Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Soaring: Use RCX_OPTION #14815

Merged
merged 4 commits into from
Jul 20, 2020
Merged

Soaring: Use RCX_OPTION #14815

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
0bfe037
RC_Channel: Add soaring aux function.
samuelctabor Jul 13, 2020
36eaed0
AP_Soaring: Use RC aux function rather than read RC directly.
samuelctabor Jul 13, 2020
7897997
Plane: Convert SOAR_ENABLE_CH to RCX_OPTION.
samuelctabor Jul 13, 2020
c459d28
autotest: Update soaring test to use RCX_OPTION.
samuelctabor Jul 13, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 15 additions & 0 deletions ArduPlane/Parameters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1422,6 +1422,21 @@ void Plane::load_parameters(void)
}
}

// Convert SOAR_ENABLE_CH to RCx_OPTION
AP_Int8 soar_enable_ch;
AP_Param::ConversionInfo soar_info = {
Parameters::k_param_g2,
968,
AP_PARAM_INT8,
nullptr
};
if (AP_Param::find_old_parameter(&soar_info, &soar_enable_ch) && soar_enable_ch.get() != 0) {
peterbarker marked this conversation as resolved.
Show resolved Hide resolved
RC_Channel *soar_ch = rc().channel(soar_enable_ch - 1);
if (soar_ch != nullptr && !soar_ch->option.configured()) {
soar_ch->option.set_and_save((int16_t)RC_Channel::AUX_FUNC::SOARING); // save the new param
}
}

hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
}

Expand Down
26 changes: 26 additions & 0 deletions ArduPlane/RC_Channel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -96,6 +96,27 @@ void RC_Channel_Plane::do_aux_function_crow_mode(AuxSwitchPos ch_flag)
}
}

void RC_Channel_Plane::do_aux_function_soaring_3pos(AuxSwitchPos ch_flag)
{
#if SOARING_ENABLED == ENABLED
SoaringController::ActiveStatus desired_state = SoaringController::ActiveStatus::SOARING_DISABLED;

switch (ch_flag) {
case AuxSwitchPos::LOW:
desired_state = SoaringController::ActiveStatus::SOARING_DISABLED;
break;
case AuxSwitchPos::MIDDLE:
desired_state = SoaringController::ActiveStatus::MANUAL_MODE_CHANGE;
break;
case AuxSwitchPos::HIGH:
desired_state = SoaringController::ActiveStatus::AUTO_MODE_CHANGE;
break;
}

plane.g2.soaring_controller.set_pilot_desired_state(desired_state);
#endif
peterbarker marked this conversation as resolved.
Show resolved Hide resolved
}

void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
const RC_Channel::AuxSwitchPos ch_flag)
{
Expand All @@ -114,6 +135,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
break;

case AUX_FUNC::Q_ASSIST:
case AUX_FUNC::SOARING:
do_aux_function(ch_option, ch_flag);
break;

Expand Down Expand Up @@ -184,6 +206,10 @@ void RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
do_aux_function_change_mode(Mode::Number::TAKEOFF, ch_flag);
break;

case AUX_FUNC::SOARING:
do_aux_function_soaring_3pos(ch_flag);
break;

case AUX_FUNC::FLAP:
break; // flap input label, nothing to do

Expand Down
2 changes: 2 additions & 0 deletions ArduPlane/RC_Channel.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,8 @@ class RC_Channel_Plane : public RC_Channel
void do_aux_function_q_assist_state(AuxSwitchPos ch_flag);

void do_aux_function_crow_mode(AuxSwitchPos ch_flag);

void do_aux_function_soaring_3pos(AuxSwitchPos ch_flag);
};

class RC_Channels_Plane : public RC_Channels
Expand Down
14 changes: 12 additions & 2 deletions Tools/autotest/arduplane.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1651,7 +1651,8 @@ def fly_soaring(self):

self.customise_SITL_commandline([],
model=model,
defaults_filepath=self.model_defaults_filepath("ArduPlane",model))
defaults_filepath=self.model_defaults_filepath("ArduPlane",model),
wipe=True)

self.load_mission('CMAC-soar.txt')

Expand All @@ -1662,7 +1663,16 @@ def fly_soaring(self):
self.arm_vehicle()

# Enable thermalling RC
rc_chan = self.get_parameter('SOAR_ENABLE_CH')
rc_chan = 0
for i in range(8):
peterbarker marked this conversation as resolved.
Show resolved Hide resolved
rcx_option = self.get_parameter('RC{0}_OPTION'.format(i+1))
if rcx_option==88:
rc_chan = i+1;
break

if rc_chan==0:
raise NotAchievedException("Did not find soaring enable channel option.")

self.send_set_rc(rc_chan, 1900)

# Wait to detect thermal
Expand Down
3 changes: 2 additions & 1 deletion Tools/autotest/default_params/plane-soaring.parm
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,15 @@ ARSPD_FBW_MIN 6
PTCH2SRV_I 0.200000
PTCH2SRV_IMAX 4000.000000

RC7_OPTION 88

SIM_THML_SCENARI 1

SOAR_ALT_CUTOFF 150.000000
SOAR_ALT_MAX 350.000000
SOAR_ALT_MIN 50.000000
SOAR_DIST_AHEAD 5.000000
SOAR_ENABLE 1.000000
SOAR_ENABLE_CH 7.000000
SOAR_MIN_CRSE_S 10.000000
SOAR_MIN_THML_S 20.000000
SOAR_POLAR_B 0.045000
Expand Down
26 changes: 4 additions & 22 deletions libraries/AP_Soaring/AP_Soaring.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,12 +114,7 @@ const AP_Param::GroupInfo SoaringController::var_info[] = {
// @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),
// 15 was SOAR_ENABLE_CH, now RCX_OPTION

// @Param: MAX_DRIFT
// @DisplayName: (Optional) Maximum drift distance to allow when thermalling.
Expand Down Expand Up @@ -376,23 +371,10 @@ float SoaringController::McCready(float alt)
SoaringController::ActiveStatus SoaringController::active_state() const
{
if (!soar_active) {
return ActiveStatus::DISABLED;
}
if (soar_active_ch <= 0) {
// no activation channel
return ActiveStatus::AUTO_MODE_CHANGE;
}

uint16_t radio_in = RC_Channels::get_radio_in(soar_active_ch-1);

// active when above 1400, with auto mode changes when above 1700
if (radio_in >= 1700) {
return ActiveStatus::AUTO_MODE_CHANGE;
} else if (radio_in >= 1400) {
return ActiveStatus::MANUAL_MODE_CHANGE;
return ActiveStatus::SOARING_DISABLED;
}

return ActiveStatus::DISABLED;
return _pilot_desired_state;
}

void SoaringController::update_active_state()
Expand All @@ -402,7 +384,7 @@ void SoaringController::update_active_state()

if (state_changed) {
switch (status) {
case ActiveStatus::DISABLED:
case ActiveStatus::SOARING_DISABLED:
peterbarker marked this conversation as resolved.
Show resolved Hide resolved
// It's not enabled, but was enabled on the last loop.
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Disabled.");
set_throttle_suppressed(false);
Expand Down
7 changes: 5 additions & 2 deletions libraries/AP_Soaring/AP_Soaring.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,6 @@ class SoaringController {

protected:
AP_Int8 soar_active;
AP_Int8 soar_active_ch;
AP_Float thermal_vspeed;
AP_Float thermal_q1;
AP_Float thermal_q2;
Expand Down Expand Up @@ -86,7 +85,7 @@ class SoaringController {
};

enum class ActiveStatus {
DISABLED,
peterbarker marked this conversation as resolved.
Show resolved Hide resolved
SOARING_DISABLED,
MANUAL_MODE_CHANGE,
AUTO_MODE_CHANGE
};
Expand Down Expand Up @@ -123,11 +122,15 @@ class SoaringController {

bool is_active() const {return _last_update_status>=SoaringController::ActiveStatus::MANUAL_MODE_CHANGE;};

void set_pilot_desired_state(ActiveStatus pilot_desired_state) {_pilot_desired_state = pilot_desired_state;};

private:
// slow down messages if they are the same. During loiter we could smap the same message. Only show new messages during loiters
LoiterStatus _cruise_criteria_msg_last;

ActiveStatus _last_update_status;

ActiveStatus _pilot_desired_state = ActiveStatus::AUTO_MODE_CHANGE;

ActiveStatus active_state() const;
};
1 change: 1 addition & 0 deletions libraries/RC_Channel/RC_Channel.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -185,6 +185,7 @@ class RC_Channel {
GENERATOR = 85, // generator control
TER_DISABLE = 86, // disable terrain following in CRUISE/FBWB modes
CROW_SELECT = 87, // select CROW mode for diff spoilers;high disables,mid forces progressive
SOARING = 88, // three-position switch to set soaring mode

// entries from 100 onwards are expected to be developer
// options used for testing
Expand Down