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

Sub: Ditch control mode RC switch logic

Browse Source 
Call set_mode() directly from joystick button handler
This commit is contained in:
Jacob Walser 2016-11-23 22:36:04 -05:00 committed by Andrew Tridgell
parent eb9aa93486
commit 8367bb4626
3 changed files with 36 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduSub/ArduSub.cpp
View File

@ -294,10 +294,9 @@ void Sub::fast_loop()
// called at 100hz
void Sub::rc_loop()
{
// Read radio and 3-position switch on radio
// Read radio
// -----------------------------------------
read_radio();
read_control_switch();
}
// throttle_loop - should be run at 50 hz

1
ArduSub/Sub.h
View File

@ -953,7 +953,6 @@ private:
void print_divider(void);
void print_enabled(bool b);
void report_version();
void read_control_switch();
bool check_if_auxsw_mode_used(uint8_t auxsw_mode_check);
bool check_duplicate_auxsw(void);
void reset_control_switch();

43
ArduSub/joystick.cpp
View File

@ -7,7 +7,7 @@
// Anonymous namespace to hold variables used only in this file
namespace {
int16_t mode = 1100;
int16_t mode_switch_pwm = 1100;
float cam_tilt = 1500.0;
float cam_tilt_goal = 1500.0;
float cam_tilt_alpha = 0.97;
@ -74,7 +74,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
channels[1] = 1500 + rollTrim; // roll
channels[2] = constrain_int16((z+zTrim)*throttleScale+throttleBase,1100,1900); // throttle
channels[3] = constrain_int16(r*rpyScale+rpyCenter,1100,1900); // yaw
channels[4] = mode; // for testing only
channels[4] = mode_switch_pwm; // for testing only
channels[5] = constrain_int16((x+xTrim)*rpyScale+rpyCenter,1100,1900); // forward for ROV
channels[6] = constrain_int16((y+yTrim)*rpyScale+rpyCenter,1100,1900); // lateral for ROV
channels[7] = 0xffff; // camera tilt (sent in camera_tilt_smooth)
@ -92,6 +92,10 @@ 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:
@ -111,22 +115,28 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held) {
init_disarm_motors();
break;
case JSButton::button_function_t::k_mode_1:
mode = 1100;
next_mode = (control_mode_t)flight_modes[0].get();
next_mode_switch_pwm = 1100;
break;
case JSButton::button_function_t::k_mode_2:
mode = 1300;
next_mode = (control_mode_t)flight_modes[1].get();
next_mode_switch_pwm = 1300;
break;
case JSButton::button_function_t::k_mode_3:
mode = 1420;
next_mode = (control_mode_t)flight_modes[2].get();
next_mode_switch_pwm = 1420;
break;
case JSButton::button_function_t::k_mode_4:
mode = 1550;
next_mode = (control_mode_t)flight_modes[3].get();
next_mode_switch_pwm = 1550;
break;
case JSButton::button_function_t::k_mode_5:
mode = 1690;
next_mode = (control_mode_t)flight_modes[4].get();
next_mode_switch_pwm = 1690;
break;
case JSButton::button_function_t::k_mode_6:
mode = 1900;
next_mode = (control_mode_t)flight_modes[5].get();
next_mode_switch_pwm = 1900;
break;
case JSButton::button_function_t::k_mount_center:
cam_tilt_goal = 1500;
@ -329,6 +339,23 @@ 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) {