void communication_send_remote_control(void) { if (global_data.param[PARAM_SEND_SLOT_REMOTE_CONTROL] == 1) { mavlink_msg_rc_channels_raw_send(global_data.param[PARAM_SEND_DEBUGCHAN], radio_control_get_channel_raw(1), radio_control_get_channel_raw(2), radio_control_get_channel_raw(3), radio_control_get_channel_raw(4), radio_control_get_channel_raw(5), radio_control_get_channel_raw(6), radio_control_get_channel_raw(7), radio_control_get_channel_raw(8), ((radio_control_status() > 0) ? 255 : 0)); // Should be global_data.rc_rssi in the future // rc_to_255(1), // rc_to_255(2), // rc_to_255(3), // rc_to_255(4), // rc_to_255(5), // rc_to_255(6), // rc_to_255(7), // rc_to_255(8), } }
inline void remote_control(void) { static uint32_t lossCounter = 0; if (global_data.state.mav_mode & (uint8_t) MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (radio_control_status() == RADIO_CONTROL_ON) { global_data.state.remote_ok=1; if (lossCounter > 0) { debug_message_buffer("REGAINED REMOTE SIGNAL AFTER LOSS!"); } lossCounter = 0; //get remote controll values float gas_remote = PPM_SCALE_FACTOR * (ppm_get_channel( global_data.param[PARAM_PPM_THROTTLE_CHANNEL]) - PPM_OFFSET); // message_debug_send(MAVLINK_COMM_1, 12, gas_remote); float yaw_remote = PPM_SCALE_FACTOR * (ppm_get_channel( global_data.param[PARAM_PPM_YAW_CHANNEL]) - PPM_CENTRE); // message_debug_send(MAVLINK_COMM_1, 13, yaw_remote); float nick_remote = PPM_SCALE_FACTOR * (ppm_get_channel( global_data.param[PARAM_PPM_NICK_CHANNEL]) - PPM_CENTRE); // message_debug_send(MAVLINK_COMM_1, 12, gas_remote); float roll_remote = PPM_SCALE_FACTOR * (ppm_get_channel( global_data.param[PARAM_PPM_ROLL_CHANNEL]) - PPM_CENTRE); // message_debug_send(MAVLINK_COMM_1, 13, yaw_remote); // if(radio_control_status()==RADIO_CONTROL_OFF){ // gas_remote=0.3; // yaw_remote=0; // nick_remote=0; // roll_remote=0; // } //MANUAL ATTITUDE CONTROL global_data.attitude_setpoint_remote.x = -roll_remote; global_data.attitude_setpoint_remote.y = -nick_remote; global_data.attitude_setpoint_remote.z = -5 * yaw_remote;// used as yaw speed!!! yaw offset1 global_data.gas_remote = gas_remote; //MANUAL POSITION CONTROL //TODO // Switch on MAV_STATE_ACTIVE if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL]) < PPM_LOW_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_YAW_CHANNEL]) < PPM_LOW_TRIG)) { // //Reset YAW integration(only needed if no vision) // global_data.attitude.z = 0; // global_data.yaw_pos_setpoint = 0; if (global_data.state.status != MAV_STATE_ACTIVE) { debug_message_buffer("MAV_STATE_ACTIVE Motors started"); //debug_message_buffer("CALIBRATING GYROS"); //start_gyro_calibration(); } //switch on motors global_data.state.status = MAV_STATE_ACTIVE; global_data.state.fly = FLY_WAIT_MOTORS; global_data.state.mav_mode |= MAV_MODE_FLAG_SAFETY_ARMED; //this will be done by setpoint if (global_data.state.mav_mode & MAV_MODE_FLAG_TEST_ENABLED) { global_data.state.fly = FLY_FLYING; } } // Switch off MAV_STATE_STANDBY if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL]) < PPM_LOW_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG)) { if (global_data.state.status != MAV_STATE_STANDBY) { debug_message_buffer("MAV_STATE_STANDBY Motors off"); } //switch off motors global_data.state.status = MAV_STATE_STANDBY; global_data.state.fly = FLY_GROUNDED; global_data.state.mav_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } // Start Gyro calibration left stick: left down. right stick right down. if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL]) < PPM_LOW_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_NICK_CHANNEL]) < PPM_LOW_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_ROLL_CHANNEL]) < PPM_LOW_TRIG)) { start_gyro_calibration(); } // Start capture: left down. right stick right up. if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL]) < PPM_LOW_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_NICK_CHANNEL]) > PPM_HIGH_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_ROLL_CHANNEL]) < PPM_LOW_TRIG)) { // FIXME LORENZ CAPTURE START //should actually go to capturer not IMU } // Stop capture: left down. right stick right up. if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL]) < PPM_LOW_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_NICK_CHANNEL]) > PPM_HIGH_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_ROLL_CHANNEL]) > PPM_HIGH_TRIG)) { // FIXME LORENZ CAPTURE END //should actually go to capturer not IMU } if (global_data.state.mav_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { // Reset position to 0,0, mostly useful for optical flow with setpoint at 0,0 if (ppm_get_channel(global_data.param[PARAM_PPM_TUNE4_CHANNEL]) < PPM_LOW_TRIG) { global_data.position.x = 0; global_data.position.y = 0; } } if (global_data.state.mav_mode & MAV_MODE_FLAG_TEST_ENABLED) { if (ppm_get_channel(global_data.param[PARAM_PPM_TUNE1_CHANNEL]) < PPM_LOW_TRIG) { //z-controller disabled global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 0; } else { //z-controller enabled global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 1; } if (ppm_get_channel(global_data.param[PARAM_PPM_TUNE4_CHANNEL]) < PPM_LOW_TRIG) { //low - Position Hold off global_data.param[PARAM_MIX_POSITION_WEIGHT] = 0; global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 0; global_data.position.x = 0; global_data.position.y = 0; } else if (ppm_get_channel( global_data.param[PARAM_PPM_TUNE4_CHANNEL]) > PPM_HIGH_TRIG) { //high - Position Hold with setpoint movement TODO global_data.param[PARAM_MIX_POSITION_WEIGHT] = 1; global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 1; } else { //center - Position Hold global_data.param[PARAM_MIX_POSITION_WEIGHT] = 1; global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 1; } } // //Integrate YAW setpoint // if (fabs( // (ppm_get_channel(global_data.param[PARAM_PPM_GIER_CHANNEL]) // - PPM_CENTRE)) > 10) // { // global_data.yaw_pos_setpoint -= 0.02 * 0.03 // * (ppm_get_channel( // global_data.param[PARAM_PPM_GIER_CHANNEL]) // - PPM_CENTRE); // } // Set PID VALUES float tune2 = 1 * (ppm_get_channel( global_data.param[PARAM_PPM_TUNE2_CHANNEL]) - 1109); float tune3 = 1 * (ppm_get_channel( global_data.param[PARAM_PPM_TUNE3_CHANNEL]) - 1109); if (tune2 < 0) { tune2 = 0; } if (tune2 > 1000) { tune2 = 1000; } if (tune3 < 0) { tune3 = 0; } if (tune3 > 1000) { tune3 = 1000; } //TUNING FOR TOBIS REMOTE CONTROL if (global_data.param[PARAM_TRIMCHAN] == 1) { global_data.param[PARAM_PID_ATT_P] = 0.1 * tune2; // global_data.param[PARAM_PID_ATT_I] = 0; global_data.param[PARAM_PID_ATT_D] = 0.1 * tune3; } else if (global_data.param[PARAM_TRIMCHAN] == 2) { global_data.param[PARAM_PID_POS_P] = 0.01 * tune2; // global_data.param[PARAM_PID_POS_I] = 0; global_data.param[PARAM_PID_POS_D] = 0.01 * tune3; } else if (global_data.param[PARAM_TRIMCHAN] == 3) { global_data.param[PARAM_PID_POS_Z_P] = 0.01 * tune2; // global_data.param[PARAM_PID_POS_Z_I] = 0; global_data.param[PARAM_PID_POS_Z_D] = 0.01 * tune3; } else if (global_data.param[PARAM_TRIMCHAN] == 4) { global_data.param[PARAM_PID_YAWSPEED_P] = 0.1 * tune2; // global_data.param[PARAM_PID_YAWSPEED_I] = 0; global_data.param[PARAM_PID_YAWSPEED_D] = 0.1 * tune3; } else if (global_data.param[PARAM_TRIMCHAN] == 5) { global_data.param[PARAM_PID_YAWPOS_P] = 0.1 * tune2; // global_data.param[PARAM_PID_YAWPOS_I] = 0; global_data.param[PARAM_PID_YAWPOS_D] = 0.1 * tune3; } //this is done at 10 Hz // pid_set_parameters(&nick_controller, // global_data.param[PARAM_PID_ATT_P], // global_data.param[PARAM_PID_ATT_I], // global_data.param[PARAM_PID_ATT_D], // global_data.param[PARAM_PID_ATT_AWU]); // pid_set_parameters(&roll_controller, // global_data.param[PARAM_PID_ATT_P], // global_data.param[PARAM_PID_ATT_I], // global_data.param[PARAM_PID_ATT_D], // global_data.param[PARAM_PID_ATT_AWU]); // // pid_set_parameters(&x_axis_controller, // global_data.param[PARAM_PID_POS_P], // global_data.param[PARAM_PID_POS_I], // global_data.param[PARAM_PID_POS_D], // global_data.param[PARAM_PID_POS_AWU]); // pid_set_parameters(&y_axis_controller, // global_data.param[PARAM_PID_POS_P], // global_data.param[PARAM_PID_POS_I], // global_data.param[PARAM_PID_POS_D], // global_data.param[PARAM_PID_POS_AWU]); //global_data.param_name[PARAM_MIX_REMOTE_WEIGHT] = 1;// add position only keep - tune3; //global_data.param[PARAM_MIX_POSITION_WEIGHT] = 1; if (global_data.param[PARAM_PPM_TUNE1_CHANNEL] != -1) { global_data.param[PARAM_CAM_ANGLE_Y_OFFSET] = ((float) (ppm_get_channel( global_data.param[PARAM_PPM_TUNE1_CHANNEL])) - 1000.0f) / 1000.0f; } } else { //No Remote signal lossCounter++; if (global_data.state.remote_ok == 1) { //Wait one round and start sinking global_data.state.remote_ok = 0; debug_message_buffer("No remote signal (1st time)"); } else { static uint16_t countdown; //already the second time // Emergency Landing if (global_data.state.fly != FLY_GROUNDED && global_data.state.fly != FLY_RAMP_DOWN && global_data.state.fly != FLY_LANDING) { debug_message_buffer_sprintf("global_data.state.fly=%i",global_data.state.fly); sys_set_state(MAV_STATE_EMERGENCY); global_data.state.fly = FLY_LANDING;//start landing debug_message_buffer( "EMERGENCY LANDING STARTED. No remote signal"); countdown = 50 * 5; // 5 seconds } else { if (global_data.state.fly == FLY_GROUNDED || global_data.state.fly == FLY_WAIT_MOTORS) { if (global_data.state.mav_mode & MAV_MODE_FLAG_SAFETY_ARMED) { if (lossCounter < 5) { debug_message_buffer( "EMERGENCY LANDING FINISHED. No remote signal"); debug_message_buffer( "EMERGENCY LANDING NOW LOCKED"); } } // Set to disarmed sys_set_mode(global_data.state.mav_mode & ~MAV_MODE_FLAG_SAFETY_ARMED); sys_set_state(MAV_STATE_STANDBY); } else { //won't come here anymore if once in locked mode debug_message_buffer( "EMERGENCY LANDING. No remote signal"); } } if((global_data.state.mav_mode & MAV_MODE_FLAG_TEST_ENABLED) && global_data.state.fly != FLY_GROUNDED){ //z-controller enabled global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 1; global_data.position_setpoint.z = global_data.position.z + 0.01; global_data.param[PARAM_POSITION_SETPOINT_Z] = global_data.position.z + 0.01; if (countdown-- <= 1) { global_data.state.fly = FLY_GROUNDED; //global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 0; debug_message_buffer( "EMERGENCY LANDING Z control finished"); } } } } } }
inline void remote_control(void) { if (global_data.state.mav_mode == (uint8_t) MAV_MODE_MANUAL || global_data.state.mav_mode == (uint8_t) MAV_MODE_GUIDED || global_data.state.mav_mode == (uint8_t) MAV_MODE_AUTO) { if (radio_control_status() == RADIO_CONTROL_ON) { global_data.state.remote_ok=1; //get remote controll values float gas_remote = PPM_SCALE_FACTOR * (ppm_get_channel( global_data.param[PARAM_PPM_THROTTLE_CHANNEL]) - PPM_OFFSET); // message_debug_send(MAVLINK_COMM_1, 12, gas_remote); float yaw_remote = PPM_SCALE_FACTOR * (ppm_get_channel( global_data.param[PARAM_PPM_YAW_CHANNEL]) - PPM_CENTRE); // message_debug_send(MAVLINK_COMM_1, 13, yaw_remote); float nick_remote = PPM_SCALE_FACTOR * (ppm_get_channel( global_data.param[PARAM_PPM_NICK_CHANNEL]) - PPM_CENTRE); // message_debug_send(MAVLINK_COMM_1, 12, gas_remote); float roll_remote = PPM_SCALE_FACTOR * (ppm_get_channel( global_data.param[PARAM_PPM_ROLL_CHANNEL]) - PPM_CENTRE); // message_debug_send(MAVLINK_COMM_1, 13, yaw_remote); // if(radio_control_status()==RADIO_CONTROL_OFF){ // gas_remote=0.3; // yaw_remote=0; // nick_remote=0; // roll_remote=0; // } //MANUAL ATTITUDE CONTROL global_data.attitude_setpoint_remote.x = -roll_remote; global_data.attitude_setpoint_remote.y = -nick_remote; global_data.attitude_setpoint_remote.z = -5 * yaw_remote;// used as yaw speed!!! yaw offset1 global_data.gas_remote = gas_remote; //MANUAL POSITION CONTROL //TODO // Switch on MAV_STATE_ACTIVE if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL]) < PPM_LOW_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_YAW_CHANNEL]) < PPM_LOW_TRIG)) { // //Reset YAW integration(only needed if no vision) // global_data.attitude.z = 0; // global_data.yaw_pos_setpoint = 0; if (global_data.state.status != MAV_STATE_ACTIVE) { debug_message_buffer("MAV_STATE_ACTIVE Motors started"); } //switch on motors global_data.state.status = MAV_STATE_ACTIVE; // global_data.state.fly = FLY_WAIT_MOTORS; // this will be done by setpoint } // Switch off MAV_STATE_STANDBY if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL]) < PPM_LOW_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG)) { if (global_data.state.status != MAV_STATE_STANDBY) { debug_message_buffer("MAV_STATE_STANDBY Motors off"); } //switch off motors global_data.state.status = MAV_STATE_STANDBY; } // Start Gyro calibration left stick: left down. right stick right down. if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL]) < PPM_LOW_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_NICK_CHANNEL]) < PPM_LOW_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_ROLL_CHANNEL]) < PPM_LOW_TRIG)) { start_gyro_calibration(); } // Start capture: left down. right stick right up. if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL]) < PPM_LOW_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_NICK_CHANNEL]) > PPM_HIGH_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_ROLL_CHANNEL]) < PPM_LOW_TRIG)) { mavlink_msg_action_send( global_data.param[PARAM_SEND_DEBUGCHAN], global_data.param[PARAM_SYSTEM_ID],MAV_COMP_ID_IMU, MAV_ACTION_REC_START); //should actually go to capturer not IMU } // Stop capture: left down. right stick right up. if ((ppm_get_channel(global_data.param[PARAM_PPM_THROTTLE_CHANNEL]) < PPM_LOW_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_YAW_CHANNEL]) > PPM_HIGH_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_NICK_CHANNEL]) > PPM_HIGH_TRIG) && (ppm_get_channel( global_data.param[PARAM_PPM_ROLL_CHANNEL]) > PPM_HIGH_TRIG)) { mavlink_msg_action_send( global_data.param[PARAM_SEND_DEBUGCHAN], global_data.param[PARAM_SYSTEM_ID], MAV_COMP_ID_IMU, MAV_ACTION_REC_STOP); //should actually go to capturer not IMU } // //Integrate YAW setpoint // if (fabs( // (ppm_get_channel(global_data.param[PARAM_PPM_GIER_CHANNEL]) // - PPM_CENTRE)) > 10) // { // global_data.yaw_pos_setpoint -= 0.02 * 0.03 // * (ppm_get_channel( // global_data.param[PARAM_PPM_GIER_CHANNEL]) // - PPM_CENTRE); // } // Set PID VALUES float tune2 = 1 * (ppm_get_channel( global_data.param[PARAM_PPM_TUNE2_CHANNEL]) - 1109); float tune3 = 1 * (ppm_get_channel( global_data.param[PARAM_PPM_TUNE3_CHANNEL]) - 1109); if (tune2 < 0) { tune2 = 0; } if (tune2 > 1000) { tune2 = 1000; } if (tune3 < 0) { tune3 = 0; } if (tune3 > 1000) { tune3 = 1000; } //TUNING FOR TOBIS REMOTE CONTROL if (global_data.param[PARAM_TRIMCHAN] == 1) { global_data.param[PARAM_PID_ATT_P] = 0.1 * tune2; // global_data.param[PARAM_PID_ATT_I] = 0; global_data.param[PARAM_PID_ATT_D] = 0.1 * tune3; } else if (global_data.param[PARAM_TRIMCHAN] == 2) { global_data.param[PARAM_PID_POS_P] = 0.01 * tune2; // global_data.param[PARAM_PID_POS_I] = 0; global_data.param[PARAM_PID_POS_D] = 0.01 * tune3; } else if (global_data.param[PARAM_TRIMCHAN] == 3) { global_data.param[PARAM_PID_POS_Z_P] = 0.01 * tune2; // global_data.param[PARAM_PID_POS_Z_I] = 0; global_data.param[PARAM_PID_POS_Z_D] = 0.01 * tune3; } else if (global_data.param[PARAM_TRIMCHAN] == 4) { global_data.param[PARAM_PID_YAWSPEED_P] = 0.1 * tune2; // global_data.param[PARAM_PID_YAWSPEED_I] = 0; global_data.param[PARAM_PID_YAWSPEED_D] = 0.1 * tune3; } else if (global_data.param[PARAM_TRIMCHAN] == 5) { global_data.param[PARAM_PID_YAWPOS_P] = 0.1 * tune2; // global_data.param[PARAM_PID_YAWPOS_I] = 0; global_data.param[PARAM_PID_YAWPOS_D] = 0.1 * tune3; } //this is done at 10 Hz // pid_set_parameters(&nick_controller, // global_data.param[PARAM_PID_ATT_P], // global_data.param[PARAM_PID_ATT_I], // global_data.param[PARAM_PID_ATT_D], // global_data.param[PARAM_PID_ATT_AWU]); // pid_set_parameters(&roll_controller, // global_data.param[PARAM_PID_ATT_P], // global_data.param[PARAM_PID_ATT_I], // global_data.param[PARAM_PID_ATT_D], // global_data.param[PARAM_PID_ATT_AWU]); // // pid_set_parameters(&x_axis_controller, // global_data.param[PARAM_PID_POS_P], // global_data.param[PARAM_PID_POS_I], // global_data.param[PARAM_PID_POS_D], // global_data.param[PARAM_PID_POS_AWU]); // pid_set_parameters(&y_axis_controller, // global_data.param[PARAM_PID_POS_P], // global_data.param[PARAM_PID_POS_I], // global_data.param[PARAM_PID_POS_D], // global_data.param[PARAM_PID_POS_AWU]); //global_data.param_name[PARAM_MIX_REMOTE_WEIGHT] = 1;// add position only keep - tune3; //global_data.param[PARAM_MIX_POSITION_WEIGHT] = 1; if (global_data.param[PARAM_PPM_TUNE1_CHANNEL] != -1) { global_data.param[PARAM_CAM_ANGLE_Y_OFFSET] = ((float) (ppm_get_channel( global_data.param[PARAM_PPM_TUNE1_CHANNEL])) - 1000.0f) / 1000.0f; } } else { //No Remote signal if (global_data.state.remote_ok == 1) { //Wait one round and start sinking global_data.state.remote_ok = 0; debug_message_buffer("No remote signal (1st time)"); } else { //already the second time // Emergency Landing if (global_data.state.fly != FLY_GROUNDED && global_data.state.fly != FLY_RAMP_DOWN) { sys_set_state(MAV_STATE_EMERGENCY); global_data.state.fly = FLY_LANDING;//start landing debug_message_buffer( "EMERGENCY LANDING STARTED. No remote signal"); } else { if (global_data.state.fly == FLY_GROUNDED) { sys_set_mode(MAV_MODE_LOCKED); sys_set_state(MAV_STATE_STANDBY); debug_message_buffer( "EMERGENCY LANDING FINISHED. No remote signal"); debug_message_buffer( "EMERGENCY LANDING NOW LOCKED"); } else { //won't come here anymore if once in locked mode debug_message_buffer( "EMERGENCY LANDING. No remote signal"); } } } } } }
/** * @brief Initialize the whole system * * All functions that need to be called before the first mainloop iteration * should be placed here. */ void main_init_generic(void) { // Reset to safe values global_data_reset(); // Load default eeprom parameters as fallback global_data_reset_param_defaults(); // LOWLEVEL INIT, ONLY VERY BASIC SYSTEM FUNCTIONS hw_init(); enableIRQ(); led_init(); led_on(LED_GREEN); buzzer_init(); sys_time_init(); sys_time_periodic_init(); sys_time_clock_init(); ppm_init(); pwm_init(); // Lowlevel periphel support init adc_init(); // FIXME SDCARD // MMC_IO_Init(); spi_init(); i2c_init(); // Sensor init sensors_init(); debug_message_buffer("Sensor initialized"); // Shutter init shutter_init(); shutter_control(0); // Debug output init debug_message_init(); debug_message_buffer("Text message buffer initialized"); // MEDIUM LEVEL INIT, INITIALIZE I2C, EEPROM, WAIT FOR MOTOR CONTROLLERS // Try to reach the EEPROM eeprom_check_start(); // WAIT FOR 2 SECONDS FOR THE USER TO NOT TOUCH THE UNIT while (sys_time_clock_get_time_usec() < 2000000) { } // Do the auto-gyro calibration for 1 second // Get current temperature led_on(LED_RED); gyro_init(); // uint8_t timeout = 3; // // Check for SD card // while (sys_time_clock_get_time_usec() < 2000000) // { // while (GetDriveInformation() != F_OK && timeout--) // { // debug_message_buffer("MMC/SD-Card not found ! retrying.."); // } // } // // if (GetDriveInformation() == F_OK) // { // debug_message_buffer("MMC/SD-Card SUCCESS: FOUND"); // } // else // { // debug_message_buffer("MMC/SD-Card FAILURE: NOT FOUND"); // } //FIXME redo init because of SD driver decreasing speed //spi_init(); led_off(LED_RED); // Stop trying to reach the EEPROM - if it has not been found by now, assume // there is no EEPROM mounted if (eeprom_check_ok()) { param_read_all(); debug_message_buffer("EEPROM detected - reading parameters from EEPROM"); for (int i = 0; i < ONBOARD_PARAM_COUNT * 2 + 20; i++) { param_handler(); //sleep 1 ms sys_time_wait(1000); } } else { debug_message_buffer("NO EEPROM - reading onboard parameters from FLASH"); } // Set mavlink system mavlink_system.compid = MAV_COMP_ID_IMU; mavlink_system.sysid = global_data.param[PARAM_SYSTEM_ID]; //Magnet sensor hmc5843_init(); acc_init(); // Comm parameter init mavlink_system.sysid = global_data.param[PARAM_SYSTEM_ID]; // System ID, 1-255 mavlink_system.compid = global_data.param[PARAM_COMPONENT_ID]; // Component/Subsystem ID, 1-255 // Comm init has to be // AFTER PARAM INIT comm_init(MAVLINK_COMM_0); comm_init(MAVLINK_COMM_1); // UART initialized, now initialize COMM peripherals communication_init(); gps_init(); us_run_init(); servos_init(); //position_kalman3_init(); // Calibration starts (this can take a few seconds) // led_on(LED_GREEN); // led_on(LED_RED); // Read out first time battery global_data.battery_voltage = battery_get_value(); global_data.state.mav_mode = MAV_MODE_PREFLIGHT; global_data.state.status = MAV_STATE_CALIBRATING; send_system_state(); float_vect3 init_state_accel; init_state_accel.x = 0.0f; init_state_accel.y = 0.0f; init_state_accel.z = -1000.0f; float_vect3 init_state_magnet; init_state_magnet.x = 1.0f; init_state_magnet.y = 0.0f; init_state_magnet.z = 0.0f; //auto_calibration(); attitude_observer_init(init_state_accel, init_state_magnet); debug_message_buffer("Attitude Filter initialized"); led_on(LED_RED); send_system_state(); debug_message_buffer("System is initialized"); // Calibration stopped led_off(LED_RED); global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; global_data.state.status = MAV_STATE_STANDBY; send_system_state(); debug_message_buffer("Checking if remote control is switched on:"); // Initialize remote control status remote_control(); remote_control(); if (radio_control_status() == RADIO_CONTROL_ON && global_data.state.remote_ok) { global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED; debug_message_buffer("RESULT: remote control switched ON"); debug_message_buffer("Now in MAV_MODE_TEST2 position hold tobi_laurens"); led_on(LED_GREEN); } else { global_data.state.mav_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; debug_message_buffer("RESULT: remote control switched OFF"); led_off(LED_GREEN); } }