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(); command_roll_trim = ap_state->command_roll_trim; command_pitch_trim = ap_state->command_pitch_trim; 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 /* 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 }
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; command_yaw_trim = ap_state->command_yaw_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 #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 (;;) {} } #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 if (ppm_valid) { ppm_valid = FALSE; radio_control_event_task(); if (rc_values_contains_avg_channels) { fbw_mode = FBW_MODE_OF_PPRZ(rc_values[RADIO_MODE]); } if (fbw_mode == FBW_MODE_MANUAL) SetCommandsFromRC(commands); #ifdef CTL_GRZ if (fbw_mode == FBW_MODE_MANUAL) ctl_grz_set_setpoints_rate(); /* else if (fbw_mode == FBW_MODE_AUTO) { */ /* SetCommandsFromRC(commands); */ /* } */ #endif /* CTL_GRZ */ } #endif #ifdef INTER_MCU #ifdef MCU_SPI_LINK if (spi_message_received) { /* Got a message on SPI. */ spi_message_received = FALSE; /* Sets link_mcu_received */ /* Sets inter_mcu_received_ap if checksum is ok */ link_mcu_event_task(); } #endif /* MCU_SPI_LINK */ if (inter_mcu_received_ap) { inter_mcu_received_ap = FALSE; inter_mcu_event_task(); if (fbw_mode == FBW_MODE_AUTO) { SetCommands(ap_state->commands); } #ifdef SINGLE_MCU inter_mcu_fill_fbw_state(); #endif /**Else the buffer is filled even if the last receive was not correct */ } #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 LINK_IMU if (spi_message_received) { /* Got a message on SPI. */ spi_message_received = FALSE; link_imu_event_task(); EstimatorSetAtt(link_imu_state.eulers[AXIS_X], link_imu_state.eulers[AXIS_Z], link_imu_state.eulers[AXIS_Y]); #ifdef CTL_GRZ ctl_grz_set_measures(); #endif /* CTL_GRZ */ } #endif /* LINK_IMU */ }