void handle_periodic_tasks_ap(void)
{
if (sys_time_check_and_ack_timer(sensors_tid))
sensors_task();//imu数据读取
if (sys_time_check_and_ack_timer(navigation_tid))
navigation_task();//估算期望的航线，故障保护
#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
if (sys_time_check_and_ack_timer(attitude_tid))
attitude_loop();//姿态循环
#endif
if (sys_time_check_and_ack_timer(modules_tid))
modules_periodic_task();//没有该函数说明
if (sys_time_check_and_ack_timer(monitor_tid))
monitor_task();//监听
if (sys_time_check_and_ack_timer(telemetry_tid))
{
reporting_task();//汇报
LED_PERIODIC();
}
}
void handle_periodic_tasks_ap(void) {
if (sys_time_check_and_ack_timer(sensors_tid))
sensors_task();
if (sys_time_check_and_ack_timer(navigation_tid))
navigation_task();
#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
if (sys_time_check_and_ack_timer(attitude_tid))
attitude_loop();
#endif
if (sys_time_check_and_ack_timer(modules_tid))
modules_periodic_task();
if (sys_time_check_and_ack_timer(monitor_tid))
monitor_task();
if (sys_time_check_and_ack_timer(telemetry_tid)) {
reporting_task();
LED_PERIODIC();
}
}
value sim_nav_task(value unit)
{
#if !USE_GENERATED_AUTOPILOT
navigation_task();
#endif
return unit;
}
void handle_periodic_tasks_ap(void)
{
if (sys_time_check_and_ack_timer(sensors_tid)) {
sensors_task();
}
#if USE_BARO_BOARD
if (sys_time_check_and_ack_timer(baro_tid)) {
baro_periodic();
}
#endif
#if USE_GENERATED_AUTOPILOT
if (sys_time_check_and_ack_timer(attitude_tid)) {
autopilot_periodic();
}
#else
// static autopilot
if (sys_time_check_and_ack_timer(navigation_tid)) {
navigation_task();
}
#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
if (sys_time_check_and_ack_timer(attitude_tid)) {
attitude_loop();
}
#endif
#endif
if (sys_time_check_and_ack_timer(modules_tid)) {
modules_periodic_task();
}
if (sys_time_check_and_ack_timer(monitor_tid)) {
monitor_task();
}
if (sys_time_check_and_ack_timer(telemetry_tid)) {
reporting_task();
LED_PERIODIC();
}
}
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();
}
value sim_nav_task(value unit) {
navigation_task();
return unit;
}
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();
}
/*! \brief Entry point of the audio management interface.
*
*/
void com_task(void)
{
static struct state_machine_context state_m = {
.state = STATE_INITIALIZATION,
.async_cmd = false,
.view = GUI_UPDATE_VIEW_NONE,
.view_elt = GUI_UPDATE_ELT_NONE,
.elapsed_time_timer.timer_state = CPU_TIMER_STATE_STOPPED
};
// Update the GUI
gui_update(state_m.view, &state_m.view_elt, &state_m.display_list, &state_m.info, &state_m.player_status);
// Ask the audio interface to execute pending tasks
ai_async_cmd_task();
if (state_m.async_cmd)
{
// If current command is not done
if (!is_ai_async_cmd_finished())
{
// If it is a new command that is being proceed
if (state_m.in_progress_timer.timer_state == CPU_TIMER_STATE_STOPPED)
cpu_set_timeout(cpu_ms_2_cy(500, FCPU_HZ), &state_m.in_progress_timer);
// If current command is not done and it is taking a long
else if (cpu_is_timeout(&state_m.in_progress_timer))
state_m.view_elt |= GUI_UPDATE_ELT_IN_PROGRESS;
return;
}
else
{
state_m.cmd_status = ai_async_cmd_out_status();
cpu_stop_timeout(&state_m.in_progress_timer);
}
}
// If a device is connected
if (state_m.state != STATE_INITIALIZATION &&
state_m.state != STATE_IDLE_ENTRY_POINT &&
state_m.state != STATE_IDLE_WAIT_FOR_EVENT)
{
// If no device is connected, then jump to the disconnection state
if (ai_is_none())
{
ai_command_abort();
state_m.state = STATE_DEVICE_DISCONNECTED;
}
}
switch (state_m.state)
{
case STATE_INITIALIZATION:
state_m.state = STATE_IDLE_ENTRY_POINT;
cpu_stop_timeout(&state_m.in_progress_timer);
// Set default volume if specified
#if defined(DEFAULT_VOLUME)
audio_mixer_dacs_set_volume(DEFAULT_VOLUME);
#endif
break;
case STATE_DEVICE_CONNECTED:
controller_init(FCPU_HZ, FHSB_HZ, FPBB_HZ, FPBA_HZ);
state_m.state = STATE_NAVIGATION_ENTRY_POINT;
state_m.view_elt |= GUI_UPDATE_ELT_CONNECTED;
break;
case STATE_DEVICE_DISCONNECTED:
controller_shutdown();
state_m.state = STATE_IDLE_ENTRY_POINT;
state_m.view_elt |= GUI_UPDATE_ELT_DISCONNECTED;
break;
case STATE_IDLE_ENTRY_POINT:
case STATE_IDLE_WAIT_FOR_EVENT:
case STATE_IDLE_DRIVE_LOAD:
idle_task(&state_m);
break;
case STATE_NAVIGATION_ENTRY_POINT:
case STATE_NAVIGATION_UPDATE_LIST:
case STATE_NAVIGATION_UPDATE_LIST_GET_NAME:
case STATE_NAVIGATION_UPDATE_LIST_STORE_NAME:
case STATE_NAVIGATION_UPDATE_ISDIR:
case STATE_NAVIGATION_WAIT_FOR_EVENT:
case STATE_NAVIGATION_UPDATE_STATUS:
case STATE_NAVIGATION_CD:
case STATE_NAVIGATION_GOTOPARENT:
case STATE_NAVIGATION_GOTOPARENT_ERROR_HANDLING:
case STATE_NAVIGATION_PLAY_SELECTED_FILE:
case STATE_NAVIGATION_WAIT_FOR_SELECTION:
case STATE_NAVIGATION_UPDATE_METADATA_AND_PLAY:
navigation_task(&state_m);
break;
case STATE_PLAYBACK_ENTRY_POINT:
case STATE_PLAYBACK_WAIT_FOR_EVENT:
case STATE_PLAYBACK_HANDLE_FAST_MODES:
case STATE_PLAYBACK_UPDATE_TIME:
case STATE_PLAYBACK_UPDATE_STATUS:
playback_task(&state_m);
break;
case STATE_CONFIG_ENTRY_POINT:
case STATE_CONFIG_WAIT_FOR_EVENT:
case STATE_CONFIG_UPDATE_STATES:
case STATE_CONFIG_READ_REPEAT_STATE:
case STATE_CONFIG_READ_SHUFFLE_STATE:
config_task(&state_m);
break;
case STATE_CHECK_DEVICE_ENTRY_POINT:
case STATE_CHECK_DEVICE_UPDATE_STATUS:
check_device_task(&state_m);
break;
case STATE_TRACK_CHANGED_ENTRY_POINT:
case STATE_TRACK_CHANGED_TOTAL_TIME:
case STATE_TRACK_CHANGED_FILE_NAME:
case STATE_TRACK_CHANGED_ARTIST:
case STATE_TRACK_CHANGED_TITLE:
case STATE_TRACK_CHANGED_IMAGE:
case STATE_TRACK_CHANGED_RESUME:
case STATE_TRACK_CHECK_RESUME:
track_changed_task(&state_m);
break;
case STATE_COMMAND_PLAY_ANY_SONG:
command_task(&state_m);
break;
default:
break;
}
/*
// Power sleep mode is managed here
if( usb_device_get_state()==DEVICE_STATE_NOT_CONNECTED ) {
if( cpu_is_timer_stopped(&sleep_timer) ) {
cpu_set_timeout(cpu_ms_2_cy(SLEEP_MODE_MS, FCPU_HZ), &sleep_timer);
}
else if( cpu_is_timeout(&sleep_timer) ) {
gui_enter_idle();
SLEEP(AVR32_PM_SMODE_IDLE);
gui_leave_idle();
}
} else {
cpu_stop_timeout(&sleep_timer);
}
*/
}
