diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 192910b698..e2d2052bd3 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -138,7 +138,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: AUTO_TRIGGER_PIN // @DisplayName: Auto mode trigger pin - // @Description: pin number to use to trigger start of auto mode. If set to -1 then don't use a trigger, otherwise this is a pin number which if held low in auto mode will start the motor. + // @Description: pin number to use to trigger start of auto mode. If set to -1 then don't use a trigger, otherwise this is a pin number which if held low in auto mode will start the motor, and otherwise will force the throttle off. This can be used in combination with INITIAL_MODE to give a 'press button to start' rover with no receiver. // @Values: -1:Disabled,0-9:TiggerPin // @User: standard GSCALAR(auto_trigger_pin, "AUTO_TRIGGER_PIN", -1), diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde index 6e235868e9..c1cc86d4cf 100644 --- a/APMrover2/Steering.pde +++ b/APMrover2/Steering.pde @@ -27,6 +27,13 @@ static bool auto_check_trigger(void) return true; } + // check for user pressing the auto trigger to off + if (auto_triggered && g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 1) { + gcs_send_text_P(SEVERITY_LOW, PSTR("AUTO triggered off")); + auto_triggered = false; + return false; + } + // if already triggered, then return true, so you don't // need to hold the switch down if (auto_triggered) { @@ -39,21 +46,10 @@ static bool auto_check_trigger(void) return true; } - if (g.auto_trigger_pin != -1) { - int8_t pin = hal.gpio->analogPinToDigitalPin(g.auto_trigger_pin); - if (pin != -1) { - // ensure we are in input mode - hal.gpio->pinMode(pin, GPIO_INPUT); - - // enable pullup - hal.gpio->write(pin, 1); - - if (hal.gpio->read(pin) == 0) { - gcs_send_text_P(SEVERITY_LOW, PSTR("Triggered AUTO with pin")); - auto_triggered = true; - return true; - } - } + if (g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 0) { + gcs_send_text_P(SEVERITY_LOW, PSTR("Triggered AUTO with pin")); + auto_triggered = true; + return true; } if (g.auto_kickstart != 0.0f) { diff --git a/APMrover2/system.pde b/APMrover2/system.pde index ecb52d6738..66c269b7c2 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -545,3 +545,21 @@ static void reboot_apm(void) hal.scheduler->reboot(); while (1); } + +/* + check a digitial pin for high,low (1/0) + */ +static uint8_t check_digital_pin(uint8_t pin) +{ + int8_t dpin = hal.gpio->analogPinToDigitalPin(pin); + if (dpin == -1) { + return 0; + } + // ensure we are in input mode + hal.gpio->pinMode(dpin, GPIO_INPUT); + + // enable pullup + hal.gpio->write(dpin, 1); + + return hal.gpio->read(dpin); +}