void autopilot_on_rc_frame(void) {
uint8_t new_autopilot_mode = 0;
AP_MODE_OF_PPRZ(radio_control.values[RADIO_MODE], new_autopilot_mode);
autopilot_set_mode(new_autopilot_mode);
#ifdef RADIO_KILL_SWITCH
if (radio_control.values[RADIO_KILL_SWITCH] < 0)
autopilot_set_mode(AP_MODE_KILL);
#endif
#ifdef AUTOPILOT_KILL_WITHOUT_AHRS
if (!ahrs_is_aligned())
autopilot_set_mode(AP_MODE_KILL);
#endif
autopilot_check_motors_on();
autopilot_check_in_flight();
kill_throttle = !autopilot_motors_on;
if (autopilot_mode > AP_MODE_FAILSAFE) {
guidance_v_read_rc();
guidance_h_read_rc(autopilot_in_flight);
}
}
STATIC_INLINE void failsafe_check(void)
{
if (radio_control.status == RC_REALLY_LOST &&
autopilot_mode != AP_MODE_KILL &&
autopilot_mode != AP_MODE_HOME &&
autopilot_mode != AP_MODE_FAILSAFE &&
autopilot_mode != AP_MODE_NAV) {
autopilot_set_mode(RC_LOST_MODE);
}
#if FAILSAFE_ON_BAT_CRITICAL
if (autopilot_mode != AP_MODE_KILL &&
electrical.bat_critical) {
autopilot_set_mode(AP_MODE_FAILSAFE);
}
#endif
#if USE_GPS
if (autopilot_mode == AP_MODE_NAV &&
autopilot_motors_on &&
#if NO_GPS_LOST_WITH_RC_VALID
radio_control.status != RC_OK &&
#endif
GpsIsLost()) {
autopilot_set_mode(AP_MODE_FAILSAFE);
}
if (autopilot_mode == AP_MODE_HOME &&
autopilot_motors_on && GpsIsLost()) {
autopilot_set_mode(AP_MODE_FAILSAFE);
}
#endif
autopilot_check_in_flight(autopilot_motors_on);
}
void autopilot_on_rc_frame(void)
{
if (kill_switch_is_on()) {
autopilot_set_mode(AP_MODE_KILL);
} else {
#ifdef RADIO_AUTO_MODE
INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
uint8_t new_autopilot_mode = ap_mode_of_two_switches();
#else
uint8_t new_autopilot_mode = ap_mode_of_3way_switch();
#endif
/* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
if (!(new_autopilot_mode == AP_MODE_NAV && GpsIsLost())) {
/* always allow to switch to manual */
if (new_autopilot_mode == MODE_MANUAL) {
autopilot_set_mode(new_autopilot_mode);
}
/* if in HOME mode, don't allow switching to non-manual modes */
else if ((autopilot_mode != AP_MODE_HOME)
#if UNLOCKED_HOME_MODE
/* Allowed to leave home mode when UNLOCKED_HOME_MODE */
|| !too_far_from_home
#endif
) {
autopilot_set_mode(new_autopilot_mode);
}
}
}
/* an arming sequence is used to start/stop motors.
* only allow arming if ahrs is aligned
*/
if (ahrs_is_aligned()) {
autopilot_arming_check_motors_on();
kill_throttle = ! autopilot_motors_on;
}
/* if not in FAILSAFE or HOME mode, read RC and set commands accordingly */
if (autopilot_mode != AP_MODE_FAILSAFE && autopilot_mode != AP_MODE_HOME) {
/* 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
guidance_v_read_rc();
guidance_h_read_rc(autopilot_in_flight);
}
}
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);
}
}
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);
}
}
/** AP mode setting handler
*
* Checks RC status before calling autopilot_set_mode function
*/
void autopilot_SetModeHandler(float mode)
{
if (mode == AP_MODE_KILL || mode == AP_MODE_FAILSAFE || mode == AP_MODE_HOME) {
// safety modes are always accessible via settings
autopilot_set_mode(mode);
} else {
if (radio_control.status != RC_OK &&
(mode == AP_MODE_NAV || mode == AP_MODE_GUIDED || mode == AP_MODE_FLIP || mode == AP_MODE_MODULE)) {
// without RC, only nav-like modes are accessible
autopilot_set_mode(mode);
}
}
// with RC, other modes can only be changed from the RC
}
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;})
}
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_on_rc_frame(void) {
if (kill_switch_is_on()) {
autopilot_set_mode(AP_MODE_KILL);
}
else if ((autopilot_mode != AP_MODE_HOME)
#ifdef UNLOCKED_HOME_MODE
|| !too_far_from_home
#endif
)
{
uint8_t new_autopilot_mode = 0;
AP_MODE_OF_PPRZ(radio_control.values[RADIO_MODE], new_autopilot_mode);
#if USE_GPS
/* don't enter NAV mode if GPS is lost (this also prevents mode oscillations) */
if (!(new_autopilot_mode == AP_MODE_NAV && GpsIsLost()))
#endif
autopilot_set_mode(new_autopilot_mode);
}
/* if not in FAILSAFE or HOME mode check motor and in_flight status, read RC */
if (autopilot_mode != AP_MODE_FAILSAFE && autopilot_mode != AP_MODE_HOME) {
/* 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;
guidance_v_read_rc();
guidance_h_read_rc(autopilot_in_flight);
}
}
static inline void autopilot_main_periodic( void ) {
static uint8_t _cnt = 0;
/* read analog baro */
analog_periodic_task();
altimeter_periodic_task();
/* read imu */
imu_periodic_task();
/* run control loops */
autopilot_periodic();
/* set actuators */
autopilot_set_actuators();
/* Run the following tasks 10x times slower than the periodic rate */
_cnt++;
if (_cnt >= 10)
_cnt = 0;
switch (_cnt)
{
case 0:
rc_periodic_task();
if (rc_status == RC_OK) {
led_on(LED_RC);
} else {
led_off(LED_RC);
autopilot_set_mode(AP_MODE_FAILSAFE);
}
break;
case 1:
comm_periodic_task(COMM_TELEMETRY);
#if !HARDWARE_ENABLED_GPS
comm_periodic_task(COMM_0);
#endif
break;
case 2:
fms_periodic_task();
break;
#if BOMB_ENABLED
case 3:
bomb_periodic_task();
break;
#endif
}
/* flash leds... */
led_periodic_task();
if (ahrs_status == STATUS_INITIALIZING) {
RunOnceEvery(50, {
led_toggle(LED_AHRS);
});
void autopilot_init(void)
{
/* mode is finally set at end of init if MODE_STARTUP is not KILL */
autopilot_mode = AP_MODE_KILL;
autopilot_motors_on = false;
kill_throttle = ! autopilot_motors_on;
autopilot_in_flight = false;
autopilot_in_flight_counter = 0;
autopilot_mode_auto2 = MODE_AUTO2;
autopilot_ground_detected = false;
autopilot_detect_ground_once = false;
autopilot_flight_time = 0;
autopilot_rc = true;
autopilot_power_switch = false;
#ifdef POWER_SWITCH_GPIO
gpio_setup_output(POWER_SWITCH_GPIO);
gpio_clear(POWER_SWITCH_GPIO); // POWER OFF
#endif
autopilot_arming_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
stabilization_none_init();
#if USE_STABILIZATION_RATE
stabilization_rate_init();
#endif
stabilization_attitude_init();
/* set startup mode, propagates through to guidance h/v */
autopilot_set_mode(MODE_STARTUP);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ALIVE, send_alive);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_STATUS, send_status);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ATTITUDE, send_attitude);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ENERGY, send_energy);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP, send_fp);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP_MIN, send_fp_min);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_CMD, send_rotorcraft_cmd);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DL_VALUE, send_dl_value);
#ifdef ACTUATORS
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);
#endif
#ifdef RADIO_CONTROL
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC, send_rc);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_RADIO_CONTROL, send_rotorcraft_rc);
#endif
}
void autopilot_init(void)
{
/* mode is finally set at end of init if MODE_STARTUP is not KILL */
autopilot_mode = AP_MODE_KILL;
autopilot_motors_on = FALSE;
kill_throttle = ! autopilot_motors_on;
autopilot_in_flight = FALSE;
autopilot_in_flight_counter = 0;
autopilot_mode_auto2 = MODE_AUTO2;
autopilot_ground_detected = FALSE;
autopilot_detect_ground_once = FALSE;
autopilot_flight_time = 0;
autopilot_rc = TRUE;
autopilot_power_switch = FALSE;
#ifdef POWER_SWITCH_GPIO
gpio_setup_output(POWER_SWITCH_GPIO);
gpio_clear(POWER_SWITCH_GPIO); // POWER OFF
#endif
autopilot_arming_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
stabilization_none_init();
stabilization_rate_init();
stabilization_attitude_init();
/* set startup mode, propagates through to guidance h/v */
autopilot_set_mode(MODE_STARTUP);
register_periodic_telemetry(DefaultPeriodic, "AUTOPILOT_VERSION", send_autopilot_version);
register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_STATUS", send_status);
register_periodic_telemetry(DefaultPeriodic, "ATTITUDE", send_attitude);
register_periodic_telemetry(DefaultPeriodic, "ENERGY", send_energy);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_FP", send_fp);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_CMD", send_rotorcraft_cmd);
register_periodic_telemetry(DefaultPeriodic, "DL_VALUE", send_dl_value);
#ifdef ACTUATORS
register_periodic_telemetry(DefaultPeriodic, "ACTUATORS", send_actuators);
#endif
#ifdef RADIO_CONTROL
register_periodic_telemetry(DefaultPeriodic, "RC", send_rc);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_RADIO_CONTROL", send_rotorcraft_rc);
#endif
}
STATIC_INLINE void main_periodic( void ) {
imu_periodic();
/* run control loops */
autopilot_periodic();
/* set actuators */
actuators_set(autopilot_motors_on);
PeriodicPrescaleBy10( \
{ \
radio_control_periodic_task(); \
if (radio_control.status != RC_OK && \
autopilot_mode != AP_MODE_KILL && \
autopilot_mode != AP_MODE_NAV) \
autopilot_set_mode(AP_MODE_FAILSAFE); \
}, \
{ \
/* booz_fms_periodic(); FIXME */ \
}, \
{ \
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);
}
}
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
}
}
void mavlink_common_message_handler(const mavlink_message_t *msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
break;
/* When requesting data streams say we can't send them */
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: {
mavlink_request_data_stream_t cmd;
mavlink_msg_request_data_stream_decode(msg, &cmd);
mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0);
MAVLinkSendMessage();
break;
}
/* Override channels with RC */
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: {
mavlink_rc_channels_override_t cmd;
mavlink_msg_rc_channels_override_decode(msg, &cmd);
#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100;
int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2;
int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2;
int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100;
parse_rc_4ch_datalink(0, thrust, roll, pitch, yaw);
//printf("RECEIVED: RC Channel Override for: %d/%d: throttle: %d; roll: %d; pitch: %d; yaw: %d;\r\n",
// cmd.target_system, cmd.target_component, thrust, roll, pitch, yaw);
#endif
break;
}
/* When a request is made of the parameters list */
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
mavlink_params_idx = 0;
break;
}
/* When a request os made for a specific parameter */
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
mavlink_param_request_read_t cmd;
mavlink_msg_param_request_read_decode(msg, &cmd);
// First check param_index and search for the ID if needed
if (cmd.param_index == -1) {
cmd.param_index = settings_idx_from_param_id(cmd.param_id);
}
// Send message only if the param_index was found (Coverity Scan)
if (cmd.param_index > -1) {
mavlink_msg_param_value_send(MAVLINK_COMM_0,
mavlink_param_names[cmd.param_index],
settings_get_value(cmd.param_index),
MAV_PARAM_TYPE_REAL32,
NB_SETTING,
cmd.param_index);
MAVLinkSendMessage();
}
break;
}
case MAVLINK_MSG_ID_PARAM_SET: {
mavlink_param_set_t set;
mavlink_msg_param_set_decode(msg, &set);
// Check if this message is for this system
if ((uint8_t) set.target_system == AC_ID) {
int16_t idx = settings_idx_from_param_id(set.param_id);
// setting found
if (idx >= 0) {
// Only write if new value is NOT "not-a-number"
// AND is NOT infinity
if (set.param_type == MAV_PARAM_TYPE_REAL32 &&
!isnan(set.param_value) && !isinf(set.param_value)) {
DlSetting(idx, set.param_value);
// Report back new value
mavlink_msg_param_value_send(MAVLINK_COMM_0,
mavlink_param_names[idx],
settings_get_value(idx),
MAV_PARAM_TYPE_REAL32,
NB_SETTING,
idx);
MAVLinkSendMessage();
}
}
}
}
break;
#ifndef AP
/* only for rotorcraft */
case MAVLINK_MSG_ID_COMMAND_LONG: {
mavlink_command_long_t cmd;
mavlink_msg_command_long_decode(msg, &cmd);
// Check if this message is for this system
if ((uint8_t) cmd.target_system == AC_ID) {
uint8_t result = MAV_RESULT_UNSUPPORTED;
switch (cmd.command) {
case MAV_CMD_NAV_GUIDED_ENABLE:
MAVLINK_DEBUG("got cmd NAV_GUIDED_ENABLE: %f\n", cmd.param1);
result = MAV_RESULT_FAILED;
if (cmd.param1 > 0.5) {
autopilot_set_mode(AP_MODE_GUIDED);
if (autopilot_mode == AP_MODE_GUIDED) {
result = MAV_RESULT_ACCEPTED;
}
}
else {
// turn guided mode off - to what? maybe NAV? or MODE_AUTO2?
}
break;
case MAV_CMD_COMPONENT_ARM_DISARM:
/* supposed to use this command to arm or SET_MODE?? */
MAVLINK_DEBUG("got cmd COMPONENT_ARM_DISARM: %f\n", cmd.param1);
result = MAV_RESULT_FAILED;
if (cmd.param1 > 0.5) {
autopilot_set_motors_on(TRUE);
if (autopilot_motors_on)
result = MAV_RESULT_ACCEPTED;
}
else {
autopilot_set_motors_on(FALSE);
if (!autopilot_motors_on)
result = MAV_RESULT_ACCEPTED;
}
break;
default:
break;
}
// confirm command with result
mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, result);
MAVLinkSendMessage();
}
break;
}
case MAVLINK_MSG_ID_SET_MODE: {
mavlink_set_mode_t mode;
mavlink_msg_set_mode_decode(msg, &mode);
if (mode.target_system == AC_ID) {
MAVLINK_DEBUG("got SET_MODE: base_mode:%d\n", mode.base_mode);
if (mode.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
autopilot_set_motors_on(TRUE);
}
else {
autopilot_set_motors_on(FALSE);
}
if (mode.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
autopilot_set_mode(AP_MODE_GUIDED);
}
else if (mode.base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
autopilot_set_mode(AP_MODE_NAV);
}
}
break;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: {
mavlink_set_position_target_local_ned_t target;
mavlink_msg_set_position_target_local_ned_decode(msg, &target);
// Check if this message is for this system
if (target.target_system == AC_ID) {
MAVLINK_DEBUG("SET_POSITION_TARGET_LOCAL_NED, byte_mask: %d\n", target.type_mask);
/* if position and yaw bits are not set to ignored, use only position for now */
if (!(target.type_mask & 0b1110000000100000)) {
switch (target.coordinate_frame) {
case MAV_FRAME_LOCAL_NED:
MAVLINK_DEBUG("set position target, frame LOCAL_NED\n");
autopilot_guided_goto_ned(target.x, target.y, target.z, target.yaw);
break;
case MAV_FRAME_LOCAL_OFFSET_NED:
MAVLINK_DEBUG("set position target, frame LOCAL_OFFSET_NED\n");
autopilot_guided_goto_ned_relative(target.x, target.y, target.z, target.yaw);
break;
case MAV_FRAME_BODY_OFFSET_NED:
MAVLINK_DEBUG("set position target, frame BODY_OFFSET_NED\n");
autopilot_guided_goto_body_relative(target.x, target.y, target.z, target.yaw);
break;
default:
break;
}
}
else if (!(target.type_mask & 0b0001110000100000)) {
/* position is set to ignore, but velocity not */
switch (target.coordinate_frame) {
case MAV_FRAME_LOCAL_NED:
MAVLINK_DEBUG("set velocity target, frame LOCAL_NED\n");
autopilot_guided_move_ned(target.vx, target.vy, target.vz, target.yaw);
break;
default:
break;
}
}
}
break;
}
#endif
default:
//Do nothing
MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid);
break;
}
}
void guidance_flip_run(void)
{
uint32_t timer;
int32_t phi;
static uint32_t timer_save = 0;
timer = (flip_counter++ << 12) / PERIODIC_FREQUENCY;
phi = stateGetNedToBodyEulers_i()->phi;
switch (flip_state) {
case 0:
flip_cmd_earth.x = 0;
flip_cmd_earth.y = 0;
stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
heading_save);
stabilization_attitude_run(autopilot_in_flight);
stabilization_cmd[COMMAND_THRUST] = 8000; //Thrust to go up first
timer_save = 0;
if (timer > BFP_OF_REAL(FIRST_THRUST_DURATION, 12)) {
flip_state++;
}
break;
case 1:
stabilization_cmd[COMMAND_ROLL] = 9000; // Rolling command
stabilization_cmd[COMMAND_PITCH] = 0;
stabilization_cmd[COMMAND_YAW] = 0;
stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
flip_state++;
}
break;
case 2:
stabilization_cmd[COMMAND_ROLL] = 0;
stabilization_cmd[COMMAND_PITCH] = 0;
stabilization_cmd[COMMAND_YAW] = 0;
stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(-110.0)) && phi < ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
timer_save = timer;
flip_state++;
}
break;
case 3:
flip_cmd_earth.x = 0;
flip_cmd_earth.y = 0;
stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
heading_save);
stabilization_attitude_run(autopilot_in_flight);
stabilization_cmd[COMMAND_THRUST] = FINAL_THRUST_LEVEL; //Thrust to stop falling
if ((timer - timer_save) > BFP_OF_REAL(0.5, 12)) {
flip_state++;
}
break;
default:
autopilot_mode_auto2 = autopilot_mode_old;
autopilot_set_mode(autopilot_mode_old);
stab_att_sp_euler.psi = heading_save;
flip_rollout = false;
flip_counter = 0;
timer_save = 0;
flip_state = 0;
stabilization_cmd[COMMAND_ROLL] = 0;
stabilization_cmd[COMMAND_PITCH] = 0;
stabilization_cmd[COMMAND_YAW] = 0;
stabilization_cmd[COMMAND_THRUST] = 8000; //Some thrust to come out of the roll?
break;
}
}