/* * initialize_components.c * * Created on: Feb 20, 2016 * Author: ucart */ #include "initialize_components.h" #include "communication.h" //#define BENCH_TEST extern int Xil_AssertWait; int protection_loops(modular_structs_t *structs) { int rc_commands[6]; // 6 "receiver_input" elements: Throttle, Pitch, Roll, Yaw, Gear, and Flap read_rec_all(rc_commands); // wait for RC receiver to connect to transmitter while(rc_commands[THROTTLE] < 75000) read_rec_all(rc_commands); // wait until throttle is low and the gear switch is engaged (so you don't immediately break out of the main loop below) // also wait for the flight mode to be set to manual while(rc_commands[THROTTLE] > 125000 || read_kill(rc_commands[GEAR]) || !read_flap(rc_commands[FLAP])) read_rec_all(rc_commands); // wait until the ground station has connected to the quad and acknowledged that its ready to start char buf[255] = {}; int length = snprintf(buf, sizeof(buf), "The quad is ready to receive VRPN data.\r\n"); length = length >= sizeof(buf) ? 255 : length; while (!structs->user_input_struct.receivedBeginUpdate) { send_data(MessageTypes[4].ID, MessageTypes[4].subtypes[1].ID, 0, buf, length); process_received(structs); usleep(10000); } // let the pilot/observers know that the quad is now active MIO7_led_on(); return 0; } int initializeAllComponents(user_input_t * user_input_struct, log_t * log_struct, raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct, setpoint_t * setpoint_struct, parameter_t * parameter_struct, user_defined_t *user_defined_struct, raw_actuator_t * raw_actuator_struct, actuator_command_t * actuator_command_struct) { // Turn off LED 7 to let observers know that the quad is not yet active MIO7_led_off(); // Initialize the controller control_algorithm_init(parameter_struct); // Xilinx given initialization init_platform(); // Initialize loop timers if (timer_init()) { return -1; } // Initialize UART0 (Bluetooth) if (initUartComms()) { return -1; } // Initialize I2C controller and start the sensor board if (initI2C0() == -1) { return -1; } // Initialize PWM Recorders and Motor outputs pwm_init(); //manual flight mode user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE; #ifndef BENCH_TEST // Get the first loop data from accelerometer for the gyroscope to use if(sensor_init(raw_sensor_struct, sensor_struct) == -1) return -1; #endif return 0; }