aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: add comments to control switch debouncing

Browse Source
This commit is contained in:
Randy Mackay 2014-10-31 15:48:28 +09:00
parent 5c8b39562f
commit 8a61f5acd2
2 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/ArduCopter.pde
View File

@ -390,13 +390,11 @@ static union {
// This is the state of the flight control system // This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO, // There are multiple states defined such as STABILIZE, ACRO,
static int8_t control_mode = STABILIZE; static int8_t control_mode = STABILIZE;
// Used to maintain the state of the previous control switch position // Structure used to detect changes in the flight mode control switch
// This is set to -1 when we need to re-read the switch
static struct { static struct {
int8_t debounced_switch_position; int8_t debounced_switch_position; // currently used switch position
int8_t last_switch_position; int8_t last_switch_position; // switch position in previous iteration
uint32_t last_edge_time_ms; uint32_t last_edge_time_ms; // system time that switch position was last changed
} control_switch_state; } control_switch_state;
static RCMapper rcmap; static RCMapper rcmap;

5
ArduCopter/switches.pde
View File

@ -1,11 +1,12 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define CONTROL_SWITCH_DEBOUNCE_TIME_MS 200 #define CONTROL_SWITCH_DEBOUNCE_TIME_MS 200
#define CONTROL_SWITCH_COUNTER 20 // 20 iterations at 100hz (i.e. 2/10th of a second) at a new switch position will cause flight mode change
static void read_control_switch() static void read_control_switch()
{ {
uint32_t tnow_ms = millis(); uint32_t tnow_ms = millis();
// calculate position of flight mode switch
int8_t switch_position; int8_t switch_position;
if (g.rc_5.radio_in < 1231) switch_position = 0; if (g.rc_5.radio_in < 1231) switch_position = 0;
else if (g.rc_5.radio_in < 1361) switch_position = 1; else if (g.rc_5.radio_in < 1361) switch_position = 1;
@ -14,10 +15,12 @@ static void read_control_switch()
else if (g.rc_5.radio_in < 1750) switch_position = 4; else if (g.rc_5.radio_in < 1750) switch_position = 4;
else switch_position = 5; else switch_position = 5;
// store time that switch last moved
if(control_switch_state.last_switch_position != switch_position) { if(control_switch_state.last_switch_position != switch_position) {
control_switch_state.last_edge_time_ms = tnow_ms; control_switch_state.last_edge_time_ms = tnow_ms;
} }
// debounce switch
bool control_switch_changed = control_switch_state.debounced_switch_position != switch_position; bool control_switch_changed = control_switch_state.debounced_switch_position != switch_position;
bool sufficient_time_elapsed = tnow_ms - control_switch_state.last_edge_time_ms > CONTROL_SWITCH_DEBOUNCE_TIME_MS; bool sufficient_time_elapsed = tnow_ms - control_switch_state.last_edge_time_ms > CONTROL_SWITCH_DEBOUNCE_TIME_MS;
bool failsafe_disengaged = !failsafe.radio && failsafe.radio_counter == 0; bool failsafe_disengaged = !failsafe.radio && failsafe.radio_counter == 0;