/********** 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 */ }