mirror of https://github.com/ArduPilot/ardupilot
ArduPlane: add CROW flaps mode select switch
This commit is contained in:
parent
966e9ddf2b
commit
16a15f5450
|
@ -1109,6 +1109,14 @@ private:
|
|||
bool ekf_over_threshold();
|
||||
void failsafe_ekf_event();
|
||||
void failsafe_ekf_off_event(void);
|
||||
|
||||
enum class CrowMode {
|
||||
NORMAL,
|
||||
PROGRESSIVE,
|
||||
CROW_DISABLED,
|
||||
};
|
||||
|
||||
CrowMode crow_mode = CrowMode::NORMAL;
|
||||
|
||||
public:
|
||||
void failsafe_check(void);
|
||||
|
|
|
@ -78,6 +78,24 @@ void RC_Channel_Plane::do_aux_function_q_assist_state(AuxSwitchPos ch_flag)
|
|||
}
|
||||
}
|
||||
|
||||
void RC_Channel_Plane::do_aux_function_crow_mode(AuxSwitchPos ch_flag)
|
||||
{
|
||||
switch(ch_flag) {
|
||||
case AuxSwitchPos::HIGH:
|
||||
plane.crow_mode = Plane::CrowMode::CROW_DISABLED;
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Crow Flaps Disabled");
|
||||
break;
|
||||
case AuxSwitchPos::MIDDLE:
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Progressive Crow Flaps");
|
||||
plane.crow_mode = Plane::CrowMode::PROGRESSIVE;
|
||||
break;
|
||||
case AuxSwitchPos::LOW:
|
||||
plane.crow_mode = Plane::CrowMode::NORMAL;
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Normal Crow Flaps");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
|
||||
const RC_Channel::AuxSwitchPos ch_flag)
|
||||
{
|
||||
|
@ -114,6 +132,10 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
|
|||
do_aux_function(ch_option, ch_flag);
|
||||
break;
|
||||
|
||||
case AUX_FUNC::CROW_SELECT:
|
||||
do_aux_function(ch_option, ch_flag);
|
||||
break;
|
||||
|
||||
default:
|
||||
// handle in parent class
|
||||
RC_Channel::init_aux_function(ch_option, ch_flag);
|
||||
|
@ -192,6 +214,9 @@ void RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
|
|||
gcs().send_text(MAV_SEVERITY_INFO, "NON AUTO TERRN: %s", plane.non_auto_terrain_disable?"OFF":"ON");
|
||||
break;
|
||||
|
||||
case AUX_FUNC::CROW_SELECT:
|
||||
do_aux_function_crow_mode(ch_flag);
|
||||
break;
|
||||
|
||||
default:
|
||||
RC_Channel::do_aux_function(ch_option, ch_flag);
|
||||
|
|
|
@ -19,6 +19,8 @@ private:
|
|||
AuxSwitchPos ch_flag);
|
||||
|
||||
void do_aux_function_q_assist_state(AuxSwitchPos ch_flag);
|
||||
|
||||
void do_aux_function_crow_mode(AuxSwitchPos ch_flag);
|
||||
};
|
||||
|
||||
class RC_Channels_Plane : public RC_Channels
|
||||
|
|
|
@ -202,7 +202,8 @@ void Plane::dspoiler_update(void)
|
|||
const int8_t bitmask = g2.crow_flap_options.get();
|
||||
const bool flying_wing = (bitmask & CrowFlapOptions::FLYINGWING) != 0;
|
||||
const bool full_span_aileron = (bitmask & CrowFlapOptions::FULLSPAN) != 0;
|
||||
const bool progresive_crow = (bitmask & CrowFlapOptions::PROGRESSIVE_CROW) != 0;
|
||||
//progressive crow when option is set or RC switch is set to progressive
|
||||
const bool progressive_crow = (bitmask & CrowFlapOptions::PROGRESSIVE_CROW) != 0 || crow_mode == CrowMode::PROGRESSIVE;
|
||||
|
||||
// if flying wing use elevons else use ailerons
|
||||
float elevon_left;
|
||||
|
@ -253,7 +254,10 @@ void Plane::dspoiler_update(void)
|
|||
}
|
||||
}
|
||||
|
||||
const int16_t weight_outer = g2.crow_flap_weight_outer.get();
|
||||
int16_t weight_outer = g2.crow_flap_weight_outer.get();
|
||||
if (crow_mode == Plane::CrowMode::CROW_DISABLED) { //override totally aileron crow if crow RC switch set to disabled
|
||||
weight_outer = 0;
|
||||
}
|
||||
const int16_t weight_inner = g2.crow_flap_weight_inner.get();
|
||||
if (weight_outer > 0 || weight_inner > 0) {
|
||||
/*
|
||||
|
@ -266,7 +270,7 @@ void Plane::dspoiler_update(void)
|
|||
if (flap_percent > 0) {
|
||||
float inner_flap_scaled = (float)flap_percent;
|
||||
float outer_flap_scaled = (float)flap_percent;
|
||||
if (progresive_crow) {
|
||||
if (progressive_crow) {
|
||||
// apply 0 - full inner from 0 to 50% flap then add in outer above 50%
|
||||
inner_flap_scaled = constrain_float(inner_flap_scaled * 2, 0,100);
|
||||
outer_flap_scaled = constrain_float(outer_flap_scaled - 50, 0,50) * 2;
|
||||
|
|
Loading…
Reference in New Issue