Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Remove unsused aux switch code

This commit is contained in:
Jacob Walser 2017-03-09 15:53:32 -05:00 committed by Randy Mackay
parent c1959952b3
commit 7c552d3545
11 changed files with 41 additions and 538 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduSub/AP_State.cpp
View File

@ -74,12 +74,6 @@ void Sub::set_pre_arm_check(bool b)
}
}
void Sub::update_using_interlock()
{
// check if we are using motor interlock control on an aux switch
ap.using_interlock = check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK);
}
void Sub::set_motor_emergency_stop(bool b)
{
if (ap.motor_emergency_stop != b) {

3
ArduSub/ArduSub.cpp
View File

@ -31,7 +31,6 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
SCHED_TASK(update_optical_flow, 200, 160),
#endif
SCHED_TASK(update_batt_compass, 10, 120),
SCHED_TASK(read_aux_switches, 10, 50),
SCHED_TASK(auto_disarm_check, 10, 50),
SCHED_TASK(auto_trim, 10, 75),
SCHED_TASK(read_rangefinder, 20, 100),
@ -381,8 +380,6 @@ void Sub::one_hz_loop()
// make it possible to change ahrs orientation at runtime during initial config
ahrs.set_orientation();
update_using_interlock();
// set all throttle channel settings
motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
}

44
ArduSub/Parameters.cpp
View File

@ -309,50 +309,6 @@ const AP_Param::Info Sub::var_info[] = {
GSCALAR(radio_tuning_high, "TUNE_HIGH", 1000),
#endif
#if AUXSW_ENABLED == ENABLED
// @Param: CH7_OPT
// @DisplayName: Channel 7 option
// @Description: Select which function if performed when CH7 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @User: Standard
GSCALAR(ch7_option, "CH7_OPT", AUXSW_DO_NOTHING),
// @Param: CH8_OPT
// @DisplayName: Channel 8 option
// @Description: Select which function if performed when CH8 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @User: Standard
GSCALAR(ch8_option, "CH8_OPT", AUXSW_DO_NOTHING),
// @Param: CH9_OPT
// @DisplayName: Channel 9 option
// @Description: Select which function if performed when CH9 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @User: Standard
GSCALAR(ch9_option, "CH9_OPT", AUXSW_DO_NOTHING),
// @Param: CH10_OPT
// @DisplayName: Channel 10 option
// @Description: Select which function if performed when CH10 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @User: Standard
GSCALAR(ch10_option, "CH10_OPT", AUXSW_DO_NOTHING),
// @Param: CH11_OPT
// @DisplayName: Channel 11 option
// @Description: Select which function if performed when CH11 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @User: Standard
GSCALAR(ch11_option, "CH11_OPT", AUXSW_DO_NOTHING),
// @Param: CH12_OPT
// @DisplayName: Channel 12 option
// @Description: Select which function if performed when CH12 is above 1800 pwm
// @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw
// @User: Standard
GSCALAR(ch12_option, "CH12_OPT", AUXSW_DO_NOTHING),
#endif
// @Param: DISARM_DELAY
// @DisplayName: Disarm delay
// @Description: Delay before automatic disarm in seconds. A value of zero disables auto disarm.

18
ArduSub/Parameters.h
View File

@ -216,17 +216,8 @@ public:
k_param_acro_balance_roll,
k_param_acro_balance_pitch,
// AUX switch options
k_param_ch7_option, // Disabled
k_param_ch8_option, // Disabled
k_param_ch9_option, // Disabled
k_param_ch10_option, // Disabled
k_param_ch11_option, // Disabled
k_param_ch12_option, // Disabled
// RPM Sensor
k_param_rpm_sensor, // Disabled
k_param_rpm_sensor = 232, // Disabled
// RC_Mapper Library
k_param_rcmap, // Disabled
@ -301,12 +292,7 @@ public:
AP_Int16 radio_tuning_high;
AP_Int16 radio_tuning_low;
#endif
AP_Int8 ch7_option;
AP_Int8 ch8_option;
AP_Int8 ch9_option;
AP_Int8 ch10_option;
AP_Int8 ch11_option;
AP_Int8 ch12_option;
AP_Int8 disarm_delay;
AP_Int8 fs_ekf_action;

10
ArduSub/Sub.h
View File

@ -241,7 +241,7 @@ private:
uint8_t compass_mot : 1; // true if we are currently performing compassmot calibration
uint8_t motor_test : 1; // true if we are currently performing the motors test
uint8_t initialised : 1; // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes
uint8_t throttle_zero : 1; // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock
uint8_t throttle_zero : 1; // true if the throttle stick is at zero
uint8_t system_time_set : 1; // true if the system time has been set from the GPS
uint8_t gps_base_pos_set : 1; // true when the gps base position has been set (used for RTK gps only)
enum HomeState home_state : 2; // home status (unset, set, locked)
@ -500,7 +500,6 @@ private:
void set_simple_mode(uint8_t b);
void set_failsafe_battery(bool b);
void set_pre_arm_check(bool b);
void update_using_interlock();
void set_motor_emergency_stop(bool b);
float get_smoothing_gain();
void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max);
@ -744,13 +743,6 @@ private:
void print_divider(void);
void print_enabled(bool b);
void report_version();
bool check_if_auxsw_mode_used(uint8_t auxsw_mode_check);
bool check_duplicate_auxsw(void);
uint8_t read_3pos_switch(int16_t radio_in);
void read_aux_switches();
void init_aux_switches();
void init_aux_switch_function(int8_t ch_option, uint8_t ch_flag);
void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag);
void save_trim();
void auto_trim();
void init_ardupilot();

8
ArduSub/config.h
View File

@ -79,14 +79,6 @@
# define CH6_TUNE_ENABLED DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Aux Switches
//
#ifndef AUXSW_ENABLED
# define AUXSW_ENABLED DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// RPM
//

55
ArduSub/defines.h
View File

@ -29,61 +29,6 @@ enum autopilot_yaw_mode {
AUTO_YAW_CORRECT_XTRACK = 6 // steer the sub in order to correct for crosstrack error during line following
};
// Ch6... Ch12 aux switch control
#define AUX_SWITCH_PWM_TRIGGER_HIGH 1800 // pwm value above which the ch7 or ch8 option will be invoked
#define AUX_SWITCH_PWM_TRIGGER_LOW 1200 // pwm value below which the ch7 or ch8 option will be disabled
#define CH6_PWM_TRIGGER_HIGH 1800
#define CH6_PWM_TRIGGER_LOW 1200
// values used by the ap.ch7_opt and ap.ch8_opt flags
#define AUX_SWITCH_LOW 0 // indicates auxiliary switch is in the low position (pwm <1200)
#define AUX_SWITCH_MIDDLE 1 // indicates auxiliary switch is in the middle position (pwm >1200, <1800)
#define AUX_SWITCH_HIGH 2 // indicates auxiliary switch is in the high position (pwm >1800)
// Aux Switch enumeration
enum aux_sw_func {
AUXSW_DO_NOTHING = 0, // aux switch disabled
// AUXSW_FLIP = 2, // flip
AUXSW_SIMPLE_MODE = 3, // change to simple mode
// No RTL mode for Sub
// AUXSW_RTL = 4, // change to RTL flight mode
AUXSW_SAVE_TRIM = 5, // save current position as level
AUXSW_SAVE_WP = 7, // save mission waypoint or RTL if in auto mode
AUXSW_CAMERA_TRIGGER = 9, // trigger camera servo or relay
AUXSW_RANGEFINDER = 10, // allow enabling or disabling rangefinder in flight which helps avoid surface tracking when you are far above the ground
AUXSW_FENCE = 11, // allow enabling or disabling fence in flight
AUXSW_RESETTOARMEDYAW = 12, // changes yaw to be same as when quad was armed
AUXSW_SUPERSIMPLE_MODE = 13, // change to simple mode in middle, super simple at top
AUXSW_ACRO_TRAINER = 14, // low = disabled, middle = leveled, high = leveled and limited
// No sprayer for Sub, remove
// AUXSW_SPRAYER = 15, // enable/disable the crop sprayer
AUXSW_AUTO = 16, // change to auto flight mode
AUXSW_GRIPPER = 19, // Operate cargo grippers low=off, middle=neutral, high=on
// No parachute for Sub, remove
// AUXSW_PARACHUTE_ENABLE = 21, // Parachute enable/disable
// AUXSW_PARACHUTE_RELEASE = 22, // Parachute release
// AUXSW_PARACHUTE_3POS = 23, // Parachute disable, enable, release with 3 position switch
AUXSW_MISSION_RESET = 24, // Reset auto mission to start from first command
AUXSW_ATTCON_FEEDFWD = 25, // enable/disable the roll and pitch rate feed forward
AUXSW_ATTCON_ACCEL_LIM = 26, // enable/disable the roll, pitch and yaw accel limiting
AUXSW_RETRACT_MOUNT = 27, // Retract Mount
AUXSW_RELAY = 28, // Relay pin on/off (only supports first relay)
AUXSW_LOST_VEHICLE_SOUND = 30, // Play lost vehicle sound
AUXSW_MOTOR_ESTOP = 31, // Emergency Stop Switch
AUXSW_MOTOR_INTERLOCK = 32, // Motor On/Off switch
// AUXSW_BRAKE = 33, // Brake flight mode
AUXSW_RELAY2 = 34, // Relay2 pin on/off (in Mission planner set CH8_OPT = 34)
AUXSW_RELAY3 = 35, // Relay3 pin on/off (in Mission planner set CH9_OPT = 35)
AUXSW_RELAY4 = 36, // Relay4 pin on/off (in Mission planner set CH10_OPT = 36)
};
// HIL enumerations
#define HIL_MODE_DISABLED 0
#define HIL_MODE_SENSORS 1

5
ArduSub/motors.cpp
View File

@ -181,11 +181,6 @@ void Sub::lost_vehicle_check()
{
static uint8_t soundalarm_counter;
// disable if aux switch is setup to vehicle alarm as the two could interfere
if (check_if_auxsw_mode_used(AUXSW_LOST_VEHICLE_SOUND)) {
return;
}
// ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch
if (ap.throttle_zero && !motors.armed() && (channel_roll->get_control_in() > 4000) && (channel_pitch->get_control_in() > 4000)) {
if (soundalarm_counter >= LOST_VEHICLE_DELAY) {

38
ArduSub/radio.cpp
View File

@ -108,3 +108,41 @@ void Sub::set_throttle_zero_flag(int16_t throttle_control)
}
}
// save_trim - adds roll and pitch trims from the radio to ahrs
void Sub::save_trim()
{
// save roll and pitch trim
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 vehicle level
void Sub::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->get_control_in() / 4000.0f);
// calculate pitch trim adjustment
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) {
AP_Notify::flags.save_trim = false;
}
}
}

384
ArduSub/switches.cpp
View File

@ -1,384 +0,0 @@
#include "Sub.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;
// check_if_auxsw_mode_used - Check to see if any of the Aux Switches are set to a given mode.
bool Sub::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 Sub::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;
}
// read_3pos_switch
uint8_t Sub::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
}
// can't take reference to a bitfield member, thus a #define:
#define read_aux_switch(chan, flag, option) \
do { \
switch_position = read_3pos_switch(chan); \
if (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 Sub::read_aux_switches()
{
uint8_t switch_position;
// exit immediately during radio failsafe
if (failsafe.manual_control) {
return;
}
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);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
read_aux_switch(CH_12, aux_con.CH12_flag, g.ch12_option);
#endif
}
#undef read_aux_switch
// init_aux_switches - invoke configured actions at start-up for aux function where it is safe to do so
void Sub::init_aux_switches()
{
// set the CH7 ~ CH12 flags
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
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 Sub::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
{
// init channel options
switch (ch_option) {
case AUXSW_SIMPLE_MODE:
case AUXSW_RANGEFINDER:
case AUXSW_FENCE:
case AUXSW_RESETTOARMEDYAW:
case AUXSW_SUPERSIMPLE_MODE:
case AUXSW_ACRO_TRAINER:
case AUXSW_GRIPPER:
case AUXSW_RETRACT_MOUNT:
case AUXSW_MISSION_RESET:
case AUXSW_ATTCON_FEEDFWD:
case AUXSW_ATTCON_ACCEL_LIM:
case AUXSW_RELAY:
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 Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
{
switch (ch_function) {
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_SAVE_TRIM:
if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (channel_throttle->get_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->get_control_in() == 0)) {
return;
}
// create new mission command
AP_Mission::Mission_Command cmd = {};
// 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);
}
}
break;
#if CAMERA == ENABLED
case AUXSW_CAMERA_TRIGGER:
if (ch_flag == AUX_SWITCH_HIGH) {
do_take_picture();
}
break;
#endif
case AUXSW_RANGEFINDER:
// enable or disable the sonar
#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;
}
#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 GRIPPER_ENABLED == ENABLED
case AUXSW_GRIPPER:
switch (ch_flag) {
case AUX_SWITCH_LOW:
g2.gripper.release();
Log_Write_Event(DATA_GRIPPER_RELEASE);
break;
case AUX_SWITCH_HIGH:
g2.gripper.grab();
Log_Write_Event(DATA_GRIPPER_GRAB);
break;
}
break;
#endif
case AUXSW_AUTO:
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();
}
}
break;
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_LOST_VEHICLE_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);
// Log new status
if (motors.get_interlock()) {
Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);
} else {
Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED);
}
break;
}
}
// save_trim - adds roll and pitch trims from the radio to ahrs
void Sub::save_trim()
{
// save roll and pitch trim
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 vehicle level
void Sub::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->get_control_in() / 4000.0f);
// calculate pitch trim adjustment
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) {
AP_Notify::flags.save_trim = false;
}
}
}

8
ArduSub/system.cpp
View File

@ -149,9 +149,6 @@ void Sub::init_ardupilot()
gcs().set_dataflash(&DataFlash);
// update motor interlock state
update_using_interlock();
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up motors and output to escs
init_joystick(); // joystick initialization
@ -275,11 +272,6 @@ void Sub::init_ardupilot()
// initialise mission library
mission.init();
// initialise the flight mode and aux switch
// ---------------------------
// reset_control_switch();
init_aux_switches();
startup_INS_ground();
// we don't want writes to the serial port to cause us to pause