STATIC_INLINE void main_periodic(void)
{
#if USE_IMU
imu_periodic();
#endif
//FIXME: temporary hack, remove me
#ifdef InsPeriodic
InsPeriodic();
#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());
}
static void check_radio_lost(void)
{
#ifdef PASSTHROUGH_CYGNUS
return;
#endif
if (radio_control.status == RC_REALLY_LOST) {
const static int32_t commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE;
SetActuatorsFromCommands(commands_failsafe);
}
}
void event_task_fbw( void) {
#ifdef RADIO_CONTROL
RadioControlEvent(handle_rc_frame);
#endif
#ifdef INTER_MCU
#ifdef MCU_SPI_LINK
link_mcu_event_task();
#endif /* MCU_SPI_LINK */
if (inter_mcu_received_ap) {
inter_mcu_received_ap = FALSE;
inter_mcu_event_task();
if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
fbw_mode = FBW_MODE_AUTO;
}
if (fbw_mode == FBW_MODE_AUTO) {
SetCommands(ap_state->commands);
}
#ifdef SetApOnlyCommands
else
{
SetApOnlyCommands(ap_state->commands);
}
#endif
fbw_new_actuators = 1;
#ifdef SINGLE_MCU
inter_mcu_fill_fbw_state();
#endif /**Else the buffer is filled even if the last receive was not correct */
}
#ifdef ACTUATORS
if (fbw_new_actuators > 0)
{
SetActuatorsFromCommands(commands);
fbw_new_actuators = 0;
}
#endif
#ifdef MCU_SPI_LINK
if (link_mcu_received) {
link_mcu_received = FALSE;
inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
link_mcu_restart(); /** Prepares the next SPI communication */
}
#endif /* MCU_SPI_LINK */
#endif /* INTER_MCU */
}
STATIC_INLINE void main_periodic( void ) {
imu_periodic();
/* run control loops */
autopilot_periodic();
/* set actuators */
//actuators_set(autopilot_motors_on);
SetActuatorsFromCommands(commands, autopilot_mode);
if (autopilot_in_flight) {
RunOnceEvery(PERIODIC_FREQUENCY, { autopilot_flight_time++; datalink_time++; });
}
/* 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_task(void)
{
/* Simply set current roll/pitch as commands.
* Scale DEMO_MAX_ROLL/PITCH to MAX_PPRZ (the max commands)
*/
commands[COMMAND_ROLL] = stateGetNedToBodyEulers_f()->phi * MAX_PPRZ / DEMO_MAX_ROLL;
commands[COMMAND_PITCH] = stateGetNedToBodyEulers_f()->theta * MAX_PPRZ / DEMO_MAX_ROLL;
/* generated macro from airframe file, seconds AP_MODE param not used */
SetActuatorsFromCommands(commands, 0);
if (sys_time.nb_sec > 1) {
imu_periodic();
}
RunOnceEvery(10, { LED_PERIODIC();});
void main_periodic(void)
{
/* run control loops */
autopilot_periodic();
/* set actuators */
//actuators_set(autopilot_get_motors_on());
SetActuatorsFromCommands(commands, autopilot_get_mode());
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());
}
/************* PERIODIC ******************************************************/
void periodic_task_fbw( void ) {
static uint8_t _10Hz; /* FIXME : sys_time should provide it */
_10Hz++;
if (_10Hz >= 6) _10Hz = 0;
#ifdef RADIO_CONTROL
radio_control_periodic_task();
if (fbw_mode == FBW_MODE_MANUAL && rc_status == RC_REALLY_LOST) {
fbw_mode = FBW_MODE_AUTO;
}
#endif
#ifdef INTER_MCU
inter_mcu_periodic_task();
if (fbw_mode == FBW_MODE_AUTO && !ap_ok)
set_failsafe_mode();
#endif
#ifdef DOWNLINK
fbw_downlink_periodic_task();
#endif
#ifdef ADC
if (!_10Hz)
fbw_vsupply_decivolt = VoltageOfAdc((10*(vsupply_adc_buf.sum/vsupply_adc_buf.av_nb_sample)));
#endif
#ifdef ACTUATORS
#ifdef CTL_GRZ
if (rc_status == RC_REALLY_LOST) {
set_failsafe_mode();
} else {
ctl_grz_rate_run();
}
#endif /* CTL_GRZ */
SetActuatorsFromCommands(commands);
#endif
}
void update_actuators(void)
{
if (fbw_new_actuators > 0) {
pprz_t trimmed_commands[COMMANDS_NB];
int i;
for (i = 0; i < COMMANDS_NB; i++) {trimmed_commands[i] = commands[i];}
#ifdef COMMAND_ROLL
trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ / 10);
#endif /* COMMAND_ROLL */
#ifdef COMMAND_PITCH
trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ / 10);
#endif /* COMMAND_PITCH */
#ifdef COMMAND_YAW
trimmed_commands[COMMAND_YAW] += ChopAbs(command_yaw_trim, MAX_PPRZ);
#endif /* COMMAND_YAW */
SetActuatorsFromCommands(trimmed_commands, autopilot_get_mode());
fbw_new_actuators = 0;
}
}
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());
}
void event_task_fbw( void) {
#ifdef RADIO_CONTROL
RadioControlEvent(handle_rc_frame);
#endif
i2c_event();
#ifdef INTER_MCU
#ifdef MCU_SPI_LINK
link_mcu_event_task();
#endif /* MCU_SPI_LINK */
if (inter_mcu_received_ap) {
inter_mcu_received_ap = FALSE;
inter_mcu_event_task();
if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
fbw_mode = FBW_MODE_AUTO;
}
if (fbw_mode == FBW_MODE_AUTO) {
SetCommands(ap_state->commands);
}
#ifdef SetApOnlyCommands
else
{
SetApOnlyCommands(ap_state->commands);
}
#endif
fbw_new_actuators = 1;
#ifdef SINGLE_MCU
inter_mcu_fill_fbw_state();
#endif /**Else the buffer is filled even if the last receive was not correct */
}
#ifdef ACTUATORS
if (fbw_new_actuators > 0)
{
pprz_t trimmed_commands[COMMANDS_NB];
int i;
for(i = 0; i < COMMANDS_NB; i++) trimmed_commands[i] = commands[i];
#ifdef COMMAND_ROLL
trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ/10);
#endif
#ifdef COMMAND_PITCH
trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ/10);
#endif
SetActuatorsFromCommands(trimmed_commands);
fbw_new_actuators = 0;
}
#endif
#ifdef MCU_SPI_LINK
if (link_mcu_received) {
link_mcu_received = FALSE;
inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
link_mcu_restart(); /** Prepares the next SPI communication */
}
#endif /* MCU_SPI_LINK */
#endif /* INTER_MCU */
}
void event_task_fbw( void) {
#ifdef RADIO_CONTROL
RadioControlEvent(handle_rc_frame);
#endif
i2c_event();
#ifdef INTER_MCU
#if defined MCU_SPI_LINK | defined MCU_UART_LINK
link_mcu_event_task();
#endif /* MCU_SPI_LINK */
if (inter_mcu_received_ap) {
inter_mcu_received_ap = FALSE;
inter_mcu_event_task();
command_roll_trim = ap_state->command_roll_trim;
command_pitch_trim = ap_state->command_pitch_trim;
#ifndef OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
fbw_mode = FBW_MODE_AUTO;
}
#endif
if (fbw_mode == FBW_MODE_AUTO) {
SetCommands(ap_state->commands);
}
#ifdef SetApOnlyCommands
else
{
SetApOnlyCommands(ap_state->commands);
}
#endif
fbw_new_actuators = 1;
#ifdef SINGLE_MCU
inter_mcu_fill_fbw_state();
#endif /**Else the buffer is filled even if the last receive was not correct */
}
#if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
#warning DANGER DANGER DANGER DANGER: Outback Challenge Rule FORCE-CRASH-RULE: DANGER DANGER: AP is now capable to FORCE your FBW in failsafe mode EVEN IF RC IS NOT LOST: Consider the consequences.
int crash = 0;
if (commands[COMMAND_FORCECRASH] >= 8000)
{
set_failsafe_mode();
crash = 1;
}
#endif
#ifdef ACTUATORS
if (fbw_new_actuators > 0)
{
pprz_t trimmed_commands[COMMANDS_NB];
int i;
for(i = 0; i < COMMANDS_NB; i++) trimmed_commands[i] = commands[i];
#ifdef COMMAND_ROLL
trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ/10);
#endif
#ifdef COMMAND_PITCH
trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ/10);
#endif
SetActuatorsFromCommands(trimmed_commands);
fbw_new_actuators = 0;
#if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
if (crash == 1)
{
for (;;) {}
}
#endif
}
#endif
#ifdef MCU_SPI_LINK
if (link_mcu_received) {
link_mcu_received = FALSE;
inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
link_mcu_restart(); /** Prepares the next SPI communication */
}
#endif /* MCU_SPI_LINK */
#endif /* INTER_MCU */
}
void event_task_fbw(void)
{
#ifdef RADIO_CONTROL
RadioControlEvent(handle_rc_frame);
#endif
/* event functions for mcu peripherals, like i2c, uart, etc.. */
mcu_event();
#ifdef INTER_MCU
#if defined MCU_SPI_LINK | defined MCU_UART_LINK
link_mcu_event_task();
#endif /* MCU_SPI_LINK */
#if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE
#warning OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_NO_AP_MUST_FAILSAFE loose ap is forced crash
if (ap_ok) {
ap_has_been_ok = TRUE;
}
if ((ap_has_been_ok) && (!ap_ok)) {
commands[COMMAND_FORCECRASH] = 9600;
}
#endif
if (inter_mcu_received_ap) {
inter_mcu_received_ap = FALSE;
inter_mcu_event_task();
command_roll_trim = ap_state->command_roll_trim;
command_pitch_trim = ap_state->command_pitch_trim;
command_yaw_trim = ap_state->command_yaw_trim;
#if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP
// LOST-RC: do NOT go to autonomous
// auto = stay in auto
// manual = stay in manual
#else
if (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
fbw_mode = FBW_MODE_AUTO;
}
#endif
if (fbw_mode == FBW_MODE_AUTO) {
SetCommands(ap_state->commands);
}
#ifdef SetApOnlyCommands
else {
SetApOnlyCommands(ap_state->commands);
}
#endif
fbw_new_actuators = 1;
#ifdef SINGLE_MCU
inter_mcu_fill_fbw_state();
#endif /**Else the buffer is filled even if the last receive was not correct */
}
#if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
#warning DANGER DANGER DANGER DANGER: Outback Challenge Rule FORCE-CRASH-RULE: DANGER DANGER: AP is now capable to FORCE your FBW in failsafe mode EVEN IF RC IS NOT LOST: Consider the consequences.
// OUTBACK: JURY REQUEST FLIGHT TERMINATION
int crash = 0;
if (commands[COMMAND_FORCECRASH] >= 8000) {
set_failsafe_mode();
crash = 1;
}
#endif
#ifdef ACTUATORS
if (fbw_new_actuators > 0) {
pprz_t trimmed_commands[COMMANDS_NB];
int i;
for (i = 0; i < COMMANDS_NB; i++) { trimmed_commands[i] = commands[i]; }
#ifdef COMMAND_ROLL
trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ / 10);
#endif
#ifdef COMMAND_PITCH
trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ / 10);
#endif
#ifdef COMMAND_YAW
trimmed_commands[COMMAND_YAW] += ChopAbs(command_yaw_trim, MAX_PPRZ);
#endif
SetActuatorsFromCommands(trimmed_commands, autopilot_mode);
fbw_new_actuators = 0;
#if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE
if (crash == 1) {
for (;;) {
#if FBW_DATALINK
fbw_datalink_event();
#endif
}
}
#endif
}
#endif
#ifdef MCU_SPI_LINK
if (link_mcu_received) {
link_mcu_received = FALSE;
inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
link_mcu_restart(); /** Prepares the next SPI communication */
}
#endif /* MCU_SPI_LINK */
#endif /* INTER_MCU */
#ifdef FBW_DATALINK
fbw_datalink_event();
#endif
}
