diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 03f6ced6fb..7e649dd8c6 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -380,6 +380,9 @@ static struct { uint32_t detected_time_ms; } obstacle; +// this is set to true when auto has been triggered to start +static bool auto_triggered; + //////////////////////////////////////////////////////////////////////////////// // Ground speed //////////////////////////////////////////////////////////////////////////////// diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index e4dcb14144..f4e9837481 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -69,6 +69,8 @@ public: k_param_speed_turn_gain, k_param_speed_turn_dist, k_param_ch7_option, + k_param_auto_trigger_pin, + k_param_auto_kickstart, // // 160: Radio settings @@ -178,6 +180,8 @@ public: AP_Int8 speed_turn_gain; AP_Float speed_turn_dist; AP_Int8 ch7_option; + AP_Int8 auto_trigger_pin; + AP_Float auto_kickstart; // RC channels RC_Channel channel_steer; diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 038cd72b93..67d2f1bf70 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -129,6 +129,22 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD", XTRACK_ENTRY_ANGLE_CENTIDEGREE), + // @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. + // @Values: -1:Disabled,0-9:TiggerPin + // @User: standard + GSCALAR(auto_trigger_pin, "AUTO_TRIGGER_PIN", -1), + + // @Param: AUTO_KICKSTART + // @DisplayName: Auto mode trigger kickstart acceleration + // @Description: X acceleration in meters/second/second to use to trigger start of auto mode. If set to zero then auto starts immediately, otherwise the rover waits for the X acceleration to go above this value before it will start the motor + // Units: m/s/s + // @Range: 0 20 + // Increment: 0.1 + // @User: standard + GSCALAR(auto_kickstart, "AUTO_KICKSTART", 0.0f), + // @Param: CRUISE_SPEED // @DisplayName: Target cruise speed in auto modes // @Description: The target speed in auto missions. diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde index 689fa30a64..9a75a033bf 100644 --- a/APMrover2/Steering.pde +++ b/APMrover2/Steering.pde @@ -17,11 +17,57 @@ static void throttle_slew_limit(int16_t last_throttle) } } +/* + check for triggering of start of auto mode + */ +static bool auto_check_trigger(void) +{ + // only applies to AUTO mode + if (control_mode != AUTO) { + return true; + } + + // if already triggered, then return true, so you don't + // need to hold the switch down + if (auto_triggered) { + return true; + } + + if (g.auto_trigger_pin == -1 && g.auto_kickstart == 0.0f) { + // no trigger configured - let's go! + auto_triggered = true; + return true; + } + + if (g.auto_trigger_pin != -1) { + hal.gpio->pinMode(g.auto_trigger_pin, GPIO_INPUT); + if (hal.gpio->read(g.auto_trigger_pin) == 0) { + auto_triggered = true; + return true; + } + } + + if (g.auto_kickstart != 0.0f) { + if (ins.get_accel().x >= g.auto_kickstart) { + auto_triggered = true; + return true; + } + } + + return false; +} + + /* calculate the throtte for auto-throttle modes */ static void calc_throttle(float target_speed) { + if (!auto_check_trigger()) { + g.channel_throttle.servo_out = g.throttle_min.get(); + return; + } + if (target_speed <= 0) { // cope with zero requested speed g.channel_throttle.servo_out = g.throttle_min.get(); diff --git a/APMrover2/system.pde b/APMrover2/system.pde index 5a4fa1705e..8b27834439 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -306,6 +306,10 @@ static void set_mode(enum mode mode) control_mode = mode; throttle_last = 0; throttle = 500; + + if (control_mode != AUTO) { + auto_triggered = false; + } switch(control_mode) {