diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index c6026a4554..cc61a9a004 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -378,7 +378,7 @@ bool AP_Arming_Copter::gps_checks(bool display_failure) } // check if flight mode requires GPS - bool mode_requires_gps = copter.mode_requires_GPS(copter.control_mode); + bool mode_requires_gps = copter.mode_requires_GPS(); // check if fence requires GPS bool fence_requires_gps = false; @@ -586,7 +586,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) control_mode_t control_mode = copter.control_mode; // always check if the current mode allows arming - if (!copter.mode_allows_arming(control_mode, arming_from_gcs)) { + if (!copter.mode_allows_arming(arming_from_gcs)) { if (display_failure) { gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Mode not armable"); } diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 95c1732afa..59778e8ac0 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -387,7 +387,7 @@ void Copter::ten_hz_logging_loop() if (should_log(MASK_LOG_RCOUT)) { DataFlash.Log_Write_RCOUT(); } - if (should_log(MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || landing_with_GPS())) { + if (should_log(MASK_LOG_NTUN) && (mode_requires_GPS() || landing_with_GPS())) { Log_Write_Nav_Tuning(); } if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) { diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index d8ee47f447..589cb989a6 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -1006,10 +1006,10 @@ private: bool set_mode(control_mode_t mode, mode_reason_t reason); void update_flight_mode(); void exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode); - bool mode_requires_GPS(control_mode_t mode); + bool mode_requires_GPS(); bool mode_has_manual_throttle(control_mode_t mode); - bool mode_allows_arming(control_mode_t mode, bool arming_from_gcs); - void notify_flight_mode(control_mode_t mode); + bool mode_allows_arming(bool arming_from_gcs); + void notify_flight_mode(); void heli_init(); void check_dynamic_flight(void); void update_heli_control_dynamics(void); diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index d1c24df2b6..1acb0b076c 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -173,7 +173,7 @@ failed: // update notify object if (success) { - notify_flight_mode(control_mode); + notify_flight_mode(); } // return success or failure @@ -339,13 +339,13 @@ void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_contr #endif //HELI_FRAME } -// returns true or false whether mode requires GPS -bool Copter::mode_requires_GPS(control_mode_t mode) +// returns true or false whether current control mode requires GPS +bool Copter::mode_requires_GPS() { if (flightmode != nullptr) { return flightmode->requires_GPS(); } - switch (mode) { + switch (control_mode) { case AUTO: case GUIDED: case LOITER: @@ -375,30 +375,31 @@ bool Copter::mode_has_manual_throttle(control_mode_t mode) } } -// mode_allows_arming - returns true if vehicle can be armed in the specified mode +// mode_allows_arming - returns true if vehicle can be armed in the current mode // arming_from_gcs should be set to true if the arming request comes from the ground station -bool Copter::mode_allows_arming(control_mode_t mode, bool arming_from_gcs) +bool Copter::mode_allows_arming(bool arming_from_gcs) { if (flightmode != nullptr) { return flightmode->allows_arming(arming_from_gcs); } + control_mode_t mode = control_mode; if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || mode == DRIFT || mode == SPORT || mode == THROW || (arming_from_gcs && (mode == GUIDED || mode == GUIDED_NOGPS))) { return true; } return false; } -// notify_flight_mode - sets notify object based on flight mode. Only used for OreoLED notify device -void Copter::notify_flight_mode(control_mode_t mode) +// 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.flight_mode = mode; + AP_Notify::flags.flight_mode = control_mode; if (flightmode != nullptr) { AP_Notify::flags.autopilot_mode = flightmode->is_autopilot(); notify.set_flight_mode_str(flightmode->name4()); return; } - switch (mode) { + switch (control_mode) { case AUTO: case GUIDED: case RTL: @@ -417,7 +418,7 @@ void Copter::notify_flight_mode(control_mode_t mode) } // set flight mode string - switch (mode) { + switch (control_mode) { case STABILIZE: notify.set_flight_mode_str("STAB"); break; diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 04820cac89..8f56c75a37 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -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 = mode_allows_arming(control_mode,false) && !mode_has_manual_throttle(control_mode); + bool mode_disarms_on_land = mode_allows_arming(false) && !mode_has_manual_throttle(control_mode); if (ap.land_complete && motors->armed() && disarm_on_land_configured && mode_disarms_on_land) { init_disarm_motors(); diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index ce30368695..e40bce70de 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -82,7 +82,7 @@ void Copter::init_ardupilot() // initialise notify system notify.init(true); - notify_flight_mode(control_mode); + notify_flight_mode(); // initialise battery monitor battery.init();