ardupilot/ArduCopter/switches.cpp

785 lines
27 KiB
C++
Raw Normal View History

#include "Copter.h"
#define CONTROL_SWITCH_DEBOUNCE_TIME_MS 200
//Documentation of Aux Switch Flags:
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
} aux_con;
void Copter::read_control_switch()
{
if (g.flight_mode_chan <= 0) {
// no flight mode channel
return;
}
uint32_t tnow_ms = millis();
// calculate position of flight mode switch
int8_t switch_position;
uint16_t mode_in = RC_Channels::rc_channel(g.flight_mode_chan-1)->get_radio_in();
if (mode_in < 1231) switch_position = 0;
else if (mode_in < 1361) switch_position = 1;
else if (mode_in < 1491) switch_position = 2;
else if (mode_in < 1621) switch_position = 3;
else if (mode_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
2016-01-25 19:40:41 -04:00
if (set_mode((control_mode_t)flight_modes[switch_position].get(), MODE_REASON_TX_COMMAND)) {
2014-10-23 15:47:00 -03:00
// play a tone
if (control_switch_state.debounced_switch_position != -1) {
// alert user to mode change (except if autopilot is just starting up)
if (ap.initialised) {
AP_Notify::events.user_mode_change = 1;
}
2014-10-23 15:47:00 -03:00
}
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));
}
}
2014-10-23 15:47:00 -03:00
2014-12-14 23:29:47 -04:00
} else if (control_switch_state.last_switch_position != -1) {
// alert user to mode change failure
2014-10-23 15:47:00 -03:00
AP_Notify::events.user_mode_change_failed = 1;
}
// set the debounced switch position
control_switch_state.debounced_switch_position = switch_position;
}
2012-12-14 17:30:48 -04:00
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)
2015-03-17 09:15:52 -03:00
{
2018-02-11 20:39:54 -04:00
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)
2015-03-17 09:15:52 -03:00
{
uint8_t auxsw_option_counts[AUXSW_SWITCH_MAX] = {};
auxsw_option_counts[g.ch7_option]++;
auxsw_option_counts[g.ch8_option]++;
auxsw_option_counts[g.ch9_option]++;
auxsw_option_counts[g.ch10_option]++;
auxsw_option_counts[g.ch11_option]++;
auxsw_option_counts[g.ch12_option]++;
for (uint8_t i=0; i<sizeof(auxsw_option_counts); i++) {
if (i == AUXSW_DO_NOTHING) {
continue;
}
if (auxsw_option_counts[i] > 1) {
return true;
}
}
return false;
}
void Copter::reset_control_switch()
{
control_switch_state.last_switch_position = control_switch_state.debounced_switch_position = -1;
read_control_switch();
}
// read_3pos_switch
2017-01-06 21:06:40 -04:00
uint8_t Copter::read_3pos_switch(uint8_t chan)
2015-03-17 09:15:52 -03:00
{
2017-01-06 21:06:40 -04:00
uint16_t radio_in = RC_Channels::rc_channel(chan)->get_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
}
// can't take reference to a bitfield member, thus a #define:
2017-01-06 21:06:40 -04:00
#define read_aux_switch(chan, flag, option) \
do { \
2017-01-06 21:06:40 -04:00
switch_position = read_3pos_switch(chan); \
if (debounce_aux_switch(chan, flag) && flag != switch_position) { \
flag = switch_position; \
do_aux_switch_function(option, flag); \
} \
} while (false)
// 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;
}
2017-01-06 21:06:40 -04:00
read_aux_switch(CH_7, aux_con.CH7_flag, g.ch7_option);
read_aux_switch(CH_8, aux_con.CH8_flag, g.ch8_option);
read_aux_switch(CH_9, aux_con.CH9_flag, g.ch9_option);
read_aux_switch(CH_10, aux_con.CH10_flag, g.ch10_option);
read_aux_switch(CH_11, aux_con.CH11_flag, g.ch11_option);
read_aux_switch(CH_12, aux_con.CH12_flag, g.ch12_option);
}
#undef read_aux_switch
// 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
2017-01-06 21:06:40 -04:00
aux_con.CH7_flag = read_3pos_switch(CH_7);
aux_con.CH8_flag = read_3pos_switch(CH_8);
aux_con.CH10_flag = read_3pos_switch(CH_10);
aux_con.CH11_flag = read_3pos_switch(CH_11);
// ch9, ch12 only supported on some boards
2017-01-06 21:06:40 -04:00
aux_con.CH9_flag = read_3pos_switch(CH_9);
aux_con.CH12_flag = read_3pos_switch(CH_12);
// 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)
2018-02-11 20:39:54 -04:00
{
// init channel options
switch(ch_option) {
case AUXSW_SIMPLE_MODE:
2016-04-27 08:37:04 -03:00
case AUXSW_RANGEFINDER:
case AUXSW_FENCE:
case AUXSW_SUPERSIMPLE_MODE:
case AUXSW_ACRO_TRAINER:
case AUXSW_GRIPPER:
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:
2015-03-17 18:53:40 -03:00
case AUXSW_MOTOR_ESTOP:
case AUXSW_MOTOR_INTERLOCK:
case AUXSW_AVOID_ADSB:
case AUXSW_PRECISION_LOITER:
case AUXSW_AVOID_PROXIMITY:
case AUXSW_INVERTED:
case AUXSW_WINCH_ENABLE:
case AUXSW_RC_OVERRIDE_ENABLE:
do_aux_switch_function(ch_option, ch_flag);
break;
}
}
/*
debounce an aux switch using a counter in copter.debounce
structure. This will return false until the same ch_flag is seen debounce_count times
*/
bool Copter::debounce_aux_switch(uint8_t chan, uint8_t ch_flag)
{
// a value of 2 means we need 3 values in a row with the same
// value to activate
const uint8_t debounce_count = 2;
if (chan < CH_7 || chan > CH_12) {
// someone has forgotten to expand the debounce channel range
return false;
}
struct debounce &db = aux_debounce[chan-CH_7];
if (db.ch_flag != ch_flag) {
db.ch_flag = ch_flag;
db.count = 0;
return false;
}
if (db.count < debounce_count) {
db.count++;
}
return db.count >= debounce_count;
}
// 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)
{
2012-12-19 12:08:59 -04:00
switch(ch_function) {
case AUXSW_FLIP:
// flip if switch is on, positive throttle and we're actually flying
if (ch_flag == AUX_SWITCH_HIGH) {
2016-01-25 19:40:41 -04:00
set_mode(FLIP, MODE_REASON_TX_COMMAND);
}
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 MODE_RTL_ENABLED == ENABLED
if (ch_flag == AUX_SWITCH_HIGH) {
// engage RTL (if not possible we remain in current flight mode)
2016-01-25 19:40:41 -04:00
set_mode(RTL, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in RTL
if (control_mode == RTL) {
reset_control_switch();
}
}
#endif
break;
case AUXSW_SAVE_TRIM:
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (channel_throttle->get_control_in() == 0)) {
save_trim();
}
break;
case AUXSW_SAVE_WP:
#if MODE_AUTO_ENABLED == ENABLED
// 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->get_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->get_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);
}
}
#endif
break;
case AUXSW_MISSION_RESET:
#if MODE_AUTO_ENABLED == ENABLED
if (ch_flag == AUX_SWITCH_HIGH) {
mission.reset();
}
#endif
break;
case AUXSW_AUTO:
#if MODE_AUTO_ENABLED == ENABLED
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(AUTO, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in AUTO
if (control_mode == AUTO) {
reset_control_switch();
}
}
#endif
break;
case AUXSW_CAMERA_TRIGGER:
#if CAMERA == ENABLED
if (ch_flag == AUX_SWITCH_HIGH) {
camera.take_picture();
}
#endif
break;
2016-04-27 08:37:04 -03:00
case AUXSW_RANGEFINDER:
// enable or disable the rangefinder
#if RANGEFINDER_ENABLED == ENABLED
if ((ch_flag == AUX_SWITCH_HIGH) && rangefinder.has_orientation(ROTATION_PITCH_270)) {
rangefinder_state.enabled = true;
} else {
rangefinder_state.enabled = false;
}
2014-07-08 02:41:39 -03:00
#endif
break;
case AUXSW_FENCE:
#if AC_FENCE == ENABLED
// 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);
}
#endif
break;
case AUXSW_ACRO_TRAINER:
#if MODE_ACRO_ENABLED == ENABLED
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;
}
#endif
2013-08-19 06:09:23 -03:00
break;
case AUXSW_GRIPPER:
#if GRIPPER_ENABLED == ENABLED
2013-12-17 00:58:11 -04:00
switch(ch_flag) {
case AUX_SWITCH_LOW:
2016-10-28 19:08:22 -03:00
g2.gripper.release();
Log_Write_Event(DATA_GRIPPER_RELEASE);
2013-12-17 00:58:11 -04:00
break;
case AUX_SWITCH_HIGH:
2016-10-28 19:08:22 -03:00
g2.gripper.grab();
Log_Write_Event(DATA_GRIPPER_GRAB);
2013-12-17 00:58:11 -04:00
break;
}
#endif
break;
case AUXSW_SPRAYER:
#if SPRAYER_ENABLED == ENABLED
sprayer.run(ch_flag == AUX_SWITCH_HIGH);
2013-09-11 05:05:25 -03:00
// if we are disarmed the pilot must want to test the pump
sprayer.test_pump((ch_flag == AUX_SWITCH_HIGH) && !motors->armed());
#endif
break;
case AUXSW_AUTOTUNE:
#if AUTOTUNE_ENABLED == ENABLED
// turn on auto tuner
switch(ch_flag) {
case AUX_SWITCH_LOW:
case AUX_SWITCH_MIDDLE:
2014-02-02 03:58:36 -04:00
// restore flight mode based on flight mode switch position
if (control_mode == AUTOTUNE) {
reset_control_switch();
2013-10-19 09:53:51 -03:00
}
break;
case AUX_SWITCH_HIGH:
2014-02-02 03:58:36 -04:00
// start an autotuning session
2016-01-25 19:40:41 -04:00
set_mode(AUTOTUNE, MODE_REASON_TX_COMMAND);
break;
}
#endif
break;
case AUXSW_LAND:
if (ch_flag == AUX_SWITCH_HIGH) {
2016-01-25 19:40:41 -04:00
set_mode(LAND, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in LAND
if (control_mode == LAND) {
reset_control_switch();
}
}
break;
case AUXSW_PARACHUTE_ENABLE:
#if PARACHUTE == ENABLED
// Parachute enable/disable
parachute.enabled(ch_flag == AUX_SWITCH_HIGH);
#endif
break;
case AUXSW_PARACHUTE_RELEASE:
#if PARACHUTE == ENABLED
if (ch_flag == AUX_SWITCH_HIGH) {
parachute_manual_release();
}
#endif
break;
case AUXSW_PARACHUTE_3POS:
#if PARACHUTE == ENABLED
// 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;
}
2014-04-02 12:20:45 -03:00
#endif
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;
case AUXSW_RETRACT_MOUNT:
#if MOUNT == ENABLE
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;
}
#endif
break;
2014-08-09 01:36:35 -03:00
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_position(AP_LandingGear::LandingGear_Deploy);
break;
case AUX_SWITCH_HIGH:
landinggear.set_position(AP_LandingGear::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
2016-01-21 22:59:47 -04:00
ap.motor_interlock_switch = (ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
break;
case AUXSW_BRAKE:
#if MODE_BRAKE_ENABLED == ENABLED
// brake flight mode
if (ch_flag == AUX_SWITCH_HIGH) {
2016-01-25 19:40:41 -04:00
set_mode(BRAKE, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in BRAKE
if (control_mode == BRAKE) {
reset_control_switch();
}
}
#endif
break;
2016-03-03 02:27:55 -04:00
case AUXSW_THROW:
#if MODE_THROW_ENABLED == ENABLED
2016-03-03 02:27:55 -04:00
// throw flight mode
if (ch_flag == AUX_SWITCH_HIGH) {
2016-01-25 19:40:41 -04:00
set_mode(THROW, MODE_REASON_TX_COMMAND);
2016-03-03 02:27:55 -04:00
} else {
// return to flight mode switch's flight mode if we are currently in throw mode
if (control_mode == THROW) {
reset_control_switch();
}
}
#endif
2016-03-03 02:27:55 -04:00
break;
case AUXSW_AVOID_ADSB:
2018-02-16 10:21:55 -04:00
#if ADSB_ENABLED == ENABLED
// enable or disable AP_Avoidance
if (ch_flag == AUX_SWITCH_HIGH) {
avoidance_adsb.enable();
Log_Write_Event(DATA_AVOIDANCE_ADSB_ENABLE);
} else {
avoidance_adsb.disable();
Log_Write_Event(DATA_AVOIDANCE_ADSB_DISABLE);
}
2018-02-16 10:21:55 -04:00
#endif
break;
case AUXSW_PRECISION_LOITER:
#if PRECISION_LANDING == ENABLED && MODE_LOITER_ENABLED == ENABLED
switch (ch_flag) {
case AUX_SWITCH_HIGH:
mode_loiter.set_precision_loiter_enabled(true);
break;
case AUX_SWITCH_LOW:
mode_loiter.set_precision_loiter_enabled(false);
break;
}
#endif
break;
case AUXSW_AVOID_PROXIMITY:
#if PROXIMITY_ENABLED == ENABLED && AC_AVOID_ENABLED == ENABLED
switch (ch_flag) {
case AUX_SWITCH_HIGH:
avoid.proximity_avoidance_enable(true);
Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_ENABLE);
break;
case AUX_SWITCH_LOW:
avoid.proximity_avoidance_enable(false);
Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_DISABLE);
break;
}
#endif
break;
case AUXSW_ARMDISARM:
// arm or disarm the vehicle
switch (ch_flag) {
case AUX_SWITCH_HIGH:
2018-06-24 23:09:23 -03:00
init_arm_motors(AP_Arming::ArmingMethod::AUXSWITCH);
// remember that we are using an arming switch, for use by set_throttle_zero_flag
ap.armed_with_switch = true;
break;
case AUX_SWITCH_LOW:
init_disarm_motors();
break;
}
break;
2017-07-26 14:14:40 -03:00
2017-09-08 23:45:31 -03:00
case AUXSW_SMART_RTL:
#if MODE_SMARTRTL_ENABLED == ENABLED
2017-07-26 14:14:40 -03:00
if (ch_flag == AUX_SWITCH_HIGH) {
// engage SmartRTL (if not possible we remain in current flight mode)
2017-09-08 23:45:31 -03:00
set_mode(SMART_RTL, MODE_REASON_TX_COMMAND);
2017-07-26 14:14:40 -03:00
} else {
// return to flight mode switch's flight mode if we are currently in RTL
2017-09-08 23:45:31 -03:00
if (control_mode == SMART_RTL) {
2017-07-26 14:14:40 -03:00
reset_control_switch();
}
}
#endif
2017-07-26 14:14:40 -03:00
break;
case AUXSW_INVERTED:
#if FRAME_CONFIG == HELI_FRAME
// inverted flight option is disabled for heli single and dual frames
if (g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_QUAD) {
switch (ch_flag) {
case AUX_SWITCH_HIGH:
motors->set_inverted_flight(true);
attitude_control->set_inverted_flight(true);
heli_flags.inverted_flight = true;
break;
case AUX_SWITCH_LOW:
motors->set_inverted_flight(false);
attitude_control->set_inverted_flight(false);
heli_flags.inverted_flight = false;
break;
}
}
#endif
break;
case AUXSW_WINCH_ENABLE:
2018-02-10 10:23:06 -04:00
#if WINCH_ENABLED == ENABLED
switch (ch_flag) {
case AUX_SWITCH_HIGH:
// high switch maintains current position
g2.winch.release_length(0.0f);
Log_Write_Event(DATA_WINCH_LENGTH_CONTROL);
break;
default:
// all other position relax winch
g2.winch.relax();
Log_Write_Event(DATA_WINCH_RELAXED);
break;
}
2018-02-10 10:23:06 -04:00
#endif
break;
case AUXSW_WINCH_CONTROL:
2018-02-10 10:23:06 -04:00
#if WINCH_ENABLED == ENABLED
switch (ch_flag) {
case AUX_SWITCH_LOW:
// raise winch at maximum speed
g2.winch.set_desired_rate(-g2.winch.get_rate_max());
break;
case AUX_SWITCH_HIGH:
// lower winch at maximum speed
g2.winch.set_desired_rate(g2.winch.get_rate_max());
break;
case AUX_SWITCH_MIDDLE:
default:
g2.winch.set_desired_rate(0.0f);
break;
}
2018-02-10 10:23:06 -04:00
#endif
break;
case AUXSW_RC_OVERRIDE_ENABLE:
// Allow or disallow RC_Override
switch (ch_flag) {
case AUX_SWITCH_HIGH: {
ap.rc_override_enable = true;
break;
}
case AUX_SWITCH_LOW: {
ap.rc_override_enable = false;
break;
}
}
break;
#ifdef USERHOOK_AUXSWITCH
case AUXSW_USER_FUNC1:
userhook_auxSwitch1(ch_flag);
break;
case AUXSW_USER_FUNC2:
userhook_auxSwitch2(ch_flag);
break;
case AUXSW_USER_FUNC3:
userhook_auxSwitch3(ch_flag);
break;
#endif
}
}
// save_trim - adds roll and pitch trims from the radio to ahrs
void Copter::save_trim()
{
// save roll and pitch trim
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
float roll_trim = ToRad((float)channel_roll->get_control_in()/100.0f);
float pitch_trim = ToRad((float)channel_pitch->get_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
2013-08-14 00:07:35 -03:00
AP_Notify::flags.save_trim = true;
// calculate roll trim adjustment
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
float roll_trim_adjustment = ToRad((float)channel_roll->get_control_in() / 4000.0f);
// calculate pitch trim adjustment
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
float pitch_trim_adjustment = ToRad((float)channel_pitch->get_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) {
2013-08-14 00:07:35 -03:00
AP_Notify::flags.save_trim = false;
}
}
}