Browse Source

AP_Soaring: added SOAR_ENABLE_CH parameter

master
Andrew Tridgell 8 years ago
parent
commit
819c70494f
  1. 17
      libraries/AP_Soaring/AP_Soaring.cpp
  2. 1
      libraries/AP_Soaring/AP_Soaring.h

17
libraries/AP_Soaring/AP_Soaring.cpp

@ -117,6 +117,13 @@ const AP_Param::GroupInfo SoaringController::var_info[] = { @@ -117,6 +117,13 @@ const AP_Param::GroupInfo SoaringController::var_info[] = {
// @Range: 0 1000.0
// @User: Advanced
AP_GROUPINFO("ALT_CUTOFF", 14, SoaringController, alt_cutoff, 250.0),
// @Param: ENABLE_CH
// @DisplayName: (Optional) RC channel that toggles the soaring controller on and off
// @Description: Toggles the soaring controller on and off. This parameter has any effect only if SOAR_ENABLE is set to 1 and this parameter is set to a valid non-zero channel number. When set, soaring will be activated when RC input to the specified channel is greater than or equal to 1700.
// @Range: 0 16
// @User: Advanced
AP_GROUPINFO("ENABLE_CH", 15, SoaringController, soar_active_ch, 0),
AP_GROUPEND
};
@ -382,5 +389,13 @@ float SoaringController::McCready(float alt) @@ -382,5 +389,13 @@ float SoaringController::McCready(float alt)
bool SoaringController::is_active()
{
return soar_active;
if (!soar_active) {
return false;
}
if (soar_active_ch <= 0) {
// no activation channel
return true;
}
// active when above 1700
return hal.rcin->read(soar_active_ch-1) >= 1700;
}

1
libraries/AP_Soaring/AP_Soaring.h

@ -67,6 +67,7 @@ class SoaringController { @@ -67,6 +67,7 @@ class SoaringController {
protected:
AP_Int8 soar_active;
AP_Int8 soar_active_ch;
AP_Float thermal_vspeed;
AP_Float thermal_q1;
AP_Float thermal_q2;

Loading…
Cancel
Save