Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Change mode button function implementation

Browse Source
This commit is contained in:
Jacob Walser 2017-03-29 17:07:33 -04:00
parent 4e700ea4df
commit d629d4ba9e
5 changed files with 26 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
ArduSub/Parameters.cpp
View File

@ -213,48 +213,6 @@ const AP_Param::Info Sub::var_info[] = {
// @Increment: 1
GSCALAR(throttle_deadzone, "THR_DZ", THR_DZ_DEFAULT),
// @Param: FLTMODE1
// @DisplayName: Flight Mode 1
// @Description: Flight mode when Channel 5 pwm is <= 1230
// @Values: 0:Stabilize,2:DepthHold,19:Manual
// @User: Standard
GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1),
// @Param: FLTMODE2
// @DisplayName: Flight Mode 2
// @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
// @Values: 0:Stabilize,2:DepthHold,19:Manual
// @User: Standard
GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2),
// @Param: FLTMODE3
// @DisplayName: Flight Mode 3
// @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
// @Values: 0:Stabilize,2:DepthHold,19:Manual
// @User: Standard
GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3),
// @Param: FLTMODE4
// @DisplayName: Flight Mode 4
// @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
// @Values: 0:Stabilize,2:DepthHold,19:Manual
// @User: Standard
GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4),
// @Param: FLTMODE5
// @DisplayName: Flight Mode 5
// @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
// @Values: 0:Stabilize,2:DepthHold,19:Manual
// @User: Standard
GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5),
// @Param: FLTMODE6
// @DisplayName: Flight Mode 6
// @Description: Flight mode when Channel 5 pwm is >=1750
// @Values: 0:Stabilize,2:DepthHold,19:Manual
// @User: Standard
GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6),
// @Param: LOG_BITMASK
// @DisplayName: Log bitmask
// @Description: 4 byte bitmap of log types to enable

20
ArduSub/Parameters.h
View File

@ -148,17 +148,8 @@ public:
k_param_jbtn_15,
// Flight mode selection
k_param_flight_mode1 = 120,
k_param_flight_mode2,
k_param_flight_mode3,
k_param_flight_mode4,
k_param_flight_mode5,
k_param_flight_mode6,
// PID Controllers
k_param_p_pos_xy,
k_param_p_pos_xy = 126,
k_param_p_alt_hold,
k_param_pi_vel_xy,
k_param_p_vel_z,
@ -259,15 +250,6 @@ public:
//
AP_Int16 throttle_deadzone;
// Flight modes
//
AP_Int8 flight_mode1;
AP_Int8 flight_mode2;
AP_Int8 flight_mode3;
AP_Int8 flight_mode4;
AP_Int8 flight_mode5;
AP_Int8 flight_mode6;
// Misc
//
AP_Int32 log_bitmask;

1
ArduSub/Sub.cpp
View File

@ -22,7 +22,6 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
*/
Sub::Sub(void) :
DataFlash {FIRMWARE_STRING},
flight_modes(&g.flight_mode1),
mission(ahrs,
FUNCTOR_BIND_MEMBER(&Sub::start_command, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Sub::verify_command_callback, bool, const AP_Mission::Mission_Command &),

3
ArduSub/Sub.h
View File

@ -169,9 +169,6 @@ private:
AP_LeakDetector leak_detector;
// flight modes convenience array
AP_Int8 *flight_modes;
TSYS01 celsius;
AP_Baro barometer;
Compass compass;

82
ArduSub/joystick.cpp
View File

@ -5,7 +5,6 @@
// Anonymous namespace to hold variables used only in this file
namespace {
int16_t mode_switch_pwm = 1100;
float cam_tilt = 1500.0;
int16_t lights1 = 1100;
int16_t lights2 = 1100;
@ -18,7 +17,6 @@ int16_t video_switch = 1100;
int16_t x_last, y_last, z_last;
uint16_t buttons_prev;
float gain;
bool toggle_mode = true;
// Servo control output channels
// TODO: Allow selecting output channels
@ -33,7 +31,7 @@ void Sub::init_joystick()
{
default_js_buttons();
set_mode((control_mode_t)flight_modes[0].get(), MODE_REASON_TX_COMMAND); // Initialize flight mode
set_mode(MANUAL, MODE_REASON_TX_COMMAND); // Initialize flight mode
if (g.numGainSettings < 1) {
g.numGainSettings.set_and_save(1);
@ -97,7 +95,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
channels[2] = constrain_int16((z+zTrim)*throttleScale+throttleBase,1100,1900); // throttle
channels[3] = constrain_int16(r*rpyScale+rpyCenter,1100,1900); // yaw
channels[4] = mode_switch_pwm; // for testing only
//channels[4] = mode_switch_pwm; // for testing only
if (!roll_pitch_flag) {
// adjust forward and lateral with joystick input instead of roll and pitch
@ -125,10 +123,6 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
{
// For attempts to change control mode
control_mode_t next_mode = control_mode;
uint16_t next_mode_switch_pwm = mode_switch_pwm;
// Act based on the function assigned to this button
switch (get_button(button)->function(shift)) {
case JSButton::button_function_t::k_arm_toggle:
@ -144,43 +138,32 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
case JSButton::button_function_t::k_disarm:
init_disarm_motors();
break;
case JSButton::button_function_t::k_mode_toggle:
if (!held) {
next_mode = (control_mode_t)flight_modes[toggle_mode?1:0].get();
next_mode_switch_pwm = toggle_mode?1300:1100;
toggle_mode = !toggle_mode;
}
case JSButton::button_function_t::k_mode_manual:
set_mode(MANUAL, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mode_1:
next_mode = (control_mode_t)flight_modes[0].get();
next_mode_switch_pwm = 1100;
toggle_mode = true;
case JSButton::button_function_t::k_mode_stabilize:
set_mode(STABILIZE, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mode_2:
next_mode = (control_mode_t)flight_modes[1].get();
next_mode_switch_pwm = 1300;
toggle_mode = false;
case JSButton::button_function_t::k_mode_depth_hold:
set_mode(ALT_HOLD, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mode_3:
next_mode = (control_mode_t)flight_modes[2].get();
next_mode_switch_pwm = 1420;
toggle_mode = false;
case JSButton::button_function_t::k_mode_auto:
set_mode(AUTO, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mode_4:
next_mode = (control_mode_t)flight_modes[3].get();
next_mode_switch_pwm = 1550;
toggle_mode = false;
case JSButton::button_function_t::k_mode_guided:
set_mode(GUIDED, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mode_5:
next_mode = (control_mode_t)flight_modes[4].get();
next_mode_switch_pwm = 1690;
toggle_mode = false;
case JSButton::button_function_t::k_mode_circle:
set_mode(CIRCLE, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mode_6:
next_mode = (control_mode_t)flight_modes[5].get();
next_mode_switch_pwm = 1900;
toggle_mode = false;
case JSButton::button_function_t::k_mode_acro:
set_mode(ACRO, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mode_poshold:
set_mode(POSHOLD, MODE_REASON_TX_COMMAND);
break;
case JSButton::button_function_t::k_mount_center:
cam_tilt = g.cam_tilt_center;
break;
@ -501,23 +484,6 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
// Not implemented
break;
}
// Update the control mode if needed
if (control_mode != next_mode) {
if (set_mode(next_mode, MODE_REASON_TX_COMMAND)) {
// Notify user
if (ap.initialised) {
AP_Notify::events.user_mode_change = 1;
}
// Update CH5 pwm value (For GCS)
mode_switch_pwm = next_mode_switch_pwm;
} else {
// Notify user
if (ap.initialised) {
AP_Notify::events.user_mode_change_failed = 1;
}
}
}
}
JSButton* Sub::get_button(uint8_t index)
@ -564,10 +530,10 @@ JSButton* Sub::get_button(uint8_t index)
void Sub::default_js_buttons()
{
JSButton::button_function_t defaults[16][2] = {
{JSButton::button_function_t::k_mode_manual, JSButton::button_function_t::k_none},
{JSButton::button_function_t::k_mode_stabilize, JSButton::button_function_t::k_none},
{JSButton::button_function_t::k_mode_depth_hold, JSButton::button_function_t::k_none},
{JSButton::button_function_t::k_none, JSButton::button_function_t::k_none},
{JSButton::button_function_t::k_mode_1, JSButton::button_function_t::k_none},
{JSButton::button_function_t::k_mode_3, JSButton::button_function_t::k_none},
{JSButton::button_function_t::k_mode_2, JSButton::button_function_t::k_none},
{JSButton::button_function_t::k_disarm, JSButton::button_function_t::k_none},
{JSButton::button_function_t::k_shift, JSButton::button_function_t::k_none},