mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
a7b69366a1
The is commit adds a new flight mode called 'Throw' to Copter that enables the copter to be thrown into the air to start motors. This mode can only be netered when the copters EKF has a valid position estimate and goes through the following states Throw_Disarmed - The copter is disarmed and motors are off. Throw_Detecting - The copter is armed, but motors will not spin unless THROW_MOT_START has been set to 1. The copter is waiting to detect the throw. A throw with an upwards velocity of at least 50cm/s is required to trigger the detector. Throw_Uprighting - The throw has been detected and the copter is being uprighted with 50% throttle to maximise control authority. This state transitions when the copter is within 30 degrees of level. Throw_HgtStabilise - The copter is kept level and height is stabilised about the target height which is 3m above the height at which the throw release was detected. This state transitions when the height is no more than 0.5m below the demanded height. Throw_PosHold - The horizontal motion is arrested and the copter is kept at a constant position and height.
633 lines
23 KiB
C++
633 lines
23 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#include "Copter.h"
|
|
|
|
#define CONTROL_SWITCH_DEBOUNCE_TIME_MS 200
|
|
|
|
//Documentation of Aux Switch Flags:
|
|
static union {
|
|
struct {
|
|
uint8_t CH6_flag : 2; // 0, 1 // ch6 aux switch : 0 is low or false, 1 is center or true, 2 is high
|
|
uint8_t CH7_flag : 2; // 2, 3 // ch7 aux switch : 0 is low or false, 1 is center or true, 2 is high
|
|
uint8_t CH8_flag : 2; // 4, 5 // ch8 aux switch : 0 is low or false, 1 is center or true, 2 is high
|
|
uint8_t CH9_flag : 2; // 6, 7 // ch9 aux switch : 0 is low or false, 1 is center or true, 2 is high
|
|
uint8_t CH10_flag : 2; // 8, 9 // ch10 aux switch : 0 is low or false, 1 is center or true, 2 is high
|
|
uint8_t CH11_flag : 2; // 10,11 // ch11 aux switch : 0 is low or false, 1 is center or true, 2 is high
|
|
uint8_t CH12_flag : 2; // 12,13 // ch12 aux switch : 0 is low or false, 1 is center or true, 2 is high
|
|
};
|
|
uint32_t value;
|
|
} aux_con;
|
|
|
|
void Copter::read_control_switch()
|
|
{
|
|
uint32_t tnow_ms = millis();
|
|
|
|
// calculate position of flight mode switch
|
|
int8_t switch_position;
|
|
if (g.rc_5.radio_in < 1231) switch_position = 0;
|
|
else if (g.rc_5.radio_in < 1361) switch_position = 1;
|
|
else if (g.rc_5.radio_in < 1491) switch_position = 2;
|
|
else if (g.rc_5.radio_in < 1621) switch_position = 3;
|
|
else if (g.rc_5.radio_in < 1750) switch_position = 4;
|
|
else switch_position = 5;
|
|
|
|
// store time that switch last moved
|
|
if(control_switch_state.last_switch_position != switch_position) {
|
|
control_switch_state.last_edge_time_ms = tnow_ms;
|
|
}
|
|
|
|
// debounce switch
|
|
bool control_switch_changed = control_switch_state.debounced_switch_position != switch_position;
|
|
bool sufficient_time_elapsed = tnow_ms - control_switch_state.last_edge_time_ms > CONTROL_SWITCH_DEBOUNCE_TIME_MS;
|
|
bool failsafe_disengaged = !failsafe.radio && failsafe.radio_counter == 0;
|
|
|
|
if (control_switch_changed && sufficient_time_elapsed && failsafe_disengaged) {
|
|
// set flight mode and simple mode setting
|
|
if (set_mode(flight_modes[switch_position])) {
|
|
// play a tone
|
|
if (control_switch_state.debounced_switch_position != -1) {
|
|
// alert user to mode change failure (except if autopilot is just starting up)
|
|
if (ap.initialised) {
|
|
AP_Notify::events.user_mode_change = 1;
|
|
}
|
|
}
|
|
|
|
if(!check_if_auxsw_mode_used(AUXSW_SIMPLE_MODE) && !check_if_auxsw_mode_used(AUXSW_SUPERSIMPLE_MODE)) {
|
|
// if none of the Aux Switches are set to Simple or Super Simple Mode then
|
|
// set Simple Mode using stored parameters from EEPROM
|
|
if (BIT_IS_SET(g.super_simple, switch_position)) {
|
|
set_simple_mode(2);
|
|
}else{
|
|
set_simple_mode(BIT_IS_SET(g.simple_modes, switch_position));
|
|
}
|
|
}
|
|
|
|
} else if (control_switch_state.last_switch_position != -1) {
|
|
// alert user to mode change failure
|
|
AP_Notify::events.user_mode_change_failed = 1;
|
|
}
|
|
|
|
// set the debounced switch position
|
|
control_switch_state.debounced_switch_position = switch_position;
|
|
}
|
|
|
|
control_switch_state.last_switch_position = switch_position;
|
|
}
|
|
|
|
// check_if_auxsw_mode_used - Check to see if any of the Aux Switches are set to a given mode.
|
|
bool Copter::check_if_auxsw_mode_used(uint8_t auxsw_mode_check)
|
|
{
|
|
bool ret = g.ch7_option == auxsw_mode_check || g.ch8_option == auxsw_mode_check || g.ch9_option == auxsw_mode_check
|
|
|| g.ch10_option == auxsw_mode_check || g.ch11_option == auxsw_mode_check || g.ch12_option == auxsw_mode_check;
|
|
|
|
return ret;
|
|
}
|
|
|
|
// check_duplicate_auxsw - Check to see if any Aux Switch Functions are duplicated
|
|
bool Copter::check_duplicate_auxsw(void)
|
|
{
|
|
bool ret = ((g.ch7_option != AUXSW_DO_NOTHING) && (g.ch7_option == g.ch8_option ||
|
|
g.ch7_option == g.ch9_option || g.ch7_option == g.ch10_option ||
|
|
g.ch7_option == g.ch11_option || g.ch7_option == g.ch12_option));
|
|
|
|
ret = ret || ((g.ch8_option != AUXSW_DO_NOTHING) && (g.ch8_option == g.ch9_option ||
|
|
g.ch8_option == g.ch10_option || g.ch8_option == g.ch11_option ||
|
|
g.ch8_option == g.ch12_option));
|
|
|
|
ret = ret || ((g.ch9_option != AUXSW_DO_NOTHING) && (g.ch9_option == g.ch10_option ||
|
|
g.ch9_option == g.ch11_option || g.ch9_option == g.ch12_option));
|
|
|
|
ret = ret || ((g.ch10_option != AUXSW_DO_NOTHING) && (g.ch10_option == g.ch11_option ||
|
|
g.ch10_option == g.ch12_option));
|
|
|
|
ret = ret || ((g.ch11_option != AUXSW_DO_NOTHING) && (g.ch11_option == g.ch12_option));
|
|
|
|
return ret;
|
|
}
|
|
|
|
void Copter::reset_control_switch()
|
|
{
|
|
control_switch_state.last_switch_position = control_switch_state.debounced_switch_position = -1;
|
|
read_control_switch();
|
|
}
|
|
|
|
// read_3pos_switch
|
|
uint8_t Copter::read_3pos_switch(int16_t radio_in)
|
|
{
|
|
if (radio_in < AUX_SWITCH_PWM_TRIGGER_LOW) return AUX_SWITCH_LOW; // switch is in low position
|
|
if (radio_in > AUX_SWITCH_PWM_TRIGGER_HIGH) return AUX_SWITCH_HIGH; // switch is in high position
|
|
return AUX_SWITCH_MIDDLE; // switch is in middle position
|
|
}
|
|
|
|
// read_aux_switches - checks aux switch positions and invokes configured actions
|
|
void Copter::read_aux_switches()
|
|
{
|
|
uint8_t switch_position;
|
|
|
|
// exit immediately during radio failsafe
|
|
if (failsafe.radio || failsafe.radio_counter != 0) {
|
|
return;
|
|
}
|
|
|
|
// check if ch7 switch has changed position
|
|
switch_position = read_3pos_switch(g.rc_7.radio_in);
|
|
if (aux_con.CH7_flag != switch_position) {
|
|
// set the CH7 flag
|
|
aux_con.CH7_flag = switch_position;
|
|
|
|
// invoke the appropriate function
|
|
do_aux_switch_function(g.ch7_option, aux_con.CH7_flag);
|
|
}
|
|
|
|
// check if Ch8 switch has changed position
|
|
switch_position = read_3pos_switch(g.rc_8.radio_in);
|
|
if (aux_con.CH8_flag != switch_position) {
|
|
// set the CH8 flag
|
|
aux_con.CH8_flag = switch_position;
|
|
|
|
// invoke the appropriate function
|
|
do_aux_switch_function(g.ch8_option, aux_con.CH8_flag);
|
|
}
|
|
|
|
// check if Ch9 switch has changed position
|
|
switch_position = read_3pos_switch(g.rc_9.radio_in);
|
|
if (aux_con.CH9_flag != switch_position) {
|
|
// set the CH9 flag
|
|
aux_con.CH9_flag = switch_position;
|
|
|
|
// invoke the appropriate function
|
|
do_aux_switch_function(g.ch9_option, aux_con.CH9_flag);
|
|
}
|
|
|
|
// check if Ch10 switch has changed position
|
|
switch_position = read_3pos_switch(g.rc_10.radio_in);
|
|
if (aux_con.CH10_flag != switch_position) {
|
|
// set the CH10 flag
|
|
aux_con.CH10_flag = switch_position;
|
|
|
|
// invoke the appropriate function
|
|
do_aux_switch_function(g.ch10_option, aux_con.CH10_flag);
|
|
}
|
|
|
|
// check if Ch11 switch has changed position
|
|
switch_position = read_3pos_switch(g.rc_11.radio_in);
|
|
if (aux_con.CH11_flag != switch_position) {
|
|
// set the CH11 flag
|
|
aux_con.CH11_flag = switch_position;
|
|
|
|
// invoke the appropriate function
|
|
do_aux_switch_function(g.ch11_option, aux_con.CH11_flag);
|
|
}
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
|
// check if Ch12 switch has changed position
|
|
switch_position = read_3pos_switch(g.rc_12.radio_in);
|
|
if (aux_con.CH12_flag != switch_position) {
|
|
// set the CH12 flag
|
|
aux_con.CH12_flag = switch_position;
|
|
|
|
// invoke the appropriate function
|
|
do_aux_switch_function(g.ch12_option, aux_con.CH12_flag);
|
|
}
|
|
#endif
|
|
}
|
|
|
|
// init_aux_switches - invoke configured actions at start-up for aux function where it is safe to do so
|
|
void Copter::init_aux_switches()
|
|
{
|
|
// set the CH7 ~ CH12 flags
|
|
aux_con.CH7_flag = read_3pos_switch(g.rc_7.radio_in);
|
|
aux_con.CH8_flag = read_3pos_switch(g.rc_8.radio_in);
|
|
aux_con.CH10_flag = read_3pos_switch(g.rc_10.radio_in);
|
|
aux_con.CH11_flag = read_3pos_switch(g.rc_11.radio_in);
|
|
|
|
// ch9, ch12 only supported on some boards
|
|
aux_con.CH9_flag = read_3pos_switch(g.rc_9.radio_in);
|
|
aux_con.CH12_flag = read_3pos_switch(g.rc_12.radio_in);
|
|
|
|
// initialise functions assigned to switches
|
|
init_aux_switch_function(g.ch7_option, aux_con.CH7_flag);
|
|
init_aux_switch_function(g.ch8_option, aux_con.CH8_flag);
|
|
init_aux_switch_function(g.ch10_option, aux_con.CH10_flag);
|
|
init_aux_switch_function(g.ch11_option, aux_con.CH11_flag);
|
|
|
|
// ch9, ch12 only supported on some boards
|
|
init_aux_switch_function(g.ch9_option, aux_con.CH9_flag);
|
|
init_aux_switch_function(g.ch12_option, aux_con.CH12_flag);
|
|
}
|
|
|
|
// init_aux_switch_function - initialize aux functions
|
|
void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
|
|
{
|
|
// init channel options
|
|
switch(ch_option) {
|
|
case AUXSW_SIMPLE_MODE:
|
|
case AUXSW_SONAR:
|
|
case AUXSW_FENCE:
|
|
case AUXSW_RESETTOARMEDYAW:
|
|
case AUXSW_SUPERSIMPLE_MODE:
|
|
case AUXSW_ACRO_TRAINER:
|
|
case AUXSW_EPM:
|
|
case AUXSW_SPRAYER:
|
|
case AUXSW_PARACHUTE_ENABLE:
|
|
case AUXSW_PARACHUTE_3POS: // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release
|
|
case AUXSW_RETRACT_MOUNT:
|
|
case AUXSW_MISSION_RESET:
|
|
case AUXSW_ATTCON_FEEDFWD:
|
|
case AUXSW_ATTCON_ACCEL_LIM:
|
|
case AUXSW_LANDING_GEAR:
|
|
case AUXSW_MOTOR_ESTOP:
|
|
case AUXSW_MOTOR_INTERLOCK:
|
|
do_aux_switch_function(ch_option, ch_flag);
|
|
break;
|
|
}
|
|
}
|
|
|
|
// do_aux_switch_function - implement the function invoked by the ch7 or ch8 switch
|
|
void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
|
|
{
|
|
|
|
switch(ch_function) {
|
|
case AUXSW_FLIP:
|
|
// flip if switch is on, positive throttle and we're actually flying
|
|
if(ch_flag == AUX_SWITCH_HIGH) {
|
|
set_mode(FLIP);
|
|
}
|
|
break;
|
|
|
|
case AUXSW_SIMPLE_MODE:
|
|
// low = simple mode off, middle or high position turns simple mode on
|
|
set_simple_mode(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
|
|
break;
|
|
|
|
case AUXSW_SUPERSIMPLE_MODE:
|
|
// low = simple mode off, middle = simple mode, high = super simple mode
|
|
set_simple_mode(ch_flag);
|
|
break;
|
|
|
|
case AUXSW_RTL:
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
|
// engage RTL (if not possible we remain in current flight mode)
|
|
set_mode(RTL);
|
|
}else{
|
|
// return to flight mode switch's flight mode if we are currently in RTL
|
|
if (control_mode == RTL) {
|
|
reset_control_switch();
|
|
}
|
|
}
|
|
break;
|
|
|
|
case AUXSW_SAVE_TRIM:
|
|
if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (channel_throttle->control_in == 0)) {
|
|
save_trim();
|
|
}
|
|
break;
|
|
|
|
case AUXSW_SAVE_WP:
|
|
// save waypoint when switch is brought high
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
|
|
|
// do not allow saving new waypoints while we're in auto or disarmed
|
|
if(control_mode == AUTO || !motors.armed()) {
|
|
return;
|
|
}
|
|
|
|
// do not allow saving the first waypoint with zero throttle
|
|
if((mission.num_commands() == 0) && (channel_throttle->control_in == 0)){
|
|
return;
|
|
}
|
|
|
|
// create new mission command
|
|
AP_Mission::Mission_Command cmd = {};
|
|
|
|
// if the mission is empty save a takeoff command
|
|
if(mission.num_commands() == 0) {
|
|
// set our location ID to 16, MAV_CMD_NAV_WAYPOINT
|
|
cmd.id = MAV_CMD_NAV_TAKEOFF;
|
|
cmd.content.location.options = 0;
|
|
cmd.p1 = 0;
|
|
cmd.content.location.lat = 0;
|
|
cmd.content.location.lng = 0;
|
|
cmd.content.location.alt = MAX(current_loc.alt,100);
|
|
|
|
// use the current altitude for the target alt for takeoff.
|
|
// only altitude will matter to the AP mission script for takeoff.
|
|
if(mission.add_cmd(cmd)) {
|
|
// log event
|
|
Log_Write_Event(DATA_SAVEWP_ADD_WP);
|
|
}
|
|
}
|
|
|
|
// set new waypoint to current location
|
|
cmd.content.location = current_loc;
|
|
|
|
// if throttle is above zero, create waypoint command
|
|
if(channel_throttle->control_in > 0) {
|
|
cmd.id = MAV_CMD_NAV_WAYPOINT;
|
|
}else{
|
|
// with zero throttle, create LAND command
|
|
cmd.id = MAV_CMD_NAV_LAND;
|
|
}
|
|
|
|
// save command
|
|
if(mission.add_cmd(cmd)) {
|
|
// log event
|
|
Log_Write_Event(DATA_SAVEWP_ADD_WP);
|
|
}
|
|
}
|
|
break;
|
|
|
|
#if CAMERA == ENABLED
|
|
case AUXSW_CAMERA_TRIGGER:
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
|
do_take_picture();
|
|
}
|
|
break;
|
|
#endif
|
|
|
|
case AUXSW_SONAR:
|
|
// enable or disable the sonar
|
|
#if CONFIG_SONAR == ENABLED
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
|
sonar_enabled = true;
|
|
}else{
|
|
sonar_enabled = false;
|
|
}
|
|
#endif
|
|
break;
|
|
|
|
#if AC_FENCE == ENABLED
|
|
case AUXSW_FENCE:
|
|
// enable or disable the fence
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
|
fence.enable(true);
|
|
Log_Write_Event(DATA_FENCE_ENABLE);
|
|
}else{
|
|
fence.enable(false);
|
|
Log_Write_Event(DATA_FENCE_DISABLE);
|
|
}
|
|
break;
|
|
#endif
|
|
// To-Do: add back support for this feature
|
|
//case AUXSW_RESETTOARMEDYAW:
|
|
// if (ch_flag == AUX_SWITCH_HIGH) {
|
|
// set_yaw_mode(YAW_RESETTOARMEDYAW);
|
|
// }else{
|
|
// set_yaw_mode(YAW_HOLD);
|
|
// }
|
|
// break;
|
|
|
|
case AUXSW_ACRO_TRAINER:
|
|
switch(ch_flag) {
|
|
case AUX_SWITCH_LOW:
|
|
g.acro_trainer = ACRO_TRAINER_DISABLED;
|
|
Log_Write_Event(DATA_ACRO_TRAINER_DISABLED);
|
|
break;
|
|
case AUX_SWITCH_MIDDLE:
|
|
g.acro_trainer = ACRO_TRAINER_LEVELING;
|
|
Log_Write_Event(DATA_ACRO_TRAINER_LEVELING);
|
|
break;
|
|
case AUX_SWITCH_HIGH:
|
|
g.acro_trainer = ACRO_TRAINER_LIMITED;
|
|
Log_Write_Event(DATA_ACRO_TRAINER_LIMITED);
|
|
break;
|
|
}
|
|
break;
|
|
#if EPM_ENABLED == ENABLED
|
|
case AUXSW_EPM:
|
|
switch(ch_flag) {
|
|
case AUX_SWITCH_LOW:
|
|
epm.release();
|
|
Log_Write_Event(DATA_EPM_RELEASE);
|
|
break;
|
|
case AUX_SWITCH_HIGH:
|
|
epm.grab();
|
|
Log_Write_Event(DATA_EPM_GRAB);
|
|
break;
|
|
}
|
|
break;
|
|
#endif
|
|
#if SPRAYER == ENABLED
|
|
case AUXSW_SPRAYER:
|
|
sprayer.enable(ch_flag == AUX_SWITCH_HIGH);
|
|
// if we are disarmed the pilot must want to test the pump
|
|
sprayer.test_pump((ch_flag == AUX_SWITCH_HIGH) && !motors.armed());
|
|
break;
|
|
#endif
|
|
|
|
case AUXSW_AUTO:
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
|
set_mode(AUTO);
|
|
}else{
|
|
// return to flight mode switch's flight mode if we are currently in AUTO
|
|
if (control_mode == AUTO) {
|
|
reset_control_switch();
|
|
}
|
|
}
|
|
break;
|
|
|
|
#if AUTOTUNE_ENABLED == ENABLED
|
|
case AUXSW_AUTOTUNE:
|
|
// turn on auto tuner
|
|
switch(ch_flag) {
|
|
case AUX_SWITCH_LOW:
|
|
case AUX_SWITCH_MIDDLE:
|
|
// restore flight mode based on flight mode switch position
|
|
if (control_mode == AUTOTUNE) {
|
|
reset_control_switch();
|
|
}
|
|
break;
|
|
case AUX_SWITCH_HIGH:
|
|
// start an autotuning session
|
|
set_mode(AUTOTUNE);
|
|
break;
|
|
}
|
|
break;
|
|
#endif
|
|
|
|
case AUXSW_LAND:
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
|
set_mode(LAND);
|
|
}else{
|
|
// return to flight mode switch's flight mode if we are currently in LAND
|
|
if (control_mode == LAND) {
|
|
reset_control_switch();
|
|
}
|
|
}
|
|
break;
|
|
|
|
#if PARACHUTE == ENABLED
|
|
case AUXSW_PARACHUTE_ENABLE:
|
|
// Parachute enable/disable
|
|
parachute.enabled(ch_flag == AUX_SWITCH_HIGH);
|
|
break;
|
|
|
|
case AUXSW_PARACHUTE_RELEASE:
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
|
parachute_manual_release();
|
|
}
|
|
break;
|
|
|
|
case AUXSW_PARACHUTE_3POS:
|
|
// Parachute disable, enable, release with 3 position switch
|
|
switch (ch_flag) {
|
|
case AUX_SWITCH_LOW:
|
|
parachute.enabled(false);
|
|
Log_Write_Event(DATA_PARACHUTE_DISABLED);
|
|
break;
|
|
case AUX_SWITCH_MIDDLE:
|
|
parachute.enabled(true);
|
|
Log_Write_Event(DATA_PARACHUTE_ENABLED);
|
|
break;
|
|
case AUX_SWITCH_HIGH:
|
|
parachute.enabled(true);
|
|
parachute_manual_release();
|
|
break;
|
|
}
|
|
break;
|
|
#endif
|
|
|
|
case AUXSW_MISSION_RESET:
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
|
mission.reset();
|
|
}
|
|
break;
|
|
|
|
case AUXSW_ATTCON_FEEDFWD:
|
|
// enable or disable feed forward
|
|
attitude_control.bf_feedforward(ch_flag == AUX_SWITCH_HIGH);
|
|
break;
|
|
|
|
case AUXSW_ATTCON_ACCEL_LIM:
|
|
// enable or disable accel limiting by restoring defaults
|
|
attitude_control.accel_limiting(ch_flag == AUX_SWITCH_HIGH);
|
|
break;
|
|
|
|
#if MOUNT == ENABLE
|
|
case AUXSW_RETRACT_MOUNT:
|
|
switch (ch_flag) {
|
|
case AUX_SWITCH_HIGH:
|
|
camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT);
|
|
break;
|
|
case AUX_SWITCH_LOW:
|
|
camera_mount.set_mode_to_default();
|
|
break;
|
|
}
|
|
break;
|
|
#endif
|
|
|
|
case AUXSW_RELAY:
|
|
ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH);
|
|
break;
|
|
|
|
case AUXSW_RELAY2:
|
|
ServoRelayEvents.do_set_relay(1, ch_flag == AUX_SWITCH_HIGH);
|
|
break;
|
|
|
|
case AUXSW_RELAY3:
|
|
ServoRelayEvents.do_set_relay(2, ch_flag == AUX_SWITCH_HIGH);
|
|
break;
|
|
|
|
case AUXSW_RELAY4:
|
|
ServoRelayEvents.do_set_relay(3, ch_flag == AUX_SWITCH_HIGH);
|
|
break;
|
|
|
|
case AUXSW_LANDING_GEAR:
|
|
switch (ch_flag) {
|
|
case AUX_SWITCH_LOW:
|
|
landinggear.set_cmd_mode(LandingGear_Deploy);
|
|
break;
|
|
case AUX_SWITCH_MIDDLE:
|
|
landinggear.set_cmd_mode(LandingGear_Auto);
|
|
break;
|
|
case AUX_SWITCH_HIGH:
|
|
landinggear.set_cmd_mode(LandingGear_Retract);
|
|
break;
|
|
}
|
|
break;
|
|
|
|
case AUXSW_LOST_COPTER_SOUND:
|
|
switch (ch_flag) {
|
|
case AUX_SWITCH_HIGH:
|
|
AP_Notify::flags.vehicle_lost = true;
|
|
break;
|
|
case AUX_SWITCH_LOW:
|
|
AP_Notify::flags.vehicle_lost = false;
|
|
break;
|
|
}
|
|
break;
|
|
|
|
case AUXSW_MOTOR_ESTOP:
|
|
// Turn on Emergency Stop logic when channel is high
|
|
set_motor_emergency_stop(ch_flag == AUX_SWITCH_HIGH);
|
|
break;
|
|
|
|
case AUXSW_MOTOR_INTERLOCK:
|
|
// Turn on when above LOW, because channel will also be used for speed
|
|
// control signal in tradheli
|
|
motors.set_interlock(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
|
|
|
|
// remember the current value of the motor interlock so that this condition can be restored if we exit the throw mode early
|
|
throw_early_exit_interlock = motors.get_interlock();
|
|
|
|
// Log new status
|
|
if (motors.get_interlock()){
|
|
Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);
|
|
} else {
|
|
Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED);
|
|
}
|
|
break;
|
|
|
|
case AUXSW_BRAKE:
|
|
// brake flight mode
|
|
if (ch_flag == AUX_SWITCH_HIGH) {
|
|
set_mode(BRAKE);
|
|
}else{
|
|
// return to flight mode switch's flight mode if we are currently in BRAKE
|
|
if (control_mode == BRAKE) {
|
|
reset_control_switch();
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
// save_trim - adds roll and pitch trims from the radio to ahrs
|
|
void Copter::save_trim()
|
|
{
|
|
// save roll and pitch trim
|
|
float roll_trim = ToRad((float)channel_roll->control_in/100.0f);
|
|
float pitch_trim = ToRad((float)channel_pitch->control_in/100.0f);
|
|
ahrs.add_trim(roll_trim, pitch_trim);
|
|
Log_Write_Event(DATA_SAVE_TRIM);
|
|
gcs_send_text(MAV_SEVERITY_INFO, "Trim saved");
|
|
}
|
|
|
|
// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions
|
|
// meant to be called continuously while the pilot attempts to keep the copter level
|
|
void Copter::auto_trim()
|
|
{
|
|
if(auto_trim_counter > 0) {
|
|
auto_trim_counter--;
|
|
|
|
// flash the leds
|
|
AP_Notify::flags.save_trim = true;
|
|
|
|
// calculate roll trim adjustment
|
|
float roll_trim_adjustment = ToRad((float)channel_roll->control_in / 4000.0f);
|
|
|
|
// calculate pitch trim adjustment
|
|
float pitch_trim_adjustment = ToRad((float)channel_pitch->control_in / 4000.0f);
|
|
|
|
// add trim to ahrs object
|
|
// save to eeprom on last iteration
|
|
ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0));
|
|
|
|
// on last iteration restore leds and accel gains to normal
|
|
if(auto_trim_counter == 0) {
|
|
AP_Notify::flags.save_trim = false;
|
|
}
|
|
}
|
|
}
|
|
|