void outdoor_position_kalman(void) { //Transform accelerometer used in all directions float_vect3 acc_nav; body2navi(&global_data.accel_si, &global_data.attitude, &acc_nav); //X &Y Kalman Filter kalman_predict(&outdoor_position_kalman_x); kalman_predict(&outdoor_position_kalman_y); m_elem x_measurement[2] = { }; m_elem x_mask[2] = { 0, 1 };//only acceleromenters normaly m_elem y_measurement[2] = { }; m_elem y_mask[2] = { 0, 1 };//only acceleromenters normaly // static int i = 0; // if (i++ == 200) // { // i = 0; // x_measurement[0]=0; // x_mask[0]=1;//simulate GPS position 0 at 1 Hz // } static int i = 0; if (global_data.state.gps_ok && global_data.state.gps_new_data) { global_data.state.gps_new_data=0; float_vect3 gps_local; gps_get_local_position(&gps_local); x_measurement[0] = gps_local.x; x_mask[0] = 1;// GPS position at 1 Hz y_measurement[0] = gps_local.y; y_mask[0] = 1;// GPS position at 1 Hz } x_measurement[1] = acc_nav.x; y_measurement[1] = acc_nav.y; //Put measurements into filter kalman_correct(&outdoor_position_kalman_x, x_measurement, x_mask); kalman_correct(&outdoor_position_kalman_y, y_measurement, y_mask); // static int i=2; // if(i++==4){ // i=0; //debug // mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 50, // x_measurement[1]); // float_vect3 out_kal_x; // out_kal_x.x = kalman_get_state(&outdoor_position_kalman_x,0); // out_kal_x.y = kalman_get_state(&outdoor_position_kalman_x,1); // out_kal_x.z = kalman_get_state(&outdoor_position_kalman_x,3); // debug_vect("out_kal_x", out_kal_x); //Altitude Kalman Filter kalman_predict(&outdoor_position_kalman_z); m_elem z_measurement[2] = { }; m_elem z_mask[2] = { 0, 1 };//we normaly only have acceleration an no pressure measurement //prepare measurement data //measurement #1 pressure => relative altitude static int nopressure = 1; nopressure++; if (nopressure == 2 || nopressure == 4) { sensors_pressure_bmp085_read_out(); } if (nopressure == 4) { nopressure = 0; if (global_data.state.pressure_ok) { if (altitude_local_origin) { z_measurement[0] = -calc_altitude_pressure( global_data.pressure_raw) - altitude_local_origin; } else { altitude_set_local_origin(); } z_mask[0] = 1;//we have a pressure measurement to update //debug output // mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 50, // outdoor_position_kalman_z.gainfactor); } } //measurement #2 acceleration z_measurement[1] = acc_nav.z; //Put measurements into filter kalman_correct(&outdoor_position_kalman_z, z_measurement, z_mask); float_vect3 debug, debugv; debug.x = kalman_get_state(&outdoor_position_kalman_x, 0); debug.y = kalman_get_state(&outdoor_position_kalman_y, 0); debug.z = kalman_get_state(&outdoor_position_kalman_z, 0); outdoor_z_position = debug.z; debugv.x = kalman_get_state(&outdoor_position_kalman_x, 1); debugv.y = kalman_get_state(&outdoor_position_kalman_y, 1); debugv.z = kalman_get_state(&outdoor_position_kalman_z, 1); static uint8_t i; if (i++ > 20) { i = 0; debug_vect("outdoot_pos", debug); debug_vect("outdoot_vel", debugv); } // save outputs // global_data.position.x = kalman_get_state(&outdoor_position_kalman_x,0); // global_data.position.y = kalman_get_state(&outdoor_position_kalman_y,0); // global_data.position.z = kalman_get_state(&outdoor_position_kalman_z,0); // // global_data.velocity.x = kalman_get_state(&outdoor_position_kalman_x,1); // global_data.velocity.y = kalman_get_state(&outdoor_position_kalman_y,1); // global_data.velocity.z = kalman_get_state(&outdoor_position_kalman_z,1); }
/** * @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) }
/** * @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) }
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) }