From 88708978473c4d67cc170eb2ff74180cb9497aad Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 10 Nov 2017 11:55:29 +0900 Subject: [PATCH] Copter: FlightMode objects use lower case --- ArduCopter/Copter.h | 40 ++++++------ ArduCopter/FlightMode.h | 92 +++++++++++++-------------- ArduCopter/FlightMode_AVOID_ADSB.h | 6 +- ArduCopter/Log.cpp | 4 +- ArduCopter/control_acro.cpp | 6 +- ArduCopter/control_althold.cpp | 4 +- ArduCopter/control_auto.cpp | 56 ++++++++-------- ArduCopter/control_autotune.cpp | 58 ++++++++--------- ArduCopter/control_avoid_adsb.cpp | 12 ++-- ArduCopter/control_brake.cpp | 6 +- ArduCopter/control_circle.cpp | 4 +- ArduCopter/control_drift.cpp | 6 +- ArduCopter/control_flip.cpp | 4 +- ArduCopter/control_guided.cpp | 50 +++++++-------- ArduCopter/control_guided_nogps.cpp | 8 +-- ArduCopter/control_land.cpp | 12 ++-- ArduCopter/control_loiter.cpp | 8 +-- ArduCopter/control_poshold.cpp | 18 +++--- ArduCopter/control_smart_rtl.cpp | 14 ++-- ArduCopter/control_sport.cpp | 4 +- ArduCopter/control_stabilize.cpp | 4 +- ArduCopter/control_throw.cpp | 12 ++-- ArduCopter/heli_control_acro.cpp | 4 +- ArduCopter/heli_control_stabilize.cpp | 4 +- 24 files changed, 218 insertions(+), 218 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 0720044a5a..22c011e6d4 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -946,54 +946,54 @@ private: Copter::FlightMode *flightmode; #if FRAME_CONFIG == HELI_FRAME - Copter::FlightMode_ACRO_Heli flightmode_acro{*this}; + Copter::FlightMode_Acro_Heli flightmode_acro{*this}; #else - Copter::FlightMode_ACRO flightmode_acro{*this}; + Copter::FlightMode_Acro flightmode_acro{*this}; #endif - Copter::FlightMode_ALTHOLD flightmode_althold{*this}; + Copter::FlightMode_AltHold flightmode_althold{*this}; - Copter::FlightMode_AUTO flightmode_auto{*this, mission, circle_nav}; + Copter::FlightMode_Auto flightmode_auto{*this, mission, circle_nav}; #if AUTOTUNE_ENABLED == ENABLED - Copter::FlightMode_AUTOTUNE flightmode_autotune{*this}; + Copter::FlightMode_AutoTune flightmode_autotune{*this}; #endif - Copter::FlightMode_BRAKE flightmode_brake{*this}; + Copter::FlightMode_Brake flightmode_brake{*this}; - Copter::FlightMode_CIRCLE flightmode_circle{*this, circle_nav}; + Copter::FlightMode_Circle flightmode_circle{*this, circle_nav}; - Copter::FlightMode_DRIFT flightmode_drift{*this}; + Copter::FlightMode_Drift flightmode_drift{*this}; - Copter::FlightMode_FLIP flightmode_flip{*this}; + Copter::FlightMode_Flip flightmode_flip{*this}; - Copter::FlightMode_GUIDED flightmode_guided{*this}; + Copter::FlightMode_Guided flightmode_guided{*this}; - Copter::FlightMode_LAND flightmode_land{*this}; + Copter::FlightMode_Land flightmode_land{*this}; - Copter::FlightMode_LOITER flightmode_loiter{*this}; + Copter::FlightMode_Loiter flightmode_loiter{*this}; #if POSHOLD_ENABLED == ENABLED - Copter::FlightMode_POSHOLD flightmode_poshold{*this}; + Copter::FlightMode_PosHold flightmode_poshold{*this}; #endif Copter::FlightMode_RTL flightmode_rtl{*this}; #if FRAME_CONFIG == HELI_FRAME - Copter::FlightMode_STABILIZE_Heli flightmode_stabilize{*this}; + Copter::FlightMode_Stabilize_Heli flightmode_stabilize{*this}; #else - Copter::FlightMode_STABILIZE flightmode_stabilize{*this}; + Copter::FlightMode_Stabilize flightmode_stabilize{*this}; #endif - Copter::FlightMode_SPORT flightmode_sport{*this}; + Copter::FlightMode_Sport flightmode_sport{*this}; - Copter::FlightMode_AVOID_ADSB flightmode_avoid_adsb{*this}; + Copter::FlightMode_Avoid_ADSB flightmode_avoid_adsb{*this}; - Copter::FlightMode_THROW flightmode_throw{*this}; + Copter::FlightMode_Throw flightmode_throw{*this}; - Copter::FlightMode_GUIDED_NOGPS flightmode_guided_nogps{*this}; + Copter::FlightMode_Guided_NoGPS flightmode_guided_nogps{*this}; - Copter::FlightMode_SMARTRTL flightmode_smartrtl{*this}; + Copter::FlightMode_SmartRTL flightmode_smartrtl{*this}; Copter::FlightMode *flightmode_for_mode(const uint8_t mode); diff --git a/ArduCopter/FlightMode.h b/ArduCopter/FlightMode.h index f2b35dba15..094e87ab49 100644 --- a/ArduCopter/FlightMode.h +++ b/ArduCopter/FlightMode.h @@ -166,11 +166,11 @@ protected: }; -class FlightMode_ACRO : public FlightMode { +class FlightMode_Acro : public FlightMode { public: - FlightMode_ACRO(Copter &copter) : + FlightMode_Acro(Copter &copter) : Copter::FlightMode(copter) { } virtual bool init(bool ignore_checks) override; @@ -193,12 +193,12 @@ private: }; #if FRAME_CONFIG == HELI_FRAME -class FlightMode_ACRO_Heli : public FlightMode_ACRO { +class FlightMode_Acro_Heli : public FlightMode_Acro { public: - FlightMode_ACRO_Heli(Copter &copter) : - Copter::FlightMode_ACRO(copter) + FlightMode_Acro_Heli(Copter &copter) : + Copter::FlightMode_Acro(copter) { } bool init(bool ignore_checks) override; @@ -211,11 +211,11 @@ private: -class FlightMode_ALTHOLD : public FlightMode { +class FlightMode_AltHold : public FlightMode { public: - FlightMode_ALTHOLD(Copter &copter) : + FlightMode_AltHold(Copter &copter) : Copter::FlightMode(copter) { } @@ -238,11 +238,11 @@ private: -class FlightMode_STABILIZE : public FlightMode { +class FlightMode_Stabilize : public FlightMode { public: - FlightMode_STABILIZE(Copter &copter) : + FlightMode_Stabilize(Copter &copter) : Copter::FlightMode(copter) { } @@ -264,16 +264,16 @@ private: }; #if FRAME_CONFIG == HELI_FRAME -class FlightMode_STABILIZE_Heli : public FlightMode_STABILIZE { +class FlightMode_Stabilize_Heli : public FlightMode_Stabilize { public: - FlightMode_STABILIZE_Heli(Copter &copter) : - Copter::FlightMode_STABILIZE(copter) + FlightMode_Stabilize_Heli(Copter &copter) : + Copter::FlightMode_Stabilize(copter) { } bool init(bool ignore_checks) override; - void run() override; // should be called at 100hz or more + void run() override; protected: @@ -284,11 +284,11 @@ private: -class FlightMode_AUTO : public FlightMode { +class FlightMode_Auto : public FlightMode { public: - FlightMode_AUTO(Copter &copter, AP_Mission &_mission, AC_Circle *& _circle_nav) : + FlightMode_Auto(Copter &copter, AP_Mission &_mission, AC_Circle *& _circle_nav) : Copter::FlightMode(copter), mission(_mission), circle_nav(_circle_nav) @@ -363,11 +363,11 @@ private: -class FlightMode_CIRCLE : public FlightMode { +class FlightMode_Circle : public FlightMode { public: - FlightMode_CIRCLE(Copter &copter, AC_Circle *& _circle_nav) : + FlightMode_Circle(Copter &copter, AC_Circle *& _circle_nav) : Copter::FlightMode(copter), circle_nav(_circle_nav) { } @@ -402,11 +402,11 @@ private: -class FlightMode_LOITER : public FlightMode { +class FlightMode_Loiter : public FlightMode { public: - FlightMode_LOITER(Copter &copter) : + FlightMode_Loiter(Copter &copter) : Copter::FlightMode(copter) { } @@ -449,11 +449,11 @@ private: -class FlightMode_GUIDED : public FlightMode { +class FlightMode_Guided : public FlightMode { public: - FlightMode_GUIDED(Copter &copter) : + FlightMode_Guided(Copter &copter) : Copter::FlightMode(copter) { } bool init(bool ignore_checks) override; @@ -514,11 +514,11 @@ private: -class FlightMode_LAND : public FlightMode { +class FlightMode_Land : public FlightMode { public: - FlightMode_LAND(Copter &copter) : + FlightMode_Land(Copter &copter) : Copter::FlightMode(copter) { } @@ -626,11 +626,11 @@ private: -class FlightMode_DRIFT : public FlightMode { +class FlightMode_Drift : public FlightMode { public: - FlightMode_DRIFT(Copter &copter) : + FlightMode_Drift(Copter &copter) : Copter::FlightMode(copter) { } @@ -655,11 +655,11 @@ private: -class FlightMode_SPORT : public FlightMode { +class FlightMode_Sport : public FlightMode { public: - FlightMode_SPORT(Copter &copter) : + FlightMode_Sport(Copter &copter) : Copter::FlightMode(copter) { } @@ -682,11 +682,11 @@ private: -class FlightMode_FLIP : public FlightMode { +class FlightMode_Flip : public FlightMode { public: - FlightMode_FLIP(Copter &copter) : + FlightMode_Flip(Copter &copter) : Copter::FlightMode(copter) { } @@ -713,11 +713,11 @@ private: #if AUTOTUNE_ENABLED == ENABLED -class FlightMode_AUTOTUNE : public FlightMode { +class FlightMode_AutoTune : public FlightMode { public: - FlightMode_AUTOTUNE(Copter &copter) : + FlightMode_AutoTune(Copter &copter) : Copter::FlightMode(copter) { } @@ -867,11 +867,11 @@ private: #if POSHOLD_ENABLED == ENABLED -class FlightMode_POSHOLD : public FlightMode { +class FlightMode_PosHold : public FlightMode { public: - FlightMode_POSHOLD(Copter &copter) : + FlightMode_PosHold(Copter &copter) : Copter::FlightMode(copter) { } @@ -903,11 +903,11 @@ private: -class FlightMode_BRAKE : public FlightMode { +class FlightMode_Brake : public FlightMode { public: - FlightMode_BRAKE(Copter &copter) : + FlightMode_Brake(Copter &copter) : Copter::FlightMode(copter) { } @@ -935,12 +935,12 @@ private: -class FlightMode_AVOID_ADSB : public FlightMode_GUIDED { +class FlightMode_Avoid_ADSB : public FlightMode_Guided { public: - FlightMode_AVOID_ADSB(Copter &copter) : - Copter::FlightMode_GUIDED(copter) { } + FlightMode_Avoid_ADSB(Copter &copter) : + Copter::FlightMode_Guided(copter) { } bool init(bool ignore_checks) override; void run() override; @@ -963,11 +963,11 @@ private: -class FlightMode_THROW : public FlightMode { +class FlightMode_Throw : public FlightMode { public: - FlightMode_THROW(Copter &copter) : + FlightMode_Throw(Copter &copter) : Copter::FlightMode(copter) { } @@ -1003,12 +1003,12 @@ private: -class FlightMode_GUIDED_NOGPS : public FlightMode_GUIDED { +class FlightMode_Guided_NoGPS : public FlightMode_Guided { public: - FlightMode_GUIDED_NOGPS(Copter &copter) : - Copter::FlightMode_GUIDED(copter) { } + FlightMode_Guided_NoGPS(Copter &copter) : + Copter::FlightMode_Guided(copter) { } bool init(bool ignore_checks) override; void run() override; @@ -1033,12 +1033,12 @@ private: }; -class FlightMode_SMARTRTL : public FlightMode_RTL { +class FlightMode_SmartRTL : public FlightMode_RTL { public: - FlightMode_SMARTRTL(Copter &copter) : - FlightMode_SMARTRTL::FlightMode_RTL(copter) + FlightMode_SmartRTL(Copter &copter) : + FlightMode_SmartRTL::FlightMode_RTL(copter) { } bool init(bool ignore_checks) override; diff --git a/ArduCopter/FlightMode_AVOID_ADSB.h b/ArduCopter/FlightMode_AVOID_ADSB.h index c42e6bb9c5..826242b573 100644 --- a/ArduCopter/FlightMode_AVOID_ADSB.h +++ b/ArduCopter/FlightMode_AVOID_ADSB.h @@ -1,9 +1,9 @@ -class FlightMode_AVOID_ADSB : protected FlightMode_GUIDED { +class FlightMode_Avoid_ADSB : protected FlightMode_Guided { public: - FlightMode_AVOID_ADSB(Copter &copter) : - Copter::FlightMode_GUIDED(copter) { } + FlightMode_Avoid_ADSB(Copter &copter) : + Copter::FlightMode_Guided(copter) { } bool init(bool ignore_checks) override; void run() override; diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index abcb9fda84..8ed76210c8 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -21,7 +21,7 @@ struct PACKED log_AutoTune { }; // Write an Autotune data packet -void Copter::FlightMode_AUTOTUNE::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt) +void Copter::FlightMode_AutoTune::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt) { struct log_AutoTune pkt = { LOG_PACKET_HEADER_INIT(LOG_AUTOTUNE_MSG), @@ -47,7 +47,7 @@ struct PACKED log_AutoTuneDetails { }; // Write an Autotune data packet -void Copter::FlightMode_AUTOTUNE::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds) +void Copter::FlightMode_AutoTune::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds) { struct log_AutoTuneDetails pkt = { LOG_PACKET_HEADER_INIT(LOG_AUTOTUNEDETAILS_MSG), diff --git a/ArduCopter/control_acro.cpp b/ArduCopter/control_acro.cpp index 67f26e38d3..f39758fbe5 100644 --- a/ArduCopter/control_acro.cpp +++ b/ArduCopter/control_acro.cpp @@ -6,7 +6,7 @@ * Init and run calls for acro flight mode */ -bool Copter::FlightMode_ACRO::init(bool ignore_checks) +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.flightmode->has_manual_throttle() && @@ -19,7 +19,7 @@ bool Copter::FlightMode_ACRO::init(bool ignore_checks) return true; } -void Copter::FlightMode_ACRO::run() +void Copter::FlightMode_Acro::run() { float target_roll, target_pitch, target_yaw; float pilot_throttle_scaled; @@ -52,7 +52,7 @@ void Copter::FlightMode_ACRO::run() // get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates // returns desired angle rates in centi-degrees-per-second -void Copter::FlightMode_ACRO::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out) +void Copter::FlightMode_Acro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out) { float rate_limit; Vector3f rate_ef_level, rate_bf_level, rate_bf_request; diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp index f6025a5558..d1a723b922 100644 --- a/ArduCopter/control_althold.cpp +++ b/ArduCopter/control_althold.cpp @@ -6,7 +6,7 @@ */ // althold_init - initialise althold controller -bool Copter::FlightMode_ALTHOLD::init(bool ignore_checks) +bool Copter::FlightMode_AltHold::init(bool ignore_checks) { // initialize vertical speeds and leash lengths pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); @@ -26,7 +26,7 @@ bool Copter::FlightMode_ALTHOLD::init(bool ignore_checks) // althold_run - runs the althold controller // should be called at 100hz or more -void Copter::FlightMode_ALTHOLD::run() +void Copter::FlightMode_AltHold::run() { AltHoldModeState althold_state; float takeoff_climb_rate = 0.0f; diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index 64003d8455..c55bd86834 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -18,7 +18,7 @@ */ // auto_init - initialise auto controller -bool Copter::FlightMode_AUTO::init(bool ignore_checks) +bool Copter::FlightMode_Auto::init(bool ignore_checks) { if ((_copter.position_ok() && mission.num_commands() > 1) || ignore_checks) { _mode = Auto_Loiter; @@ -52,7 +52,7 @@ bool Copter::FlightMode_AUTO::init(bool ignore_checks) // auto_run - runs the auto controller // should be called at 100hz or more // relies on run_autopilot being called at 10hz which handles decision making and non-navigation related commands -void Copter::FlightMode_AUTO::run() +void Copter::FlightMode_Auto::run() { // call the correct auto controller switch (_mode) { @@ -99,7 +99,7 @@ void Copter::FlightMode_AUTO::run() } // auto_takeoff_start - initialises waypoint controller to implement take-off -void Copter::FlightMode_AUTO::takeoff_start(const Location& dest_loc) +void Copter::FlightMode_Auto::takeoff_start(const Location& dest_loc) { _mode = Auto_TakeOff; @@ -147,7 +147,7 @@ void Copter::FlightMode_AUTO::takeoff_start(const Location& dest_loc) // auto_takeoff_run - takeoff in auto mode // called by auto_run at 100hz or more -void Copter::FlightMode_AUTO::takeoff_run() +void Copter::FlightMode_Auto::takeoff_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { @@ -200,7 +200,7 @@ void Copter::FlightMode_AUTO::takeoff_run() } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination -void Copter::FlightMode_AUTO::wp_start(const Vector3f& destination) +void Copter::FlightMode_Auto::wp_start(const Vector3f& destination) { _mode = Auto_WP; @@ -215,7 +215,7 @@ void Copter::FlightMode_AUTO::wp_start(const Vector3f& destination) } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination -void Copter::FlightMode_AUTO::wp_start(const Location_Class& dest_loc) +void Copter::FlightMode_Auto::wp_start(const Location_Class& dest_loc) { _mode = Auto_WP; @@ -235,7 +235,7 @@ void Copter::FlightMode_AUTO::wp_start(const Location_Class& dest_loc) // auto_wp_run - runs the auto waypoint controller // called by auto_run at 100hz or more -void Copter::FlightMode_AUTO::wp_run() +void Copter::FlightMode_Auto::wp_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { @@ -285,7 +285,7 @@ void Copter::FlightMode_AUTO::wp_run() // auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller // seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE. If Straight or Spline the next_destination should be provided -void Copter::FlightMode_AUTO::spline_start(const Location_Class& destination, bool stopped_at_start, +void Copter::FlightMode_Auto::spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination) { @@ -307,7 +307,7 @@ void Copter::FlightMode_AUTO::spline_start(const Location_Class& destination, bo // auto_spline_run - runs the auto spline controller // called by auto_run at 100hz or more -void Copter::FlightMode_AUTO::spline_run() +void Copter::FlightMode_Auto::spline_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { @@ -356,7 +356,7 @@ void Copter::FlightMode_AUTO::spline_run() } // auto_land_start - initialises controller to implement a landing -void Copter::FlightMode_AUTO::land_start() +void Copter::FlightMode_Auto::land_start() { // set target to stopping point Vector3f stopping_point; @@ -367,7 +367,7 @@ void Copter::FlightMode_AUTO::land_start() } // auto_land_start - initialises controller to implement a landing -void Copter::FlightMode_AUTO::land_start(const Vector3f& destination) +void Copter::FlightMode_Auto::land_start(const Vector3f& destination) { _mode = Auto_Land; @@ -386,7 +386,7 @@ void Copter::FlightMode_AUTO::land_start(const Vector3f& destination) // auto_land_run - lands in auto mode // called by auto_run at 100hz or more -void Copter::FlightMode_AUTO::land_run() +void Copter::FlightMode_Auto::land_run() { // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { @@ -411,7 +411,7 @@ void Copter::FlightMode_AUTO::land_run() _copter.land_run_vertical_control(); } -bool Copter::FlightMode_AUTO::landing_gear_should_be_deployed() +bool Copter::FlightMode_Auto::landing_gear_should_be_deployed() { switch(_mode) { case Auto_Land: @@ -433,7 +433,7 @@ bool Copter::FlightMode_AUTO::landing_gear_should_be_deployed() } // auto_rtl_start - initialises RTL in AUTO flight mode -void Copter::FlightMode_AUTO::rtl_start() +void Copter::FlightMode_Auto::rtl_start() { _mode = Auto_RTL; @@ -443,7 +443,7 @@ void Copter::FlightMode_AUTO::rtl_start() // auto_rtl_run - rtl in AUTO flight mode // called by auto_run at 100hz or more -void Copter::FlightMode_AUTO::rtl_run() +void Copter::FlightMode_Auto::rtl_run() { // call regular rtl flight mode run function _copter.flightmode_rtl.run(false); @@ -451,7 +451,7 @@ void Copter::FlightMode_AUTO::rtl_run() // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location // we assume the caller has performed all required GPS_ok checks -void Copter::FlightMode_AUTO::circle_movetoedge_start(const Location_Class &circle_center, float radius_m) +void Copter::FlightMode_Auto::circle_movetoedge_start(const Location_Class &circle_center, float radius_m) { // convert location to vector from ekf origin Vector3f circle_center_neu; @@ -505,7 +505,7 @@ void Copter::FlightMode_AUTO::circle_movetoedge_start(const Location_Class &circ // auto_circle_start - initialises controller to fly a circle in AUTO flight mode // assumes that circle_nav object has already been initialised with circle center and radius -void Copter::FlightMode_AUTO::circle_start() +void Copter::FlightMode_Auto::circle_start() { _mode = Auto_Circle; @@ -515,7 +515,7 @@ void Copter::FlightMode_AUTO::circle_start() // auto_circle_run - circle in AUTO flight mode // called by auto_run at 100hz or more -void Copter::FlightMode_AUTO::circle_run() +void Copter::FlightMode_Auto::circle_run() { // call circle controller circle_nav->update(); @@ -529,7 +529,7 @@ void Copter::FlightMode_AUTO::circle_run() #if NAV_GUIDED == ENABLED // auto_nav_guided_start - hand over control to external navigation controller in AUTO mode -void Copter::FlightMode_AUTO::nav_guided_start() +void Copter::FlightMode_Auto::nav_guided_start() { _mode = Auto_NavGuided; @@ -542,7 +542,7 @@ void Copter::FlightMode_AUTO::nav_guided_start() // auto_nav_guided_run - allows control by external navigation controller // called by auto_run at 100hz or more -void Copter::FlightMode_AUTO::nav_guided_run() +void Copter::FlightMode_Auto::nav_guided_run() { // call regular guided flight mode run function _copter.flightmode_guided.run(); @@ -551,7 +551,7 @@ void Copter::FlightMode_AUTO::nav_guided_run() // auto_loiter_start - initialises loitering in auto mode // returns success/failure because this can be called by exit_mission -bool Copter::FlightMode_AUTO::loiter_start() +bool Copter::FlightMode_Auto::loiter_start() { // return failure if GPS is bad if (!_copter.position_ok()) { @@ -574,7 +574,7 @@ bool Copter::FlightMode_AUTO::loiter_start() // auto_loiter_run - loiter in AUTO flight mode // called by auto_run at 100hz or more -void Copter::FlightMode_AUTO::loiter_run() +void Copter::FlightMode_Auto::loiter_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { @@ -792,7 +792,7 @@ float Copter::get_auto_yaw_rate_cds(void) } // auto_payload_place_start - initialises controller to implement a placing -void Copter::FlightMode_AUTO::payload_place_start() +void Copter::FlightMode_Auto::payload_place_start() { // set target to stopping point Vector3f stopping_point; @@ -804,7 +804,7 @@ void Copter::FlightMode_AUTO::payload_place_start() } // auto_payload_place_start - initialises controller to implement placement of a load -void Copter::FlightMode_AUTO::payload_place_start(const Vector3f& destination) +void Copter::FlightMode_Auto::payload_place_start(const Vector3f& destination) { _mode = Auto_NavPayloadPlace; _copter.nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; @@ -822,7 +822,7 @@ void Copter::FlightMode_AUTO::payload_place_start(const Vector3f& destination) set_auto_yaw_mode(AUTO_YAW_HOLD); } -bool Copter::FlightMode_AUTO::payload_place_run_should_run() +bool Copter::FlightMode_Auto::payload_place_run_should_run() { // muts be armed if (!motors->armed()) { @@ -846,7 +846,7 @@ bool Copter::FlightMode_AUTO::payload_place_run_should_run() // auto_payload_place_run - places an object in auto mode // called by auto_run at 100hz or more -void Copter::FlightMode_AUTO::payload_place_run() +void Copter::FlightMode_Auto::payload_place_run() { if (!payload_place_run_should_run()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw @@ -884,7 +884,7 @@ void Copter::FlightMode_AUTO::payload_place_run() } } -void Copter::FlightMode_AUTO::payload_place_run_loiter() +void Copter::FlightMode_Auto::payload_place_run_loiter() { // loiter... _copter.land_run_horizontal_control(); @@ -900,7 +900,7 @@ void Copter::FlightMode_AUTO::payload_place_run_loiter() pos_control->update_z_controller(); } -void Copter::FlightMode_AUTO::payload_place_run_descend() +void Copter::FlightMode_Auto::payload_place_run_descend() { _copter.land_run_horizontal_control(); _copter.land_run_vertical_control(); diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index 939a59135b..fab21dc9e4 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -93,7 +93,7 @@ #define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000 // autotune_init - should be called when autotune mode is selected -bool Copter::FlightMode_AUTOTUNE::init(bool ignore_checks) +bool Copter::FlightMode_AutoTune::init(bool ignore_checks) { bool success = true; @@ -145,7 +145,7 @@ bool Copter::FlightMode_AUTOTUNE::init(bool ignore_checks) } // stop - should be called when the ch7/ch8 switch is switched OFF -void Copter::FlightMode_AUTOTUNE::stop() +void Copter::FlightMode_AutoTune::stop() { // set gains to their original values load_orig_gains(); @@ -162,7 +162,7 @@ void Copter::FlightMode_AUTOTUNE::stop() } // start - Initialize autotune flight mode -bool Copter::FlightMode_AUTOTUNE::start(bool ignore_checks) +bool Copter::FlightMode_AutoTune::start(bool ignore_checks) { // only allow flip from Stabilize, AltHold, PosHold or Loiter modes if (_copter.control_mode != STABILIZE && _copter.control_mode != ALT_HOLD && @@ -193,7 +193,7 @@ bool Copter::FlightMode_AUTOTUNE::start(bool ignore_checks) return true; } -const char *Copter::FlightMode_AUTOTUNE::level_issue_string() const +const char *Copter::FlightMode_AutoTune::level_issue_string() const { switch (level_problem.issue) { case LEVEL_ISSUE_NONE: @@ -214,7 +214,7 @@ const char *Copter::FlightMode_AUTOTUNE::level_issue_string() const return "Bug"; } -void Copter::FlightMode_AUTOTUNE::send_step_string() +void Copter::FlightMode_AutoTune::send_step_string() { if (pilot_override) { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Paused: Pilot Override Active"); @@ -234,7 +234,7 @@ void Copter::FlightMode_AUTOTUNE::send_step_string() gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: unknown step"); } -const char *Copter::FlightMode_AUTOTUNE::type_string() const +const char *Copter::FlightMode_AutoTune::type_string() const { switch (tune_type) { case RD_UP: @@ -251,7 +251,7 @@ const char *Copter::FlightMode_AUTOTUNE::type_string() const return "Bug"; } -void Copter::FlightMode_AUTOTUNE::do_gcs_announcements() +void Copter::FlightMode_AutoTune::do_gcs_announcements() { const uint32_t now = millis(); if (now - announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) { @@ -312,7 +312,7 @@ void Copter::FlightMode_AUTOTUNE::do_gcs_announcements() // run - runs the autotune flight mode // should be called at 100hz or more -void Copter::FlightMode_AUTOTUNE::run() +void Copter::FlightMode_AutoTune::run() { float target_roll, target_pitch; float target_yaw_rate; @@ -419,7 +419,7 @@ void Copter::FlightMode_AUTOTUNE::run() } } -bool Copter::FlightMode_AUTOTUNE::check_level(const LEVEL_ISSUE issue, const float current, const float maximum) +bool Copter::FlightMode_AutoTune::check_level(const LEVEL_ISSUE issue, const float current, const float maximum) { if (current > maximum) { level_problem.current = current; @@ -430,7 +430,7 @@ bool Copter::FlightMode_AUTOTUNE::check_level(const LEVEL_ISSUE issue, const flo return true; } -bool Copter::FlightMode_AUTOTUNE::currently_level() +bool Copter::FlightMode_AutoTune::currently_level() { if (!check_level(LEVEL_ISSUE_ANGLE_ROLL, labs(ahrs.roll_sensor - roll_cd), @@ -467,7 +467,7 @@ bool Copter::FlightMode_AUTOTUNE::currently_level() } // attitude_controller - sets attitude control targets during tuning -void Copter::FlightMode_AUTOTUNE::autotune_attitude_control() +void Copter::FlightMode_AutoTune::autotune_attitude_control() { rotation_rate = 0.0f; // rotation rate in radians/second lean_angle = 0.0f; @@ -871,7 +871,7 @@ void Copter::FlightMode_AUTOTUNE::autotune_attitude_control() // backup_gains_and_initialise - store current gains as originals // called before tuning starts to backup original gains -void Copter::FlightMode_AUTOTUNE::backup_gains_and_initialise() +void Copter::FlightMode_AutoTune::backup_gains_and_initialise() { // initialise state because this is our first time if (roll_enabled()) { @@ -929,7 +929,7 @@ void Copter::FlightMode_AUTOTUNE::backup_gains_and_initialise() // load_orig_gains - set gains to their original values // called by stop and failed functions -void Copter::FlightMode_AUTOTUNE::load_orig_gains() +void Copter::FlightMode_AutoTune::load_orig_gains() { attitude_control->bf_feedforward(orig_bf_feedforward); if (roll_enabled()) { @@ -963,7 +963,7 @@ void Copter::FlightMode_AUTOTUNE::load_orig_gains() } // load_tuned_gains - load tuned gains -void Copter::FlightMode_AUTOTUNE::load_tuned_gains() +void Copter::FlightMode_AutoTune::load_tuned_gains() { if (!attitude_control->get_bf_feedforward()) { attitude_control->bf_feedforward(true); @@ -1002,7 +1002,7 @@ void Copter::FlightMode_AUTOTUNE::load_tuned_gains() // load_intra_test_gains - gains used between tests // called during testing mode's update-gains step to set gains ahead of return-to-level step -void Copter::FlightMode_AUTOTUNE::load_intra_test_gains() +void Copter::FlightMode_AutoTune::load_intra_test_gains() { // we are restarting tuning so reset gains to tuning-start gains (i.e. low I term) // sanity check the gains @@ -1030,7 +1030,7 @@ void Copter::FlightMode_AUTOTUNE::load_intra_test_gains() // load_twitch_gains - load the to-be-tested gains for a single axis // called by attitude_control() just before it beings testing a gain (i.e. just before it twitches) -void Copter::FlightMode_AUTOTUNE::load_twitch_gains() +void Copter::FlightMode_AutoTune::load_twitch_gains() { switch (axis) { case ROLL: @@ -1057,7 +1057,7 @@ void Copter::FlightMode_AUTOTUNE::load_twitch_gains() // save_tuning_gains - save the final tuned gains for each axis // save discovered gains to eeprom if autotuner is enabled (i.e. switch is in the high position) -void Copter::FlightMode_AUTOTUNE::save_tuning_gains() +void Copter::FlightMode_AutoTune::save_tuning_gains() { // if we successfully completed tuning if (mode == SUCCESS) { @@ -1145,7 +1145,7 @@ void Copter::FlightMode_AUTOTUNE::save_tuning_gains() } // update_gcs - send message to ground station -void Copter::FlightMode_AUTOTUNE::update_gcs(uint8_t message_id) +void Copter::FlightMode_AutoTune::update_gcs(uint8_t message_id) { switch (message_id) { case AUTOTUNE_MESSAGE_STARTED: @@ -1167,21 +1167,21 @@ void Copter::FlightMode_AUTOTUNE::update_gcs(uint8_t message_id) } // axis helper functions -inline bool Copter::FlightMode_AUTOTUNE::roll_enabled() { +inline bool Copter::FlightMode_AutoTune::roll_enabled() { return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_ROLL; } -inline bool Copter::FlightMode_AUTOTUNE::pitch_enabled() { +inline bool Copter::FlightMode_AutoTune::pitch_enabled() { return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_PITCH; } -inline bool Copter::FlightMode_AUTOTUNE::yaw_enabled() { +inline bool Copter::FlightMode_AutoTune::yaw_enabled() { return g.autotune_axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW; } // twitching_test - twitching tests // update min and max and test for end conditions -void Copter::FlightMode_AUTOTUNE::twitching_test(float measurement, float target, float &measurement_min, float &measurement_max) +void Copter::FlightMode_AutoTune::twitching_test(float measurement, float target, float &measurement_min, float &measurement_max) { // capture maximum measurement if (measurement > measurement_max) { @@ -1221,7 +1221,7 @@ void Copter::FlightMode_AUTOTUNE::twitching_test(float measurement, float target // updating_d_up - increase D and adjust P to optimize the D term for a little bounce back // optimize D term while keeping the maximum just below the target by adjusting P -void Copter::FlightMode_AUTOTUNE::updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) +void Copter::FlightMode_AutoTune::updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) { if (measurement_max > target) { // if maximum measurement was higher than target @@ -1276,7 +1276,7 @@ void Copter::FlightMode_AUTOTUNE::updating_d_up(float &tune_d, float tune_d_min, // updating_d_down - decrease D and adjust P to optimize the D term for no bounce back // optimize D term while keeping the maximum just below the target by adjusting P -void Copter::FlightMode_AUTOTUNE::updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) +void Copter::FlightMode_AutoTune::updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) { if (measurement_max > target) { // if maximum measurement was higher than target @@ -1331,7 +1331,7 @@ void Copter::FlightMode_AUTOTUNE::updating_d_down(float &tune_d, float tune_d_mi // updating_p_down - decrease P until we don't reach the target before time out // P is decreased to ensure we are not overshooting the target -void Copter::FlightMode_AUTOTUNE::updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max) +void Copter::FlightMode_AutoTune::updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max) { if (measurement_max < target*(1+0.5f*g.autotune_aggressiveness)) { if (ignore_next == false) { @@ -1360,7 +1360,7 @@ void Copter::FlightMode_AUTOTUNE::updating_p_down(float &tune_p, float tune_p_mi // updating_p_up - increase P to ensure the target is reached // P is increased until we achieve our target within a reasonable time -void Copter::FlightMode_AUTOTUNE::updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max) +void Copter::FlightMode_AutoTune::updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max) { if (measurement_max > target*(1+0.5f*g.autotune_aggressiveness)) { // ignore the next result unless it is the same as this one @@ -1389,7 +1389,7 @@ void Copter::FlightMode_AUTOTUNE::updating_p_up(float &tune_p, float tune_p_max, // updating_p_up - increase P to ensure the target is reached while checking bounce back isn't increasing // P is increased until we achieve our target within a reasonable time while reducing D if bounce back increases above the threshold -void Copter::FlightMode_AUTOTUNE::updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) +void Copter::FlightMode_AutoTune::updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max) { if (measurement_max > target*(1+0.5f*g.autotune_aggressiveness)) { // ignore the next result unless it is the same as this one @@ -1438,7 +1438,7 @@ void Copter::FlightMode_AUTOTUNE::updating_p_up_d_down(float &tune_d, float tune } // twitching_measure_acceleration - measure rate of change of measurement -void Copter::FlightMode_AUTOTUNE::twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) +void Copter::FlightMode_AutoTune::twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) { if (rate_measurement_max < rate_measurement) { rate_measurement_max = rate_measurement; @@ -1447,7 +1447,7 @@ void Copter::FlightMode_AUTOTUNE::twitching_measure_acceleration(float &rate_of_ } // get attitude for slow position hold in autotune mode -void Copter::FlightMode_AUTOTUNE::get_poshold_attitude(float &roll_cd_out, float &pitch_cd_out, float &yaw_cd_out) +void Copter::FlightMode_AutoTune::get_poshold_attitude(float &roll_cd_out, float &pitch_cd_out, float &yaw_cd_out) { roll_cd_out = pitch_cd_out = 0; diff --git a/ArduCopter/control_avoid_adsb.cpp b/ArduCopter/control_avoid_adsb.cpp index 8450507205..57ea69454b 100644 --- a/ArduCopter/control_avoid_adsb.cpp +++ b/ArduCopter/control_avoid_adsb.cpp @@ -10,13 +10,13 @@ */ // initialise avoid_adsb controller -bool Copter::FlightMode_AVOID_ADSB::init(const bool ignore_checks) +bool Copter::FlightMode_Avoid_ADSB::init(const bool ignore_checks) { // re-use guided mode - return Copter::FlightMode_GUIDED::init(ignore_checks); + return Copter::FlightMode_Guided::init(ignore_checks); } -bool Copter::FlightMode_AVOID_ADSB::set_velocity(const Vector3f& velocity_neu) +bool Copter::FlightMode_Avoid_ADSB::set_velocity(const Vector3f& velocity_neu) { // check flight mode if (_copter.control_mode != AVOID_ADSB) { @@ -24,15 +24,15 @@ bool Copter::FlightMode_AVOID_ADSB::set_velocity(const Vector3f& velocity_neu) } // re-use guided mode's velocity controller - Copter::FlightMode_GUIDED::set_velocity(velocity_neu); + Copter::FlightMode_Guided::set_velocity(velocity_neu); return true; } // runs the AVOID_ADSB controller -void Copter::FlightMode_AVOID_ADSB::run() +void Copter::FlightMode_Avoid_ADSB::run() { // re-use guided mode's velocity controller // Note: this is safe from interference from GCSs and companion computer's whose guided mode // position and velocity requests will be ignored while the vehicle is not in guided mode - Copter::FlightMode_GUIDED::run(); + Copter::FlightMode_Guided::run(); } diff --git a/ArduCopter/control_brake.cpp b/ArduCopter/control_brake.cpp index acb219423a..9ea1974672 100644 --- a/ArduCopter/control_brake.cpp +++ b/ArduCopter/control_brake.cpp @@ -5,7 +5,7 @@ */ // brake_init - initialise brake controller -bool Copter::FlightMode_BRAKE::init(bool ignore_checks) +bool Copter::FlightMode_Brake::init(bool ignore_checks) { if (_copter.position_ok() || ignore_checks) { @@ -35,7 +35,7 @@ bool Copter::FlightMode_BRAKE::init(bool ignore_checks) // brake_run - runs the brake controller // should be called at 100hz or more -void Copter::FlightMode_BRAKE::run() +void Copter::FlightMode_Brake::run() { // if not auto armed set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { @@ -85,7 +85,7 @@ void Copter::FlightMode_BRAKE::run() } } -void Copter::FlightMode_BRAKE::timeout_to_loiter_ms(uint32_t timeout_ms) +void Copter::FlightMode_Brake::timeout_to_loiter_ms(uint32_t timeout_ms) { _timeout_start = millis(); _timeout_ms = timeout_ms; diff --git a/ArduCopter/control_circle.cpp b/ArduCopter/control_circle.cpp index a78001686b..74542f2f48 100644 --- a/ArduCopter/control_circle.cpp +++ b/ArduCopter/control_circle.cpp @@ -5,7 +5,7 @@ */ // circle_init - initialise circle controller flight mode -bool Copter::FlightMode_CIRCLE::init(bool ignore_checks) +bool Copter::FlightMode_Circle::init(bool ignore_checks) { if (_copter.position_ok() || ignore_checks) { pilot_yaw_override = false; @@ -28,7 +28,7 @@ bool Copter::FlightMode_CIRCLE::init(bool ignore_checks) // circle_run - runs the circle flight mode // should be called at 100hz or more -void Copter::FlightMode_CIRCLE::run() +void Copter::FlightMode_Circle::run() { float target_yaw_rate = 0; float target_climb_rate = 0; diff --git a/ArduCopter/control_drift.cpp b/ArduCopter/control_drift.cpp index 380d3e9810..9609c3d441 100644 --- a/ArduCopter/control_drift.cpp +++ b/ArduCopter/control_drift.cpp @@ -27,7 +27,7 @@ #endif // drift_init - initialise drift controller -bool Copter::FlightMode_DRIFT::init(bool ignore_checks) +bool Copter::FlightMode_Drift::init(bool ignore_checks) { if (_copter.position_ok() || ignore_checks) { return true; @@ -38,7 +38,7 @@ bool Copter::FlightMode_DRIFT::init(bool ignore_checks) // drift_run - runs the drift controller // should be called at 100hz or more -void Copter::FlightMode_DRIFT::run() +void Copter::FlightMode_Drift::run() { static float breaker = 0.0f; static float roll_input = 0.0f; @@ -108,7 +108,7 @@ void Copter::FlightMode_DRIFT::run() } // get_throttle_assist - return throttle output (range 0 ~ 1) based on pilot input and z-axis velocity -float Copter::FlightMode_DRIFT::get_throttle_assist(float velz, float pilot_throttle_scaled) +float Copter::FlightMode_Drift::get_throttle_assist(float velz, float pilot_throttle_scaled) { // throttle assist - adjusts throttle to slow the vehicle's vertical velocity // Only active when pilot's throttle is between 213 ~ 787 diff --git a/ArduCopter/control_flip.cpp b/ArduCopter/control_flip.cpp index 2fd692b400..b96d3faf33 100644 --- a/ArduCopter/control_flip.cpp +++ b/ArduCopter/control_flip.cpp @@ -38,7 +38,7 @@ int8_t flip_roll_dir; // roll direction (-1 = roll left, 1 = roll int8_t flip_pitch_dir; // pitch direction (-1 = pitch forward, 1 = pitch back) // flip_init - initialise flip controller -bool Copter::FlightMode_FLIP::init(bool ignore_checks) +bool Copter::FlightMode_Flip::init(bool ignore_checks) { // only allow flip from ACRO, Stabilize, AltHold or Drift flight modes if (_copter.control_mode != ACRO && _copter.control_mode != STABILIZE && _copter.control_mode != ALT_HOLD) { @@ -94,7 +94,7 @@ bool Copter::FlightMode_FLIP::init(bool ignore_checks) // flip_run - runs the flip controller // should be called at 100hz or more -void Copter::FlightMode_FLIP::run() +void Copter::FlightMode_Flip::run() { float throttle_out; float recovery_angle; diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 10eea7272e..20b4470df9 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -36,7 +36,7 @@ struct Guided_Limit { } guided_limit; // guided_init - initialise guided controller -bool Copter::FlightMode_GUIDED::init(bool ignore_checks) +bool Copter::FlightMode_Guided::init(bool ignore_checks) { if (_copter.position_ok() || ignore_checks) { // initialise yaw @@ -51,7 +51,7 @@ bool Copter::FlightMode_GUIDED::init(bool ignore_checks) // guided_takeoff_start - initialises waypoint controller to implement take-off -bool Copter::FlightMode_GUIDED::takeoff_start(float final_alt_above_home) +bool Copter::FlightMode_Guided::takeoff_start(float final_alt_above_home) { guided_mode = Guided_TakeOff; @@ -79,7 +79,7 @@ bool Copter::FlightMode_GUIDED::takeoff_start(float final_alt_above_home) } // initialise guided mode's position controller -void Copter::FlightMode_GUIDED::pos_control_start() +void Copter::FlightMode_Guided::pos_control_start() { // set to position control mode guided_mode = Guided_WP; @@ -99,7 +99,7 @@ void Copter::FlightMode_GUIDED::pos_control_start() } // initialise guided mode's velocity controller -void Copter::FlightMode_GUIDED::vel_control_start() +void Copter::FlightMode_Guided::vel_control_start() { // set guided_mode to velocity controller guided_mode = Guided_Velocity; @@ -118,7 +118,7 @@ void Copter::FlightMode_GUIDED::vel_control_start() } // initialise guided mode's posvel controller -void Copter::FlightMode_GUIDED::posvel_control_start() +void Copter::FlightMode_Guided::posvel_control_start() { // set guided_mode to velocity controller guided_mode = Guided_PosVel; @@ -146,7 +146,7 @@ void Copter::FlightMode_GUIDED::posvel_control_start() } // initialise guided mode's angle controller -void Copter::FlightMode_GUIDED::angle_control_start() +void Copter::FlightMode_Guided::angle_control_start() { // set guided_mode to velocity controller guided_mode = Guided_Angle; @@ -177,7 +177,7 @@ void Copter::FlightMode_GUIDED::angle_control_start() // guided_set_destination - sets guided mode's target destination // Returns true if the fence is enabled and guided waypoint is within the fence // else return false if the waypoint is outside the fence -bool Copter::FlightMode_GUIDED::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +bool Copter::FlightMode_Guided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) { // ensure we are in position control mode if (guided_mode != Guided_WP) { @@ -208,7 +208,7 @@ bool Copter::FlightMode_GUIDED::set_destination(const Vector3f& destination, boo // sets guided mode's target from a Location object // returns false if destination could not be set (probably caused by missing terrain data) // or if the fence is enabled and guided waypoint is outside the fence -bool Copter::FlightMode_GUIDED::set_destination(const Location_Class& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +bool Copter::FlightMode_Guided::set_destination(const Location_Class& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) { // ensure we are in position control mode if (guided_mode != Guided_WP) { @@ -241,7 +241,7 @@ bool Copter::FlightMode_GUIDED::set_destination(const Location_Class& dest_loc, } // guided_set_velocity - sets guided mode's target velocity -void Copter::FlightMode_GUIDED::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +void Copter::FlightMode_Guided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) { // check we are in velocity control mode if (guided_mode != Guided_Velocity) { @@ -260,7 +260,7 @@ void Copter::FlightMode_GUIDED::set_velocity(const Vector3f& velocity, bool use_ } // set guided mode posvel target -bool Copter::FlightMode_GUIDED::set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +bool Copter::FlightMode_Guided::set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) { // check we are in velocity control mode if (guided_mode != Guided_PosVel) { @@ -292,7 +292,7 @@ bool Copter::FlightMode_GUIDED::set_destination_posvel(const Vector3f& destinati } // set guided mode angle target -void Copter::FlightMode_GUIDED::set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads) +void Copter::FlightMode_Guided::set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads) { // check we are in velocity control mode if (guided_mode != Guided_Angle) { @@ -323,7 +323,7 @@ void Copter::FlightMode_GUIDED::set_angle(const Quaternion &q, float climb_rate_ // guided_run - runs the guided controller // should be called at 100hz or more -void Copter::FlightMode_GUIDED::run() +void Copter::FlightMode_Guided::run() { // call the correct auto controller switch (guided_mode) { @@ -357,7 +357,7 @@ void Copter::FlightMode_GUIDED::run() // guided_takeoff_run - takeoff in guided mode // called by guided_run at 100hz or more -void Copter::FlightMode_GUIDED::takeoff_run() +void Copter::FlightMode_Guided::takeoff_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { @@ -411,7 +411,7 @@ void Copter::FlightMode_GUIDED::takeoff_run() // guided_pos_control_run - runs the guided position controller // called from guided_run -void Copter::FlightMode_GUIDED::pos_control_run() +void Copter::FlightMode_Guided::pos_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) { @@ -461,7 +461,7 @@ void Copter::FlightMode_GUIDED::pos_control_run() // guided_vel_control_run - runs the guided velocity controller // called from guided_run -void Copter::FlightMode_GUIDED::vel_control_run() +void Copter::FlightMode_Guided::vel_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) { @@ -523,7 +523,7 @@ void Copter::FlightMode_GUIDED::vel_control_run() // guided_posvel_control_run - runs the guided spline controller // called from guided_run -void Copter::FlightMode_GUIDED::posvel_control_run() +void Copter::FlightMode_Guided::posvel_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) { @@ -603,7 +603,7 @@ void Copter::FlightMode_GUIDED::posvel_control_run() // guided_angle_control_run - runs the guided angle controller // called from guided_run -void Copter::FlightMode_GUIDED::angle_control_run() +void Copter::FlightMode_Guided::angle_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) { @@ -667,7 +667,7 @@ void Copter::FlightMode_GUIDED::angle_control_run() } // helper function to update position controller's desired velocity while respecting acceleration limits -void Copter::FlightMode_GUIDED::set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des) +void Copter::FlightMode_Guided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des) { // get current desired velocity Vector3f curr_vel_des = pos_control->get_desired_velocity(); @@ -701,7 +701,7 @@ void Copter::FlightMode_GUIDED::set_desired_velocity_with_accel_and_fence_limits } // helper function to set yaw state and targets -void Copter::FlightMode_GUIDED::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle) +void Copter::FlightMode_Guided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle) { if (use_yaw) { set_auto_yaw_look_at_heading(yaw_cd / 100.0f, 0.0f, 0, relative_angle); @@ -713,7 +713,7 @@ void Copter::FlightMode_GUIDED::set_yaw_state(bool use_yaw, float yaw_cd, bool u // Guided Limit code // guided_limit_clear - clear/turn off guided limits -void Copter::FlightMode_GUIDED::limit_clear() +void Copter::FlightMode_Guided::limit_clear() { guided_limit.timeout_ms = 0; guided_limit.alt_min_cm = 0.0f; @@ -722,7 +722,7 @@ void Copter::FlightMode_GUIDED::limit_clear() } // guided_limit_set - set guided timeout and movement limits -void Copter::FlightMode_GUIDED::limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm) +void Copter::FlightMode_Guided::limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm) { guided_limit.timeout_ms = timeout_ms; guided_limit.alt_min_cm = alt_min_cm; @@ -732,7 +732,7 @@ void Copter::FlightMode_GUIDED::limit_set(uint32_t timeout_ms, float alt_min_cm, // guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking // only called from AUTO mode's auto_nav_guided_start function -void Copter::FlightMode_GUIDED::limit_init_time_and_pos() +void Copter::FlightMode_Guided::limit_init_time_and_pos() { // initialise start time guided_limit.start_time = AP_HAL::millis(); @@ -743,7 +743,7 @@ void Copter::FlightMode_GUIDED::limit_init_time_and_pos() // guided_limit_check - returns true if guided mode has breached a limit // used when guided is invoked from the NAV_GUIDED_ENABLE mission command -bool Copter::FlightMode_GUIDED::limit_check() +bool Copter::FlightMode_Guided::limit_check() { // check if we have passed the timeout if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) { @@ -776,7 +776,7 @@ bool Copter::FlightMode_GUIDED::limit_check() } -uint32_t Copter::FlightMode_GUIDED::wp_distance() const +uint32_t Copter::FlightMode_Guided::wp_distance() const { switch(mode()) { case Guided_WP: @@ -790,7 +790,7 @@ uint32_t Copter::FlightMode_GUIDED::wp_distance() const } } -int32_t Copter::FlightMode_GUIDED::wp_bearing() const +int32_t Copter::FlightMode_Guided::wp_bearing() const { switch(mode()) { case Guided_WP: diff --git a/ArduCopter/control_guided_nogps.cpp b/ArduCopter/control_guided_nogps.cpp index 8c0f368e87..c6d6273333 100644 --- a/ArduCopter/control_guided_nogps.cpp +++ b/ArduCopter/control_guided_nogps.cpp @@ -5,17 +5,17 @@ */ // initialise guided_nogps controller -bool Copter::FlightMode_GUIDED_NOGPS::init(bool ignore_checks) +bool Copter::FlightMode_Guided_NoGPS::init(bool ignore_checks) { // start in angle control mode - Copter::FlightMode_GUIDED::angle_control_start(); + Copter::FlightMode_Guided::angle_control_start(); return true; } // guided_run - runs the guided controller // should be called at 100hz or more -void Copter::FlightMode_GUIDED_NOGPS::run() +void Copter::FlightMode_Guided_NoGPS::run() { // run angle controller - Copter::FlightMode_GUIDED::angle_control_run(); + Copter::FlightMode_Guided::angle_control_run(); } diff --git a/ArduCopter/control_land.cpp b/ArduCopter/control_land.cpp index 18cfb130e5..3d38d6992f 100644 --- a/ArduCopter/control_land.cpp +++ b/ArduCopter/control_land.cpp @@ -7,7 +7,7 @@ static uint32_t land_start_time; static bool land_pause; // land_init - initialise land controller -bool Copter::FlightMode_LAND::init(bool ignore_checks) +bool Copter::FlightMode_Land::init(bool ignore_checks) { // check if we have GPS and decide which LAND we're going to do land_with_gps = _copter.position_ok(); @@ -40,7 +40,7 @@ bool Copter::FlightMode_LAND::init(bool ignore_checks) // land_run - runs the land controller // should be called at 100hz or more -void Copter::FlightMode_LAND::run() +void Copter::FlightMode_Land::run() { if (land_with_gps) { gps_run(); @@ -52,7 +52,7 @@ void Copter::FlightMode_LAND::run() // land_gps_run - runs the land controller // horizontal position controlled with loiter controller // should be called at 100hz or more -void Copter::FlightMode_LAND::gps_run() +void Copter::FlightMode_Land::gps_run() { // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { @@ -89,7 +89,7 @@ void Copter::FlightMode_LAND::gps_run() // land_nogps_run - runs the land controller // pilot controls roll and pitch angles // should be called at 100hz or more -void Copter::FlightMode_LAND::nogps_run() +void Copter::FlightMode_Land::nogps_run() { float target_roll = 0.0f, target_pitch = 0.0f; float target_yaw_rate = 0; @@ -150,7 +150,7 @@ void Copter::FlightMode_LAND::nogps_run() /* get a height above ground estimate for landing */ -int32_t Copter::FlightMode_LAND::get_alt_above_ground(void) +int32_t Copter::FlightMode_Land::get_alt_above_ground(void) { int32_t alt_above_ground; if (_copter.rangefinder_alt_ok()) { @@ -303,7 +303,7 @@ void Copter::land_run_horizontal_control() // do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch // called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS // has no effect if we are not already in LAND mode -void Copter::FlightMode_LAND::do_not_use_GPS() +void Copter::FlightMode_Land::do_not_use_GPS() { land_with_gps = false; } diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp index 4474521d2f..f03f117e47 100644 --- a/ArduCopter/control_loiter.cpp +++ b/ArduCopter/control_loiter.cpp @@ -5,7 +5,7 @@ */ // loiter_init - initialise loiter controller -bool Copter::FlightMode_LOITER::init(bool ignore_checks) +bool Copter::FlightMode_Loiter::init(bool ignore_checks) { if (_copter.position_ok() || ignore_checks) { @@ -29,7 +29,7 @@ bool Copter::FlightMode_LOITER::init(bool ignore_checks) } #if PRECISION_LANDING == ENABLED -bool Copter::FlightMode_LOITER::do_precision_loiter() +bool Copter::FlightMode_Loiter::do_precision_loiter() { if (!_precision_loiter_enabled) { return false; @@ -47,7 +47,7 @@ bool Copter::FlightMode_LOITER::do_precision_loiter() return true; } -void Copter::FlightMode_LOITER::precision_loiter_xy() +void Copter::FlightMode_Loiter::precision_loiter_xy() { wp_nav->clear_pilot_desired_acceleration(); Vector2f target_pos, target_vel_rel; @@ -66,7 +66,7 @@ void Copter::FlightMode_LOITER::precision_loiter_xy() // loiter_run - runs the loiter controller // should be called at 100hz or more -void Copter::FlightMode_LOITER::run() +void Copter::FlightMode_Loiter::run() { LoiterModeState loiter_state; float target_yaw_rate = 0.0f; diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp index 46c4de04b8..386980a444 100644 --- a/ArduCopter/control_poshold.cpp +++ b/ArduCopter/control_poshold.cpp @@ -73,7 +73,7 @@ static struct { } poshold; // poshold_init - initialise PosHold controller -bool Copter::FlightMode_POSHOLD::init(bool ignore_checks) +bool Copter::FlightMode_PosHold::init(bool ignore_checks) { // fail to initialise PosHold mode if no GPS lock if (!_copter.position_ok() && !ignore_checks) { @@ -124,7 +124,7 @@ bool Copter::FlightMode_POSHOLD::init(bool ignore_checks) // poshold_run - runs the PosHold controller // should be called at 100hz or more -void Copter::FlightMode_POSHOLD::run() +void Copter::FlightMode_PosHold::run() { float target_roll, target_pitch; // pilot's roll and pitch angle inputs float target_yaw_rate = 0; // pilot desired yaw rate in centi-degrees/sec @@ -544,7 +544,7 @@ void Copter::FlightMode_POSHOLD::run() } // poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received -void Copter::FlightMode_POSHOLD::poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw) +void Copter::FlightMode_PosHold::poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw) { // if raw input is large or reversing the vehicle's lean angle immediately set the fitlered angle to the new raw angle if ((lean_angle_filtered > 0 && lean_angle_raw < 0) || (lean_angle_filtered < 0 && lean_angle_raw > 0) || (fabsf(lean_angle_raw) > POSHOLD_STICK_RELEASE_SMOOTH_ANGLE)) { @@ -566,7 +566,7 @@ void Copter::FlightMode_POSHOLD::poshold_update_pilot_lean_angle(float &lean_ang // poshold_mix_controls - mixes two controls based on the mix_ratio // mix_ratio of 1 = use first_control completely, 0 = use second_control completely, 0.5 = mix evenly -int16_t Copter::FlightMode_POSHOLD::poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control) +int16_t Copter::FlightMode_PosHold::poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control) { mix_ratio = constrain_float(mix_ratio, 0.0f, 1.0f); return (int16_t)((mix_ratio * first_control) + ((1.0f-mix_ratio)*second_control)); @@ -575,7 +575,7 @@ int16_t Copter::FlightMode_POSHOLD::poshold_mix_controls(float mix_ratio, int16_ // poshold_update_brake_angle_from_velocity - updates the brake_angle based on the vehicle's velocity and brake_gain // brake_angle is slewed with the wpnav.poshold_brake_rate and constrained by the wpnav.poshold_braking_angle_max // velocity is assumed to be in the same direction as lean angle so for pitch you should provide the velocity backwards (i.e. -ve forward velocity) -void Copter::FlightMode_POSHOLD::poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity) +void Copter::FlightMode_PosHold::poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity) { float lean_angle; int16_t brake_rate = g.poshold_brake_rate; @@ -601,7 +601,7 @@ void Copter::FlightMode_POSHOLD::poshold_update_brake_angle_from_velocity(int16_ // poshold_update_wind_comp_estimate - updates wind compensation estimate // should be called at the maximum loop rate when loiter is engaged -void Copter::FlightMode_POSHOLD::poshold_update_wind_comp_estimate() +void Copter::FlightMode_PosHold::poshold_update_wind_comp_estimate() { // check wind estimate start has not been delayed if (poshold.wind_comp_start_timer > 0) { @@ -637,7 +637,7 @@ void Copter::FlightMode_POSHOLD::poshold_update_wind_comp_estimate() // poshold_get_wind_comp_lean_angles - retrieve wind compensation angles in body frame roll and pitch angles // should be called at the maximum loop rate -void Copter::FlightMode_POSHOLD::poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle) +void Copter::FlightMode_PosHold::poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle) { // reduce rate to 10hz poshold.wind_comp_timer++; @@ -652,7 +652,7 @@ void Copter::FlightMode_POSHOLD::poshold_get_wind_comp_lean_angles(int16_t &roll } // poshold_roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis -void Copter::FlightMode_POSHOLD::poshold_roll_controller_to_pilot_override() +void Copter::FlightMode_PosHold::poshold_roll_controller_to_pilot_override() { poshold.roll_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE; poshold.controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; @@ -663,7 +663,7 @@ void Copter::FlightMode_POSHOLD::poshold_roll_controller_to_pilot_override() } // poshold_pitch_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis -void Copter::FlightMode_POSHOLD::poshold_pitch_controller_to_pilot_override() +void Copter::FlightMode_PosHold::poshold_pitch_controller_to_pilot_override() { poshold.pitch_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE; poshold.controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; diff --git a/ArduCopter/control_smart_rtl.cpp b/ArduCopter/control_smart_rtl.cpp index 4764ecdff7..83c6169778 100644 --- a/ArduCopter/control_smart_rtl.cpp +++ b/ArduCopter/control_smart_rtl.cpp @@ -7,7 +7,7 @@ * Once the copter is close to home, it will run a standard land controller. */ -bool Copter::FlightMode_SMARTRTL::init(bool ignore_checks) +bool Copter::FlightMode_SmartRTL::init(bool ignore_checks) { if ((copter.position_ok() || ignore_checks) && g2.smart_rtl.is_active()) { // initialise waypoint and spline controller @@ -31,12 +31,12 @@ bool Copter::FlightMode_SMARTRTL::init(bool ignore_checks) } // perform cleanup required when leaving smart_rtl -void Copter::FlightMode_SMARTRTL::exit() +void Copter::FlightMode_SmartRTL::exit() { g2.smart_rtl.cancel_request_for_thorough_cleanup(); } -void Copter::FlightMode_SMARTRTL::run() +void Copter::FlightMode_SmartRTL::run() { switch (smart_rtl_state) { case SmartRTL_WaitForPathCleanup: @@ -57,7 +57,7 @@ void Copter::FlightMode_SMARTRTL::run() } } -void Copter::FlightMode_SMARTRTL::wait_cleanup_run() +void Copter::FlightMode_SmartRTL::wait_cleanup_run() { // hover at current target position motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); @@ -71,7 +71,7 @@ void Copter::FlightMode_SMARTRTL::wait_cleanup_run() } } -void Copter::FlightMode_SMARTRTL::path_follow_run() +void Copter::FlightMode_SmartRTL::path_follow_run() { // if we are close to current target point, switch the next point to be our target. if (wp_nav->reached_wp_destination()) { @@ -108,7 +108,7 @@ void Copter::FlightMode_SMARTRTL::path_follow_run() } } -void Copter::FlightMode_SMARTRTL::pre_land_position_run() +void Copter::FlightMode_SmartRTL::pre_land_position_run() { // if we are close to 2m above start point, we are ready to land. if (wp_nav->reached_wp_destination()) { @@ -131,7 +131,7 @@ void Copter::FlightMode_SMARTRTL::pre_land_position_run() } // save current position for use by the smart_rtl flight mode -void Copter::FlightMode_SMARTRTL::save_position() +void Copter::FlightMode_SmartRTL::save_position() { const bool should_save_position = motors->armed() && (copter.control_mode != SMART_RTL); diff --git a/ArduCopter/control_sport.cpp b/ArduCopter/control_sport.cpp index 566aacea96..7827abfd91 100644 --- a/ArduCopter/control_sport.cpp +++ b/ArduCopter/control_sport.cpp @@ -5,7 +5,7 @@ */ // sport_init - initialise sport controller -bool Copter::FlightMode_SPORT::init(bool ignore_checks) +bool Copter::FlightMode_Sport::init(bool ignore_checks) { // initialize vertical speed and acceleration pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); @@ -22,7 +22,7 @@ bool Copter::FlightMode_SPORT::init(bool ignore_checks) // sport_run - runs the sport controller // should be called at 100hz or more -void Copter::FlightMode_SPORT::run() +void Copter::FlightMode_Sport::run() { SportModeState sport_state; float takeoff_climb_rate = 0.0f; diff --git a/ArduCopter/control_stabilize.cpp b/ArduCopter/control_stabilize.cpp index e5a90a0afd..1b51979b78 100644 --- a/ArduCopter/control_stabilize.cpp +++ b/ArduCopter/control_stabilize.cpp @@ -5,7 +5,7 @@ */ // stabilize_init - initialise stabilize controller -bool Copter::FlightMode_STABILIZE::init(bool ignore_checks) +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.flightmode->has_manual_throttle() && @@ -20,7 +20,7 @@ bool Copter::FlightMode_STABILIZE::init(bool ignore_checks) // stabilize_run - runs the main stabilize controller // should be called at 100hz or more -void Copter::FlightMode_STABILIZE::run() +void Copter::FlightMode_Stabilize::run() { float target_roll, target_pitch; float target_yaw_rate; diff --git a/ArduCopter/control_throw.cpp b/ArduCopter/control_throw.cpp index c68269d5ac..3f3ec6311f 100644 --- a/ArduCopter/control_throw.cpp +++ b/ArduCopter/control_throw.cpp @@ -2,7 +2,7 @@ // throw_init - initialise throw controller -bool Copter::FlightMode_THROW::init(bool ignore_checks) +bool Copter::FlightMode_Throw::init(bool ignore_checks) { #if FRAME_CONFIG == HELI_FRAME // do not allow helis to use throw to start @@ -23,7 +23,7 @@ bool Copter::FlightMode_THROW::init(bool ignore_checks) // runs the throw to start controller // should be called at 100hz or more -void Copter::FlightMode_THROW::run() +void Copter::FlightMode_Throw::run() { /* Throw State Machine Throw_Disarmed - motors are off @@ -204,7 +204,7 @@ void Copter::FlightMode_THROW::run() } } -bool Copter::FlightMode_THROW::throw_detected() +bool Copter::FlightMode_Throw::throw_detected() { // Check that we have a valid navigation solution nav_filter_status filt_status = inertial_nav.get_filter_status(); @@ -249,20 +249,20 @@ bool Copter::FlightMode_THROW::throw_detected() } } -bool Copter::FlightMode_THROW::throw_attitude_good() +bool Copter::FlightMode_Throw::throw_attitude_good() { // Check that we have uprighted the copter const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned(); return (rotMat.c.z > 0.866f); // is_upright } -bool Copter::FlightMode_THROW::throw_height_good() +bool Copter::FlightMode_Throw::throw_height_good() { // Check that we are no more than 0.5m below the demanded height return (pos_control->get_alt_error() < 50.0f); } -bool Copter::FlightMode_THROW::throw_position_good() +bool Copter::FlightMode_Throw::throw_position_good() { // check that our horizontal position error is within 50cm return (pos_control->get_horizontal_error() < 50.0f); diff --git a/ArduCopter/heli_control_acro.cpp b/ArduCopter/heli_control_acro.cpp index 3f2d3e54a9..2baadde5c1 100644 --- a/ArduCopter/heli_control_acro.cpp +++ b/ArduCopter/heli_control_acro.cpp @@ -6,7 +6,7 @@ */ // heli_acro_init - initialise acro controller -bool Copter::FlightMode_ACRO_Heli::init(bool ignore_checks) +bool Copter::FlightMode_Acro_Heli::init(bool ignore_checks) { // if heli is equipped with a flybar, then tell the attitude controller to pass through controls directly to servos attitude_control->use_flybar_passthrough(motors->has_flybar(), motors->supports_yaw_passthrough()); @@ -22,7 +22,7 @@ bool Copter::FlightMode_ACRO_Heli::init(bool ignore_checks) // heli_acro_run - runs the acro controller // should be called at 100hz or more -void Copter::FlightMode_ACRO_Heli::run() +void Copter::FlightMode_Acro_Heli::run() { float target_roll, target_pitch, target_yaw; float pilot_throttle_scaled; diff --git a/ArduCopter/heli_control_stabilize.cpp b/ArduCopter/heli_control_stabilize.cpp index e7407ed97e..78fa56a86f 100644 --- a/ArduCopter/heli_control_stabilize.cpp +++ b/ArduCopter/heli_control_stabilize.cpp @@ -6,7 +6,7 @@ */ // stabilize_init - initialise stabilize controller -bool Copter::FlightMode_STABILIZE_Heli::init(bool ignore_checks) +bool Copter::FlightMode_Stabilize_Heli::init(bool ignore_checks) { // set target altitude to zero for reporting // To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error? @@ -20,7 +20,7 @@ bool Copter::FlightMode_STABILIZE_Heli::init(bool ignore_checks) // stabilize_run - runs the main stabilize controller // should be called at 100hz or more -void Copter::FlightMode_STABILIZE_Heli::run() +void Copter::FlightMode_Stabilize_Heli::run() { float target_roll, target_pitch; float target_yaw_rate;