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 }