Browse Source

Copter: eliminate mode_has_manual_throttle

master
Peter Barker 7 years ago committed by Randy Mackay
parent
commit
7b637334f4
  1. 2
      ArduCopter/AP_Arming.cpp
  2. 2
      ArduCopter/Attitude.cpp
  3. 1
      ArduCopter/Copter.h
  4. 2
      ArduCopter/baro_ground_effect.cpp
  5. 2
      ArduCopter/control_acro.cpp
  6. 2
      ArduCopter/control_stabilize.cpp
  7. 2
      ArduCopter/fence.cpp
  8. 14
      ArduCopter/flight_mode.cpp
  9. 4
      ArduCopter/land_detector.cpp
  10. 4
      ArduCopter/motors.cpp
  11. 2
      ArduCopter/system.cpp

2
ArduCopter/AP_Arming.cpp

@ -668,7 +668,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) @@ -668,7 +668,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs)
return false;
}
// in manual modes throttle must be at zero
if ((copter.mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && copter.channel_throttle->get_control_in() > 0) {
if ((copter.flightmode->has_manual_throttle() || control_mode == DRIFT) && copter.channel_throttle->get_control_in() > 0) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high");

2
ArduCopter/Attitude.cpp

@ -112,7 +112,7 @@ void Copter::update_throttle_hover() @@ -112,7 +112,7 @@ void Copter::update_throttle_hover()
}
// do not update in manual throttle modes or Drift
if (mode_has_manual_throttle(control_mode) || (control_mode == DRIFT)) {
if (flightmode->has_manual_throttle() || (control_mode == DRIFT)) {
return;
}

1
ArduCopter/Copter.h

@ -803,7 +803,6 @@ private: @@ -803,7 +803,6 @@ private:
void update_sensor_status_flags(void);
bool set_mode(control_mode_t mode, mode_reason_t reason);
void update_flight_mode();
bool mode_has_manual_throttle(control_mode_t mode);
void notify_flight_mode();
void heli_init();
void check_dynamic_flight(void);

2
ArduCopter/baro_ground_effect.cpp

@ -37,7 +37,7 @@ void Copter::update_ground_effect_detector(void) @@ -37,7 +37,7 @@ void Copter::update_ground_effect_detector(void)
}
// if we aren't taking off yet, reset the takeoff timer, altitude and complete flag
bool throttle_up = mode_has_manual_throttle(control_mode) && channel_throttle->get_control_in() > 0;
const bool throttle_up = flightmode->has_manual_throttle() && channel_throttle->get_control_in() > 0;
if (!throttle_up && ap.land_complete) {
gndeffect_state.takeoff_time_ms = tnow_ms;
gndeffect_state.takeoff_alt_cm = inertial_nav.get_altitude();

2
ArduCopter/control_acro.cpp

@ -9,7 +9,7 @@ @@ -9,7 +9,7 @@
bool Copter::FlightMode_ACRO::init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors->armed() && ap.land_complete && !_copter.mode_has_manual_throttle(_copter.control_mode) &&
if (motors->armed() && ap.land_complete && !_copter.flightmode->has_manual_throttle() &&
(get_pilot_desired_throttle(channel_throttle->get_control_in(), _copter.g2.acro_thr_mid) > _copter.get_non_takeoff_throttle())) {
return false;
}

2
ArduCopter/control_stabilize.cpp

@ -8,7 +8,7 @@ @@ -8,7 +8,7 @@
bool Copter::FlightMode_STABILIZE::init(bool ignore_checks)
{
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
if (motors->armed() && ap.land_complete && !_copter.mode_has_manual_throttle(_copter.control_mode) &&
if (motors->armed() && ap.land_complete && !_copter.flightmode->has_manual_throttle() &&
(get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
return false;
}

2
ArduCopter/fence.cpp

@ -30,7 +30,7 @@ void Copter::fence_check() @@ -30,7 +30,7 @@ void Copter::fence_check()
// disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle
// don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
if (ap.land_complete || (mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){
if (ap.land_complete || (flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){
init_disarm_motors();
}else{
// if we are within 100m of the fence, RTL

14
ArduCopter/flight_mode.cpp

@ -130,7 +130,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) @@ -130,7 +130,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter a non-manual throttle mode if the
// rotor runup is not complete
if (!ignore_checks && !mode_has_manual_throttle(mode) && !motors->rotor_runup_complete()){
if (!ignore_checks && !new_flightmode->has_manual_throttle() && !motors->rotor_runup_complete()){
return false;
}
#endif
@ -231,18 +231,6 @@ void Copter::exit_mode(Copter::FlightMode *&old_flightmode, @@ -231,18 +231,6 @@ void Copter::exit_mode(Copter::FlightMode *&old_flightmode,
#endif //HELI_FRAME
}
// mode_has_manual_throttle - returns true if the flight mode has a manual throttle (i.e. pilot directly controls throttle)
bool Copter::mode_has_manual_throttle(control_mode_t mode)
{
switch (mode) {
case ACRO:
case STABILIZE:
return true;
default:
return false;
}
}
// notify_flight_mode - sets notify object based on current flight mode. Only used for OreoLED notify device
void Copter::notify_flight_mode() {
AP_Notify::flags.autopilot_mode = flightmode->is_autopilot();

4
ArduCopter/land_detector.cpp

@ -111,7 +111,7 @@ void Copter::set_land_complete(bool b) @@ -111,7 +111,7 @@ void Copter::set_land_complete(bool b)
// trigger disarm-on-land if configured
bool disarm_on_land_configured = (g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0;
bool mode_disarms_on_land = flightmode->allows_arming(false) && !mode_has_manual_throttle(control_mode);
const bool mode_disarms_on_land = flightmode->allows_arming(false) && !flightmode->has_manual_throttle();
if (ap.land_complete && motors->armed() && disarm_on_land_configured && mode_disarms_on_land) {
init_disarm_motors();
@ -143,7 +143,7 @@ void Copter::update_throttle_thr_mix() @@ -143,7 +143,7 @@ void Copter::update_throttle_thr_mix()
return;
}
if (mode_has_manual_throttle(control_mode)) {
if (flightmode->has_manual_throttle()) {
// manual throttle
if(channel_throttle->get_control_in() <= 0) {
attitude_control->set_throttle_mix_min();

4
ArduCopter/motors.cpp

@ -46,7 +46,7 @@ void Copter::arm_motors_check() @@ -46,7 +46,7 @@ void Copter::arm_motors_check()
// full left
}else if (tmp < -4000) {
if (!mode_has_manual_throttle(control_mode) && !ap.land_complete) {
if (!flightmode->has_manual_throttle() && !ap.land_complete) {
arming_counter = 0;
return;
}
@ -98,7 +98,7 @@ void Copter::auto_disarm_check() @@ -98,7 +98,7 @@ void Copter::auto_disarm_check()
} else {
bool sprung_throttle_stick = (g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0;
bool thr_low;
if (mode_has_manual_throttle(control_mode) || !sprung_throttle_stick) {
if (flightmode->has_manual_throttle() || !sprung_throttle_stick) {
thr_low = ap.throttle_zero;
} else {
float deadband_top = channel_throttle->get_control_mid() + g.throttle_deadzone;

2
ArduCopter/system.cpp

@ -386,7 +386,7 @@ void Copter::update_auto_armed() @@ -386,7 +386,7 @@ void Copter::update_auto_armed()
return;
}
// if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
if(mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.radio) {
if(flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio) {
set_auto_armed(false);
}
#if FRAME_CONFIG == HELI_FRAME

Loading…
Cancel
Save