void autopilot_periodic(void) {
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
#ifdef FAILSAFE_GROUND_DETECT
if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_detect_ground) {
autopilot_set_mode(AP_MODE_KILL);
autopilot_detect_ground = FALSE;
}
#endif
if ( !autopilot_motors_on ||
#ifndef FAILSAFE_GROUND_DETECT
autopilot_mode == AP_MODE_FAILSAFE ||
#endif
autopilot_mode == AP_MODE_KILL ) {
SetCommands(commands_failsafe,
autopilot_in_flight, autopilot_motors_on);
}
else {
guidance_v_run( autopilot_in_flight );
guidance_h_run( autopilot_in_flight );
SetCommands(stabilization_cmd,
autopilot_in_flight, autopilot_motors_on);
}
}
void autopilot_periodic(void) {
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
#ifdef FAILSAFE_GROUND_DETECT
if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_detect_ground) {
autopilot_set_mode(AP_MODE_KILL);
autopilot_detect_ground = FALSE;
}
#endif
if ( !autopilot_motors_on ||
#ifndef FAILSAFE_GROUND_DETECT
autopilot_mode == AP_MODE_FAILSAFE ||
#endif
autopilot_mode == AP_MODE_KILL ) {
SetCommands(commands_failsafe,
autopilot_in_flight, autopilot_motors_on);
}
else {
guidance_v_run( autopilot_in_flight );
guidance_h_run( autopilot_in_flight );
SetCommands(stabilization_cmd,
autopilot_in_flight, autopilot_motors_on);
}
#ifdef AUTOPILOT_LOBATT_WING_WAGGLE
if (electrical.vsupply < (MIN_BAT_LEVEL * 10)){
RunOnceEvery(autopilot_lobatt_wing_waggle_interval,{setpoint_lobatt_wing_waggle_num=0;})
}
/********** 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
}
/** Callback when we received an RC frame */
static void fbw_on_rc_frame(void)
{
/* get autopilot fbw mode as set by RADIO_MODE 3-way switch */
if (radio_control.values[RADIO_FBW_MODE] < (MIN_PPRZ / 2)) {
fbw_mode = FBW_MODE_MANUAL;
} else {
fbw_mode = FBW_MODE_AUTO;
}
/* Failsafe check if intermcu is lost while AP was in control */
if ((intermcu.status == INTERMCU_LOST) &&
(fbw_mode == FBW_MODE_AUTO)) {
fbw_mode = AP_LOST_FBW_MODE;
}
/* If the FBW is in control */
if (fbw_mode == FBW_MODE_MANUAL) {
fbw_motors_on = true;
SetCommands(commands_failsafe);
#ifdef SetCommandsFromRC
SetCommandsFromRC(commands, radio_control.values);
#else
#warning "FBW: needs commands from RC in order to be useful."
#endif
}
/* Forward radiocontrol to AP */
intermcu_on_rc_frame(fbw_mode);
}
/** Callback when receive commands from the AP */
static void fbw_on_ap_command(void)
{
// Only set the command from AP when we are in AUTO mode
if (fbw_mode == FBW_MODE_AUTO) {
SetCommands(intermcu_commands);
}
}
static void autopilot_on_rc_frame(void)
{
/* get autopilot fbw mode as set by RADIO_MODE 3-way switch */
if (radio_control.values[RADIO_FBW_MODE] < (MIN_PPRZ / 2)) {
//TODO, check whether the aircraft can actually be flown in manual mode
//most rotory aircraft can't, at least not without additional IMU aid
//for now, just turn set to failsafe instead of manual mode.
fbw_mode = FBW_MODE_FAILSAFE;
} else {
fbw_mode = FBW_MODE_AUTO;
}
/* Trying to switch to auto when AP is lost */
if ((inter_mcu.status == INTERMCU_LOST) &&
(fbw_mode == FBW_MODE_AUTO)) {
fbw_mode = AP_LOST_FBW_MODE;
}
/* if manual */
if (fbw_mode == FBW_MODE_MANUAL) {
autopilot_motors_on = true;
SetCommands(commands_failsafe);
#ifdef SetCommandsFromRC
SetCommandsFromRC(commands, radio_control.values);
#else
#warning "FBW: needs commands from RC in order to be useful."
#endif
}
/* Forward radiocontrol to AP */
intermcu_on_rc_frame(fbw_mode);
}
/********** 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
}
void autopilot_periodic(void)
{
RunOnceEvery(NAV_PRESCALER, compute_dist2_to_home());
if (autopilot_in_flight && autopilot_mode == AP_MODE_NAV) {
if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) {
if (dist2_to_home > failsafe_mode_dist2) {
autopilot_set_mode(FAILSAFE_MODE_TOO_FAR_FROM_HOME);
} else {
autopilot_set_mode(AP_MODE_HOME);
}
}
}
if (autopilot_mode == AP_MODE_HOME) {
RunOnceEvery(NAV_PRESCALER, nav_home());
} else {
// otherwise always call nav_periodic_task so that carrot is always updated in GCS for other modes
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
}
/* If in FAILSAFE mode and either already not in_flight anymore
* or just "detected" ground, go to KILL mode.
*/
if (autopilot_mode == AP_MODE_FAILSAFE) {
if (!autopilot_in_flight) {
autopilot_set_mode(AP_MODE_KILL);
}
#if FAILSAFE_GROUND_DETECT
INFO("Using FAILSAFE_GROUND_DETECT: KILL")
if (autopilot_ground_detected) {
autopilot_set_mode(AP_MODE_KILL);
}
#endif
}
/* Reset ground detection _after_ running flight plan
*/
if (!autopilot_in_flight) {
autopilot_ground_detected = false;
autopilot_detect_ground_once = false;
}
/* Set fixed "failsafe" commands from airframe file if in KILL mode.
* If in FAILSAFE mode, run normal loops with failsafe attitude and
* downwards velocity setpoints.
*/
if (autopilot_mode == AP_MODE_KILL) {
SetCommands(commands_failsafe);
} else {
guidance_v_run(autopilot_in_flight);
guidance_h_run(autopilot_in_flight);
SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
}
}
/********** 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 */
}
static void autopilot_on_ap_command(void)
{
if (fbw_mode != FBW_MODE_MANUAL) {
SetCommands(intermcu_commands);
} else if (fbw_mode == FBW_MODE_AUTO) {
autopilot_motors_on = true;
} else {
autopilot_motors_on = false;
}
}
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 */
}
void autopilot_periodic(void) {
if (autopilot_in_flight) {
if (too_far_from_home) {
if (dist2_to_home > failsafe_mode_dist2)
autopilot_set_mode(FAILSAFE_MODE_TOO_FAR_FROM_HOME);
else
autopilot_set_mode(AP_MODE_HOME);
}
}
if (autopilot_mode == AP_MODE_HOME) {
RunOnceEvery(NAV_PRESCALER, nav_home());
}
else {
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
}
/* If in FAILSAFE mode and either already not in_flight anymore
* or just "detected" ground, go to KILL mode.
*/
if (autopilot_mode == AP_MODE_FAILSAFE) {
if (!autopilot_in_flight)
autopilot_set_mode(AP_MODE_KILL);
#if FAILSAFE_GROUND_DETECT
INFO("Using FAILSAFE_GROUND_DETECT: KILL")
if (autopilot_ground_detected)
autopilot_set_mode(AP_MODE_KILL);
#endif
}
/* Reset ground detection _after_ running flight plan
*/
if (!autopilot_in_flight || autopilot_ground_detected) {
autopilot_ground_detected = FALSE;
autopilot_detect_ground_once = FALSE;
}
/* Set fixed "failsafe" commands from airframe file if in KILL mode.
* If in FAILSAFE mode, run normal loops with failsafe attitude and
* downwards velocity setpoints.
*/
if (autopilot_mode == AP_MODE_KILL) {
SetCommands(commands_failsafe);
}
else {
guidance_v_run( autopilot_in_flight );
guidance_h_run( autopilot_in_flight );
SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
}
}
/* 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());
}
/********** 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
}
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 autopilot_periodic(void) {
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
#if FAILSAFE_GROUND_DETECT
INFO("Using FAILSAFE_GROUND_DETECT")
if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_detect_ground) {
autopilot_set_mode(AP_MODE_KILL);
autopilot_detect_ground = FALSE;
}
#endif
/* set failsafe commands, if in FAILSAFE or KILL mode */
#if !FAILSAFE_GROUND_DETECT
if (autopilot_mode == AP_MODE_KILL ||
autopilot_mode == AP_MODE_FAILSAFE) {
#else
if (autopilot_mode == AP_MODE_KILL) {
#endif
SetCommands(commands_failsafe);
}
else {
guidance_v_run( autopilot_in_flight );
guidance_h_run( autopilot_in_flight );
SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
}
}
void autopilot_set_mode(uint8_t new_autopilot_mode) {
/* force kill mode as long as AHRS is not aligned */
if (!ahrs_is_aligned())
new_autopilot_mode = AP_MODE_KILL;
if (new_autopilot_mode != autopilot_mode) {
/* horizontal mode */
switch (new_autopilot_mode) {
case AP_MODE_FAILSAFE:
#ifndef KILL_AS_FAILSAFE
stab_att_sp_euler.phi = 0;
stab_att_sp_euler.theta = 0;
guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
break;
#endif
case AP_MODE_KILL:
autopilot_set_motors_on(FALSE);
autopilot_in_flight = FALSE;
autopilot_in_flight_counter = 0;
guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
break;
case AP_MODE_RC_DIRECT:
guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT);
break;
case AP_MODE_RATE_DIRECT:
case AP_MODE_RATE_Z_HOLD:
guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
break;
case AP_MODE_ATTITUDE_RC_CLIMB:
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_ATTITUDE_Z_HOLD:
guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
break;
case AP_MODE_FORWARD:
guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD);
break;
case AP_MODE_CARE_FREE_DIRECT:
guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE);
break;
case AP_MODE_HOVER_DIRECT:
case AP_MODE_HOVER_CLIMB:
case AP_MODE_HOVER_Z_HOLD:
guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER);
break;
case AP_MODE_NAV:
guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
break;
default:
break;
}
/* vertical mode */
switch (new_autopilot_mode) {
case AP_MODE_FAILSAFE:
#ifndef KILL_AS_FAILSAFE
guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5);
guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
break;
#endif
case AP_MODE_KILL:
guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
break;
case AP_MODE_RC_DIRECT:
case AP_MODE_RATE_DIRECT:
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_HOVER_DIRECT:
case AP_MODE_CARE_FREE_DIRECT:
case AP_MODE_FORWARD:
guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT);
break;
case AP_MODE_RATE_RC_CLIMB:
case AP_MODE_ATTITUDE_RC_CLIMB:
guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB);
break;
case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_HOVER_CLIMB:
guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
break;
case AP_MODE_RATE_Z_HOLD:
case AP_MODE_ATTITUDE_Z_HOLD:
case AP_MODE_HOVER_Z_HOLD:
guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER);
break;
case AP_MODE_NAV:
guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
break;
default:
break;
}
autopilot_mode = new_autopilot_mode;
}
}
static inline void autopilot_check_in_flight( bool_t motors_on ) {
if (autopilot_in_flight) {
if (autopilot_in_flight_counter > 0) {
if (THROTTLE_STICK_DOWN()) {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
autopilot_in_flight = FALSE;
}
}
else { /* !THROTTLE_STICK_DOWN */
autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
}
}
}
else { /* not in flight */
if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
motors_on) {
if (!THROTTLE_STICK_DOWN()) {
autopilot_in_flight_counter++;
if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME)
autopilot_in_flight = TRUE;
}
else { /* THROTTLE_STICK_DOWN */
autopilot_in_flight_counter = 0;
}
}
}
}
void autopilot_set_motors_on(bool_t motors_on) {
if (ahrs_is_aligned() && motors_on)
autopilot_motors_on = TRUE;
else
autopilot_motors_on = FALSE;
kill_throttle = ! autopilot_motors_on;
autopilot_arming_set(autopilot_motors_on);
}
void autopilot_on_rc_frame(void) {
if (kill_switch_is_on())
autopilot_set_mode(AP_MODE_KILL);
else {
uint8_t new_autopilot_mode = 0;
AP_MODE_OF_PPRZ(radio_control.values[RADIO_MODE], new_autopilot_mode);
/* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
if (!(new_autopilot_mode == AP_MODE_NAV
#if USE_GPS
&& GpsIsLost()
#endif
))
autopilot_set_mode(new_autopilot_mode);
}
/* if not in FAILSAFE mode check motor and in_flight status, read RC */
if (autopilot_mode > AP_MODE_FAILSAFE) {
/* if there are some commands that should always be set from RC, do it */
#ifdef SetAutoCommandsFromRC
SetAutoCommandsFromRC(commands, radio_control.values);
#endif
/* if not in NAV_MODE set commands from the rc */
#ifdef SetCommandsFromRC
if (autopilot_mode != AP_MODE_NAV) {
SetCommandsFromRC(commands, radio_control.values);
}
#endif
/* an arming sequence is used to start/stop motors */
autopilot_arming_check_motors_on();
kill_throttle = ! autopilot_motors_on;
autopilot_check_in_flight(autopilot_motors_on);
guidance_v_read_rc();
guidance_h_read_rc(autopilot_in_flight);
}
}
/**
* Failsafe function
*/
static inline void set_failsafe_mode(void)
{
fbw_mode = FBW_MODE_FAILSAFE;
SetCommands(commands_failsafe);
fbw_new_actuators = 1;
}
void autopilot_periodic(void) {
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
#if FAILSAFE_GROUND_DETECT
INFO("Using FAILSAFE_GROUND_DETECT")//使用模式FAILSAFE_GROUND_DETECT失效保护_
if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_detect_ground) {
autopilot_set_mode(AP_MODE_KILL);
autopilot_detect_ground = FALSE;
}
#endif
/* set failsafe commands, if in FAILSAFE or KILL mode */
#if !FAILSAFE_GROUND_DETECT
if (autopilot_mode == AP_MODE_KILL ||
autopilot_mode == AP_MODE_FAILSAFE) {
#else
if (autopilot_mode == AP_MODE_KILL) {
#endif
SetCommands(commands_failsafe);
}
else {
/* 计算向导模式下的两个方向水平和垂直方向的姿态信息*/
guidance_v_run( autopilot_in_flight );
guidance_h_run( autopilot_in_flight );
/*设置旋翼的命令：稳定模式配置，飞行模式，电机打开状态*/
SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
}
}
/*飞控模式设置函数*/
void autopilot_set_mode(uint8_t new_autopilot_mode) {
/* force kill mode as long as AHRS is not aligned
强制杀死模式只要ahrs不是均衡的 */
if (!ahrs_is_aligned())
new_autopilot_mode = AP_MODE_KILL;
/* 新的飞行模式*/
if (new_autopilot_mode != autopilot_mode) {
/* horizontal mode 水平模式 */
switch (new_autopilot_mode) {
case AP_MODE_FAILSAFE://失效保护模式
#ifndef KILL_AS_FAILSAFE
stab_att_sp_euler.phi = 0;
stab_att_sp_euler.theta = 0;
guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
break;
#endif
case AP_MODE_KILL://kill模式
autopilot_set_motors_on(FALSE);
autopilot_in_flight = FALSE;
autopilot_in_flight_counter = 0;
guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
break;
case AP_MODE_RC_DIRECT://RC指挥模式
guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT);
break;
case AP_MODE_RATE_DIRECT://速度指挥模式
case AP_MODE_RATE_Z_HOLD://Z轴（高度）速度指挥模式
guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
break;
case AP_MODE_ATTITUDE_RC_CLIMB://RC 姿态爬升模式
case AP_MODE_ATTITUDE_DIRECT://姿态向导模式
case AP_MODE_ATTITUDE_CLIMB://姿态爬升模式
case AP_MODE_ATTITUDE_Z_HOLD://高度保持模式
guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
break;
case AP_MODE_FORWARD://前进模式
guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD);
break;
case AP_MODE_CARE_FREE_DIRECT://自由模式
guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE);
break;
case AP_MODE_HOVER_DIRECT://盘旋向导模式
case AP_MODE_HOVER_CLIMB://盘旋爬升模式
case AP_MODE_HOVER_Z_HOLD://盘旋高度保持模式
guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER);
break;
case AP_MODE_NAV://导航模式
guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
break;
default:
break;
}
/* vertical mode 垂直模式*/
switch (new_autopilot_mode) {
case AP_MODE_FAILSAFE:
#ifndef KILL_AS_FAILSAFE
guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5);
guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
break;
#endif
case AP_MODE_KILL:
guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
break;
case AP_MODE_RC_DIRECT:
case AP_MODE_RATE_DIRECT:
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_HOVER_DIRECT:
case AP_MODE_CARE_FREE_DIRECT:
case AP_MODE_FORWARD:
guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT);
break;
case AP_MODE_RATE_RC_CLIMB:
case AP_MODE_ATTITUDE_RC_CLIMB:
guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB);
break;
case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_HOVER_CLIMB:
guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
break;
case AP_MODE_RATE_Z_HOLD:
case AP_MODE_ATTITUDE_Z_HOLD:
case AP_MODE_HOVER_Z_HOLD:
guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER);
break;
case AP_MODE_NAV:
guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
break;
default:
break;
}
autopilot_mode = new_autopilot_mode;
}
}
static inline void autopilot_check_in_flight( bool_t motors_on ) {
if (autopilot_in_flight) {
if (autopilot_in_flight_counter > 0) {
if (THROTTLE_STICK_DOWN()) {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
autopilot_in_flight = FALSE;
}
}
else { /* !THROTTLE_STICK_DOWN */
autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
}
}
}
else { /* not in flight */
if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
motors_on) {
if (!THROTTLE_STICK_DOWN()) {
autopilot_in_flight_counter++;
if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME)
autopilot_in_flight = TRUE;
}
else { /* THROTTLE_STICK_DOWN */
autopilot_in_flight_counter = 0;
}
}
}
}
void autopilot_set_motors_on(bool_t motors_on) {
if (ahrs_is_aligned() && motors_on)
autopilot_motors_on = TRUE;
else
autopilot_motors_on = FALSE;
kill_throttle = ! autopilot_motors_on;
autopilot_arming_set(autopilot_motors_on);
}
void autopilot_on_rc_frame(void) {
//是否关闭开关
if (kill_switch_is_on())
autopilot_set_mode(AP_MODE_KILL);//关闭自驾模式
else {
uint8_t new_autopilot_mode = 0;
AP_MODE_OF_PPRZ(radio_control.values[RADIO_MODE], new_autopilot_mode);
/* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
if (!(new_autopilot_mode == AP_MODE_NAV
#if USE_GPS
&& GpsIsLost()//GPS丢失时不要使用NAV导航模式
#endif
))
autopilot_set_mode(new_autopilot_mode);
}
/* if not in FAILSAFE mode check motor and in_flight status, read RC */
//飞行模式不是：失效保护时，检查电机和飞行状态，读RC
if (autopilot_mode > AP_MODE_FAILSAFE) {
/* if there are some commands that should always be set from RC, do it */
//如果有来自于RC的命令，则去执行命令
#ifdef SetAutoCommandsFromRC
SetAutoCommandsFromRC(commands, radio_control.values);
#endif
/* if not in NAV_MODE set commands from the rc */
//如果不是导航模式，设置来自RC的命令
#ifdef SetCommandsFromRC
if (autopilot_mode != AP_MODE_NAV) {
SetCommandsFromRC(commands, radio_control.values);
}
#endif
/* an arming sequence is used to start/stop motors */
// 一个。。。序列用来启动和关闭电机
autopilot_arming_check_motors_on();
kill_throttle = ! autopilot_motors_on;//关闭电机
autopilot_check_in_flight(autopilot_motors_on);
guidance_v_read_rc();//的垂直方向的信息
guidance_h_read_rc(autopilot_in_flight);//读EC
}
}
BatchCommand::BatchCommand(const CommandCollection& commands)
{
SetCommands(commands);
}
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
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 */
}
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;
#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 */
}
static inline void set_failsafe_mode( void ) {
fbw_mode = FBW_MODE_FAILSAFE;
SetCommands(commands_failsafe);
}
