void estimator_update_state_gps( void ) { float gps_east = gps_utm_east / 100.; float gps_north = gps_utm_north / 100.; /* Relative position to reference */ gps_east -= nav_utm_east0; gps_north -= nav_utm_north0; EstimatorSetPosXY(gps_east, gps_north); #ifndef USE_BARO_ETS float falt = gps_alt / 100.; EstimatorSetAlt(falt); #endif float fspeed = gps_gspeed / 100.; float fclimb = gps_climb / 100.; float fcourse = RadOfDeg(gps_course / 10.); EstimatorSetSpeedPol(fspeed, fcourse, fclimb); // Heading estimator from wind-information, usually computed with -DWIND_INFO // wind_north and wind_east initialized to 0, so still correct if not updated float w_vn = cosf(estimator_hspeed_dir) * estimator_hspeed_mod - wind_north; float w_ve = sinf(estimator_hspeed_dir) * estimator_hspeed_mod - wind_east; estimator_psi = atan2f(w_ve, w_vn); if (estimator_psi < 0.) estimator_psi += 2 * M_PI; #ifdef EXTRA_DOWNLINK_DEVICE DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta); #endif }
void periodic_task_ap( void ) { static uint8_t _60Hz = 0; static uint8_t _20Hz = 0; static uint8_t _10Hz = 0; static uint8_t _4Hz = 0; static uint8_t _1Hz = 0; #ifdef USE_IMU // Run at PERIODIC_FREQUENCY (60Hz if not defined) imu_periodic(); #endif // USE_IMU #define _check_periodic_freq_ PERIODIC_FREQUENCY % 60 #if _check_periodic_freq_ #error Using HighSpeed Periodic: PERIODIC_FREQUENCY has to be a multiple of 60! #endif _60Hz++; if (_60Hz >= (PERIODIC_FREQUENCY / 60)) { _60Hz = 0; } else { return; } // Rest of the periodic function still runs at 60Hz like always _20Hz++; if (_20Hz>=3) _20Hz=0; _10Hz++; if (_10Hz>=6) _10Hz=0; _4Hz++; if (_4Hz>=15) _4Hz=0; _1Hz++; if (_1Hz>=60) _1Hz=0; reporting_task(); if (!_1Hz) { if (estimator_flight_time) estimator_flight_time++; #if defined DATALINK || defined SITL datalink_time++; #endif static uint8_t t = 0; if (vsupply < LOW_BATTERY_DECIVOLT) t++; else t = 0; kill_throttle |= (t >= LOW_BATTERY_DELAY); kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE)); } switch(_4Hz) { case 0: #ifdef SITL #ifdef GPS_TRIGGERED_FUNCTION GPS_TRIGGERED_FUNCTION(); #endif #endif estimator_propagate_state(); #ifdef EXTRA_DOWNLINK_DEVICE DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta); #endif navigation_task(); break; case 1: if (!estimator_flight_time && estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) { estimator_flight_time = 1; launch = TRUE; /* Not set in non auto launch */ DOWNLINK_SEND_TAKEOFF(DefaultChannel, &cpu_time_sec); default: break; } break; #ifdef USE_GPIO case 3: GpioUpdate1(); break; #endif /* default: */ } #ifndef CONTROL_RATE #define CONTROL_RATE 20 #endif #if CONTROL_RATE != 60 && CONTROL_RATE != 20 #error "Only 20 and 60 allowed for CONTROL_RATE" #endif #if CONTROL_RATE == 20 if (!_20Hz) #endif { #ifndef AHRS_TRIGGERED_ATTITUDE_LOOP attitude_loop(); #endif } modules_periodic_task(); }
static void send_attitude(void) { struct FloatEulers* att = stateGetNedToBodyEulers_f(); DOWNLINK_SEND_ATTITUDE(DefaultChannel, DefaultDevice, &(att->phi), &(att->psi), &(att->theta)); };
void periodic_task_ap( void ) { static uint8_t _20Hz = 0; static uint8_t _10Hz = 0; static uint8_t _4Hz = 0; static uint8_t _1Hz = 0; _20Hz++; if (_20Hz>=3) _20Hz=0; _10Hz++; if (_10Hz>=6) _10Hz=0; _4Hz++; if (_4Hz>=15) _4Hz=0; _1Hz++; if (_1Hz>=60) _1Hz=0; reporting_task(); if (!_1Hz) { if (estimator_flight_time) estimator_flight_time++; #if defined DATALINK || defined SITL datalink_time++; #endif static uint8_t t = 0; if (vsupply < LOW_BATTERY_DECIVOLT) t++; else t = 0; kill_throttle |= (t >= LOW_BATTERY_DELAY); kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE)); } switch(_4Hz) { case 0: #ifdef SITL #ifdef GPS_TRIGGERED_FUNCTION GPS_TRIGGERED_FUNCTION(); #endif #endif estimator_propagate_state(); #ifdef EXTRA_DOWNLINK_DEVICE DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta); #endif navigation_task(); break; case 1: if (!estimator_flight_time && estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) { estimator_flight_time = 1; launch = TRUE; /* Not set in non auto launch */ DOWNLINK_SEND_TAKEOFF(DefaultChannel, &cpu_time_sec); default: break; } break; #ifdef USE_GPIO case 3: GpioUpdate1(); break; #endif /* default: */ } #ifndef CONTROL_RATE #define CONTROL_RATE 20 #endif #if CONTROL_RATE != 60 && CONTROL_RATE != 20 #error "Only 20 and 60 allowed for CONTROL_RATE" #endif #ifdef USE_ANALOG_IMU if (!_20Hz) { imu_periodic(); } #endif // USE_ANALOG_IMU #if CONTROL_RATE == 20 if (!_20Hz) #endif { #ifdef USE_GYRO gyro_update(); #endif #ifdef USE_INFRARED infrared_update(); estimator_update_state_infrared(); #endif /* USE_INFRARED */ h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */ v_ctl_throttle_slew(); ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed; ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint; ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint; #if defined MCU_SPI_LINK link_mcu_send(); #elif defined INTER_MCU && defined SINGLE_MCU /**Directly set the flag indicating to FBW that shared buffer is available*/ inter_mcu_received_ap = TRUE; #endif } modules_periodic_task(); }