mirror of https://github.com/ArduPilot/ardupilot
Sub: Remove unsused aux switch code
This commit is contained in:
Jacob Walser
Randy Mackay
parent
c1959952b3
commit
7c552d3545
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -79,14 +79,6 @@
|
|||
# define CH6_TUNE_ENABLED DISABLED
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Aux Switches
|
||||
//
|
||||
|
||||
#ifndef AUXSW_ENABLED
|
||||
# define AUXSW_ENABLED DISABLED
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// RPM
|
||||
//
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue