zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Sub: Change mode button function implementation

master
Jacob Walser 8 years ago
parent
4e700ea4df
commit
d629d4ba9e
5 changed files with 26 additions and 124 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 42
      ArduSub/Parameters.cpp
  2. 20
      ArduSub/Parameters.h
  3. 1
      ArduSub/Sub.cpp
  4. 3
      ArduSub/Sub.h
  5. 82
      ArduSub/joystick.cpp

42
ArduSub/Parameters.cpp
Unescape View File

@ -213,48 +213,6 @@ const AP_Param::Info Sub::var_info[] = { @@ -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
Unescape View File

@ -148,17 +148,8 @@ public: @@ -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: @@ -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
Unescape View File

@ -22,7 +22,6 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); @@ -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
Unescape View File

@ -169,9 +169,6 @@ private: @@ -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
Unescape View File

@ -5,7 +5,6 @@ @@ -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; @@ -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() @@ -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 @@ -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 @@ -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) @@ -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_stabilize:
set_mode(STABILIZE, 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_depth_hold:
set_mode(ALT_HOLD, 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_auto:
set_mode(AUTO, 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_guided:
set_mode(GUIDED, 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_circle:
set_mode(CIRCLE, 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_acro:
set_mode(ACRO, 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_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) @@ -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) @@ -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},

Write Preview
Loading…
Cancel
Save