/************* PERIODIC ******************************************************/ void periodic_task_fbw( void ) { #ifdef RADIO_CONTROL radio_control_periodic_task(); if (fbw_mode == FBW_MODE_MANUAL && radio_control.status == RC_REALLY_LOST) { #ifdef OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP #warning WARNING DANGER: OUTBACK_CHALLENGE RULE RC_LOST_NO_AP defined. If you loose RC you will NOT go to automatically go to AUTO2 Anymore!! set_failsafe_mode(); #else fbw_mode = FBW_MODE_AUTO; #endif } #endif #ifdef INTER_MCU inter_mcu_periodic_task(); if (fbw_mode == FBW_MODE_AUTO && !ap_ok) { set_failsafe_mode(); } #endif #ifdef MCU_UART_LINK inter_mcu_fill_fbw_state(); link_mcu_periodic_task(); #endif #ifdef DOWNLINK fbw_downlink_periodic_task(); #endif }
/************* 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 (radio_control.status == RC_REALLY_LOST) { fbw_mode = fbw_rc_really_lost_mode; // pprz_mode= PPRZ_MODE_HOME; } if (fbw_mode == FBW_MODE_FAILSAFE) set_failsafe_mode(); if (fbw_mode == FBW_MODE_MANUAL && radio_control.status == RC_REALLY_LOST) { fbw_mode = FBW_MODE_AUTO; } #endif /*if (radio_control.status == RC_REALLY_LOST) { fbw_mode = FBW_MODE_FAILSAFE; set_failsafe_mode(); }*/ if (radio_control.status==RC_OK) { if (pprz_mode == PPRZ_MODE_MANUAL) { fbw_mode = FBW_MODE_MANUAL; } else { if ((pprz_mode == PPRZ_MODE_AUTO1) || (pprz_mode == PPRZ_MODE_AUTO2)) fbw_mode = FBW_MODE_AUTO; } } #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 if (!_10Hz) { electrical_periodic(); } #ifdef ACTUATORS #ifdef USE_RC_GYRO if ((fbw_mode == FBW_MODE_MANUAL) || (fbw_mode==FBW_MODE_AUTO)) { rc_gyro_apply_damping(commands); SetActuatorsFromCommands(rc_gyro_damped_commands) }
/************* 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 && radio_control.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 if (!_10Hz) { electrical_periodic(); } }
void post_inter_mcu_received_ap(void) { int crash = 0; if (commands[COMMAND_FORCECRASH] >= 8000) { set_failsafe_mode(); crash = 1; } }
/************* PERIODIC ******************************************************/ void periodic_task_fbw(void) { #ifdef FBW_DATALINK fbw_datalink_periodic(); #endif #ifdef RADIO_CONTROL radio_control_periodic_task(); if (fbw_mode == FBW_MODE_MANUAL && radio_control.status == RC_REALLY_LOST) { #if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP #warning WARNING DANGER: OUTBACK_CHALLENGE RULE RC_LOST_NO_AP defined. If you loose RC you will NOT go to automatically go to AUTO2 Anymore!! set_failsafe_mode(); #if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE #warning WARNING DANGER: OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE defined. If you ever temporarly lost RC while in manual, you will failsafe forever even if RC is restored commands[COMMAND_FORCECRASH] = 9600; #endif #else fbw_mode = FBW_MODE_AUTO; #endif } #endif #ifdef INTER_MCU inter_mcu_periodic_task(); if (fbw_mode == FBW_MODE_AUTO && !ap_ok) { set_failsafe_mode(); } #endif #ifdef MCU_UART_LINK inter_mcu_fill_fbw_state(); link_mcu_periodic_task(); #endif #ifdef MCU_CAN_LINK inter_mcu_fill_fbw_state(); link_mcu_periodic_task(); #endif #if PERIODIC_TELEMETRY periodic_telemetry_send_Fbw(&(DefaultChannel).trans_tx, &(DefaultDevice).device); #endif }
/** * Handles inter_mcu periodic checks */ void inter_mcu_periodic_handle(void) { inter_mcu_periodic_task(); if (fbw_mode == FBW_MODE_AUTO && !ap_ok) { set_failsafe_mode(); } #if defined MCU_UART_LINK || defined MCU_CAN_LINK inter_mcu_fill_fbw_state(); link_mcu_periodic_task(); #endif /* defined MCU_UART_LINK || defined MCU_CAN_LINK */ }
/************* 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 }
/************* PERIODIC ******************************************************/ void periodic_task_fbw( void ) { #ifdef RADIO_CONTROL radio_control_periodic_task(); if (fbw_mode == FBW_MODE_MANUAL && radio_control.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 }
void radio_lost_mode(void) { set_failsafe_mode(); }
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 }