/********** INIT *************************************************************/
void init_fbw( void ) {
mcu_init();
sys_time_init();
electrical_init();
#ifdef ACTUATORS
actuators_init();
/* Load the failsafe defaults */
SetCommands(commands_failsafe);
fbw_new_actuators = 1;
#endif
#ifdef RADIO_CONTROL
radio_control_init();
#endif
#ifdef INTER_MCU
inter_mcu_init();
#endif
#ifdef MCU_SPI_LINK
link_mcu_restart();
#endif
fbw_mode = FBW_MODE_FAILSAFE;
#ifndef SINGLE_MCU
mcu_int_enable();
#endif
}
/********** INIT *************************************************************/
void init_fbw( void ) {
mcu_init();
electrical_init();
#ifdef ACTUATORS
actuators_init();
/* Load the failsafe defaults */
SetCommands(commands_failsafe);
fbw_new_actuators = 1;
#endif
#ifdef RADIO_CONTROL
radio_control_init();
#endif
#ifdef INTER_MCU
inter_mcu_init();
#endif
#ifdef MCU_SPI_LINK
link_mcu_restart();
#endif
fbw_mode = FBW_MODE_FAILSAFE;
/**** start timers for periodic functions *****/
fbw_periodic_tid = sys_time_register_timer((1./60.), NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
#ifndef SINGLE_MCU
mcu_int_enable();
#endif
}
/********** INIT *************************************************************/
void init_fbw(void)
{
mcu_init();
#if !(DISABLE_ELECTRICAL)
electrical_init();
#endif
#ifdef ACTUATORS
actuators_init();
/* Load the failsafe defaults */
SetCommands(commands_failsafe);
fbw_new_actuators = 1;
#endif /* ACTUATORS */
#ifdef RADIO_CONTROL
radio_control_init();
#endif /* RADIO_CONTROL */
#ifdef INTER_MCU
inter_mcu_init();
#endif /* INTER_MCU */
#if defined MCU_SPI_LINK || defined MCU_CAN_LINK
link_mcu_init();
#endif /* MCU_SPI_LINK || MCU_CAN_LINK */
#ifdef MCU_SPI_LINK
link_mcu_restart();
#endif /* MCU_SPI_LINK */
fbw_mode = FBW_MODE_FAILSAFE;
/**** start timers for periodic functions *****/
fbw_periodic_tid = sys_time_register_timer((1. / 60.), NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
#ifndef SINGLE_MCU
mcu_int_enable();
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_fbw_status);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_COMMANDS, send_commands);
#ifdef ACTUATORS
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);
#endif /* ACTUATORS */
#ifdef RADIO_CONTROL
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC, send_rc);
#endif /* RADIO_CONTROL */
#endif /* PERIODIC_TELEMETRY */
}
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 */
}
/********** INIT *************************************************************/
void init_fbw( void ) {
mcu_init();
#if !(DISABLE_ELECTRICAL)
electrical_init();
#endif
#ifdef ACTUATORS
actuators_init();
/* Load the failsafe defaults */
SetCommands(commands_failsafe);
fbw_new_actuators = 1;
#endif
#ifdef RADIO_CONTROL
radio_control_init();
#endif
#ifdef INTER_MCU
inter_mcu_init();
#endif
#ifdef MCU_SPI_LINK
link_mcu_init();
link_mcu_restart();
#endif
fbw_mode = FBW_MODE_FAILSAFE;
/**** start timers for periodic functions *****/
fbw_periodic_tid = sys_time_register_timer((1./60.), NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
#ifndef SINGLE_MCU
mcu_int_enable();
#endif
#if DOWNLINK
register_periodic_telemetry(&telemetry_Fbw, "FBW_STATUS", send_fbw_status);
register_periodic_telemetry(&telemetry_Fbw, "COMMANDS", send_commands);
#ifdef ACTUATORS
register_periodic_telemetry(&telemetry_Fbw, "ACTUATORS", send_actuators);
#endif
#ifdef RADIO_CONTROL
register_periodic_telemetry(&telemetry_Fbw, "RC", send_rc);
#endif
#endif
}
/**
* Handles inter_mcu event
*/
void inter_mcu_event_handle(void)
{
#if defined MCU_SPI_LINK | defined MCU_UART_LINK
link_mcu_event_task();
#endif /* MCU_SPI_LINK */
pre_inter_mcu_received_ap();
if (inter_mcu_received_ap) {
inter_mcu_received_ap = false;
inter_mcu_event_task();
PPRZ_MUTEX_LOCK(ap_state_mtx);
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 (ap_ok && fbw_mode == FBW_MODE_FAILSAFE) {
fbw_mode = FBW_MODE_INTER_MCU_FAILSAFE;
}
if (fbw_mode == FBW_MODE_AUTO) {
SetCommands(ap_state->commands);
} else {
#if SET_AP_ONLY_COMMANDS
SetApOnlyCommands(ap_state->commands);
#endif /* SET_AP_ONLY_COMMANDS */
}
fbw_new_actuators = 1;
PPRZ_MUTEX_UNLOCK(ap_state_mtx);
#ifdef SINGLE_MCU
inter_mcu_fill_fbw_state();
#endif /* SINGLE_MCU - The buffer is filled even if the last receive was not correct */
}
post_inter_mcu_received_ap();
update_actuators();
#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 */
}
/********** INIT *************************************************************/
void init_fbw( void ) {
hw_init();
sys_time_init();
#ifdef LED
led_init();
#endif
#ifdef USE_UART0
uart0_init_tx();
#endif
#ifdef USE_UART1
uart1_init_tx();
#endif
#ifdef ADC
adc_init();
adc_buf_channel(ADC_CHANNEL_VSUPPLY, &vsupply_adc_buf, DEFAULT_AV_NB_SAMPLE);
#endif
#ifdef ACTUATORS
actuators_init();
/* Load the failsafe defaults */
SetCommands(commands_failsafe);
#endif
#ifdef RADIO_CONTROL
ppm_init();
#endif
#ifdef INTER_MCU
inter_mcu_init();
#endif
#ifdef MCU_SPI_LINK
spi_init();
link_mcu_restart();
#endif
#ifdef LINK_IMU
spi_init();
link_imu_init();
#endif
#ifdef CTL_GRZ
ctl_grz_init();
#endif
#ifndef SINGLE_MCU
int_enable();
#endif
}
/********** INIT *************************************************************/
void init_fbw( void ) //fbw初始化
{
mcu_init();//mcu初始化，如：led,uart,i2c,vocm,spi,dac
#if !(DISABLE_ELECTRICAL)
electrical_init();//供电初始化
#endif
#ifdef ACTUATORS
actuators_init();//执行器初始化
/* Load the failsafe defaults */
SetCommands(commands_failsafe);
fbw_new_actuators = 1;
#endif
#ifdef RADIO_CONTROL
radio_control_init();//无线电台初始化
#endif
#ifdef INTER_MCU
inter_mcu_init();
#endif
#ifdef MCU_SPI_LINK
link_mcu_init();
link_mcu_restart();
#endif
fbw_mode = FBW_MODE_FAILSAFE;
command_roll_trim = COMMAND_ROLL_TRIM;
command_pitch_trim = COMMAND_PITCH_TRIM;
/**** start timers for periodic functions *****/
fbw_periodic_tid = sys_time_register_timer((1./60.), NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
#ifndef SINGLE_MCU
mcu_int_enable();
#endif
}
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
}
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 */
}
