bool calibration_enter(void) { // If not flying if (!sys_state_is_flying()) { calibration_prev_state = sys_get_state(); calibration_prev_mode = sys_get_mode(); // Lock vehicle during calibration sys_set_mode((uint8_t)MAV_MODE_LOCKED); sys_set_state((uint8_t)MAV_STATE_CALIBRATING); debug_message_buffer("Starting calibration."); mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode, global_data.state.status, global_data.cpu_usage, global_data.battery_voltage, global_data.motor_block, communication_get_uart_drop_rate()); mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode, global_data.state.status, global_data.cpu_usage, global_data.battery_voltage, global_data.motor_block, communication_get_uart_drop_rate()); debug_message_send_one(); debug_message_send_one(); return true; } else { //Can't calibrate during flight debug_message_buffer("Can't calibrate during flight!!!"); return false; } }
void execute_action(uint8_t action) { switch (action) { case MAV_ACTION_LAUNCH: if (global_data.state.mav_mode > (uint8_t)MAV_MODE_LOCKED) { global_data.state.status = (uint8_t)MAV_STATE_ACTIVE; } break; case MAV_ACTION_MOTORS_START: if (global_data.state.mav_mode > (uint8_t)MAV_MODE_LOCKED) { global_data.state.status = (uint8_t)MAV_STATE_ACTIVE; } break; case MAV_ACTION_MOTORS_STOP: global_data.state.status = (uint8_t)MAV_STATE_STANDBY; break; case MAV_ACTION_EMCY_KILL: global_data.state.status = (uint8_t)MAV_STATE_EMERGENCY; break; case MAV_ACTION_STORAGE_READ: param_read_all(); debug_message_buffer("Started reading params from eeprom"); break; case MAV_ACTION_STORAGE_WRITE: debug_message_buffer("Started writing params to eeprom"); param_write_all(); break; case MAV_ACTION_CALIBRATE_GYRO: start_gyro_calibration(); break; case MAV_ACTION_CALIBRATE_RC: start_rc_calibration(); break; case MAV_ACTION_CALIBRATE_MAG: start_mag_calibration(); break; case MAV_ACTION_CALIBRATE_PRESSURE: start_pressure_calibration(); break; case MAV_ACTION_SET_ORIGIN: // If not flying if (!sys_state_is_flying()) { gps_set_local_origin(); altitude_set_local_origin(); } break; default: // Should never be reached, ignore unknown commands debug_message_buffer_sprintf("Rejected unknown action Number: %u", action); break; } }
void handle_eeprom_write_request(void) { //GET UPDATES FROM PARAMS //testing eeprom params if (global_data.state.status == MAV_STATE_STANDBY) { switch ((uint8_t) global_data.param[PARAM_IMU_RESET]) { case 2: debug_message_buffer("start writing params to eeprom"); param_write_all(); global_data.param[PARAM_IMU_RESET] = 0; break; case 3: param_read_all(); debug_message_buffer("start reading params from eeprom"); global_data.param[PARAM_IMU_RESET] = 0; break; case 4: global_data.param[PARAM_IMU_RESET] = 0; debug_message_buffer("params reset to defaults"); global_data_reset_param_defaults(); break; case 5: global_data.param[PARAM_IMU_RESET] = 0; if (param_size_check()) { debug_message_buffer("check true"); } else { debug_message_buffer("check false"); } break; case 6: global_data.param[PARAM_IMU_RESET] = 0; eeprom_check_start(); break; default: break; } } }
void gps_set_local_origin(void) { gps_local_origin.x = gps_lat / 1e7f; gps_local_origin.y = gps_lon / 1e7f; gps_local_origin.z = gps_alt / 100e0f; gps_cos_origin_lat = cos(gps_local_origin.x * 3.1415 / 180); gps_local_origin_init = true; debug_message_buffer("GPS Local Origin saved"); }
/** * @brief Set the current system state * @param state the new system state */ bool sys_set_state(uint8_t state) { if (state == MAV_STATE_ACTIVE) { global_data.state.status = MAV_STATE_ACTIVE; return true; } else if (state == MAV_STATE_BOOT) { global_data.state.status = MAV_STATE_BOOT; return true; } else if (state == MAV_STATE_CALIBRATING) { global_data.state.status = MAV_STATE_CALIBRATING; return true; } else if (state == MAV_STATE_CRITICAL) { global_data.state.status = MAV_STATE_CRITICAL; return true; } else if (state == MAV_STATE_EMERGENCY) { global_data.state.status = MAV_STATE_EMERGENCY; return true; } else if (state == MAV_STATE_POWEROFF) { global_data.state.status = MAV_STATE_POWEROFF; return true; } else if (state == MAV_STATE_STANDBY) { global_data.state.status = MAV_STATE_STANDBY; return true; } else if (state == MAV_STATE_UNINIT) { global_data.state.status = MAV_STATE_STANDBY; return true; } else { // UNINIT or invalid state, ignore value and return false debug_message_buffer("WARNING: Attempted to set invalid state"); return false; } // FIXME Remove this once new interface is existent global_data.state.status = state; }
/** * @note Buffer one debug message with a integer variable in it * @param string * */ uint8_t debug_message_buffer_sprintf(const char* string, const uint32_t num) { if (debug_message_buffer(string)) { // Could write, save integer to buffer m_debug_buf_int[m_debug_index_write] = num; return 1; } else { // Could not write, do nothing return 0; } }
/** * @brief Add a string debug message to the send buffer * * @param string Message * @return 1 if successfully added, 0 else */ uint8_t debug_string_message_buffer(const char* string) { if (debug_message_buffer(string)) { /* Could write, save message to buffer */ m_debug_buf_type[m_debug_index_write] = DEBUG_STRING; return 1; } else { /* Could not write, do nothing */ return 0; } }
void start_rc_calibration(void) { if (calibration_enter()) { // Calibration routine: Read out remote control and wait for all channels. // Read initial values. const uint8_t chan_count = 9; uint32_t chan_initial[chan_count]; uint32_t chan_min[chan_count]; uint32_t chan_max[chan_count]; for (int i = 0; i < chan_count; i++) { chan_initial[i] = ppm_get_channel(i + 1); chan_min[i]=2000; chan_max[i]=1000; } bool abort_rc_cal = 0; uint64_t timeout = sys_time_clock_get_time_usec() + 30 * 1000000; while (!abort_rc_cal) { // Now save the min and max values for each channel for (int i = 0; i < chan_count; i++) { uint32_t ppm_value = (uint32_t) ppm_get_channel(i + 1); chan_min[i] = min(chan_min[i], ppm_value); chan_min[i] = min(chan_min[i], ppm_value); } // until motors stop conditions are met. 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)) { abort_rc_cal = 1; } if(sys_time_clock_get_time_usec()>timeout){ abort_rc_cal = 1; debug_message_buffer("RC calibration abborted after 30 seconds"); } } calibration_exit(); } }
/** * @brief Add a string-float debug message to the send buffers * * @param string Message * @return 1 if successfully added, 0 else */ uint8_t debug_float_message_buffer(const char* string, float num) { if (debug_message_buffer(string)) { /* Could write, save float to buffer */ m_debug_buf_float[m_debug_index_write] = num; m_debug_buf_type[m_debug_index_write] = DEBUG_FLOAT; return 1; } else { /* Could not write, do nothing */ return 0; } }
/** * @brief Add a string-integer debug message to the send buffers * * @param string Message * @return 1 if successfully added, 0 else */ uint8_t debug_int_message_buffer(const char* string, int32_t num) { if (debug_message_buffer(string)) { /* Could write, save integer to buffer */ m_debug_buf_int[m_debug_index_write] = num; m_debug_buf_type[m_debug_index_write] = DEBUG_INT; return 1; } else { /* Could not write, do nothing */ return 0; } }
void calibration_exit(void) { // Go back to old state sys_set_mode(calibration_prev_mode); sys_set_state(calibration_prev_state); // Clear debug message buffers for (int i = 0; i < DEBUG_COUNT; i++) { debug_message_send_one(); } // Clear UART buffers while (uart0_char_available()) {uart0_get_char();} while (uart1_char_available()) {uart1_get_char();} debug_message_buffer("Calibration finished. UART buffers cleared."); }
void attitude_tobi_laurens(void) { //Transform accelerometer used in all directions // float_vect3 acc_nav; //body2navi(&global_data.accel_si, &global_data.attitude, &acc_nav); // Kalman Filter //Calculate new linearized A matrix attitude_tobi_laurens_update_a(); kalman_predict(&attitude_tobi_laurens_kal); //correction update m_elem measurement[9] = { }; m_elem mask[9] = { 1.0f, 1.0f, 1.0f, 0, 0, 0, 1.0f, 1.0f, 1.0f }; float_vect3 acc; float_vect3 mag; float_vect3 gyro; // acc.x = global_data.accel_raw.x * 9.81f /690; // acc.y = global_data.accel_raw.y * 9.81f / 690; // acc.z = global_data.accel_raw.z * 9.81f / 690; #ifdef IMU_PIXHAWK_V250 acc.x = global_data.accel_raw.x * (650.0f/900.0f); acc.y = global_data.accel_raw.y * (650.0f/900.0f); acc.z = global_data.accel_raw.z * (650.0f/900.0f); #else acc.x = global_data.accel_raw.x; acc.y = global_data.accel_raw.y; acc.z = global_data.accel_raw.z; #endif float acc_norm = sqrt(global_data.accel_raw.x * global_data.accel_raw.x + global_data.accel_raw.y * global_data.accel_raw.y + global_data.accel_raw.z * global_data.accel_raw.z); static float acc_norm_filt = SCA3100_COUNTS_PER_G; float acc_norm_lp = 0.05f; acc_norm_filt = (1.0f - acc_norm_lp) * acc_norm_filt + acc_norm_lp * acc_norm; // static float acc_norm_filtz = SCA3100_COUNTS_PER_G; // float acc_norm_lpz = 0.05; // acc_norm_filtz = (1.0f - acc_norm_lpz) * acc_norm_filtz + acc_norm_lpz * -acc.z; float acc_diff = fabs(acc_norm_filt - SCA3100_COUNTS_PER_G); if (acc_diff > 200.0f) { //Don't correct when acc high mask[0] = 0; mask[1] = 0; mask[2] = 0; } else if (acc_diff > 100.0f) { //fade linearely out between 100 and 200 float mask_lin = (200.0f - acc_diff) / 100.0f; mask[0] = mask_lin; mask[1] = mask_lin; mask[2] = mask_lin; } //else mask stays 1 // static uint8_t i = 0; // if (i++ > 10) // { // i = 0; // float_vect3 debug; // debug.x = mask[0]; // debug.y = acc_norm; // debug.z = acc_norm_filt; // debug_vect("acc_norm", debug); // } // mag.x = (global_data.magnet_corrected.x ) * 1.f / 510.f; // mag.y = (global_data.magnet_corrected.y) * 1.f / 510.f; // mag.z = (global_data.magnet_corrected.z) * 1.f / 510.f; mag.x = (global_data.magnet_corrected.x ) ; mag.y = (global_data.magnet_corrected.y) ; mag.z = (global_data.magnet_corrected.z) ; // gyro.x = -(global_data.gyros_raw.x-global_data.param[PARAM_GYRO_OFFSET_X]) * 0.000955; // gyro.y = (global_data.gyros_raw.y-global_data.param[PARAM_GYRO_OFFSET_Y]) * 0.000955; // gyro.z = -(global_data.gyros_raw.z-global_data.param[PARAM_GYRO_OFFSET_Z]) * 0.001010; gyro.x = -(global_data.gyros_raw.x-global_data.param[PARAM_GYRO_OFFSET_X]) * 0.001008f; gyro.y = (global_data.gyros_raw.y-global_data.param[PARAM_GYRO_OFFSET_Y]) * 0.001008f; gyro.z = -(global_data.gyros_raw.z-global_data.param[PARAM_GYRO_OFFSET_Z]) * 0.001010f; measurement[0] = acc.x; measurement[1] = acc.y; measurement[2] = acc.z; if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VISION) { measurement[3] = global_data.vision_magnetometer_replacement.x; measurement[4] = global_data.vision_magnetometer_replacement.y; measurement[5] = global_data.vision_magnetometer_replacement.z; } else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_GLOBAL_VISION) { // measurement[3] = global_data.vision_global_magnetometer_replacement.x; // measurement[4] = global_data.vision_global_magnetometer_replacement.y; // measurement[5] = global_data.vision_global_magnetometer_replacement.z; } else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VICON) { measurement[3] = global_data.vicon_magnetometer_replacement.x; measurement[4] = global_data.vicon_magnetometer_replacement.y; measurement[5] = global_data.vicon_magnetometer_replacement.z; } else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_INTEGRATION) { // Do nothing // KEEP THIS IN HERE } else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_MAGNETOMETER) // YAW_ESTIMATION_MODE_MAGNETOMETER { measurement[3] = mag.x; measurement[4] = mag.y; measurement[5] = mag.z; // debug_vect("mag_f", mag); } else { static uint8_t errcount = 0; if (errcount == 0) debug_message_buffer("ATT EST. ERROR: No valid yaw estimation mode set!"); errcount++; global_data.state.status = MAV_STATE_CRITICAL; } measurement[6] = gyro.x; measurement[7] = gyro.y; measurement[8] = gyro.z; //Put measurements into filter static int j = 0; // MASK if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_INTEGRATION) { // Do nothing, pure integration } else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VICON) { // If our iteration count is right and new vicon data is available, update measurement if (j >= 3 && global_data.state.vicon_attitude_new_data == 1) { j = 0; mask[3]=1; mask[4]=1; mask[5]=1; } else { j++; } global_data.state.vicon_attitude_new_data = 0; } else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VISION) { // If the iteration count is right and new vision data is available, update measurement if (j >= 3 && global_data.state.vision_attitude_new_data == 1) { j = 0; mask[3]=1; mask[4]=1; mask[5]=1; } else { j++; } global_data.state.vision_attitude_new_data = 0; } else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_GLOBAL_VISION) { // // If the iteration count is right and new vision data is available, update measurement // if (j >= 3 && global_data.state.global_vision_attitude_new_data == 1) // { // j = 0; // // mask[3]=1; // mask[4]=1; // mask[5]=1; // j=0; // } // else // { // j++; // } // global_data.state.global_vision_attitude_new_data = 0; } else { if (j >= 3 && global_data.state.magnet_ok) { j = 0; mask[3]=1; mask[4]=1; mask[5]=1; } else { j++; } } kalman_correct(&attitude_tobi_laurens_kal, measurement, mask); //debug // save outputs float_vect3 kal_acc, kal_mag, kal_w0, kal_w; kal_acc.x = kalman_get_state(&attitude_tobi_laurens_kal, 0); kal_acc.y = kalman_get_state(&attitude_tobi_laurens_kal, 1); kal_acc.z = kalman_get_state(&attitude_tobi_laurens_kal, 2); kal_mag.x = kalman_get_state(&attitude_tobi_laurens_kal, 3); kal_mag.y = kalman_get_state(&attitude_tobi_laurens_kal, 4); kal_mag.z = kalman_get_state(&attitude_tobi_laurens_kal, 5); kal_w0.x = kalman_get_state(&attitude_tobi_laurens_kal, 6); kal_w0.y = kalman_get_state(&attitude_tobi_laurens_kal, 7); kal_w0.z = kalman_get_state(&attitude_tobi_laurens_kal, 8); kal_w.x = kalman_get_state(&attitude_tobi_laurens_kal, 9); kal_w.y = kalman_get_state(&attitude_tobi_laurens_kal, 10); kal_w.z = kalman_get_state(&attitude_tobi_laurens_kal, 11); float_vect3 x_n_b, y_n_b, z_n_b; z_n_b.x = -kal_acc.x; z_n_b.y = -kal_acc.y; z_n_b.z = -kal_acc.z; vect_norm(&z_n_b); vect_cross_product(&z_n_b, &kal_mag, &y_n_b); vect_norm(&y_n_b); vect_cross_product(&y_n_b, &z_n_b, &x_n_b); //save euler angles global_data.attitude.x = atan2f(z_n_b.y, z_n_b.z); global_data.attitude.y = -asinf(z_n_b.x); if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_INTEGRATION) { global_data.attitude.z += 0.005f * global_data.gyros_si.z; } else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_GLOBAL_VISION) { global_data.attitude.z += 0.0049f * global_data.gyros_si.z; if (global_data.state.global_vision_attitude_new_data == 1) { global_data.attitude.z = 0.995f*global_data.attitude.z + 0.005f*global_data.vision_data_global.ang.z; // Reset new data flag at roughly 1 Hz, detecting a vision timeout static uint8_t new_data_reset = 0; if (new_data_reset == 0) { global_data.state.global_vision_attitude_new_data = 0; } new_data_reset++; } } else { // static hackMagLowpass = 0.0f; global_data.attitude.z = global_data.attitude.z*0.9f+0.1f*atan2f(y_n_b.x, x_n_b.x); } //save rates global_data.attitude_rate.x = kal_w.x; global_data.attitude_rate.y = kal_w.y; global_data.attitude_rate.z = kal_w.z; global_data.yaw_lowpass = 0.99f * global_data.yaw_lowpass + 0.01f * global_data.attitude_rate.z; }
/** * @brief This is the main loop * * It will be executed at maximum MCU speed (60 Mhz) */ void main_loop_fixed_wing(void) { last_mainloop_idle = sys_time_clock_get_time_usec(); debug_message_buffer("Starting main loop"); while (1) { // Time Measurement uint64_t loop_start_time = sys_time_clock_get_time_usec(); /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// Camera Shutter /////////////////////////////////////////////////////////////////////////// // Set camera shutter with 2.5ms resolution if (us_run_every(2500, COUNTER1, loop_start_time)) { camera_shutter_handling(loop_start_time); } if (global_data.state.mav_mode == MAV_MODE_RC_TRAINING) { /////////////////////////////////////////////////////////////////////////// /// RC INTERFACE HACK AT 50 Hz /////////////////////////////////////////////////////////////////////////// if (us_run_every(20000, COUNTER8, loop_start_time)) { // Write start byte uart1_transmit(0xFF); // Write channels 1-6 for (int i = 1; i < 7; i++) { uart1_transmit((radio_control_get_channel(1)+1)*127); } } led_toggle(LED_GREEN); led_toggle(LED_RED); // Do not execute any of the functions below continue; } /////////////////////////////////////////////////////////////////////////// /// CRITICAL 200 Hz functions /////////////////////////////////////////////////////////////////////////// if (us_run_every(5000, COUNTER2, loop_start_time)) { // Kalman Attitude filter, used on all systems gyro_read(); sensors_read_acc(); // Read out magnetometer at its default 50 Hz rate static uint8_t mag_count = 0; if (mag_count == 3) { sensors_read_mag(); attitude_observer_correct_magnet(global_data.magnet_corrected); mag_count = 0; } else { mag_count++; } // Correction step of observer filter attitude_observer_correct_accel(global_data.accel_raw); // Write in roll and pitch static float_vect3 att; //if not static we have spikes in roll and pitch on bravo !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! attitude_observer_get_angles(&att); global_data.attitude.x = att.x; global_data.attitude.y = att.y; if (global_data.param[PARAM_ATT_KAL_IYAW]) { global_data.attitude.z += 0.005 * global_data.gyros_si.z; } else { global_data.attitude.z = att.z; } // Prediction step of observer attitude_observer_predict(global_data.gyros_si); control_fixed_wing_attitude(); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// CRITICAL FAST 50 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(20000, COUNTER3, loop_start_time)) { // Read Analog-to-Digital converter adc_read(); // Read remote control remote_control(); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 100 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(10000, COUNTER6, loop_start_time)) { // Send the raw sensor/ADC values communication_send_raw_data(loop_start_time); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// UNCRITICAL SLOW 5 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(200000, COUNTER8, loop_start_time)) { // The onboard controllers go into failsafe mode once // position data is missing handle_controller_timeouts(loop_start_time); // Send buffered data such as debug text messages communication_queued_send(); // Empty one message out of the buffer debug_message_send_one(); // Toggle status led //led_toggle(LED_YELLOW); led_toggle(LED_RED); // just for green LED on alpha at the moment // Toggle active mode led if (global_data.state.mav_mode == MAV_MODE_MANUAL || global_data.state.mav_mode == MAV_MODE_GUIDED || global_data.state.mav_mode == MAV_MODE_AUTO) { led_on(LED_GREEN); } else { led_off(LED_GREEN); } handle_eeprom_write_request(); handle_reset_request(); communication_send_controller_feedback(); communication_send_remote_control(); // Pressure sensor driver works, but not tested regarding stability sensors_pressure_bmp085_read_out(); if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 1) { mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 90, global_data.param[PARAM_POSITION_SETPOINT_YAW]); mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 91, global_data.yaw_pos_setpoint); } } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 1 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(1000000, COUNTER9, loop_start_time)) { // Send system state, mode, battery voltage, etc. send_system_state(); if (global_data.param[PARAM_GPS_MODE] >= 10) { //Send GPS information float_vect3 gps; gps.x = gps_utm_north / 100.0f;//m gps.y = gps_utm_east / 100.0f;//m gps.z = gps_utm_zone;// gps_week; debug_vect("GPS", gps); } beep_on_low_voltage(); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 20 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(50000, COUNTER7, loop_start_time)) { led_toggle(LED_YELLOW); if (global_data.param[PARAM_GPS_MODE] >= 10) { //get thru all gps messages debug_message_send_one(); } communication_send_attitude_position(loop_start_time); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 200 Hz functions // /////////////////////////////////////////////////////////////////////////// else if (us_run_every(5000, COUNTER5, loop_start_time)) { if (global_data.state.status == MAV_STATE_STANDBY) { //Check if parameters should be written or read param_handler(); } } /////////////////////////////////////////////////////////////////////////// else { // All Tasks are fine and we have no starvation last_mainloop_idle = loop_start_time; } // Read out comm at max rate - takes only a few microseconds in worst case communication_receive(); // MCU load measurement uint64_t loop_stop_time = sys_time_clock_get_time_usec(); global_data.cpu_usage = measure_avg_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time); global_data.cpu_peak = measure_peak_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time); if (loop_start_time - last_mainloop_idle >= 5000) { debug_message_buffer( "CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!"); last_mainloop_idle = loop_start_time;//reset to prevent multiple messages } if (global_data.cpu_usage > 800) { // CPU load higher than 80% debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%"); } } // End while(1) }
void update_system_statemachine(uint64_t loop_start_time) { // Update state machine, enable and disable controllers switch (global_data.state.mav_mode) { case MAV_MODE_MANUAL: global_data.param[PARAM_MIX_POSITION_WEIGHT] = 0; global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 0; global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 0; global_data.param[PARAM_MIX_OFFSET_WEIGHT] = 1; global_data.param[PARAM_MIX_REMOTE_WEIGHT] = 1; break; case MAV_MODE_AUTO: // Same as guided mode, NO BREAK case MAV_MODE_TEST1: // Same as guided mode, NO BREAK case MAV_MODE_GUIDED: if (global_data.state.position_fix) { if (global_data.state.status == MAV_STATE_CRITICAL) { //get active again if we are in critical //if we are in Emergency we don't want to start again!!!! global_data.state.status = MAV_STATE_ACTIVE; } } else { if (global_data.state.status == MAV_STATE_ACTIVE) { global_data.state.status = MAV_STATE_CRITICAL; global_data.entry_critical = loop_start_time; debug_message_buffer("CRITICAL! POSITION LOST"); } else if (global_data.state.status == MAV_STATE_CRITICAL && (loop_start_time - global_data.entry_critical > 3 * (uint32_t) global_data.param[PARAM_POSITION_TIMEOUT])) { //wait 3 times as long as waited for critical global_data.state.status = MAV_STATE_EMERGENCY; //Initiate Landing even if we didn't reach 0.7m global_data.state.fly=FLY_LANDING; debug_message_buffer("EMERGENCY! MAYDAY! MAYDAY! LANDING!"); } } switch (global_data.state.status) { case MAV_STATE_ACTIVE: global_data.param[PARAM_MIX_POSITION_WEIGHT] = 1; global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 1; global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 1; break; case MAV_STATE_CRITICAL: //global_data.param[PARAM_MIX_POSITION_WEIGHT] = 1; // estimate path without vision global_data.param[PARAM_MIX_POSITION_WEIGHT] = 0; // try to hover on place global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 1; global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 1; break; case MAV_STATE_EMERGENCY: global_data.param[PARAM_MIX_POSITION_WEIGHT] = 0; global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 1; global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 1; break; default: global_data.param[PARAM_MIX_POSITION_WEIGHT] = 0; global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 0; global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 0; } global_data.param[PARAM_MIX_OFFSET_WEIGHT] = 1; global_data.param[PARAM_MIX_REMOTE_WEIGHT] = 1; break; case MAV_MODE_TEST2: //allow other mix params to be set by hand global_data.param[PARAM_MIX_OFFSET_WEIGHT] = 1; global_data.param[PARAM_MIX_REMOTE_WEIGHT] = 1; break; case MAV_MODE_TEST3: // break; case MAV_MODE_LOCKED: // break; case MAV_MODE_READY: // break; case MAV_MODE_UNINIT: // break; default: global_data.param[PARAM_MIX_POSITION_WEIGHT] = 0; global_data.param[PARAM_MIX_POSITION_YAW_WEIGHT] = 0; global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] = 0; global_data.param[PARAM_MIX_OFFSET_WEIGHT] = 0; global_data.param[PARAM_MIX_REMOTE_WEIGHT] = 0; } if (global_data.state.remote_ok == 0) { global_data.param[PARAM_MIX_REMOTE_WEIGHT] = 0; } }
/* Interrupt handler for I2C0 interrupt */ static void I2C0_ISR(void) { //sprintf((char*)buffer, "h "); //message_send_debug(COMM_1, buffer); //uint8_t buffer[200]; // string buffer for debug messages ISR_ENTRY(); if (error_counter0 > I2C_PERMANENT_ERROR_LIMIT) { //uint8_t buffer[50]; //sprintf(buffer, "i2c error limit %d \n", 1); // sprintf((char*)buffer, package_buffer0[i2c_package_buffer0_current_idx].slave_address) ; //message_send_debug(COMM_1, buffer); // debug_message_buffer("i2c error limit reached"); debug_message_buffer_sprintf("i2c0 error limit reached. Dest: %i", package_buffer0[i2c_package_buffer0_current_idx].slave_address); if (i2c0_permanent_error_count++ % 256 == 0) { debug_message_buffer_sprintf( "i2c0 error limit reached. Total: %i errors.", i2c0_permanent_error_count); } package_buffer0[i2c_package_buffer0_current_idx].i2c_error_code = I2C_CODE_ERROR;//set error code to error //TODO set this to ok if everzthing was ok if (package_buffer0[i2c_package_buffer0_current_idx].i2c_done_handler != NULL) { // check if there is a package handler registered package_buffer0[i2c_package_buffer0_current_idx].i2c_done_handler( &(package_buffer0[i2c_package_buffer0_current_idx])); // call package handler } i2c_package_buffer0_insert_idx = (i2c_package_buffer0_insert_idx + 1) % I2C_PACKAGE_BUFFER_SIZE; // increment package insertion pointer error_counter0 = 0; if (i2c_package_buffer0_current_idx == i2c_package_buffer0_insert_idx) { // no unhandled packages I2C0CONCLR = 1 << STAC; I2C0CONSET = 1 << STO; // generate I2C stop condition on the bus without restart I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag i2c0_busy = 0; // release I2C resource } else { // unhandled packages in package buffer if (!(package_buffer0[i2c_package_buffer0_current_idx].write_read == 1)) { I2C0CONSET = 1 << STO; // generate stop condition on the bus if no repeated start is necessary } I2C0CONSET = 1 << STA; // generate "repeated start" condition on the bus -> next state: 0x10 I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag } } if (i2c0_busy == 1) { //Return if Package error //message_debug_send(MAVLINK_COMM_1, 30, I2C0STAT); switch (I2C0STAT) { // check current I2C0 state case 0x08: // I2C start condition has been generated on the bus -> transmit slave address and read/write bit -> next state: 0x18 or 0x40 I2C0DAT = package_buffer0[i2c_package_buffer0_current_idx].slave_address | package_buffer0[i2c_package_buffer0_current_idx].direction; I2C0CONCLR = 1 << STAC; // do not restart (continue normal I2C operation) I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag break; case 0x10: // "repeated start" condition has been generated on the bus -> transmit slave address and read/write bit -> next state: 0x18 or 0x40 I2C0DAT = package_buffer0[i2c_package_buffer0_current_idx].slave_address | package_buffer0[i2c_package_buffer0_current_idx].direction; I2C0CONCLR = 1 << STAC; I2C0CONCLR = 1 << SIC; break; case 0x18: // slave address and write bit has been transmitted -> transmit first data byte -> next state: 0x28 current_data_byte_i2c0 = 0; I2C0CONCLR = 1 << STAC; I2C0DAT = package_buffer0[i2c_package_buffer0_current_idx].data[current_data_byte_i2c0]; current_data_byte_i2c0++; I2C0CONCLR = 1 << SIC; break; case 0x28: // data byte has been transmitted // if there's another byte to be transmitted, do it if (current_data_byte_i2c0 < package_buffer0[i2c_package_buffer0_current_idx].length) { I2C0CONCLR = 1 << STAC; I2C0DAT = package_buffer0[i2c_package_buffer0_current_idx].data[current_data_byte_i2c0]; current_data_byte_i2c0++; I2C0CONCLR = 1 << SIC; } // it the last byte has been transmitted, check if there are unhandled packages in the package buffer else { i2c_package_buffer0_current_idx = (i2c_package_buffer0_current_idx + 1) % I2C_PACKAGE_BUFFER_SIZE; // increment pointer to package in processing error_counter0 = 0; if (i2c_package_buffer0_current_idx == i2c_package_buffer0_insert_idx) { // no unhandled packages I2C0CONCLR = 1 << STAC; I2C0CONSET = 1 << STO; // generate I2C stop condition on the bus without restart I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag i2c0_busy = 0; // release I2C resource } else { // unhandled packages in package buffer if (!(package_buffer0[i2c_package_buffer0_current_idx].write_read == 1)) { I2C0CONSET = 1 << STO; // generate stop condition on the bus if no repeated start is necessary } I2C0CONSET = 1 << STA; // generate "repeated start" condition on the bus -> next state: 0x10 I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag } } break; case 0x20: // I2C error state detection I2C0CONSET = 1 << STO; I2C0CONSET = 1 << STA; // restart I2C state machine with current package error_counter0++; //debug_message_buffer("I2C error: slave address not acknowledged (write)\n"); debug_message_buffer_sprintf( "I2C0 error: slave address not acknowledged (write). Dest: %i", package_buffer0[i2c_package_buffer0_current_idx].slave_address); //message_send_debug(COMM_1, buffer); I2C0CONCLR = 1 << SIC; break; case 0x30: // I2C error state detection I2C0CONSET = 1 << STO; I2C0CONSET = 1 << STA; // restart I2C state machine with current package error_counter0++; // debug_message_buffer("I2C error: data not acknowledged\n"); debug_message_buffer_sprintf( "I2C0 error: data not acknowledged. Dest: %i", package_buffer0[i2c_package_buffer0_current_idx].slave_address); //message_send_debug(COMM_1, buffer); I2C0CONCLR = 1 << SIC; break; case 0x38: // I2C error state detection I2C0CONSET = 1 << STA; // restart I2C state machine with current package error_counter0++; // debug_message_buffer("I2C error: arbitration lost\n"); debug_message_buffer_sprintf( "I2C0 error: arbitration lost. Dest: %i", package_buffer0[i2c_package_buffer0_current_idx].slave_address); //message_send_debug(COMM_1, buffer); I2C0CONCLR = 1 << SIC; break; case 0x40: // slave address and read bit has been transmitted -> clear interrupt and wait for first data byte -> next state: 0x50 or 0x58 current_data_byte_i2c0 = 0; if (package_buffer0[i2c_package_buffer0_current_idx].length > 1) I2C0CONSET = 1 << AA; // if there's more than one byte to be received -> next state: 0x50 else I2C0CONCLR = 1 << AAC; // if there's only one byte to be received -> next state: 0x58 I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag break; case 0x50: // data byte has been received package_buffer0[i2c_package_buffer0_current_idx].data[current_data_byte_i2c0] = I2C0DAT; // copy data byte to data array in I2C package current_data_byte_i2c0++; // increment data byte if ((current_data_byte_i2c0 + 1) < package_buffer0[i2c_package_buffer0_current_idx].length) { // there's more than one byte left to be received I2C0CONSET = 1 << AA; // acknowledge next data byte -> next state: 0x50 } else { // there's only one byte left to be received I2C0CONCLR = 1 << AAC; // do not acknowledge next data byte -> next state: 0x58 } I2C0CONCLR = 1 << SIC; break; case 0x58: // last data byte has been received package_buffer0[i2c_package_buffer0_current_idx].data[current_data_byte_i2c0] = I2C0DAT; // copy data byte to data array in I2C package I2C0CONSET = 1 << STO; // generate STOP condition on the I2C bus //uart0_transmit(package_buffer0[i2c_package_buffer0_current_idx].data[current_data_byte_i2c0]); if (package_buffer0[i2c_package_buffer0_current_idx].i2c_done_handler != NULL) { // check if there is a package handler registered package_buffer0[i2c_package_buffer0_current_idx].i2c_done_handler( &(package_buffer0[i2c_package_buffer0_current_idx])); // call package handler } i2c_package_buffer0_current_idx = (i2c_package_buffer0_current_idx + 1) % I2C_PACKAGE_BUFFER_SIZE; // increment pointer to package in processing error_counter0 = 0; // check if there are unhandled packages in the package buffer if (i2c_package_buffer0_current_idx == i2c_package_buffer0_insert_idx) { // no unhandled packages I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag i2c0_busy = 0; // release I2C resource } else { // unhandled packages in package buffer I2C0CONSET = 1 << STA; // generate "repeated start" condition on the bus -> next state: 0x10 I2C0CONCLR = 1 << SIC; // clear I2C interrupt flag } break; case 0x48: // I2C error state detection I2C0CONSET = 1 << STO; I2C0CONSET = 1 << STA; // restart I2C state machine with current package error_counter0++; debug_message_buffer( "I2C0 error: slave address not acknowledged (read)\n"); //message_send_debug(COMM_1, buffer); I2C0CONCLR = 1 << SIC; break; default: // I2C error state detection I2C0CONSET = 1 << STO; I2C0CONSET = 1 << STA; // restart I2C state machine with current package error_counter0++; debug_message_buffer_sprintf("I2C0 error: undefined I2C state: %i",I2C0STAT); debug_message_buffer_sprintf("I2C0 error: prior state: %X",i2c0stat_prior_state); // message_send_debug(COMM_1, buffer); I2C0CONCLR = 1 << SIC; } } // Sum up errors global_data.i2c0_err_count += error_counter0; VICVectAddr = 0x00000000; // clear this interrupt from the VIC ISR_EXIT(); // exit ISR }
/** * @brief Set the current mode of operation * @param mode the new mode */ bool sys_set_mode(uint8_t mode) { if (mode == MAV_MODE_AUTO) { global_data.state.mav_mode = MAV_MODE_AUTO; return true; } else if (mode == MAV_MODE_GUIDED) { global_data.state.mav_mode = MAV_MODE_GUIDED; return true; } else if (mode == MAV_MODE_LOCKED) { global_data.state.mav_mode = MAV_MODE_LOCKED; return true; } else if (mode == MAV_MODE_MANUAL) { global_data.state.mav_mode = MAV_MODE_MANUAL; return true; } else if (mode == MAV_MODE_READY) { global_data.state.mav_mode = MAV_MODE_READY; return true; } else if (mode == MAV_MODE_TEST1) { global_data.state.mav_mode = MAV_MODE_TEST1; return true; } else if (mode == MAV_MODE_TEST2) { global_data.state.mav_mode = MAV_MODE_TEST2; return true; } else if (mode == MAV_MODE_TEST3) { global_data.state.mav_mode = MAV_MODE_TEST3; return true; } else if (mode == MAV_MODE_RC_TRAINING) { // Only go into RC training if not flying if (! sys_state_is_flying()) { global_data.state.mav_mode = MAV_MODE_RC_TRAINING; return true; } else { debug_message_buffer("WARNING: SYSTEM IS IN FLIGHT! Denied to switch to RC mode"); return false; } } // UNINIT is not a mode that should be actively set // it will thus be rejected like any other invalid mode else { // No valid mode debug_message_buffer("WARNING: Attempted to set invalid mode"); return false; } }
void main_loop_quadrotor(void) { /** * @brief Initialize the whole system * * All functions that need to be called before the first mainloop iteration * should be placed here. */ main_init_generic(); control_quadrotor_position_init(); control_quadrotor_attitude_init(); attitude_tobi_laurens_init(); // FIXME XXX Make proper mode switching // outdoor_position_kalman_init(); //vision_position_kalman_init(); // Default filters, allow Vision, Vicon and optical flow inputs vicon_position_kalman_init(); optflow_speed_kalman_init(); /** * @brief This is the main loop * * It will be executed at maximum MCU speed (60 Mhz) */ // Executiontime debugging time_debug.x = 0; time_debug.y = 0; time_debug.z = 0; last_mainloop_idle = sys_time_clock_get_time_usec(); debug_message_buffer("Starting main loop"); led_off(LED_GREEN); led_off(LED_RED); while (1) { // Time Measurement uint64_t loop_start_time = sys_time_clock_set_loop_start_time(); // loop_start_time should not be used anymore /////////////////////////////////////////////////////////////////////////// /// CRITICAL 200 Hz functions /////////////////////////////////////////////////////////////////////////// if (us_run_every(5000, COUNTER2, loop_start_time)) { // Kalman Attitude filter, used on all systems gyro_read(); sensors_read_acc(); sensors_pressure_bmp085_read_out(); // Read out magnetometer at its default 50 Hz rate static uint8_t mag_count = 0; if (mag_count == 3) { sensors_read_mag(); //attitude_observer_correct_magnet(global_data.magnet_corrected); mag_count = 0; }else if(mag_count==1){ hmc5843_start_read(); mag_count++; } else { mag_count++; } // Correction step of observer filter attitude_tobi_laurens(); if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_VICON_ONLY || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_VISION_VICON_BACKUP) { vicon_position_kalman(); } else if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_GPS_ONLY) { outdoor_position_kalman(); } control_quadrotor_attitude(); //debug counting number of executions count++; } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// Camera Shutter - This takes 50 usecs!!! /////////////////////////////////////////////////////////////////////////// // Set camera shutter with 2.5ms resolution else if (us_run_every(5000, COUNTER1, loop_start_time)) //was 2500 !!! { camera_shutter_handling(loop_start_time); // Measure time for debugging time_debug.x = max(time_debug.x, sys_time_clock_get_time_usec() - loop_start_time); } /////////////////////////////////////////////////////////////////////////// /// CRITICAL FAST 50 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(20000, COUNTER3, loop_start_time)) { // Read infrared sensor //adc_read(); // Control the quadrotor position control_quadrotor_position(); // Read remote control remote_control(); control_camera_angle(); // //float_vect3 opt; // static float_vect3 opt_int; // uint8_t valid = optical_flow_get_dxy(80, &global_data.optflow.x, &global_data.optflow.y, &global_data.optflow.z); // if (valid) // { // opt_int.x += global_data.optflow.x; // opt_int.y += global_data.optflow.y; // // } // // uint8_t supersampling = 10; // for (int i = 0; i < supersampling; ++i) // { // global_data.sonar_distance += sonar_distance_get(ADC_5_CHANNEL); // } // // global_data.sonar_distance /= supersampling; // // opt_int.z = valid; // static unsigned int i = 0; // if (i == 10) // { // mavlink_msg_optical_flow_send(global_data.param[PARAM_SEND_DEBUGCHAN], sys_time_clock_get_unix_loop_start_time(), 0, global_data.optflow.x, global_data.optflow.y, global_data.optflow.z, global_data.sonar_distance_filtered); // // i = 0; // } // i++; //optical_flow_debug_vect_send(); //debug_vect("opt_int", opt_int); // optical_flow_start_read(80); if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_INTEGRATING || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_NON_INTEGRATING || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ADD_VICON_AS_OFFSET || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ADD_VISION_AS_OFFSET || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_ODOMETRY_ADD_VISION_AS_OFFSET || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_VICON || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_GPS_OPTICAL_FLOW || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_GLOBAL_VISION || global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_VISUAL_ODOMETRY_GLOBAL_VISION) { optflow_speed_kalman(); } // Send the raw sensor/ADC values communication_send_raw_data(loop_start_time); float_vect3 yy; yy.x = global_data.yaw_lowpass; yy.y = 0.f; yy.z = 0.f; debug_vect("yaw_low", yy); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// CRITICAL FAST 20 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(50000, COUNTER4, loop_start_time)) { //*** this happens in handle_controller_timeouts already!!!!! *** // //update global_data.state // if (global_data.param[PARAM_VICON_MODE] == 1) // { // //VICON_MODE 1 only accepts vicon position // global_data.state.position_fix = global_data.state.vicon_ok; // } // else // { // //VICON_MODEs 0, 2, 3 accepts vision additionally, so check vision // global_data.state.position_fix = global_data.state.vision_ok; // } update_system_statemachine(loop_start_time); update_controller_setpoints(); mavlink_msg_roll_pitch_yaw_thrust_setpoint_send( global_data.param[PARAM_SEND_DEBUGCHAN], sys_time_clock_get_loop_start_time_boot_ms(), global_data.attitude_setpoint.x, global_data.attitude_setpoint.y, global_data.position_yaw_control_output, global_data.thrust_control_output); //STARTING AND LANDING quadrotor_start_land_handler(loop_start_time); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 100 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(5000, COUNTER6, loop_start_time)) { if (global_data.param[PARAM_SEND_SLOT_DEBUG_6]) { debug_vect("att_ctrl_o", global_data.attitude_control_output); } } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// UNCRITICAL SLOW 5 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(200000, COUNTER8, loop_start_time)) { // The onboard controllers go into failsafe mode once // position data is missing handle_controller_timeouts(loop_start_time); // Send buffered data such as debug text messages // Empty one message out of the buffer debug_message_send_one(); // Toggle status led led_toggle(LED_RED); // Toggle active mode led if (global_data.state.mav_mode & MAV_MODE_FLAG_SAFETY_ARMED) { led_on(LED_GREEN); } else { led_off(LED_GREEN); } handle_eeprom_write_request(); handle_reset_request(); update_controller_parameters(); communication_send_controller_feedback(); communication_send_remote_control(); // Pressure sensor driver works, but not tested regarding stability // sensors_pressure_bmp085_read_out(); if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 1) { mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 90, global_data.param[PARAM_POSITION_SETPOINT_YAW]); mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 91, global_data.yaw_pos_setpoint); } } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 1 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(1000000, COUNTER9, loop_start_time)) { // Send system state, mode, battery voltage, etc. send_system_state(); // Send position setpoint offset //debug_vect("pos offs", global_data.position_setpoint_offset); // Send current onboard time mavlink_msg_system_time_send(MAVLINK_COMM_1, sys_time_clock_get_unix_loop_start_time(),sys_time_clock_get_loop_start_time_boot_ms()); mavlink_msg_system_time_send(MAVLINK_COMM_0, sys_time_clock_get_unix_loop_start_time(),sys_time_clock_get_loop_start_time_boot_ms()); //update state from received parameters sync_state_parameters(); //debug number of execution count = 0; if (global_data.param[PARAM_GPS_MODE] >= 10) { //Send GPS information float_vect3 gps; gps.x = gps_utm_north / 100.0f;//m gps.y = gps_utm_east / 100.0f;//m gps.z = gps_utm_zone;// gps_week; debug_vect("GPS", gps); } else if (global_data.param[PARAM_GPS_MODE] == 9 || global_data.param[PARAM_GPS_MODE] == 8) { if (global_data.param[PARAM_GPS_MODE] == 8) { gps_set_local_origin(); // gps_local_home_init = false; } if (gps_lat == 0) { debug_message_buffer("GPS Signal Lost"); } else { float_vect3 gps_local, gps_local_velocity; gps_get_local_position(&gps_local); debug_vect("GPS local", gps_local); gps_get_local_velocity(&gps_local_velocity); debug_vect("GPS loc velocity", gps_local_velocity); } } if (global_data.state.gps_mode) { gps_send_local_origin(); } beep_on_low_voltage(); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 20 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(50000, COUNTER7, loop_start_time)) { //led_toggle(LED_YELLOW); if (global_data.param[PARAM_GPS_MODE] >= 10) { //get thru all gps messages debug_message_send_one(); } communication_send_attitude_position(loop_start_time); // Send parameter communication_queued_send(); // //infrared distance // float_vect3 infra; // infra.x = global_data.ground_distance; // infra.y = global_data.ground_distance_unfiltered; // infra.z = global_data.state.ground_distance_ok; // debug_vect("infrared", infra); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 200 Hz functions // /////////////////////////////////////////////////////////////////////////// else if (us_run_every(5000, COUNTER5, loop_start_time)) { if (global_data.state.status == MAV_STATE_STANDBY) { //Check if parameters should be written or read param_handler(); } } /////////////////////////////////////////////////////////////////////////// else { // All Tasks are fine and we have no starvation last_mainloop_idle = loop_start_time; } // Read out comm at max rate - takes only a few microseconds in worst case communication_receive(); // MCU load measurement uint64_t loop_stop_time = sys_time_clock_get_time_usec(); global_data.cpu_usage = measure_avg_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time); global_data.cpu_peak = measure_peak_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time); time_debug.y = max(time_debug.y, global_data.cpu_usage); time_debug.z = max(time_debug.z, global_data.cpu_peak); if (loop_start_time - last_mainloop_idle >= 20000) { debug_message_buffer( "CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!"); last_mainloop_idle = loop_start_time;//reset to prevent multiple messages } if (global_data.cpu_usage > 800) { // CPU load higher than 80% debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%"); } } // End while(1) }
/** * @brief Send one of the buffered messages * @param pos data from vision */ void vision_buffer_handle_data(mavlink_vision_position_estimate_t* pos) { if (vision_buffer_index_write == vision_buffer_index_read) { //buffer empty return; } vision_buffer_index_read = (vision_buffer_index_read + 1) % VISION_BUFFER_COUNT; //TODO: find data and process it uint8_t for_count = 0; uint8_t i = vision_buffer_index_read; for (; (vision_buffer[i].time_captured < pos->usec) && (vision_buffer_index_write - i != 1); i = (i + 1) % VISION_BUFFER_COUNT) { if (++for_count > VISION_BUFFER_COUNT) { debug_message_buffer("vision_buffer PREVENTED HANG"); break; } } if (vision_buffer[i].time_captured == pos->usec) { //we found the right data if (!isnumber(pos->x) || !isnumber(pos->y) || !isnumber(pos->z) || !isnumber(pos->roll) || !isnumber(pos->pitch) || !isnumber(pos->yaw) || pos->x == 0.0 || pos->y == 0.0 || pos->z == 0.0) { //reject invalid data debug_message_buffer("vision_buffer invalid data (inf,nan,0) rejected"); } else if (fabs(vision_buffer[i].ang.x - pos->roll) < global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD] && fabs(vision_buffer[i].ang.y - pos->pitch) < global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD]) { // Update validity time global_data.pos_last_valid = sys_time_clock_get_time_usec(); //Pack new vision_data package global_data.vision_data.time_captured = vision_buffer[i].time_captured; global_data.vision_data.comp_end = sys_time_clock_get_unix_time(); //Set data from Vision directly global_data.vision_data.ang.x = pos->roll; global_data.vision_data.ang.y = pos->pitch; global_data.vision_data.ang.z = pos->yaw; global_data.vision_data.pos.x = pos->x; global_data.vision_data.pos.y = pos->y; global_data.vision_data.pos.z = pos->z; // If yaw input from vision is enabled, feed vision // directly into state estimator global_data.vision_magnetometer_replacement.x = 200.0f*lookup_cos(pos->yaw); global_data.vision_magnetometer_replacement.y = -200.0f*lookup_sin(pos->yaw); global_data.vision_magnetometer_replacement.z = 0.f; //If yaw goes to infinity (no idea why) set it to setpoint, next time will be better if (global_data.attitude.z > 18.8495559 || global_data.attitude.z < -18.8495559) { global_data.attitude.z = global_data.yaw_pos_setpoint; debug_message_buffer("vision_buffer CRITICAL FAULT yaw was bigger than 6 PI! prevented crash"); } global_data.vision_data.new_data = 1; //TODO correct also all buffer data needed if we are going to have overlapping vision data } else { //rejected outlayer if (vision_buffer_reject_count++ % 16 == 0) { debug_message_buffer_sprintf("vision_buffer rejected outlier #%u", vision_buffer_reject_count); } } if (global_data.param[PARAM_SEND_SLOT_DEBUG_1] == 1) { //mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 202, global_data.attitude.z); mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 210, pos->x); mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 211, pos->y); mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 212, pos->z); mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 215, pos->yaw); //mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 212, pos.z); //mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 203, pos.r1); //mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 204, pos.confidence); } if (for_count) { debug_message_buffer_sprintf( "vision_buffer data found skipped %i data sets", for_count); } } else { //we didn't find it // debug_message_buffer("vision_buffer data NOT found"); if (for_count) { debug_message_buffer_sprintf( "vision_buffer data NOT found skipped %i data sets", for_count); } } vision_buffer_index_read = i;//skip all images that are older; // if (for_count) // { // debug_message_buffer_sprintf("vision_buffer skipped %i data sets", // for_count); // } }
/** * @brief This is the main loop * * It will be executed at maximum MCU speed (60 Mhz) */ void main_loop_ground_car(void) { last_mainloop_idle = sys_time_clock_get_time_usec(); debug_message_buffer("Starting main loop"); while (1) { // Time Measurement uint64_t loop_start_time = sys_time_clock_get_time_usec(); /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// CRITICAL 200 Hz functions /////////////////////////////////////////////////////////////////////////// if (us_run_every(5000, COUNTER2, loop_start_time)) { // Kalman Attitude filter, used on all systems gyro_read(); sensors_read_acc(); // Read out magnetometer at its default 50 Hz rate static uint8_t mag_count = 0; if (mag_count == 3) { sensors_read_mag(); attitude_observer_correct_magnet(global_data.magnet_corrected); mag_count = 0; } else { mag_count++; } // Correction step of observer filter attitude_observer_correct_accel(global_data.accel_raw); // Write in roll and pitch static float_vect3 att; //if not static we have spikes in roll and pitch on bravo !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! attitude_observer_get_angles(&att); global_data.attitude.x = att.x; global_data.attitude.y = att.y; if (global_data.param[PARAM_ATT_KAL_IYAW]) { global_data.attitude.z += 0.005 * global_data.gyros_si.z; } else { global_data.attitude.z = att.z; } // Prediction step of observer attitude_observer_predict(global_data.gyros_si); // TODO READ OUT MOUSE SENSOR } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// CRITICAL FAST 50 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(20000, COUNTER3, loop_start_time)) { // Read Analog-to-Digital converter adc_read(); // Read remote control remote_control(); // Send the raw sensor/ADC values communication_send_raw_data(loop_start_time); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// UNCRITICAL SLOW 5 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(200000, COUNTER8, loop_start_time)) { // Send buffered data such as debug text messages communication_queued_send(); // Empty one message out of the buffer debug_message_send_one(); // Toggle status led //led_toggle(LED_YELLOW); led_toggle(LED_RED); // just for green LED on alpha at the moment // Toggle active mode led if (global_data.state.mav_mode == MAV_MODE_MANUAL || global_data.state.mav_mode == MAV_MODE_GUIDED || global_data.state.mav_mode == MAV_MODE_AUTO) { led_on(LED_GREEN); } else { led_off(LED_GREEN); } handle_eeprom_write_request(); handle_reset_request(); communication_send_remote_control(); // Pressure sensor driver works, but not tested regarding stability sensors_pressure_bmp085_read_out(); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 20 Hz functions /////////////////////////////////////////////////////////////////////////// else if (us_run_every(50000, COUNTER7, loop_start_time)) { led_toggle(LED_YELLOW); communication_send_attitude_position(loop_start_time); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// /// NON-CRITICAL SLOW 200 Hz functions // /////////////////////////////////////////////////////////////////////////// else if (us_run_every(5000, COUNTER5, loop_start_time)) { if (global_data.state.status == MAV_STATE_STANDBY) { //Check if parameters should be written or read param_handler(); } } /////////////////////////////////////////////////////////////////////////// else { // All Tasks are fine and we have no starvation last_mainloop_idle = loop_start_time; } // Read out comm at max rate - takes only a few microseconds in worst case communication_receive(); // MCU load measurement uint64_t loop_stop_time = sys_time_clock_get_time_usec(); global_data.cpu_usage = measure_avg_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time); global_data.cpu_peak = measure_peak_cpu_load(loop_start_time, loop_stop_time, min_mainloop_time); if (loop_start_time - last_mainloop_idle >= 5000) { debug_message_buffer( "CRITICAL WARNING! CPU LOAD TO HIGH. STARVATION!"); last_mainloop_idle = loop_start_time;//reset to prevent multiple messages } if (global_data.cpu_usage > 800) { // CPU load higher than 80% debug_message_buffer("CRITICAL WARNING! CPU LOAD HIGHER THAN 80%"); } } // End while(1) }
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"); } } } } } }
/** * @brief Send one of the buffered messages * @param pos data from vision */ void vision_buffer_handle_global_data(mavlink_global_vision_position_estimate_t* pos) { //#define PROJECT_GLOBAL_DATA_FORWARD #ifdef PROJECT_GLOBAL_DATA_FORWARD if (vision_buffer_index_write == vision_buffer_index_read) { //buffer empty debug_message_buffer("ERROR VIS BUF: buffer empty"); return; } vision_buffer_index_read = (vision_buffer_index_read + 1) % VISION_BUFFER_COUNT; //TODO: find data and process it uint8_t for_count = 0; uint8_t i = vision_buffer_index_read; for (; (vision_buffer[i].time_captured < pos->usec) && (vision_buffer_index_write - i != 1); i = (i + 1) % VISION_BUFFER_COUNT) { if (++for_count > VISION_BUFFER_COUNT) break; } if (vision_buffer[i].time_captured == pos->usec) { //we found the right data if (!isnumber(pos->x) || !isnumber(pos->y) || !isnumber(pos->z) || !isnumber(pos->roll) || !isnumber(pos->pitch) || !isnumber(pos->yaw) || pos->x == 0.0 || pos->y == 0.0 || pos->z == 0.0) { //reject invalid data debug_message_buffer("ERROR VIS BUF: invalid data (inf,nan,0) rejected"); } else if (fabs(vision_buffer[i].ang.x - pos->roll) < global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD] && fabs(vision_buffer[i].ang.y - pos->pitch) < global_data.param[PARAM_VISION_ANG_OUTLAYER_TRESHOLD]) { // Update validity time global_data.pos_last_valid = sys_time_clock_get_time_usec(); //Pack new vision_data package global_data.vision_data_global.time_captured = vision_buffer[i].time_captured; global_data.vision_data_global.comp_end = sys_time_clock_get_unix_time(); // FIXME currently visodo is not allowed to run in parallel, else race condititions! // Project position measurement global_data.vision_data_global.pos.x = pos->x + (global_data.position.x - vision_buffer[i].pos.x); global_data.vision_data_global.pos.y = pos->y + (global_data.position.y - vision_buffer[i].pos.y); global_data.vision_data_global.pos.z = pos->z + (global_data.position.z - vision_buffer[i].pos.z); // Set roll and pitch absolutely global_data.vision_data_global.ang.x = pos->roll; global_data.vision_data_global.ang.y = pos->pitch; global_data.vision_data_global.ang.z = pos->yaw; // Project yaw //global_data.vision_data_global.ang.z = pos->yaw-vision_buffer[i].ang.z; for (uint8_t j = (i+1) % VISION_BUFFER_COUNT; j != i; j = (j + 1) % VISION_BUFFER_COUNT) { vision_buffer[j].pos.x = vision_buffer[j].pos.x + (pos->x - vision_buffer[i].pos.x); vision_buffer[j].pos.y = vision_buffer[j].pos.y + (pos->y - vision_buffer[i].pos.y); vision_buffer[j].pos.z = vision_buffer[j].pos.z + (pos->z - vision_buffer[i].pos.z); } // If yaw input from vision is enabled, feed vision // directly into state estimator float lpYaw = pos->yaw*0.5f+global_data.attitude.z*0.5f; global_data.vision_global_magnetometer_replacement.x = 200.0f*lookup_cos(lpYaw); global_data.vision_global_magnetometer_replacement.y = -200.0f*lookup_sin(lpYaw); global_data.vision_global_magnetometer_replacement.z = 0.f; //If yaw goes to infinity (no idea why) set it to setpoint, next time will be better if (global_data.attitude.z > 18.8495559 || global_data.attitude.z < -18.8495559) { global_data.attitude.z = global_data.yaw_pos_setpoint; debug_message_buffer("vision_buffer CRITICAL FAULT yaw was bigger than 6 PI! prevented crash"); } global_data.vision_data_global.new_data = 1; global_data.state.global_vision_attitude_new_data = 1; debug_message_buffer_sprintf("vision_buffer data found skipped %i data sets", for_count); //TODO correct also all buffer data needed if we are going to have overlapping vision data if (global_data.param[PARAM_SEND_SLOT_DEBUG_1] == 1) { //mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 202, global_data.attitude.z); mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 220, pos->x); mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 221, pos->y); mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 222, pos->z); mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 0, 225, pos->yaw); //mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 212, pos.z); //mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 203, pos.r1); //mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 204, pos.confidence); } } else { //rejected outlier //if (vision_buffer_reject_count++ % 16 == 0) { debug_message_buffer_sprintf("vision_buffer rejected outlier #%u", vision_buffer_reject_count); } } } else { //we didn't find it debug_message_buffer_sprintf("vision_buffer data NOT found skipped %i data sets", for_count); } vision_buffer_index_read = i;//skip all images that are older; #else global_data.vision_data_global.pos.x = pos->x; global_data.vision_data_global.pos.y = pos->y; global_data.vision_data_global.pos.z = pos->z; //Set data from Vision directly global_data.vision_data_global.ang.x = pos->roll; global_data.vision_data_global.ang.y = pos->pitch; global_data.vision_data_global.ang.z = pos->yaw; global_data.vision_data_global.new_data = 1; global_data.state.global_vision_attitude_new_data = 1; #endif mavlink_msg_global_vision_position_estimate_send(MAVLINK_COMM_0, sys_time_clock_get_unix_loop_start_time(), global_data.vision_data_global.pos.x, global_data.vision_data_global.pos.y, global_data.vision_data_global.pos.z, global_data.vision_data_global.ang.x, global_data.vision_data_global.ang.y, global_data.vision_data_global.ang.z); }
//@{ void handle_mavlink_message(mavlink_channel_t chan, mavlink_message_t* msg) { uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint32_t len; switch (chan) { case MAVLINK_COMM_0: { if (msg->msgid != MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) { // Copy to COMM 1 len = mavlink_msg_to_send_buffer(buf, msg); for (int i = 0; i < len; i++) { uart1_transmit(buf[i]); } } } break; case MAVLINK_COMM_1: { if (msg->msgid != MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE && msg->msgid != MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { // Copy to COMM 0 len = mavlink_msg_to_send_buffer(buf, msg); for (int i = 0; i < len; i++) { uart0_transmit(buf[i]); } break; } } default: break; } switch (msg->msgid) { case MAVLINK_MSG_ID_SET_MODE: { mavlink_set_mode_t mode; mavlink_msg_set_mode_decode(msg, &mode); // Check if this system should change the mode if (mode.target == (uint8_t)global_data.param[PARAM_SYSTEM_ID]) { sys_set_mode(mode.mode); // Emit current mode mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode, global_data.state.status, global_data.cpu_usage, global_data.battery_voltage, global_data.motor_block, communication_get_uart_drop_rate()); mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode, global_data.state.status, global_data.cpu_usage, global_data.battery_voltage, global_data.motor_block, communication_get_uart_drop_rate()); } } break; case MAVLINK_MSG_ID_ACTION: { execute_action(mavlink_msg_action_get_action(msg)); //Forwart actions from Xbee to Onboard Computer and vice versa if (chan == MAVLINK_COMM_1) { mavlink_send_uart(MAVLINK_COMM_0, msg); } else if (chan == MAVLINK_COMM_0) { mavlink_send_uart(MAVLINK_COMM_1, msg); } } break; case MAVLINK_MSG_ID_SYSTEM_TIME: { if (!sys_time_clock_get_unix_offset()) { int64_t offset = ((int64_t) mavlink_msg_system_time_get_time_usec( msg)) - (int64_t) sys_time_clock_get_time_usec(); sys_time_clock_set_unix_offset(offset); debug_message_buffer("UNIX offset updated"); } else { // debug_message_buffer("UNIX offset REFUSED"); } } break; case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { mavlink_request_data_stream_t stream; mavlink_msg_request_data_stream_decode(msg, &stream); switch (stream.req_stream_id) { case 0: // UNIMPLEMENTED break; case 1: // RAW SENSOR DATA global_data.param[PARAM_SEND_SLOT_RAW_IMU] = stream.start_stop; break; case 2: // EXTENDED SYSTEM STATUS global_data.param[PARAM_SEND_SLOT_ATTITUDE] = stream.start_stop; break; case 3: // REMOTE CONTROL CHANNELS global_data.param[PARAM_SEND_SLOT_REMOTE_CONTROL] = stream.start_stop; break; case 4: // RAW CONTROLLER //global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop; //global_data.param[PARAM_SEND_SLOT_DEBUG_3] = stream.start_stop; global_data.param[PARAM_SEND_SLOT_CONTROLLER_OUTPUT] = stream.start_stop; break; case 5: // SENSOR FUSION //LOST IN GROUDNCONTROL // global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop; break; case 6: // POSITION global_data.param[PARAM_SEND_SLOT_DEBUG_5] = stream.start_stop; break; case 7: // EXTRA1 global_data.param[PARAM_SEND_SLOT_DEBUG_2] = stream.start_stop; break; case 8: // EXTRA2 global_data.param[PARAM_SEND_SLOT_DEBUG_4] = stream.start_stop; break; case 9: // EXTRA3 global_data.param[PARAM_SEND_SLOT_DEBUG_6] = stream.start_stop; break; default: // Do nothing break; } } break; case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { mavlink_param_request_read_t set; mavlink_msg_param_request_read_decode(msg, &set); // Check if this message is for this system if ((uint8_t) set.target_system == (uint8_t) global_data.param[PARAM_SYSTEM_ID] && (uint8_t) set.target_component == (uint8_t) global_data.param[PARAM_COMPONENT_ID]) { char* key = (char*) set.param_id; if (set.param_id[0] == '\0') { // Choose parameter based on index if (set.param_index < ONBOARD_PARAM_COUNT) { // Report back value mavlink_msg_param_value_send(chan, (int8_t*) global_data.param_name[set.param_index], global_data.param[set.param_index], ONBOARD_PARAM_COUNT, set.param_index); } } else { for (int i = 0; i < ONBOARD_PARAM_COUNT; i++) { bool match = true; for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++) { // Compare if (((char) (global_data.param_name[i][j])) != (char) (key[j])) { match = false; } // End matching if null termination is reached if (((char) global_data.param_name[i][j]) == '\0') { break; } } // Check if matched if (match) { // Report back value mavlink_msg_param_value_send(chan, (int8_t*) global_data.param_name[i], global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i); } } } } } break; case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { // Start sending parameters m_parameter_i = 0; } break; case MAVLINK_MSG_ID_PARAM_SET: { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); // Check if this message is for this system if ((uint8_t) set.target_system == (uint8_t) global_data.param[PARAM_SYSTEM_ID] && (uint8_t) set.target_component == (uint8_t) global_data.param[PARAM_COMPONENT_ID]) { char* key = (char*) set.param_id; for (int i = 0; i < ONBOARD_PARAM_COUNT; i++) { bool match = true; for (int j = 0; j < ONBOARD_PARAM_NAME_LENGTH; j++) { // Compare if (((char) (global_data.param_name[i][j])) != (char) (key[j])) { match = false; } // End matching if null termination is reached if (((char) global_data.param_name[i][j]) == '\0') { break; } } // Check if matched if (match) { // Only write and emit changes if there is actually a difference // AND only write if new value is NOT "not-a-number" // AND is NOT infy if (global_data.param[i] != set.param_value && !isnan(set.param_value) && !isinf(set.param_value)) { global_data.param[i] = set.param_value; // Report back new value mavlink_msg_param_value_send(MAVLINK_COMM_0, (int8_t*) global_data.param_name[i], global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i); mavlink_msg_param_value_send(MAVLINK_COMM_1, (int8_t*) global_data.param_name[i], global_data.param[i], ONBOARD_PARAM_COUNT, m_parameter_i); debug_message_buffer_sprintf("Parameter received param id=%i",i); } } } } } break; case MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET: { mavlink_position_control_setpoint_set_t pos; mavlink_msg_position_control_setpoint_set_decode(msg, &pos); if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1) { // global_data.position_setpoint.x = pos.x; // global_data.position_setpoint.y = pos.y; // global_data.position_setpoint.z = pos.z; debug_message_buffer("Position setpoint updated. OLD?\n"); } else { debug_message_buffer( "Position setpoint recieved but not updated. OLD?\n"); } // Send back a message confirming the new position mavlink_msg_position_control_setpoint_send(MAVLINK_COMM_0, pos.id, pos.x, pos.y, pos.z, pos.yaw); mavlink_msg_position_control_setpoint_send(MAVLINK_COMM_1, pos.id, pos.x, pos.y, pos.z, pos.yaw); } break; case MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET: { mavlink_position_control_offset_set_t set; mavlink_msg_position_control_offset_set_decode(msg, &set); //global_data.attitude_setpoint_pos_body_offset.z = set.yaw; //Ball Tracking if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1 && global_data.param[PARAM_POSITION_YAW_TRACKING]==1) { global_data.param[PARAM_POSITION_SETPOINT_YAW] = global_data.attitude.z + set.yaw; mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 92, set.yaw); } } break; case MAVLINK_MSG_ID_SET_CAM_SHUTTER: { // Decode the desired shutter mavlink_set_cam_shutter_t cam; mavlink_msg_set_cam_shutter_decode(msg, &cam); shutter_set(cam.interval, cam.exposure); debug_message_buffer_sprintf("set_cam_shutter. interval %i", cam.interval); } break; case MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL: { uint8_t enable = mavlink_msg_image_trigger_control_get_enable(msg); shutter_control(enable); if (enable) { debug_message_buffer("CAM: Enabling hardware trigger"); } else { debug_message_buffer("CAM: Disabling hardware trigger"); } } break; case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: { mavlink_vision_position_estimate_t pos; mavlink_msg_vision_position_estimate_decode(msg, &pos); vision_buffer_handle_data(&pos); // Update validity time is done in vision buffer } break; case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: { mavlink_vicon_position_estimate_t pos; mavlink_msg_vicon_position_estimate_decode(msg, &pos); global_data.vicon_data.x = pos.x; global_data.vicon_data.y = pos.y; global_data.vicon_data.z = pos.z; global_data.state.vicon_new_data=1; // Update validity time global_data.vicon_last_valid = sys_time_clock_get_time_usec(); global_data.state.vicon_ok=1; // //Set data from Vicon into vision filter // global_data.vision_data.ang.x = pos.roll; // global_data.vision_data.ang.y = pos.pitch; // global_data.vision_data.ang.z = pos.yaw; // // global_data.vision_data.pos.x = pos.x; // global_data.vision_data.pos.y = pos.y; // global_data.vision_data.pos.z = pos.z; // // global_data.vision_data.new_data = 1; if (!global_data.state.vision_ok) { //Correct YAW global_data.attitude.z = pos.yaw; //If yaw goes to infy (no idea why) set it to setpoint, next time will be better if (global_data.attitude.z > 18.8495559 || global_data.attitude.z < -18.8495559) { global_data.attitude.z = global_data.yaw_pos_setpoint; debug_message_buffer( "vicon CRITICAL FAULT yaw was bigger than 6 PI! prevented crash"); } } //send the vicon message to UART0 with new timestamp mavlink_msg_vicon_position_estimate_send(MAVLINK_COMM_0, global_data.vicon_last_valid, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw); } break; case MAVLINK_MSG_ID_PING: { mavlink_ping_t ping; mavlink_msg_ping_decode(msg, &ping); if (ping.target_system == 0 && ping.target_component == 0) { // Respond to ping uint64_t r_timestamp = sys_time_clock_get_unix_time(); mavlink_msg_ping_send(chan, ping.seq, msg->sysid, msg->compid, r_timestamp); } } break; case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET: { mavlink_local_position_setpoint_set_t sp; mavlink_msg_local_position_setpoint_set_decode(msg, &sp); if (sp.target_system == global_data.param[PARAM_SYSTEM_ID]) { if (global_data.param[PARAM_POSITIONSETPOINT_ACCEPT] == 1) { if (sp.x >= global_data.position_setpoint_min.x && sp.y >= global_data.position_setpoint_min.y && sp.z >= global_data.position_setpoint_min.z && sp.x <= global_data.position_setpoint_max.x && sp.y <= global_data.position_setpoint_max.y && sp.z <= global_data.position_setpoint_max.z) { debug_message_buffer("Setpoint accepted and set."); global_data.param[PARAM_POSITION_SETPOINT_X] = sp.x; global_data.param[PARAM_POSITION_SETPOINT_Y] = sp.y; global_data.param[PARAM_POSITION_SETPOINT_Z] = sp.z; if (global_data.param[PARAM_POSITION_YAW_TRACKING] == 0) { // Only update yaw if we are not tracking ball. global_data.param[PARAM_POSITION_SETPOINT_YAW] = sp.yaw; } //check if we want to start or land if (global_data.state.status == MAV_STATE_ACTIVE || global_data.state.status == MAV_STATE_CRITICAL) { if (sp.z > -0.1) { if (!(global_data.state.fly == FLY_GROUNDED || global_data.state.fly == FLY_SINKING || global_data.state.fly == FLY_WAIT_LANDING || global_data.state.fly == FLY_LANDING || global_data.state.fly == FLY_RAMP_DOWN)) { //if setpoint is lower that ground iate landing global_data.state.fly = FLY_SINKING; global_data.param[PARAM_POSITION_SETPOINT_Z] = -0.2;//with lowpass debug_message_buffer( "Sinking for LANDING. (z-sp lower than 10cm)"); } else if (!(global_data.state.fly == FLY_GROUNDED)) { global_data.param[PARAM_POSITION_SETPOINT_Z] = -0.2;//with lowpass } } else if (global_data.state.fly == FLY_GROUNDED && sp.z < -0.50) { //start if we were grounded and get a sp over 0.5m global_data.state.fly = FLY_WAIT_MOTORS; debug_message_buffer( "STARTING wait motors. (z-sp higher than 50cm)"); //set point changed with lowpass, after 5s it will be ok. } } //SINK TO 0.7m if we are critical or emergency if (global_data.state.status == MAV_STATE_EMERGENCY || global_data.state.status == MAV_STATE_CRITICAL) { global_data.param[PARAM_POSITION_SETPOINT_Z] = -0.7;//with lowpass } } else { debug_message_buffer("Setpoint refused. Out of range."); } } else { debug_message_buffer("Setpoint refused. Param setpoint accept=0."); } } } break; default: break; } }
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 Receive communication packets and handle them * * This function decodes packets on the protocol level and also handles * their value by calling the appropriate functions. */ void communication_receive(void) { mavlink_message_t msg; mavlink_status_t status = { 0 }; status.packet_rx_drop_count = 0; // COMMUNICATION WITH ONBOARD COMPUTER while (uart0_char_available()) { uint8_t c = uart0_get_char(); if (global_data.state.uart0mode == UART_MODE_MAVLINK) { // Try to get a new message if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { // Handle message handle_mavlink_message(MAVLINK_COMM_0, &msg); } } else if (global_data.state.uart0mode == UART_MODE_BYTE_FORWARD) { uart1_transmit(c); } // And get the next one } // Update global packet drops counter global_data.comm.uart0_rx_drop_count += status.packet_rx_drop_count; global_data.comm.uart0_rx_success_count += status.packet_rx_success_count; status.packet_rx_drop_count = 0; // COMMUNICATION WITH EXTERNAL COMPUTER while (uart1_char_available()) { uint8_t c = uart1_get_char(); // Check if this link is used for MAVLink or GPS if (global_data.state.uart1mode == UART_MODE_MAVLINK) { //uart0_transmit((unsigned char)c); // Try to get a new message if (mavlink_parse_char(MAVLINK_COMM_1, c, &msg, &status)) { // Handle message handle_mavlink_message(MAVLINK_COMM_1, &msg); } } else if (global_data.state.uart1mode == UART_MODE_GPS) { if (global_data.state.gps_mode == 10) { static uint8_t gps_i = 0; static char gps_chars[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; if (c == '$' || gps_i == MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN - 1) { gps_i = 0; char gps_chars_buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; strncpy(gps_chars_buf, gps_chars, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); debug_message_buffer(gps_chars_buf); } gps_chars[gps_i++] = c; } if (gps_parse(c)) { // New GPS data received //debug_message_buffer("RECEIVED NEW GPS DATA"); parse_gps_msg(); if (gps_lat == 0) { global_data.state.gps_ok = 0; //debug_message_buffer("GPS Signal Lost"); } else { global_data.state.gps_ok = 1; mavlink_msg_gps_raw_send( global_data.param[PARAM_SEND_DEBUGCHAN], sys_time_clock_get_unix_time(), gps_mode, gps_lat / 1e7f, gps_lon / 1e7f, gps_alt / 100.0f, 0.0f, 0.0f, gps_gspeed / 100.0f, gps_course / 10.0f); } // // Output satellite info // for (int i = 0; i < gps_nb_channels; i++) // { // mavlink_msg_gps_status_send(global_data.param[PARAM_SEND_DEBUGCHAN], gps_numSV, gps_svinfos[i].svid, gps_satellite_used(gps_svinfos[i].qi), gps_svinfos[i].elev, ((gps_svinfos[i].azim/360.0f)*255.0f), gps_svinfos[i].cno); // } } } else if (global_data.state.uart1mode == UART_MODE_BYTE_FORWARD) { uart0_transmit(c); led_toggle(LED_YELLOW); } // And get the next one } // Update global packet drops counter global_data.comm.uart0_rx_drop_count += status.packet_rx_drop_count; global_data.comm.uart0_rx_success_count += status.packet_rx_success_count; status.packet_rx_drop_count = 0; }
/** * @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); } }