STATIC_INLINE void main_periodic(void)
{
#if INTER_MCU_AP
/* Inter-MCU watchdog */
intermcu_periodic();
#endif
/* run control loops */
autopilot_periodic();
/* set actuators */
//actuators_set(autopilot_motors_on);
#ifndef INTER_MCU_AP
SetActuatorsFromCommands(commands, autopilot_mode);
#else
intermcu_set_actuators(commands, autopilot_mode);
#endif
if (autopilot_in_flight) {
RunOnceEvery(PERIODIC_FREQUENCY, autopilot_flight_time++);
}
#if defined DATALINK || defined SITL
RunOnceEvery(PERIODIC_FREQUENCY, datalink_time++);
#endif
RunOnceEvery(10, LED_PERIODIC());
}
/* Sets the actual actuator commands */
STATIC_INLINE void main_periodic(void)
{
/* Inter-MCU watchdog */
intermcu_periodic();
/* Safety check and set FBW mode */
fbw_safety_check();
#ifdef BOARD_PX4IO
//due to a baud rate issue on PX4, for a few seconds the baud is 1500000 however this may result in package loss, causing the motors to spin at random
//to prevent this situation:
if (intermcu.stable_px4_baud != PPRZ_BAUD) {
fbw_mode = FBW_MODE_FAILSAFE;
fbw_motors_on = false;
//signal to user whether fbw can be flashed:
#ifdef FBW_MODE_LED
LED_OFF(FBW_MODE_LED); // causes really fast blinking
#endif
}
#endif
// TODO make module out of led blink?
#ifdef FBW_MODE_LED
static uint16_t dv = 0;
if (fbw_mode == FBW_MODE_FAILSAFE) {
if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);}
} else if (fbw_mode == FBW_MODE_MANUAL) {
if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);}
} else if (fbw_mode == FBW_MODE_AUTO) {
LED_ON(FBW_MODE_LED);
}
#endif // FWB_MODE_LED
/* Set failsafe commands */
if (fbw_mode == FBW_MODE_FAILSAFE) {
fbw_motors_on = false;
SetCommands(commands_failsafe);
}
/* If in auto copy autopilot motors on */
if (fbw_mode == FBW_MODE_AUTO) {
fbw_motors_on = autopilot_motors_on;
}
/* Set actuators */
SetActuatorsFromCommands(commands, autopilot_mode);
/* Periodic blinking */
RunOnceEvery(10, LED_PERIODIC());
}
STATIC_INLINE void main_periodic(void)
{
/* Inter-MCU watchdog */
intermcu_periodic();
/* Safety logic */
bool ap_lost = (inter_mcu.status == INTERMCU_LOST);
bool rc_lost = (radio_control.status == RC_REALLY_LOST);
if (rc_lost) {
if (ap_lost) {
// Both are lost
fbw_mode = FBW_MODE_FAILSAFE;
} else {
if (fbw_mode == FBW_MODE_MANUAL) {
fbw_mode = RC_LOST_FBW_MODE;
} else {
if (fbw_mode == FBW_MODE_FAILSAFE) {
// No change: failsafe stays failsafe
} else {
// Lost RC while in working Auto mode
fbw_mode = RC_LOST_IN_AUTO_FBW_MODE;
}
}
}
} else { // rc_is_good
if (fbw_mode == FBW_MODE_AUTO) {
if (ap_lost) {
fbw_mode = AP_LOST_FBW_MODE;
}
}
}
#ifdef BOARD_PX4IO
//due to a baud rate issue on PX4, for a few seconds the baud is 1500000 however this may result in package loss, causing the motors to spin at random
//to prevent this situation:
if (inter_mcu.stable_px4_baud != PPRZ_BAUD) {
fbw_mode = FBW_MODE_FAILSAFE;
autopilot_motors_on = false;
//signal to user whether fbw can be flashed:
#ifdef FBW_MODE_LED
LED_OFF(FBW_MODE_LED); // causes really fast blinking
#endif
}
#endif
static uint16_t dv = 0;
// TODO make module out of led blink?
/* set failsafe commands */
if (fbw_mode == FBW_MODE_FAILSAFE) {
autopilot_motors_on = false;
SetCommands(commands_failsafe);
#ifdef FBW_MODE_LED
if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);}
} else if (fbw_mode == FBW_MODE_MANUAL) {
if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);}
} else if (fbw_mode == FBW_MODE_AUTO) {
intermcu_blink_fbw_led(dv++);
#endif // FWB_MODE_LED
}
/* set actuators */
SetActuatorsFromCommands(commands, autopilot_mode);
/* blink */
RunOnceEvery(10, LED_PERIODIC());
}
