From 676d75c391926ddc481b5f4d0a6a4efb95145ac8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 10 May 2019 12:18:49 +1000 Subject: [PATCH] Copter: correct namespacing of Copter modes This makes us look like Rover and Plane in terms of namespacing for the Mode classes, and removes a wart where we #include mode.h in the middle of the Mode class. This was done mechanically for the most part. I've had to remove the convenience reference for ap as part of this. --- ArduCopter/Copter.cpp | 2 +- ArduCopter/Copter.h | 53 ++++++--- ArduCopter/GCS_Mavlink.cpp | 2 +- ArduCopter/Parameters.cpp | 6 +- ArduCopter/Parameters.h | 10 ++ ArduCopter/autoyaw.cpp | 22 ++-- ArduCopter/mode.cpp | 83 +++++++-------- ArduCopter/mode.h | 57 +++++----- ArduCopter/mode_acro.cpp | 6 +- ArduCopter/mode_acro_heli.cpp | 8 +- ArduCopter/mode_althold.cpp | 4 +- ArduCopter/mode_auto.cpp | 166 ++++++++++++++--------------- ArduCopter/mode_autotune.cpp | 28 ++--- ArduCopter/mode_avoid_adsb.cpp | 12 +-- ArduCopter/mode_brake.cpp | 10 +- ArduCopter/mode_circle.cpp | 10 +- ArduCopter/mode_drift.cpp | 8 +- ArduCopter/mode_flip.cpp | 8 +- ArduCopter/mode_flowhold.cpp | 22 ++-- ArduCopter/mode_follow.cpp | 16 +-- ArduCopter/mode_guided.cpp | 66 ++++++------ ArduCopter/mode_guided_nogps.cpp | 8 +- ArduCopter/mode_land.cpp | 16 +-- ArduCopter/mode_loiter.cpp | 16 +-- ArduCopter/mode_poshold.cpp | 22 ++-- ArduCopter/mode_rtl.cpp | 44 ++++---- ArduCopter/mode_smart_rtl.cpp | 18 ++-- ArduCopter/mode_sport.cpp | 4 +- ArduCopter/mode_stabilize.cpp | 4 +- ArduCopter/mode_stabilize_heli.cpp | 4 +- ArduCopter/mode_throw.cpp | 12 +-- ArduCopter/mode_zigzag.cpp | 16 +-- ArduCopter/takeoff.cpp | 24 ++--- 33 files changed, 411 insertions(+), 376 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 0ac51fc271..6e804c8dce 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -119,7 +119,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(run_nav_updates, 50, 100), SCHED_TASK(update_throttle_hover,100, 90), #if MODE_SMARTRTL_ENABLED == ENABLED - SCHED_TASK_CLASS(Copter::ModeSmartRTL, &copter.mode_smartrtl, save_position, 3, 100), + SCHED_TASK_CLASS(ModeSmartRTL, &copter.mode_smartrtl, save_position, 3, 100), #endif #if SPRAYER_ENABLED == ENABLED SCHED_TASK_CLASS(AC_Sprayer, &copter.sprayer, update, 3, 90), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index ede49688e1..b94813bf3f 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -176,6 +176,19 @@ #include #endif +#if FRAME_CONFIG == HELI_FRAME + #define AC_AttitudeControl_t AC_AttitudeControl_Heli +#else + #define AC_AttitudeControl_t AC_AttitudeControl_Multi +#endif + +#if FRAME_CONFIG == HELI_FRAME + #define MOTOR_CLASS AP_MotorsHeli +#else + #define MOTOR_CLASS AP_MotorsMulticopter +#endif + +#include "mode.h" class Copter : public AP_HAL::HAL::Callbacks { public: @@ -194,6 +207,33 @@ public: friend class RC_Channel_Copter; friend class RC_Channels_Copter; + friend class AutoTune; + + friend class Mode; + friend class ModeAcro; + friend class ModeAcro_Heli; + friend class ModeAltHold; + friend class ModeAuto; + friend class ModeAutoTune; + friend class ModeAvoidADSB; + friend class ModeBrake; + friend class ModeCircle; + friend class ModeDrift; + friend class ModeFlip; + friend class ModeFlowHold; + friend class ModeFollow; + friend class ModeGuided; + friend class ModeLand; + friend class ModeLoiter; + friend class ModePosHold; + friend class ModeRTL; + friend class ModeSmartRTL; + friend class ModeSport; + friend class ModeStabilize; + friend class ModeStabilize_Heli; + friend class ModeThrow; + friend class ModeZigZag; + Copter(void); // HAL::Callbacks implementation. @@ -374,12 +414,6 @@ private: } sensor_health; // Motor Output -#if FRAME_CONFIG == HELI_FRAME - #define MOTOR_CLASS AP_MotorsHeli -#else - #define MOTOR_CLASS AP_MotorsMulticopter -#endif - MOTOR_CLASS *motors; const struct AP_Param::GroupInfo *motors_var_info; @@ -428,11 +462,6 @@ private: // Attitude, Position and Waypoint navigation objects // To-Do: move inertial nav up or other navigation variables down here -#if FRAME_CONFIG == HELI_FRAME - #define AC_AttitudeControl_t AC_AttitudeControl_Heli -#else - #define AC_AttitudeControl_t AC_AttitudeControl_Multi -#endif AC_AttitudeControl_t *attitude_control; AC_PosControl *pos_control; AC_WPNav *wp_nav; @@ -847,8 +876,6 @@ private: void publish_osd_info(); #endif -#include "mode.h" - Mode *flightmode; #if MODE_ACRO_ENABLED == ENABLED #if FRAME_CONFIG == HELI_FRAME diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index a2bd0046be..c478f10eef 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -164,7 +164,7 @@ void GCS_MAVLINK_Copter::send_nav_controller_output() const return; } const Vector3f &targets = copter.attitude_control->get_att_target_euler_cd(); - const Copter::Mode *flightmode = copter.flightmode; + const Mode *flightmode = copter.flightmode; mavlink_msg_nav_controller_output_send( chan, targets.x * 1.0e-2f, diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index e9a552be48..60381ec6ce 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -742,7 +742,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Description: Used by THROW mode. Specifies whether Copter is thrown upward or dropped. // @Values: 0:Upward Throw,1:Drop // @User: Standard - AP_GROUPINFO("THROW_TYPE", 4, ParametersG2, throw_type, Copter::ModeThrow::ThrowType_Upward), + AP_GROUPINFO("THROW_TYPE", 4, ParametersG2, throw_type, ModeThrow::ThrowType_Upward), #endif // @Param: GND_EFFECT_COMP @@ -882,7 +882,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { #if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED // @Group: FHLD // @Path: mode_flowhold.cpp - AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, Copter::ModeFlowHold), + AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, ModeFlowHold), #endif #if MODE_FOLLOW_ENABLED == ENABLED @@ -898,7 +898,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { #if AUTOTUNE_ENABLED == ENABLED // @Group: AUTOTUNE_ // @Path: ../libraries/AC_AutoTune/AC_AutoTune.cpp - AP_SUBGROUPPTR(autotune_ptr, "AUTOTUNE_", 29, ParametersG2, Copter::AutoTune), + AP_SUBGROUPPTR(autotune_ptr, "AUTOTUNE_", 29, ParametersG2, AutoTune), #endif #ifdef ENABLE_SCRIPTING diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 4da1333191..a0dc9b26a9 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -2,6 +2,16 @@ #include +#if GRIPPER_ENABLED == ENABLED + # include +#endif +#if MODE_FOLLOW_ENABLED == ENABLED + # include +#endif +#if VISUAL_ODOMETRY_ENABLED == ENABLED + # include +#endif + // Global parameter class. // class Parameters { diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 0845d4cf4b..2c0a527b82 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -1,9 +1,9 @@ #include "Copter.h" -Copter::Mode::AutoYaw Copter::Mode::auto_yaw; +Mode::AutoYaw Mode::auto_yaw; // roi_yaw - returns heading towards location held in roi -float Copter::Mode::AutoYaw::roi_yaw() +float Mode::AutoYaw::roi_yaw() { roi_yaw_counter++; if (roi_yaw_counter >= 4) { @@ -14,7 +14,7 @@ float Copter::Mode::AutoYaw::roi_yaw() return _roi_yaw; } -float Copter::Mode::AutoYaw::look_ahead_yaw() +float Mode::AutoYaw::look_ahead_yaw() { const Vector3f& vel = copter.inertial_nav.get_velocity(); float speed = norm(vel.x,vel.y); @@ -25,14 +25,14 @@ float Copter::Mode::AutoYaw::look_ahead_yaw() return _look_ahead_yaw; } -void Copter::Mode::AutoYaw::set_mode_to_default(bool rtl) +void Mode::AutoYaw::set_mode_to_default(bool rtl) { set_mode(default_mode(rtl)); } // default_mode - returns auto_yaw.mode() based on WP_YAW_BEHAVIOR parameter // set rtl parameter to true if this is during an RTL -autopilot_yaw_mode Copter::Mode::AutoYaw::default_mode(bool rtl) const +autopilot_yaw_mode Mode::AutoYaw::default_mode(bool rtl) const { switch (copter.g.wp_yaw_behavior) { @@ -56,7 +56,7 @@ autopilot_yaw_mode Copter::Mode::AutoYaw::default_mode(bool rtl) const } // set_mode - sets the yaw mode for auto -void Copter::Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode) +void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode) { // return immediately if no change if (_mode == yaw_mode) { @@ -98,7 +98,7 @@ void Copter::Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode) } // set_fixed_yaw - sets the yaw look at heading for auto mode -void Copter::Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle) +void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle) { const int32_t curr_yaw_target = copter.attitude_control->get_att_target_euler_cd().z; @@ -130,7 +130,7 @@ void Copter::Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_dps, } // set_roi - sets the yaw to look at roi for auto mode -void Copter::Mode::AutoYaw::set_roi(const Location &roi_location) +void Mode::AutoYaw::set_roi(const Location &roi_location) { // if location is zero lat, lon and altitude turn off ROI if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) { @@ -169,14 +169,14 @@ void Copter::Mode::AutoYaw::set_roi(const Location &roi_location) } // set auto yaw rate in centi-degrees per second -void Copter::Mode::AutoYaw::set_rate(float turn_rate_cds) +void Mode::AutoYaw::set_rate(float turn_rate_cds) { set_mode(AUTO_YAW_RATE); _rate_cds = turn_rate_cds; } // yaw - returns target heading depending upon auto_yaw.mode() -float Copter::Mode::AutoYaw::yaw() +float Mode::AutoYaw::yaw() { switch (_mode) { @@ -207,7 +207,7 @@ float Copter::Mode::AutoYaw::yaw() // returns yaw rate normally set by SET_POSITION_TARGET mavlink // messages (positive is clockwise, negative is counter clockwise) -float Copter::Mode::AutoYaw::rate_cds() const +float Mode::AutoYaw::rate_cds() const { if (_mode == AUTO_YAW_RATE) { return _rate_cds; diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index f881d68b75..10d0260816 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -8,7 +8,7 @@ /* constructor for Mode object */ -Copter::Mode::Mode(void) : +Mode::Mode(void) : g(copter.g), g2(copter.g2), wp_nav(copter.wp_nav), @@ -22,16 +22,15 @@ Copter::Mode::Mode(void) : channel_pitch(copter.channel_pitch), channel_throttle(copter.channel_throttle), channel_yaw(copter.channel_yaw), - G_Dt(copter.G_Dt), - ap(copter.ap) + G_Dt(copter.G_Dt) { }; -float Copter::Mode::auto_takeoff_no_nav_alt_cm = 0; +float Mode::auto_takeoff_no_nav_alt_cm = 0; // return the static controller object corresponding to supplied mode -Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode) +Mode *Copter::mode_from_mode_num(const uint8_t mode) { - Copter::Mode *ret = nullptr; + Mode *ret = nullptr; switch (mode) { #if MODE_ACRO_ENABLED == ENABLED @@ -144,7 +143,7 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode) #if OPTFLOW == ENABLED case FLOWHOLD: - ret = (Copter::Mode *)g2.mode_flowhold_ptr; + ret = (Mode *)g2.mode_flowhold_ptr; break; #endif @@ -181,7 +180,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) return true; } - Copter::Mode *new_flightmode = mode_from_mode_num(mode); + Mode *new_flightmode = mode_from_mode_num(mode); if (new_flightmode == nullptr) { gcs().send_text(MAV_SEVERITY_WARNING,"No such mode"); AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); @@ -271,8 +270,8 @@ void Copter::update_flight_mode() } // exit_mode - high level call to organise cleanup as a flight mode is exited -void Copter::exit_mode(Copter::Mode *&old_flightmode, - Copter::Mode *&new_flightmode) +void Copter::exit_mode(Mode *&old_flightmode, + Mode *&new_flightmode) { #if AUTOTUNE_ENABLED == ENABLED if (old_flightmode == &mode_autotune) { @@ -335,7 +334,7 @@ void Copter::notify_flight_mode() { notify.set_flight_mode_str(flightmode->name4()); } -void Copter::Mode::update_navigation() +void Mode::update_navigation() { // run autopilot to make high level decisions about control modes run_autopilot(); @@ -343,7 +342,7 @@ void Copter::Mode::update_navigation() // get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle // returns desired angle in centi-degrees -void Copter::Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const +void Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const { // fetch roll and pitch inputs roll_out = channel_roll->get_control_in(); @@ -371,7 +370,7 @@ void Copter::Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_o // roll_out and pitch_out are returned } -bool Copter::Mode::_TakeOff::triggered(const float target_climb_rate) const +bool Mode::_TakeOff::triggered(const float target_climb_rate) const { if (!copter.ap.land_complete) { // can't take off if we're already flying @@ -390,15 +389,15 @@ bool Copter::Mode::_TakeOff::triggered(const float target_climb_rate) const return true; } -bool Copter::Mode::is_disarmed_or_landed() const +bool Mode::is_disarmed_or_landed() const { - if (!motors->armed() || !ap.auto_armed || ap.land_complete) { + if (!motors->armed() || !copter.ap.auto_armed || copter.ap.land_complete) { return true; } return false; } -void Copter::Mode::zero_throttle_and_relax_ac(bool spool_up) +void Mode::zero_throttle_and_relax_ac(bool spool_up) { if (spool_up) { motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); @@ -409,14 +408,14 @@ void Copter::Mode::zero_throttle_and_relax_ac(bool spool_up) attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt); } -void Copter::Mode::zero_throttle_and_hold_attitude() +void Mode::zero_throttle_and_hold_attitude() { // run attitude controller attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt); } -void Copter::Mode::make_safe_spool_down() +void Mode::make_safe_spool_down() { // command aircraft to initiate the shutdown process motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); @@ -445,7 +444,7 @@ void Copter::Mode::make_safe_spool_down() /* get a height above ground estimate for landing */ -int32_t Copter::Mode::get_alt_above_ground_cm(void) +int32_t Mode::get_alt_above_ground_cm(void) { int32_t alt_above_ground; if (copter.rangefinder_alt_ok()) { @@ -459,11 +458,11 @@ int32_t Copter::Mode::get_alt_above_ground_cm(void) return alt_above_ground; } -void Copter::Mode::land_run_vertical_control(bool pause_descent) +void Mode::land_run_vertical_control(bool pause_descent) { #if PRECISION_LANDING == ENABLED const bool navigating = pos_control->is_active_xy(); - bool doing_precision_landing = !ap.land_repo_active && copter.precland.target_acquired() && navigating; + bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired() && navigating; #else bool doing_precision_landing = false; #endif @@ -502,14 +501,14 @@ void Copter::Mode::land_run_vertical_control(bool pause_descent) pos_control->update_z_controller(); } -void Copter::Mode::land_run_horizontal_control() +void Mode::land_run_horizontal_control() { float target_roll = 0.0f; float target_pitch = 0.0f; float target_yaw_rate = 0; // relax loiter target if we might be landed - if (ap.land_complete_maybe) { + if (copter.ap.land_complete_maybe) { loiter_nav->soften_for_landing(); } @@ -532,10 +531,10 @@ void Copter::Mode::land_run_horizontal_control() // record if pilot has overridden roll or pitch if (!is_zero(target_roll) || !is_zero(target_pitch)) { - if (!ap.land_repo_active) { + if (!copter.ap.land_repo_active) { copter.Log_Write_Event(DATA_LAND_REPO_ACTIVE); } - ap.land_repo_active = true; + copter.ap.land_repo_active = true; } } @@ -547,7 +546,7 @@ void Copter::Mode::land_run_horizontal_control() } #if PRECISION_LANDING == ENABLED - bool doing_precision_landing = !ap.land_repo_active && copter.precland.target_acquired(); + bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired(); // run precision landing if (doing_precision_landing) { Vector2f target_pos, target_vel_rel; @@ -603,7 +602,7 @@ void Copter::Mode::land_run_horizontal_control() } } -float Copter::Mode::throttle_hover() const +float Mode::throttle_hover() const { return motors->get_throttle_hover(); } @@ -612,7 +611,7 @@ float Copter::Mode::throttle_hover() const // used only for manual throttle modes // thr_mid should be in the range 0 to 1 // returns throttle output 0 to 1 -float Copter::Mode::get_pilot_desired_throttle() const +float Mode::get_pilot_desired_throttle() const { const float thr_mid = throttle_hover(); int16_t throttle_control = channel_throttle->get_control_in(); @@ -640,7 +639,7 @@ float Copter::Mode::get_pilot_desired_throttle() const return throttle_out; } -Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_climb_rate_cms) +Mode::AltHoldModeState Mode::get_alt_hold_state(float target_climb_rate_cms) { // Alt Hold State Machine Determination if (!motors->armed()) { @@ -665,9 +664,9 @@ Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_cli // the aircraft should progress through the take off procedure return AltHold_Takeoff; - } else if (!ap.auto_armed || ap.land_complete) { + } else if (!copter.ap.auto_armed || copter.ap.land_complete) { // the aircraft is armed and landed - if (target_climb_rate_cms < 0.0f && !ap.using_interlock) { + if (target_climb_rate_cms < 0.0f && !copter.ap.using_interlock) { // the aircraft should move to a ground idle state motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); @@ -695,56 +694,56 @@ Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_cli // pass-through functions to reduce code churn on conversion; // these are candidates for moving into the Mode base // class. -float Copter::Mode::get_pilot_desired_yaw_rate(int16_t stick_angle) +float Mode::get_pilot_desired_yaw_rate(int16_t stick_angle) { return copter.get_pilot_desired_yaw_rate(stick_angle); } -float Copter::Mode::get_pilot_desired_climb_rate(float throttle_control) +float Mode::get_pilot_desired_climb_rate(float throttle_control) { return copter.get_pilot_desired_climb_rate(throttle_control); } -float Copter::Mode::get_non_takeoff_throttle() +float Mode::get_non_takeoff_throttle() { return copter.get_non_takeoff_throttle(); } -void Copter::Mode::update_simple_mode(void) { +void Mode::update_simple_mode(void) { copter.update_simple_mode(); } -bool Copter::Mode::set_mode(control_mode_t mode, mode_reason_t reason) +bool Mode::set_mode(control_mode_t mode, mode_reason_t reason) { return copter.set_mode(mode, reason); } -void Copter::Mode::set_land_complete(bool b) +void Mode::set_land_complete(bool b) { return copter.set_land_complete(b); } -GCS_Copter &Copter::Mode::gcs() +GCS_Copter &Mode::gcs() { return copter.gcs(); } -void Copter::Mode::Log_Write_Event(Log_Event id) +void Mode::Log_Write_Event(Log_Event id) { return copter.logger.Write_Event(id); } -void Copter::Mode::set_throttle_takeoff() +void Mode::set_throttle_takeoff() { return copter.set_throttle_takeoff(); } -float Copter::Mode::get_avoidance_adjusted_climbrate(float target_rate) +float Mode::get_avoidance_adjusted_climbrate(float target_rate) { return copter.get_avoidance_adjusted_climbrate(target_rate); } -uint16_t Copter::Mode::get_pilot_speed_dn() +uint16_t Mode::get_pilot_speed_dn() { return copter.get_pilot_speed_dn(); } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 4e20e773c0..0faa22b1d4 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1,6 +1,6 @@ #pragma once -// this class is #included into the Copter:: namespace +#include "Copter.h" class Mode { @@ -99,7 +99,6 @@ protected: RC_Channel *&channel_throttle; RC_Channel *&channel_yaw; float &G_Dt; - ap_t ≈ // note that we support two entirely different automatic takeoffs: @@ -221,7 +220,7 @@ class ModeAcro : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; virtual void run() override; @@ -241,7 +240,7 @@ protected: if (g2.acro_thr_mid > 0) { return g2.acro_thr_mid; } - return Copter::Mode::throttle_hover(); + return Mode::throttle_hover(); } private: @@ -254,7 +253,7 @@ class ModeAcro_Heli : public ModeAcro { public: // inherit constructor - using Copter::ModeAcro::Mode; + using ModeAcro::Mode; bool init(bool ignore_checks) override; void run() override; @@ -270,7 +269,7 @@ class ModeAltHold : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; bool init(bool ignore_checks) override; void run() override; @@ -297,7 +296,7 @@ class ModeAuto : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; bool init(bool ignore_checks) override; void run() override; @@ -339,9 +338,9 @@ public: bool do_guided(const AP_Mission::Mission_Command& cmd); AP_Mission mission{ - FUNCTOR_BIND_MEMBER(&Copter::ModeAuto::start_command, bool, const AP_Mission::Mission_Command &), - FUNCTOR_BIND_MEMBER(&Copter::ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &), - FUNCTOR_BIND_MEMBER(&Copter::ModeAuto::exit_mission, void)}; + FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command &), + FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &), + FUNCTOR_BIND_MEMBER(&ModeAuto::exit_mission, void)}; protected: @@ -488,7 +487,7 @@ class ModeAutoTune : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; bool init(bool ignore_checks) override; void run() override; @@ -514,7 +513,7 @@ class ModeBrake : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; bool init(bool ignore_checks) override; void run() override; @@ -543,7 +542,7 @@ class ModeCircle : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; bool init(bool ignore_checks) override; void run() override; @@ -572,7 +571,7 @@ class ModeDrift : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; bool init(bool ignore_checks) override; void run() override; @@ -598,7 +597,7 @@ class ModeFlip : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; bool init(bool ignore_checks) override; void run() override; @@ -725,7 +724,7 @@ class ModeGuided : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; bool init(bool ignore_checks) override; void run() override; @@ -789,7 +788,7 @@ class ModeGuidedNoGPS : public ModeGuided { public: // inherit constructor - using Copter::ModeGuided::Mode; + using ModeGuided::Mode; bool init(bool ignore_checks) override; void run() override; @@ -813,7 +812,7 @@ class ModeLand : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; bool init(bool ignore_checks) override; void run() override; @@ -844,7 +843,7 @@ class ModeLoiter : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; bool init(bool ignore_checks) override; void run() override; @@ -885,7 +884,7 @@ class ModePosHold : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; bool init(bool ignore_checks) override; void run() override; @@ -964,7 +963,7 @@ class ModeRTL : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; bool init(bool ignore_checks) override; void run() override { @@ -1037,7 +1036,7 @@ class ModeSmartRTL : public ModeRTL { public: // inherit constructor - using Copter::ModeRTL::Mode; + using ModeRTL::Mode; bool init(bool ignore_checks) override; void run() override; @@ -1074,7 +1073,7 @@ class ModeSport : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; bool init(bool ignore_checks) override; void run() override; @@ -1101,7 +1100,7 @@ class ModeStabilize : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; virtual void run() override; @@ -1124,7 +1123,7 @@ class ModeStabilize_Heli : public ModeStabilize { public: // inherit constructor - using Copter::ModeStabilize::Mode; + using ModeStabilize::Mode; bool init(bool ignore_checks) override; void run() override; @@ -1141,7 +1140,7 @@ class ModeThrow : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; bool init(bool ignore_checks) override; void run() override; @@ -1192,7 +1191,7 @@ class ModeAvoidADSB : public ModeGuided { public: // inherit constructor - using Copter::ModeGuided::Mode; + using ModeGuided::Mode; bool init(bool ignore_checks) override; void run() override; @@ -1218,7 +1217,7 @@ class ModeFollow : public ModeGuided { public: // inherit constructor - using Copter::ModeGuided::Mode; + using ModeGuided::Mode; bool init(bool ignore_checks) override; void run() override; @@ -1244,7 +1243,7 @@ class ModeZigZag : public Mode { public: // inherit constructor - using Copter::Mode::Mode; + using Mode::Mode; bool init(bool ignore_checks) override; void run() override; diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp index 4241f9528b..1620c78833 100644 --- a/ArduCopter/mode_acro.cpp +++ b/ArduCopter/mode_acro.cpp @@ -8,7 +8,7 @@ * Init and run calls for acro flight mode */ -void Copter::ModeAcro::run() +void ModeAcro::run() { // convert the input to the desired body frame rate float target_roll, target_pitch, target_yaw; @@ -17,7 +17,7 @@ void Copter::ModeAcro::run() if (!motors->armed()) { // Motors should be Stopped motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); - } else if (ap.throttle_zero) { + } else if (copter.ap.throttle_zero) { // Attempting to Land motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); } else { @@ -59,7 +59,7 @@ void Copter::ModeAcro::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::ModeAcro::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 ModeAcro::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/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp index 4d104cfda4..eadeb4893f 100644 --- a/ArduCopter/mode_acro_heli.cpp +++ b/ArduCopter/mode_acro_heli.cpp @@ -8,7 +8,7 @@ */ // heli_acro_init - initialise acro controller -bool Copter::ModeAcro_Heli::init(bool ignore_checks) +bool ModeAcro_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()); @@ -24,7 +24,7 @@ bool Copter::ModeAcro_Heli::init(bool ignore_checks) // heli_acro_run - runs the acro controller // should be called at 100hz or more -void Copter::ModeAcro_Heli::run() +void ModeAcro_Heli::run() { float target_roll, target_pitch, target_yaw; float pilot_throttle_scaled; @@ -73,7 +73,7 @@ void Copter::ModeAcro_Heli::run() // only mimic flybar response when trainer mode is disabled if (g.acro_trainer == ACRO_TRAINER_DISABLED) { // while landed always leak off target attitude to current attitude - if (ap.land_complete) { + if (copter.ap.land_complete) { virtual_flybar(target_roll, target_pitch, target_yaw, 3.0f, 3.0f); // while flying use acro balance parameters for leak rate } else { @@ -123,7 +123,7 @@ void Copter::ModeAcro_Heli::run() // virtual_flybar - acts like a flybar by leaking target atttitude back to current attitude -void Copter::ModeAcro_Heli::virtual_flybar( float &roll_out, float &pitch_out, float &yaw_out, float pitch_leak, float roll_leak) +void ModeAcro_Heli::virtual_flybar( float &roll_out, float &pitch_out, float &yaw_out, float pitch_leak, float roll_leak) { Vector3f rate_ef_level, rate_bf_level; diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 36e1a8c5b6..154dbdd26d 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -6,7 +6,7 @@ */ // althold_init - initialise althold controller -bool Copter::ModeAltHold::init(bool ignore_checks) +bool ModeAltHold::init(bool ignore_checks) { // initialise position and desired velocity if (!pos_control->is_active_z()) { @@ -19,7 +19,7 @@ bool Copter::ModeAltHold::init(bool ignore_checks) // althold_run - runs the althold controller // should be called at 100hz or more -void Copter::ModeAltHold::run() +void ModeAltHold::run() { float takeoff_climb_rate = 0.0f; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 85ae014669..d5138d47e5 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -20,13 +20,13 @@ */ // auto_init - initialise auto controller -bool Copter::ModeAuto::init(bool ignore_checks) +bool ModeAuto::init(bool ignore_checks) { if (mission.num_commands() > 1 || ignore_checks) { _mode = Auto_Loiter; // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips) - if (motors->armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) { + if (motors->armed() && copter.ap.land_complete && !mission.starts_with_takeoff_cmd()) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd"); return false; } @@ -54,7 +54,7 @@ bool Copter::ModeAuto::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::ModeAuto::run() +void ModeAuto::run() { // call the correct auto controller switch (_mode) { @@ -106,7 +106,7 @@ void Copter::ModeAuto::run() // auto_loiter_start - initialises loitering in auto mode // returns success/failure because this can be called by exit_mission -bool Copter::ModeAuto::loiter_start() +bool ModeAuto::loiter_start() { // return failure if GPS is bad if (!copter.position_ok()) { @@ -128,7 +128,7 @@ bool Copter::ModeAuto::loiter_start() } // auto_rtl_start - initialises RTL in AUTO flight mode -void Copter::ModeAuto::rtl_start() +void ModeAuto::rtl_start() { _mode = Auto_RTL; @@ -137,7 +137,7 @@ void Copter::ModeAuto::rtl_start() } // auto_takeoff_start - initialises waypoint controller to implement take-off -void Copter::ModeAuto::takeoff_start(const Location& dest_loc) +void ModeAuto::takeoff_start(const Location& dest_loc) { _mode = Auto_TakeOff; @@ -183,7 +183,7 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc) } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination -void Copter::ModeAuto::wp_start(const Vector3f& destination) +void ModeAuto::wp_start(const Vector3f& destination) { _mode = Auto_WP; @@ -198,7 +198,7 @@ void Copter::ModeAuto::wp_start(const Vector3f& destination) } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination -void Copter::ModeAuto::wp_start(const Location& dest_loc) +void ModeAuto::wp_start(const Location& dest_loc) { _mode = Auto_WP; @@ -217,7 +217,7 @@ void Copter::ModeAuto::wp_start(const Location& dest_loc) } // auto_land_start - initialises controller to implement a landing -void Copter::ModeAuto::land_start() +void ModeAuto::land_start() { // set target to stopping point Vector3f stopping_point; @@ -228,7 +228,7 @@ void Copter::ModeAuto::land_start() } // auto_land_start - initialises controller to implement a landing -void Copter::ModeAuto::land_start(const Vector3f& destination) +void ModeAuto::land_start(const Vector3f& destination) { _mode = Auto_Land; @@ -247,7 +247,7 @@ void Copter::ModeAuto::land_start(const Vector3f& destination) // 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::ModeAuto::circle_movetoedge_start(const Location &circle_center, float radius_m) +void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radius_m) { // convert location to vector from ekf origin Vector3f circle_center_neu; @@ -305,7 +305,7 @@ void Copter::ModeAuto::circle_movetoedge_start(const Location &circle_center, fl // 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::ModeAuto::circle_start() +void ModeAuto::circle_start() { _mode = Auto_Circle; @@ -315,7 +315,7 @@ void Copter::ModeAuto::circle_start() // 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::ModeAuto::spline_start(const Location& destination, bool stopped_at_start, +void ModeAuto::spline_start(const Location& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location& next_destination) { @@ -337,7 +337,7 @@ void Copter::ModeAuto::spline_start(const Location& destination, bool stopped_at #if NAV_GUIDED == ENABLED // auto_nav_guided_start - hand over control to external navigation controller in AUTO mode -void Copter::ModeAuto::nav_guided_start() +void ModeAuto::nav_guided_start() { _mode = Auto_NavGuided; @@ -349,7 +349,7 @@ void Copter::ModeAuto::nav_guided_start() } #endif //NAV_GUIDED -bool Copter::ModeAuto::is_landing() const +bool ModeAuto::is_landing() const { switch(_mode) { case Auto_Land: @@ -362,12 +362,12 @@ bool Copter::ModeAuto::is_landing() const return false; } -bool Copter::ModeAuto::is_taking_off() const +bool ModeAuto::is_taking_off() const { return _mode == Auto_TakeOff; } -bool Copter::ModeAuto::landing_gear_should_be_deployed() const +bool ModeAuto::landing_gear_should_be_deployed() const { switch(_mode) { case Auto_Land: @@ -381,7 +381,7 @@ bool Copter::ModeAuto::landing_gear_should_be_deployed() const } // auto_payload_place_start - initialises controller to implement a placing -void Copter::ModeAuto::payload_place_start() +void ModeAuto::payload_place_start() { // set target to stopping point Vector3f stopping_point; @@ -393,7 +393,7 @@ void Copter::ModeAuto::payload_place_start() } // start_command - this function will be called when the ap_mission lib wishes to start a new command -bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) { // To-Do: logging when new commands start/end if (copter.should_log(MASK_LOG_CMD)) { @@ -525,12 +525,12 @@ bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) } // exit_mission - function that is called once the mission completes -void Copter::ModeAuto::exit_mission() +void ModeAuto::exit_mission() { // play a tone AP_Notify::events.mission_complete = 1; // if we are not on the ground switch to loiter or land - if (!ap.land_complete) { + if (!copter.ap.land_complete) { // try to enter loiter but if that fails land if (!loiter_start()) { set_mode(LAND, MODE_REASON_MISSION_END); @@ -542,7 +542,7 @@ void Copter::ModeAuto::exit_mission() } // do_guided - start guided mode -bool Copter::ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd) { // only process guided waypoint if we are in guided mode if (copter.control_mode != GUIDED && !(copter.control_mode == AUTO && mode() == Auto_NavGuided)) { @@ -571,7 +571,7 @@ bool Copter::ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd) return true; } -uint32_t Copter::ModeAuto::wp_distance() const +uint32_t ModeAuto::wp_distance() const { switch (_mode) { case Auto_Circle: @@ -583,7 +583,7 @@ uint32_t Copter::ModeAuto::wp_distance() const } } -int32_t Copter::ModeAuto::wp_bearing() const +int32_t ModeAuto::wp_bearing() const { switch (_mode) { case Auto_Circle: @@ -595,7 +595,7 @@ int32_t Copter::ModeAuto::wp_bearing() const } } -bool Copter::ModeAuto::get_wp(Location& destination) +bool ModeAuto::get_wp(Location& destination) { switch (_mode) { case Auto_NavGuided: @@ -608,7 +608,7 @@ bool Copter::ModeAuto::get_wp(Location& destination) } // update mission -void Copter::ModeAuto::run_autopilot() +void ModeAuto::run_autopilot() { mission.update(); } @@ -624,7 +624,7 @@ Return true if we do not recognize the command so that we move on to the next co // verify_command - callback function called from ap-mission at 10hz or higher when a command is being run // we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode -bool Copter::ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) { if (copter.flightmode != &copter.mode_auto) { return false; @@ -730,7 +730,7 @@ bool Copter::ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) // auto_takeoff_run - takeoff in auto mode // called by auto_run at 100hz or more -void Copter::ModeAuto::takeoff_run() +void ModeAuto::takeoff_run() { auto_takeoff_run(); if (wp_nav->reached_wp_destination()) { @@ -741,7 +741,7 @@ void Copter::ModeAuto::takeoff_run() // auto_wp_run - runs the auto waypoint controller // called by auto_run at 100hz or more -void Copter::ModeAuto::wp_run() +void ModeAuto::wp_run() { // process pilot's yaw input float target_yaw_rate = 0; @@ -781,7 +781,7 @@ void Copter::ModeAuto::wp_run() // auto_spline_run - runs the auto spline controller // called by auto_run at 100hz or more -void Copter::ModeAuto::spline_run() +void ModeAuto::spline_run() { // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { @@ -821,7 +821,7 @@ void Copter::ModeAuto::spline_run() // auto_land_run - lands in auto mode // called by auto_run at 100hz or more -void Copter::ModeAuto::land_run() +void ModeAuto::land_run() { // if not armed set throttle to zero and exit immediately @@ -840,7 +840,7 @@ void Copter::ModeAuto::land_run() // auto_rtl_run - rtl in AUTO flight mode // called by auto_run at 100hz or more -void Copter::ModeAuto::rtl_run() +void ModeAuto::rtl_run() { // call regular rtl flight mode run function copter.mode_rtl.run(false); @@ -848,7 +848,7 @@ void Copter::ModeAuto::rtl_run() // auto_circle_run - circle in AUTO flight mode // called by auto_run at 100hz or more -void Copter::ModeAuto::circle_run() +void ModeAuto::circle_run() { // call circle controller copter.circle_nav->update(); @@ -868,7 +868,7 @@ void Copter::ModeAuto::circle_run() #if NAV_GUIDED == ENABLED // auto_nav_guided_run - allows control by external navigation controller // called by auto_run at 100hz or more -void Copter::ModeAuto::nav_guided_run() +void ModeAuto::nav_guided_run() { // call regular guided flight mode run function copter.mode_guided.run(); @@ -877,7 +877,7 @@ void Copter::ModeAuto::nav_guided_run() // auto_loiter_run - loiter in AUTO flight mode // called by auto_run at 100hz or more -void Copter::ModeAuto::loiter_run() +void ModeAuto::loiter_run() { // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { @@ -904,7 +904,7 @@ void Copter::ModeAuto::loiter_run() // auto_loiter_run - loiter to altitude in AUTO flight mode // called by auto_run at 100hz or more -void Copter::ModeAuto::loiter_to_alt_run() +void ModeAuto::loiter_to_alt_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (is_disarmed_or_landed() || !motors->get_interlock()) { @@ -955,7 +955,7 @@ void Copter::ModeAuto::loiter_to_alt_run() } // auto_payload_place_start - initialises controller to implement placement of a load -void Copter::ModeAuto::payload_place_start(const Vector3f& destination) +void ModeAuto::payload_place_start(const Vector3f& destination) { _mode = Auto_NavPayloadPlace; nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; @@ -975,7 +975,7 @@ void Copter::ModeAuto::payload_place_start(const Vector3f& destination) // auto_payload_place_run - places an object in auto mode // called by auto_run at 100hz or more -void Copter::ModeAuto::payload_place_run() +void ModeAuto::payload_place_run() { if (!payload_place_run_should_run()) { zero_throttle_and_relax_ac(); @@ -1007,18 +1007,18 @@ void Copter::ModeAuto::payload_place_run() } } -bool Copter::ModeAuto::payload_place_run_should_run() +bool ModeAuto::payload_place_run_should_run() { // must be armed if (!motors->armed()) { return false; } // must be auto-armed - if (!ap.auto_armed) { + if (!copter.ap.auto_armed) { return false; } // must not be landed - if (ap.land_complete) { + if (copter.ap.land_complete) { return false; } // interlock must be enabled (i.e. unsafe) @@ -1029,7 +1029,7 @@ bool Copter::ModeAuto::payload_place_run_should_run() return true; } -void Copter::ModeAuto::payload_place_run_loiter() +void ModeAuto::payload_place_run_loiter() { // loiter... land_run_horizontal_control(); @@ -1045,7 +1045,7 @@ void Copter::ModeAuto::payload_place_run_loiter() pos_control->update_z_controller(); } -void Copter::ModeAuto::payload_place_run_descend() +void ModeAuto::payload_place_run_descend() { land_run_horizontal_control(); land_run_vertical_control(); @@ -1053,7 +1053,7 @@ void Copter::ModeAuto::payload_place_run_descend() // terrain_adjusted_location: returns a Location with lat/lon from cmd // and altitude from our current altitude adjusted for location -Location Copter::ModeAuto::terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const +Location ModeAuto::terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const { // convert to location class Location target_loc(cmd.content.location); @@ -1077,13 +1077,13 @@ Location Copter::ModeAuto::terrain_adjusted_location(const AP_Mission::Mission_C /********************************************************************************/ // do_takeoff - initiate takeoff navigation command -void Copter::ModeAuto::do_takeoff(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_takeoff(const AP_Mission::Mission_Command& cmd) { // Set wp navigation target to safe altitude above current position takeoff_start(cmd.content.location); } -Location Copter::ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd) const +Location ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd) const { Location ret(cmd.content.location); @@ -1108,7 +1108,7 @@ Location Copter::ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd) } // do_nav_wp - initiate move to next waypoint -void Copter::ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) { Location target_loc = loc_from_cmd(cmd); @@ -1153,7 +1153,7 @@ void Copter::ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) } // do_land - initiate landing procedure -void Copter::ModeAuto::do_land(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_land(const AP_Mission::Mission_Command& cmd) { // To-Do: check if we have already landed @@ -1176,7 +1176,7 @@ void Copter::ModeAuto::do_land(const AP_Mission::Mission_Command& cmd) // do_loiter_unlimited - start loitering with no end conditions // note: caller should set yaw_mode -void Copter::ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) { // convert back to location Location target_loc(cmd.content.location); @@ -1210,7 +1210,7 @@ void Copter::ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cm } // do_circle - initiate moving in a circle -void Copter::ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd) { const Location circle_center = loc_from_cmd(cmd); @@ -1223,7 +1223,7 @@ void Copter::ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd) // do_loiter_time - initiate loitering at a point for a given time period // note: caller should set yaw_mode -void Copter::ModeAuto::do_loiter_time(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_loiter_time(const AP_Mission::Mission_Command& cmd) { // re-use loiter unlimited do_loiter_unlimited(cmd); @@ -1235,7 +1235,7 @@ void Copter::ModeAuto::do_loiter_time(const AP_Mission::Mission_Command& cmd) // do_loiter_alt - initiate loitering at a point until a given altitude is reached // note: caller should set yaw_mode -void Copter::ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) { // re-use loiter unlimited do_loiter_unlimited(cmd); @@ -1270,7 +1270,7 @@ void Copter::ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) } // do_spline_wp - initiate move to next waypoint -void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd) { const Location target_loc = loc_from_cmd(cmd); @@ -1328,7 +1328,7 @@ void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd) #if NAV_GUIDED == ENABLED // do_nav_guided_enable - initiate accepting commands from external nav computer -void Copter::ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd) { if (cmd.p1 > 0) { // start guided within auto @@ -1337,7 +1337,7 @@ void Copter::ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& c } // do_guided_limits - pass guided limits to guided controller -void Copter::ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd) { copter.mode_guided.limit_set( cmd.p1 * 1000, // convert seconds to ms @@ -1348,7 +1348,7 @@ void Copter::ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd) #endif // NAV_GUIDED // do_nav_delay - Delay the next navigation command -void Copter::ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) { nav_delay_time_start = millis(); @@ -1366,18 +1366,18 @@ void Copter::ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) // Condition (May) commands /********************************************************************************/ -void Copter::ModeAuto::do_wait_delay(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_wait_delay(const AP_Mission::Mission_Command& cmd) { condition_start = millis(); condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds } -void Copter::ModeAuto::do_within_distance(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_within_distance(const AP_Mission::Mission_Command& cmd) { condition_value = cmd.content.distance.meters * 100; } -void Copter::ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd) { auto_yaw.set_fixed_yaw( cmd.content.yaw.angle_deg, @@ -1392,7 +1392,7 @@ void Copter::ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd) -void Copter::ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd) { if (cmd.content.speed.target_ms > 0) { if (cmd.content.speed.speed_type == 2) { @@ -1405,7 +1405,7 @@ void Copter::ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd) } } -void Copter::ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd) { if (cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) { if (!copter.set_home_to_current_location(false)) { @@ -1422,13 +1422,13 @@ void Copter::ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd) // this involves either moving the camera to point at the ROI (region of interest) // and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature // TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint -void Copter::ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd) { auto_yaw.set_roi(cmd.content.location); } // point the camera to a specified angle -void Copter::ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd) { #if MOUNT == ENABLED if (!copter.camera_mount.has_pan_control()) { @@ -1440,7 +1440,7 @@ void Copter::ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd) #if WINCH_ENABLED == ENABLED // control winch based on mission command -void Copter::ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd) { // Note: we ignore the gripper num parameter because we only support one gripper switch (cmd.content.winch.action) { @@ -1464,7 +1464,7 @@ void Copter::ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd) #endif // do_payload_place - initiate placing procedure -void Copter::ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd) { // if location provided we fly to that location at current altitude if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) { @@ -1484,7 +1484,7 @@ void Copter::ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd) } // do_RTL - start Return-to-Launch -void Copter::ModeAuto::do_RTL(void) +void ModeAuto::do_RTL(void) { // start rtl in auto flight mode rtl_start(); @@ -1495,14 +1495,14 @@ void Copter::ModeAuto::do_RTL(void) /********************************************************************************/ // verify_takeoff - check if we have completed the takeoff -bool Copter::ModeAuto::verify_takeoff() +bool ModeAuto::verify_takeoff() { // have we reached our target altitude? return copter.wp_nav->reached_wp_destination(); } // verify_land - returns true if landing has been completed -bool Copter::ModeAuto::verify_land() +bool ModeAuto::verify_land() { bool retval = false; @@ -1523,7 +1523,7 @@ bool Copter::ModeAuto::verify_land() case LandStateType_Descending: // rely on THROTTLE_LAND mode to correctly update landing status - retval = ap.land_complete && (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE); + retval = copter.ap.land_complete && (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE); break; default: @@ -1547,7 +1547,7 @@ bool Copter::ModeAuto::verify_land() #endif // verify_payload_place - returns true if placing has been completed -bool Copter::ModeAuto::verify_payload_place() +bool ModeAuto::verify_payload_place() { const uint16_t hover_throttle_calibrate_time = 2000; // milliseconds const uint16_t descend_throttle_calibrate_time = 2000; // milliseconds @@ -1559,7 +1559,7 @@ bool Copter::ModeAuto::verify_payload_place() const uint32_t now = AP_HAL::millis(); // if we discover we've landed then immediately release the load: - if (ap.land_complete) { + if (copter.ap.land_complete) { switch (nav_payload_place.state) { case PayloadPlaceStateType_FlyToLocation: case PayloadPlaceStateType_Calibrating_Hover_Start: @@ -1698,13 +1698,13 @@ bool Copter::ModeAuto::verify_payload_place() } #undef debug -bool Copter::ModeAuto::verify_loiter_unlimited() +bool ModeAuto::verify_loiter_unlimited() { return false; } // verify_loiter_time - check if we have loitered long enough -bool Copter::ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd) { // return immediately if we haven't reached our destination if (!copter.wp_nav->reached_wp_destination()) { @@ -1727,7 +1727,7 @@ bool Copter::ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd // verify_loiter_to_alt - check if we have reached both destination // (roughly) and altitude (precisely) -bool Copter::ModeAuto::verify_loiter_to_alt() +bool ModeAuto::verify_loiter_to_alt() { if (loiter_to_alt.reached_destination_xy && loiter_to_alt.reached_alt) { @@ -1739,7 +1739,7 @@ bool Copter::ModeAuto::verify_loiter_to_alt() // verify_RTL - handles any state changes required to implement RTL // do_RTL should have been called once first to initialise all variables // returns true with RTL has completed successfully -bool Copter::ModeAuto::verify_RTL() +bool ModeAuto::verify_RTL() { return (copter.mode_rtl.state_complete() && (copter.mode_rtl.state() == RTL_FinalDescent || copter.mode_rtl.state() == RTL_Land) && @@ -1750,7 +1750,7 @@ bool Copter::ModeAuto::verify_RTL() // Verify Condition (May) commands /********************************************************************************/ -bool Copter::ModeAuto::verify_wait_delay() +bool ModeAuto::verify_wait_delay() { if (millis() - condition_start > (uint32_t)MAX(condition_value,0)) { condition_value = 0; @@ -1759,7 +1759,7 @@ bool Copter::ModeAuto::verify_wait_delay() return false; } -bool Copter::ModeAuto::verify_within_distance() +bool ModeAuto::verify_within_distance() { if (wp_distance() < (uint32_t)MAX(condition_value,0)) { condition_value = 0; @@ -1769,7 +1769,7 @@ bool Copter::ModeAuto::verify_within_distance() } // verify_yaw - return true if we have reached the desired heading -bool Copter::ModeAuto::verify_yaw() +bool ModeAuto::verify_yaw() { // set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command) if (auto_yaw.mode() != AUTO_YAW_FIXED) { @@ -1781,7 +1781,7 @@ bool Copter::ModeAuto::verify_yaw() } // verify_nav_wp - check if we have reached the next way point -bool Copter::ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) { // check if we have reached the waypoint if ( !copter.wp_nav->reached_wp_destination() ) { @@ -1810,7 +1810,7 @@ bool Copter::ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) } // verify_circle - check if we have circled the point enough -bool Copter::ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd) { // check if we've reached the edge if (mode() == Auto_CircleMoveToEdge) { @@ -1843,7 +1843,7 @@ bool Copter::ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd) } // verify_spline_wp - check if we have reached the next way point using spline -bool Copter::ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd) { // check if we have reached the waypoint if ( !copter.wp_nav->reached_wp_destination() ) { @@ -1865,7 +1865,7 @@ bool Copter::ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd) #if NAV_GUIDED == ENABLED // verify_nav_guided - check if we have breached any limits -bool Copter::ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) { // if disabling guided mode then immediately return true so we move to next command if (cmd.p1 == 0) { @@ -1878,7 +1878,7 @@ bool Copter::ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Comman #endif // NAV_GUIDED // verify_nav_delay - check if we have waited long enough -bool Copter::ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd) { if (millis() - nav_delay_time_start > (uint32_t)MAX(nav_delay_time_max,0)) { nav_delay_time_max = 0; diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index ea914bb7e7..9bef508b81 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -6,7 +6,7 @@ #if AUTOTUNE_ENABLED == ENABLED -bool Copter::AutoTune::init() +bool AutoTune::init() { // use position hold while tuning if we were in QLOITER bool position_hold = (copter.control_mode == LOITER || copter.control_mode == POSHOLD); @@ -21,7 +21,7 @@ bool Copter::AutoTune::init() /* start autotune mode */ -bool Copter::AutoTune::start() +bool AutoTune::start() { // only allow flip from Stabilize, AltHold, PosHold or Loiter modes if (copter.control_mode != STABILIZE && copter.control_mode != ALT_HOLD && @@ -42,7 +42,7 @@ bool Copter::AutoTune::start() return AC_AutoTune::start(); } -void Copter::AutoTune::run() +void AutoTune::run() { // apply SIMPLE mode transform to pilot inputs copter.update_simple_mode(); @@ -77,7 +77,7 @@ void Copter::AutoTune::run() /* get stick input climb rate */ -float Copter::AutoTune::get_pilot_desired_climb_rate_cms(void) const +float AutoTune::get_pilot_desired_climb_rate_cms(void) const { float target_climb_rate = copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in()); @@ -90,7 +90,7 @@ float Copter::AutoTune::get_pilot_desired_climb_rate_cms(void) const /* get stick roll, pitch and yaw rate */ -void Copter::AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pitch_cd, float &yaw_rate_cds) +void AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pitch_cd, float &yaw_rate_cds) { copter.mode_autotune.get_pilot_desired_lean_angles(des_roll_cd, des_pitch_cd, copter.aparm.angle_max, copter.attitude_control->get_althold_lean_angle_max()); @@ -100,13 +100,13 @@ void Copter::AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float & /* setup z controller velocity and accel limits */ -void Copter::AutoTune::init_z_limits() +void AutoTune::init_z_limits() { copter.pos_control->set_max_speed_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up); copter.pos_control->set_max_accel_z(copter.g.pilot_accel_z); } -void Copter::AutoTune::log_pids() +void AutoTune::log_pids() { copter.logger.Write_PID(LOG_PIDR_MSG, copter.attitude_control->get_rate_roll_pid().get_pid_info()); copter.logger.Write_PID(LOG_PIDP_MSG, copter.attitude_control->get_rate_pitch_pid().get_pid_info()); @@ -118,7 +118,7 @@ void Copter::AutoTune::log_pids() Write an event packet. This maps from AC_AutoTune event IDs to copter event IDs */ -void Copter::AutoTune::Log_Write_Event(enum at_event id) +void AutoTune::Log_Write_Event(enum at_event id) { const struct { enum at_event eid; @@ -144,7 +144,7 @@ void Copter::AutoTune::Log_Write_Event(enum at_event id) /* check if we have a good position estimate */ -bool Copter::AutoTune::position_ok() +bool AutoTune::position_ok() { return copter.position_ok(); } @@ -152,28 +152,28 @@ bool Copter::AutoTune::position_ok() /* initialise autotune mode */ -bool Copter::ModeAutoTune::init(bool ignore_checks) +bool ModeAutoTune::init(bool ignore_checks) { return copter.autotune.init(); } -void Copter::ModeAutoTune::run() +void ModeAutoTune::run() { copter.autotune.run(); } -void Copter::ModeAutoTune::save_tuning_gains() +void ModeAutoTune::save_tuning_gains() { copter.autotune.save_tuning_gains(); } -void Copter::ModeAutoTune::stop() +void ModeAutoTune::stop() { copter.autotune.stop(); } -void Copter::ModeAutoTune::reset() +void ModeAutoTune::reset() { copter.autotune.reset(); } diff --git a/ArduCopter/mode_avoid_adsb.cpp b/ArduCopter/mode_avoid_adsb.cpp index dd4e80741b..254dda0f19 100644 --- a/ArduCopter/mode_avoid_adsb.cpp +++ b/ArduCopter/mode_avoid_adsb.cpp @@ -10,13 +10,13 @@ */ // initialise avoid_adsb controller -bool Copter::ModeAvoidADSB::init(const bool ignore_checks) +bool ModeAvoidADSB::init(const bool ignore_checks) { // re-use guided mode - return Copter::ModeGuided::init(ignore_checks); + return ModeGuided::init(ignore_checks); } -bool Copter::ModeAvoidADSB::set_velocity(const Vector3f& velocity_neu) +bool ModeAvoidADSB::set_velocity(const Vector3f& velocity_neu) { // check flight mode if (copter.control_mode != AVOID_ADSB) { @@ -24,15 +24,15 @@ bool Copter::ModeAvoidADSB::set_velocity(const Vector3f& velocity_neu) } // re-use guided mode's velocity controller - Copter::ModeGuided::set_velocity(velocity_neu); + ModeGuided::set_velocity(velocity_neu); return true; } // runs the AVOID_ADSB controller -void Copter::ModeAvoidADSB::run() +void ModeAvoidADSB::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::ModeGuided::run(); + ModeGuided::run(); } diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 7b5b90fe33..5304245e79 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -7,7 +7,7 @@ */ // brake_init - initialise brake controller -bool Copter::ModeBrake::init(bool ignore_checks) +bool ModeBrake::init(bool ignore_checks) { // set target to current position wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE); @@ -29,7 +29,7 @@ bool Copter::ModeBrake::init(bool ignore_checks) // brake_run - runs the brake controller // should be called at 100hz or more -void Copter::ModeBrake::run() +void ModeBrake::run() { // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { @@ -42,7 +42,7 @@ void Copter::ModeBrake::run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // relax stop target if we might be landed - if (ap.land_complete_maybe) { + if (copter.ap.land_complete_maybe) { loiter_nav->soften_for_landing(); } @@ -54,7 +54,7 @@ void Copter::ModeBrake::run() // update altitude target and call position controller // protects heli's from inflight motor interlock disable - if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::GROUND_IDLE && !ap.land_complete) { + if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::GROUND_IDLE && !copter.ap.land_complete) { pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); } else { pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); @@ -68,7 +68,7 @@ void Copter::ModeBrake::run() } } -void Copter::ModeBrake::timeout_to_loiter_ms(uint32_t timeout_ms) +void ModeBrake::timeout_to_loiter_ms(uint32_t timeout_ms) { _timeout_start = millis(); _timeout_ms = timeout_ms; diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index 0b4345413b..e1e60686b3 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -7,7 +7,7 @@ */ // circle_init - initialise circle controller flight mode -bool Copter::ModeCircle::init(bool ignore_checks) +bool ModeCircle::init(bool ignore_checks) { pilot_yaw_override = false; @@ -25,7 +25,7 @@ bool Copter::ModeCircle::init(bool ignore_checks) // circle_run - runs the circle flight mode // should be called at 100hz or more -void Copter::ModeCircle::run() +void ModeCircle::run() { // initialize speeds and accelerations pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy()); @@ -72,7 +72,7 @@ void Copter::ModeCircle::run() // update altitude target and call position controller // protects heli's from inflight motor interlock disable - if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::GROUND_IDLE && !ap.land_complete) { + if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::GROUND_IDLE && !copter.ap.land_complete) { pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); } else { pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); @@ -80,12 +80,12 @@ void Copter::ModeCircle::run() pos_control->update_z_controller(); } -uint32_t Copter::ModeCircle::wp_distance() const +uint32_t ModeCircle::wp_distance() const { return copter.circle_nav->get_distance_to_target(); } -int32_t Copter::ModeCircle::wp_bearing() const +int32_t ModeCircle::wp_bearing() const { return copter.circle_nav->get_bearing_to_target(); } diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index bf61a171f3..56d7ea2545 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -29,14 +29,14 @@ #endif // drift_init - initialise drift controller -bool Copter::ModeDrift::init(bool ignore_checks) +bool ModeDrift::init(bool ignore_checks) { return true; } // drift_run - runs the drift controller // should be called at 100hz or more -void Copter::ModeDrift::run() +void ModeDrift::run() { static float braker = 0.0f; static float roll_input = 0.0f; @@ -81,7 +81,7 @@ void Copter::ModeDrift::run() if (!motors->armed()) { // Motors should be Stopped motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); - } else if (ap.throttle_zero) { + } else if (copter.ap.throttle_zero) { // Attempting to Land motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); } else { @@ -120,7 +120,7 @@ void Copter::ModeDrift::run() } // get_throttle_assist - return throttle output (range 0 ~ 1) based on pilot input and z-axis velocity -float Copter::ModeDrift::get_throttle_assist(float velz, float pilot_throttle_scaled) +float ModeDrift::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/mode_flip.cpp b/ArduCopter/mode_flip.cpp index df1186a9a6..763ee47b88 100644 --- a/ArduCopter/mode_flip.cpp +++ b/ArduCopter/mode_flip.cpp @@ -33,7 +33,7 @@ #define FLIP_PITCH_FORWARD -1 // used to set flip_dir // flip_init - initialise flip controller -bool Copter::ModeFlip::init(bool ignore_checks) +bool ModeFlip::init(bool ignore_checks) { // only allow flip from ACRO, Stabilize, AltHold or Drift flight modes if (copter.control_mode != ACRO && @@ -44,7 +44,7 @@ bool Copter::ModeFlip::init(bool ignore_checks) } // if in acro or stabilize ensure throttle is above zero - if (ap.throttle_zero && (copter.control_mode == ACRO || copter.control_mode == STABILIZE)) { + if (copter.ap.throttle_zero && (copter.control_mode == ACRO || copter.control_mode == STABILIZE)) { return false; } @@ -54,7 +54,7 @@ bool Copter::ModeFlip::init(bool ignore_checks) } // only allow flip when flying - if (!motors->armed() || ap.land_complete) { + if (!motors->armed() || copter.ap.land_complete) { return false; } @@ -92,7 +92,7 @@ bool Copter::ModeFlip::init(bool ignore_checks) // run - runs the flip controller // should be called at 100hz or more -void Copter::ModeFlip::run() +void ModeFlip::run() { // if pilot inputs roll > 40deg or timeout occurs abandon flip if (!motors->armed() || (abs(channel_roll->get_control_in()) >= 4000) || (abs(channel_pitch->get_control_in()) >= 4000) || ((millis() - start_time_ms) > FLIP_TIMEOUT_MS)) { diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 85e9273587..308ccc24dd 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -8,7 +8,7 @@ without rangefinder */ -const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = { +const AP_Param::GroupInfo ModeFlowHold::var_info[] = { // @Param: _XY_P // @DisplayName: FlowHold P gain // @Description: FlowHold (horizontal) P gain. @@ -37,14 +37,14 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = { // @Range: 0 100 // @Units: Hz // @User: Advanced - AP_SUBGROUPINFO(flow_pi_xy, "_XY_", 1, Copter::ModeFlowHold, AC_PI_2D), + AP_SUBGROUPINFO(flow_pi_xy, "_XY_", 1, ModeFlowHold, AC_PI_2D), // @Param: _FLOW_MAX // @DisplayName: FlowHold Flow Rate Max // @Description: Controls maximum apparent flow rate in flowhold // @Range: 0.1 2.5 // @User: Standard - AP_GROUPINFO("_FLOW_MAX", 2, Copter::ModeFlowHold, flow_max, 0.6), + AP_GROUPINFO("_FLOW_MAX", 2, ModeFlowHold, flow_max, 0.6), // @Param: _FILT_HZ // @DisplayName: FlowHold Filter Frequency @@ -52,14 +52,14 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = { // @Range: 1 100 // @Units: Hz // @User: Standard - AP_GROUPINFO("_FILT_HZ", 3, Copter::ModeFlowHold, flow_filter_hz, 5), + AP_GROUPINFO("_FILT_HZ", 3, ModeFlowHold, flow_filter_hz, 5), // @Param: _QUAL_MIN // @DisplayName: FlowHold Flow quality minimum // @Description: Minimum flow quality to use flow position hold // @Range: 0 255 // @User: Standard - AP_GROUPINFO("_QUAL_MIN", 4, Copter::ModeFlowHold, flow_min_quality, 10), + AP_GROUPINFO("_QUAL_MIN", 4, ModeFlowHold, flow_min_quality, 10), // 5 was FLOW_SPEED @@ -69,12 +69,12 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = { // @Range: 1 30 // @User: Standard // @Units: deg/s - AP_GROUPINFO("_BRAKE_RATE", 6, Copter::ModeFlowHold, brake_rate_dps, 8), + AP_GROUPINFO("_BRAKE_RATE", 6, ModeFlowHold, brake_rate_dps, 8), AP_GROUPEND }; -Copter::ModeFlowHold::ModeFlowHold(void) : Mode() +ModeFlowHold::ModeFlowHold(void) : Mode() { AP_Param::setup_object_defaults(this, var_info); } @@ -82,7 +82,7 @@ Copter::ModeFlowHold::ModeFlowHold(void) : Mode() #define CONTROL_FLOWHOLD_EARTH_FRAME 0 // flowhold_init - initialise flowhold controller -bool Copter::ModeFlowHold::init(bool ignore_checks) +bool ModeFlowHold::init(bool ignore_checks) { if (!copter.optflow.enabled() || !copter.optflow.healthy()) { return false; @@ -116,7 +116,7 @@ bool Copter::ModeFlowHold::init(bool ignore_checks) /* calculate desired attitude from flow sensor. Called when flow sensor is healthy */ -void Copter::ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stick_input) +void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stick_input) { uint32_t now = AP_HAL::millis(); @@ -215,7 +215,7 @@ void Copter::ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stic // flowhold_run - runs the flowhold controller // should be called at 100hz or more -void Copter::ModeFlowHold::run() +void ModeFlowHold::run() { float takeoff_climb_rate = 0.0f; @@ -342,7 +342,7 @@ void Copter::ModeFlowHold::run() /* update height estimate using integrated accelerometer ratio with optical flow */ -void Copter::ModeFlowHold::update_height_estimate(void) +void ModeFlowHold::update_height_estimate(void) { float ins_height = copter.inertial_nav.get_altitude() * 0.01; diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index b123c241af..a770829fcf 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -14,17 +14,17 @@ */ // initialise follow mode -bool Copter::ModeFollow::init(const bool ignore_checks) +bool ModeFollow::init(const bool ignore_checks) { if (!g2.follow.enabled()) { gcs().send_text(MAV_SEVERITY_WARNING, "Set FOLL_ENABLE = 1"); return false; } // re-use guided mode - return Copter::ModeGuided::init(ignore_checks); + return ModeGuided::init(ignore_checks); } -void Copter::ModeFollow::run() +void ModeFollow::run() { // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { @@ -148,17 +148,17 @@ void Copter::ModeFollow::run() last_log_ms = now; } // re-use guided mode's velocity controller (takes NEU) - Copter::ModeGuided::set_velocity(desired_velocity_neu_cms, use_yaw, yaw_cd, false, 0.0f, false, log_request); + ModeGuided::set_velocity(desired_velocity_neu_cms, use_yaw, yaw_cd, false, 0.0f, false, log_request); - Copter::ModeGuided::run(); + ModeGuided::run(); } -uint32_t Copter::ModeFollow::wp_distance() const +uint32_t ModeFollow::wp_distance() const { return g2.follow.get_distance_to_target() * 100; } -int32_t Copter::ModeFollow::wp_bearing() const +int32_t ModeFollow::wp_bearing() const { return g2.follow.get_bearing_to_target() * 100; } @@ -166,7 +166,7 @@ int32_t Copter::ModeFollow::wp_bearing() const /* get target position for mavlink reporting */ -bool Copter::ModeFollow::get_wp(Location &loc) +bool ModeFollow::get_wp(Location &loc) { float dist = g2.follow.get_distance_to_target(); float bearing = g2.follow.get_bearing_to_target(); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 7dbf373261..f3ad2d44ef 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -38,7 +38,7 @@ struct Guided_Limit { } guided_limit; // guided_init - initialise guided controller -bool Copter::ModeGuided::init(bool ignore_checks) +bool ModeGuided::init(bool ignore_checks) { // start in position control mode pos_control_start(); @@ -47,7 +47,7 @@ bool Copter::ModeGuided::init(bool ignore_checks) // do_user_takeoff_start - initialises waypoint controller to implement take-off -bool Copter::ModeGuided::do_user_takeoff_start(float final_alt_above_home) +bool ModeGuided::do_user_takeoff_start(float final_alt_above_home) { guided_mode = Guided_TakeOff; @@ -75,7 +75,7 @@ bool Copter::ModeGuided::do_user_takeoff_start(float final_alt_above_home) } // initialise guided mode's position controller -void Copter::ModeGuided::pos_control_start() +void ModeGuided::pos_control_start() { // set to position control mode guided_mode = Guided_WP; @@ -95,7 +95,7 @@ void Copter::ModeGuided::pos_control_start() } // initialise guided mode's velocity controller -void Copter::ModeGuided::vel_control_start() +void ModeGuided::vel_control_start() { // set guided_mode to velocity controller guided_mode = Guided_Velocity; @@ -113,7 +113,7 @@ void Copter::ModeGuided::vel_control_start() } // initialise guided mode's posvel controller -void Copter::ModeGuided::posvel_control_start() +void ModeGuided::posvel_control_start() { // set guided_mode to velocity controller guided_mode = Guided_PosVel; @@ -139,13 +139,13 @@ void Copter::ModeGuided::posvel_control_start() auto_yaw.set_mode(AUTO_YAW_HOLD); } -bool Copter::ModeGuided::is_taking_off() const +bool ModeGuided::is_taking_off() const { return guided_mode == Guided_TakeOff; } // initialise guided mode's angle controller -void Copter::ModeGuided::angle_control_start() +void ModeGuided::angle_control_start() { // set guided_mode to velocity controller guided_mode = Guided_Angle; @@ -176,7 +176,7 @@ void Copter::ModeGuided::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::ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +bool ModeGuided::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) { @@ -204,7 +204,7 @@ bool Copter::ModeGuided::set_destination(const Vector3f& destination, bool use_y return true; } -bool Copter::ModeGuided::get_wp(Location& destination) +bool ModeGuided::get_wp(Location& destination) { if (guided_mode != Guided_WP) { return false; @@ -215,7 +215,7 @@ bool Copter::ModeGuided::get_wp(Location& destination) // 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::ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +bool ModeGuided::set_destination(const Location& 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) { @@ -248,7 +248,7 @@ bool Copter::ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, } // guided_set_velocity - sets guided mode's target velocity -void Copter::ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request) +void ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request) { // check we are in velocity control mode if (guided_mode != Guided_Velocity) { @@ -269,7 +269,7 @@ void Copter::ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, fl } // set guided mode posvel target -bool Copter::ModeGuided::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 ModeGuided::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) { @@ -301,7 +301,7 @@ bool Copter::ModeGuided::set_destination_posvel(const Vector3f& destination, con } // set guided mode angle target -void Copter::ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads) +void ModeGuided::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) { @@ -320,7 +320,7 @@ void Copter::ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bo guided_angle_state.update_time_ms = millis(); // interpret positive climb rate as triggering take-off - if (motors->armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) { + if (motors->armed() && !copter.ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) { copter.set_auto_armed(true); } @@ -332,7 +332,7 @@ void Copter::ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bo // guided_run - runs the guided controller // should be called at 100hz or more -void Copter::ModeGuided::run() +void ModeGuided::run() { // call the correct auto controller switch (guided_mode) { @@ -366,7 +366,7 @@ void Copter::ModeGuided::run() // guided_takeoff_run - takeoff in guided mode // called by guided_run at 100hz or more -void Copter::ModeGuided::takeoff_run() +void ModeGuided::takeoff_run() { auto_takeoff_run(); if (wp_nav->reached_wp_destination()) { @@ -375,10 +375,10 @@ void Copter::ModeGuided::takeoff_run() } } -void Copter::Mode::auto_takeoff_run() +void Mode::auto_takeoff_run() { // if not armed set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed) { + if (!motors->armed() || !copter.ap.auto_armed) { make_safe_spool_down(); wp_nav->shift_wp_origin_to_current_pos(); return; @@ -413,7 +413,7 @@ void Copter::Mode::auto_takeoff_run() // guided_pos_control_run - runs the guided position controller // called from guided_run -void Copter::ModeGuided::pos_control_run() +void ModeGuided::pos_control_run() { // process pilot's yaw input float target_yaw_rate = 0; @@ -455,7 +455,7 @@ void Copter::ModeGuided::pos_control_run() // guided_vel_control_run - runs the guided velocity controller // called from guided_run -void Copter::ModeGuided::vel_control_run() +void ModeGuided::vel_control_run() { // process pilot's yaw input float target_yaw_rate = 0; @@ -507,7 +507,7 @@ void Copter::ModeGuided::vel_control_run() // guided_posvel_control_run - runs the guided spline controller // called from guided_run -void Copter::ModeGuided::posvel_control_run() +void ModeGuided::posvel_control_run() { // process pilot's yaw input float target_yaw_rate = 0; @@ -572,7 +572,7 @@ void Copter::ModeGuided::posvel_control_run() // guided_angle_control_run - runs the guided angle controller // called from guided_run -void Copter::ModeGuided::angle_control_run() +void ModeGuided::angle_control_run() { // constrain desired lean angles float roll_in = guided_angle_state.roll_cd; @@ -605,14 +605,14 @@ void Copter::ModeGuided::angle_control_run() } // if not armed set throttle to zero and exit immediately - if (!motors->armed() || !ap.auto_armed || (ap.land_complete && (guided_angle_state.climb_rate_cms <= 0.0f))) { + if (!motors->armed() || !copter.ap.auto_armed || (copter.ap.land_complete && (guided_angle_state.climb_rate_cms <= 0.0f))) { make_safe_spool_down(); return; } // TODO: use get_alt_hold_state // landed with positive desired climb rate, takeoff - if (ap.land_complete && (guided_angle_state.climb_rate_cms > 0.0f)) { + if (copter.ap.land_complete && (guided_angle_state.climb_rate_cms > 0.0f)) { zero_throttle_and_relax_ac(); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) { @@ -638,7 +638,7 @@ void Copter::ModeGuided::angle_control_run() } // helper function to update position controller's desired velocity while respecting acceleration limits -void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des) +void ModeGuided::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(); @@ -672,7 +672,7 @@ void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const } // helper function to set yaw state and targets -void Copter::ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle) +void ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle) { if (use_yaw) { auto_yaw.set_fixed_yaw(yaw_cd * 0.01f, 0.0f, 0, relative_angle); @@ -684,7 +684,7 @@ void Copter::ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_ // Guided Limit code // guided_limit_clear - clear/turn off guided limits -void Copter::ModeGuided::limit_clear() +void ModeGuided::limit_clear() { guided_limit.timeout_ms = 0; guided_limit.alt_min_cm = 0.0f; @@ -693,7 +693,7 @@ void Copter::ModeGuided::limit_clear() } // guided_limit_set - set guided timeout and movement limits -void Copter::ModeGuided::limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm) +void ModeGuided::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; @@ -703,7 +703,7 @@ void Copter::ModeGuided::limit_set(uint32_t timeout_ms, float alt_min_cm, float // 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::ModeGuided::limit_init_time_and_pos() +void ModeGuided::limit_init_time_and_pos() { // initialise start time guided_limit.start_time = AP_HAL::millis(); @@ -714,7 +714,7 @@ void Copter::ModeGuided::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::ModeGuided::limit_check() +bool ModeGuided::limit_check() { // check if we have passed the timeout if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) { @@ -747,7 +747,7 @@ bool Copter::ModeGuided::limit_check() } -uint32_t Copter::ModeGuided::wp_distance() const +uint32_t ModeGuided::wp_distance() const { switch(mode()) { case Guided_WP: @@ -761,7 +761,7 @@ uint32_t Copter::ModeGuided::wp_distance() const } } -int32_t Copter::ModeGuided::wp_bearing() const +int32_t ModeGuided::wp_bearing() const { switch(mode()) { case Guided_WP: @@ -775,7 +775,7 @@ int32_t Copter::ModeGuided::wp_bearing() const } } -float Copter::ModeGuided::crosstrack_error() const +float ModeGuided::crosstrack_error() const { if (mode() == Guided_WP) { return wp_nav->crosstrack_error(); diff --git a/ArduCopter/mode_guided_nogps.cpp b/ArduCopter/mode_guided_nogps.cpp index 994589f90c..b74aaf8f2b 100644 --- a/ArduCopter/mode_guided_nogps.cpp +++ b/ArduCopter/mode_guided_nogps.cpp @@ -7,19 +7,19 @@ */ // initialise guided_nogps controller -bool Copter::ModeGuidedNoGPS::init(bool ignore_checks) +bool ModeGuidedNoGPS::init(bool ignore_checks) { // start in angle control mode - Copter::ModeGuided::angle_control_start(); + ModeGuided::angle_control_start(); return true; } // guided_run - runs the guided controller // should be called at 100hz or more -void Copter::ModeGuidedNoGPS::run() +void ModeGuidedNoGPS::run() { // run angle controller - Copter::ModeGuided::angle_control_run(); + ModeGuided::angle_control_run(); } #endif diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index d4dbb7b957..b8bbcb2227 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -7,7 +7,7 @@ static uint32_t land_start_time; static bool land_pause; // land_init - initialise land controller -bool Copter::ModeLand::init(bool ignore_checks) +bool ModeLand::init(bool ignore_checks) { // check if we have GPS and decide which LAND we're going to do land_with_gps = copter.position_ok(); @@ -32,14 +32,14 @@ bool Copter::ModeLand::init(bool ignore_checks) land_pause = false; // reset flag indicating if pilot has applied roll or pitch inputs during landing - ap.land_repo_active = false; + copter.ap.land_repo_active = false; return true; } // land_run - runs the land controller // should be called at 100hz or more -void Copter::ModeLand::run() +void ModeLand::run() { if (land_with_gps) { gps_run(); @@ -51,10 +51,10 @@ void Copter::ModeLand::run() // land_gps_run - runs the land controller // horizontal position controlled with loiter controller // should be called at 100hz or more -void Copter::ModeLand::gps_run() +void ModeLand::gps_run() { // disarm when the landing detector says we've landed - if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { + if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { copter.arming.disarm(); } @@ -79,7 +79,7 @@ void Copter::ModeLand::gps_run() // land_nogps_run - runs the land controller // pilot controls roll and pitch angles // should be called at 100hz or more -void Copter::ModeLand::nogps_run() +void ModeLand::nogps_run() { float target_roll = 0.0f, target_pitch = 0.0f; float target_yaw_rate = 0; @@ -108,7 +108,7 @@ void Copter::ModeLand::nogps_run() } // disarm when the landing detector says we've landed - if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { + if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { copter.arming.disarm(); } @@ -134,7 +134,7 @@ void Copter::ModeLand::nogps_run() // 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::ModeLand::do_not_use_GPS() +void ModeLand::do_not_use_GPS() { land_with_gps = false; } diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 9a51a9c477..5a51eadb79 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -7,7 +7,7 @@ */ // loiter_init - initialise loiter controller -bool Copter::ModeLoiter::init(bool ignore_checks) +bool ModeLoiter::init(bool ignore_checks) { if (!copter.failsafe.radio) { float target_roll, target_pitch; @@ -35,12 +35,12 @@ bool Copter::ModeLoiter::init(bool ignore_checks) } #if PRECISION_LANDING == ENABLED -bool Copter::ModeLoiter::do_precision_loiter() +bool ModeLoiter::do_precision_loiter() { if (!_precision_loiter_enabled) { return false; } - if (ap.land_complete_maybe) { + if (copter.ap.land_complete_maybe) { return false; // don't move on the ground } // if the pilot *really* wants to move the vehicle, let them.... @@ -53,7 +53,7 @@ bool Copter::ModeLoiter::do_precision_loiter() return true; } -void Copter::ModeLoiter::precision_loiter_xy() +void ModeLoiter::precision_loiter_xy() { loiter_nav->clear_pilot_desired_acceleration(); Vector2f target_pos, target_vel_rel; @@ -72,7 +72,7 @@ void Copter::ModeLoiter::precision_loiter_xy() // loiter_run - runs the loiter controller // should be called at 100hz or more -void Copter::ModeLoiter::run() +void ModeLoiter::run() { float target_roll, target_pitch; float target_yaw_rate = 0.0f; @@ -106,7 +106,7 @@ void Copter::ModeLoiter::run() } // relax loiter target if we might be landed - if (ap.land_complete_maybe) { + if (copter.ap.land_complete_maybe) { loiter_nav->soften_for_landing(); } @@ -194,12 +194,12 @@ void Copter::ModeLoiter::run() } } -uint32_t Copter::ModeLoiter::wp_distance() const +uint32_t ModeLoiter::wp_distance() const { return loiter_nav->get_distance_to_target(); } -int32_t Copter::ModeLoiter::wp_bearing() const +int32_t ModeLoiter::wp_bearing() const { return loiter_nav->get_bearing_to_target(); } diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index ec5024d6b6..1af1442738 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -24,7 +24,7 @@ #define POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX 10 // wind compensation estimates will only run when velocity is at or below this speed in cm/s // poshold_init - initialise PosHold controller -bool Copter::ModePosHold::init(bool ignore_checks) +bool ModePosHold::init(bool ignore_checks) { // initialize vertical speeds and acceleration pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); @@ -43,7 +43,7 @@ bool Copter::ModePosHold::init(bool ignore_checks) // compute brake_gain brake_gain = (15.0f * (float)g.poshold_brake_rate + 95.0f) / 100.0f; - if (ap.land_complete) { + if (copter.ap.land_complete) { // if landed begin in loiter mode roll_mode = RPMode::LOITER; pitch_mode = RPMode::LOITER; @@ -68,7 +68,7 @@ bool Copter::ModePosHold::init(bool ignore_checks) // poshold_run - runs the PosHold controller // should be called at 100hz or more -void Copter::ModePosHold::run() +void ModePosHold::run() { float takeoff_climb_rate = 0.0f; float brake_to_loiter_mix; // mix of brake and loiter controls. 0 = fully brake controls, 1 = fully loiter controls @@ -96,7 +96,7 @@ void Copter::ModePosHold::run() target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); // relax loiter target if we might be landed - if (ap.land_complete_maybe) { + if (copter.ap.land_complete_maybe) { loiter_nav->soften_for_landing(); } @@ -508,7 +508,7 @@ void Copter::ModePosHold::run() } // poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received -void Copter::ModePosHold::update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw) +void ModePosHold::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)) { @@ -530,7 +530,7 @@ void Copter::ModePosHold::update_pilot_lean_angle(float &lean_angle_filtered, fl // 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::ModePosHold::mix_controls(float mix_ratio, int16_t first_control, int16_t second_control) +int16_t ModePosHold::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)); @@ -539,7 +539,7 @@ int16_t Copter::ModePosHold::mix_controls(float mix_ratio, int16_t first_control // 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::ModePosHold::update_brake_angle_from_velocity(int16_t &brake_angle, float velocity) +void ModePosHold::update_brake_angle_from_velocity(int16_t &brake_angle, float velocity) { float lean_angle; int16_t brake_rate = g.poshold_brake_rate; @@ -565,7 +565,7 @@ void Copter::ModePosHold::update_brake_angle_from_velocity(int16_t &brake_angle, // update_wind_comp_estimate - updates wind compensation estimate // should be called at the maximum loop rate when loiter is engaged -void Copter::ModePosHold::update_wind_comp_estimate() +void ModePosHold::update_wind_comp_estimate() { // check wind estimate start has not been delayed if (wind_comp_start_timer > 0) { @@ -601,7 +601,7 @@ void Copter::ModePosHold::update_wind_comp_estimate() // 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::ModePosHold::get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle) +void ModePosHold::get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle) { // reduce rate to 10hz wind_comp_timer++; @@ -616,7 +616,7 @@ void Copter::ModePosHold::get_wind_comp_lean_angles(int16_t &roll_angle, int16_t } // roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis -void Copter::ModePosHold::roll_controller_to_pilot_override() +void ModePosHold::roll_controller_to_pilot_override() { roll_mode = RPMode::CONTROLLER_TO_PILOT_OVERRIDE; controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; @@ -627,7 +627,7 @@ void Copter::ModePosHold::roll_controller_to_pilot_override() } // pitch_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis -void Copter::ModePosHold::pitch_controller_to_pilot_override() +void ModePosHold::pitch_controller_to_pilot_override() { pitch_mode = RPMode::CONTROLLER_TO_PILOT_OVERRIDE; controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 9c6d23832d..0c72a76122 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -10,7 +10,7 @@ */ // rtl_init - initialise rtl controller -bool Copter::ModeRTL::init(bool ignore_checks) +bool ModeRTL::init(bool ignore_checks) { if (!ignore_checks) { if (!AP::ahrs().home_is_set()) { @@ -26,7 +26,7 @@ bool Copter::ModeRTL::init(bool ignore_checks) } // re-start RTL with terrain following disabled -void Copter::ModeRTL::restart_without_terrain() +void ModeRTL::restart_without_terrain() { AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RESTARTED_RTL); if (rtl_path.terrain_used) { @@ -39,7 +39,7 @@ void Copter::ModeRTL::restart_without_terrain() // rtl_run - runs the return-to-launch controller // should be called at 100hz or more -void Copter::ModeRTL::run(bool disarm_on_land) +void ModeRTL::run(bool disarm_on_land) { if (!motors->armed()) { return; @@ -105,7 +105,7 @@ void Copter::ModeRTL::run(bool disarm_on_land) } // rtl_climb_start - initialise climb to RTL altitude -void Copter::ModeRTL::climb_start() +void ModeRTL::climb_start() { _state = RTL_InitialClimb; _state_complete = false; @@ -129,7 +129,7 @@ void Copter::ModeRTL::climb_start() } // rtl_return_start - initialise return to home -void Copter::ModeRTL::return_start() +void ModeRTL::return_start() { _state = RTL_ReturnHome; _state_complete = false; @@ -145,7 +145,7 @@ void Copter::ModeRTL::return_start() // rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller // called by rtl_run at 100hz or more -void Copter::ModeRTL::climb_return_run() +void ModeRTL::climb_return_run() { // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { @@ -186,7 +186,7 @@ void Copter::ModeRTL::climb_return_run() } // rtl_loiterathome_start - initialise return to home -void Copter::ModeRTL::loiterathome_start() +void ModeRTL::loiterathome_start() { _state = RTL_LoiterAtHome; _state_complete = false; @@ -202,7 +202,7 @@ void Copter::ModeRTL::loiterathome_start() // rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller // called by rtl_run at 100hz or more -void Copter::ModeRTL::loiterathome_run() +void ModeRTL::loiterathome_run() { // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { @@ -253,7 +253,7 @@ void Copter::ModeRTL::loiterathome_run() } // rtl_descent_start - initialise descent to final alt -void Copter::ModeRTL::descent_start() +void ModeRTL::descent_start() { _state = RTL_FinalDescent; _state_complete = false; @@ -270,7 +270,7 @@ void Copter::ModeRTL::descent_start() // rtl_descent_run - implements the final descent to the RTL_ALT // called by rtl_run at 100hz or more -void Copter::ModeRTL::descent_run() +void ModeRTL::descent_run() { float target_roll = 0.0f; float target_pitch = 0.0f; @@ -301,10 +301,10 @@ void Copter::ModeRTL::descent_run() // record if pilot has overridden roll or pitch if (!is_zero(target_roll) || !is_zero(target_pitch)) { - if (!ap.land_repo_active) { + if (!copter.ap.land_repo_active) { copter.Log_Write_Event(DATA_LAND_REPO_ACTIVE); } - ap.land_repo_active = true; + copter.ap.land_repo_active = true; } } @@ -333,7 +333,7 @@ void Copter::ModeRTL::descent_run() } // rtl_loiterathome_start - initialise controllers to loiter over home -void Copter::ModeRTL::land_start() +void ModeRTL::land_start() { _state = RTL_Land; _state_complete = false; @@ -351,12 +351,12 @@ void Copter::ModeRTL::land_start() auto_yaw.set_mode(AUTO_YAW_HOLD); } -bool Copter::ModeRTL::is_landing() const +bool ModeRTL::is_landing() const { return _state == RTL_Land; } -bool Copter::ModeRTL::landing_gear_should_be_deployed() const +bool ModeRTL::landing_gear_should_be_deployed() const { switch(_state) { case RTL_LoiterAtHome: @@ -371,13 +371,13 @@ bool Copter::ModeRTL::landing_gear_should_be_deployed() const // rtl_returnhome_run - return home // called by rtl_run at 100hz or more -void Copter::ModeRTL::land_run(bool disarm_on_land) +void ModeRTL::land_run(bool disarm_on_land) { // check if we've completed this stage of RTL - _state_complete = ap.land_complete; + _state_complete = copter.ap.land_complete; // disarm when the landing detector says we've landed - if (disarm_on_land && ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { + if (disarm_on_land && copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { copter.arming.disarm(); } @@ -395,7 +395,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land) land_run_vertical_control(); } -void Copter::ModeRTL::build_path() +void ModeRTL::build_path() { // origin point is our stopping point Vector3f stopping_point; @@ -420,7 +420,7 @@ void Copter::ModeRTL::build_path() // compute the return target - home or rally point // return altitude in cm above home at which vehicle should return home // return target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set) -void Copter::ModeRTL::compute_return_target() +void ModeRTL::compute_return_target() { // set return target to nearest rally point or home position (Note: alt is absolute) #if AC_RALLY == ENABLED @@ -495,12 +495,12 @@ void Copter::ModeRTL::compute_return_target() rtl_path.return_target.alt = MAX(rtl_path.return_target.alt, curr_alt); } -uint32_t Copter::ModeRTL::wp_distance() const +uint32_t ModeRTL::wp_distance() const { return wp_nav->get_wp_distance_to_destination(); } -int32_t Copter::ModeRTL::wp_bearing() const +int32_t ModeRTL::wp_bearing() const { return wp_nav->get_wp_bearing_to_destination(); } diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index 7ebf231caf..6bb4867a27 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -9,7 +9,7 @@ * Once the copter is close to home, it will run a standard land controller. */ -bool Copter::ModeSmartRTL::init(bool ignore_checks) +bool ModeSmartRTL::init(bool ignore_checks) { if (g2.smart_rtl.is_active()) { // initialise waypoint and spline controller @@ -33,12 +33,12 @@ bool Copter::ModeSmartRTL::init(bool ignore_checks) } // perform cleanup required when leaving smart_rtl -void Copter::ModeSmartRTL::exit() +void ModeSmartRTL::exit() { g2.smart_rtl.cancel_request_for_thorough_cleanup(); } -void Copter::ModeSmartRTL::run() +void ModeSmartRTL::run() { switch (smart_rtl_state) { case SmartRTL_WaitForPathCleanup: @@ -59,7 +59,7 @@ void Copter::ModeSmartRTL::run() } } -void Copter::ModeSmartRTL::wait_cleanup_run() +void ModeSmartRTL::wait_cleanup_run() { // hover at current target position motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); @@ -73,7 +73,7 @@ void Copter::ModeSmartRTL::wait_cleanup_run() } } -void Copter::ModeSmartRTL::path_follow_run() +void ModeSmartRTL::path_follow_run() { float target_yaw_rate = 0.0f; if (!copter.failsafe.radio) { @@ -119,7 +119,7 @@ void Copter::ModeSmartRTL::path_follow_run() } } -void Copter::ModeSmartRTL::pre_land_position_run() +void ModeSmartRTL::pre_land_position_run() { // if we are close to 2m above start point, we are ready to land. if (wp_nav->reached_wp_destination()) { @@ -142,19 +142,19 @@ void Copter::ModeSmartRTL::pre_land_position_run() } // save current position for use by the smart_rtl flight mode -void Copter::ModeSmartRTL::save_position() +void ModeSmartRTL::save_position() { const bool should_save_position = motors->armed() && (copter.control_mode != SMART_RTL); copter.g2.smart_rtl.update(copter.position_ok(), should_save_position); } -uint32_t Copter::ModeSmartRTL::wp_distance() const +uint32_t ModeSmartRTL::wp_distance() const { return wp_nav->get_wp_distance_to_destination(); } -int32_t Copter::ModeSmartRTL::wp_bearing() const +int32_t ModeSmartRTL::wp_bearing() const { return wp_nav->get_wp_bearing_to_destination(); } diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index cbca35f5fd..9ada89d009 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -7,7 +7,7 @@ */ // sport_init - initialise sport controller -bool Copter::ModeSport::init(bool ignore_checks) +bool ModeSport::init(bool ignore_checks) { // initialize vertical speed and acceleration pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); @@ -24,7 +24,7 @@ bool Copter::ModeSport::init(bool ignore_checks) // sport_run - runs the sport controller // should be called at 100hz or more -void Copter::ModeSport::run() +void ModeSport::run() { float takeoff_climb_rate = 0.0f; diff --git a/ArduCopter/mode_stabilize.cpp b/ArduCopter/mode_stabilize.cpp index 4fde8d5d91..49714f3d33 100644 --- a/ArduCopter/mode_stabilize.cpp +++ b/ArduCopter/mode_stabilize.cpp @@ -6,7 +6,7 @@ // stabilize_run - runs the main stabilize controller // should be called at 100hz or more -void Copter::ModeStabilize::run() +void ModeStabilize::run() { // apply simple mode transform to pilot inputs update_simple_mode(); @@ -21,7 +21,7 @@ void Copter::ModeStabilize::run() if (!motors->armed()) { // Motors should be Stopped motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); - } else if (ap.throttle_zero) { + } else if (copter.ap.throttle_zero) { // Attempting to Land motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); } else { diff --git a/ArduCopter/mode_stabilize_heli.cpp b/ArduCopter/mode_stabilize_heli.cpp index be0b9dde56..633d0396c0 100644 --- a/ArduCopter/mode_stabilize_heli.cpp +++ b/ArduCopter/mode_stabilize_heli.cpp @@ -6,7 +6,7 @@ */ // stabilize_init - initialise stabilize controller -bool Copter::ModeStabilize_Heli::init(bool ignore_checks) +bool ModeStabilize_Heli::init(bool ignore_checks) { // set stab collective true to use stabilize scaled collective pitch range copter.input_manager.set_use_stab_col(true); @@ -16,7 +16,7 @@ bool Copter::ModeStabilize_Heli::init(bool ignore_checks) // stabilize_run - runs the main stabilize controller // should be called at 100hz or more -void Copter::ModeStabilize_Heli::run() +void ModeStabilize_Heli::run() { float target_roll, target_pitch; float target_yaw_rate; diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index 44899f0c11..b9520185a9 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -3,7 +3,7 @@ #if MODE_THROW_ENABLED == ENABLED // throw_init - initialise throw controller -bool Copter::ModeThrow::init(bool ignore_checks) +bool ModeThrow::init(bool ignore_checks) { #if FRAME_CONFIG == HELI_FRAME // do not allow helis to use throw to start @@ -24,7 +24,7 @@ bool Copter::ModeThrow::init(bool ignore_checks) // runs the throw to start controller // should be called at 100hz or more -void Copter::ModeThrow::run() +void ModeThrow::run() { /* Throw State Machine Throw_Disarmed - motors are off @@ -216,7 +216,7 @@ void Copter::ModeThrow::run() } } -bool Copter::ModeThrow::throw_detected() +bool ModeThrow::throw_detected() { // Check that we have a valid navigation solution nav_filter_status filt_status = inertial_nav.get_filter_status(); @@ -261,20 +261,20 @@ bool Copter::ModeThrow::throw_detected() } } -bool Copter::ModeThrow::throw_attitude_good() +bool ModeThrow::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::ModeThrow::throw_height_good() +bool ModeThrow::throw_height_good() { // Check that we are within 0.5m of the demanded height return (pos_control->get_alt_error() < 50.0f); } -bool Copter::ModeThrow::throw_position_good() +bool ModeThrow::throw_position_good() { // check that our horizontal position error is within 50cm return (pos_control->get_horizontal_error() < 50.0f); diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 558ad03ee2..7aa5ebc81a 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -9,7 +9,7 @@ #define ZIGZAG_WP_RADIUS_CM 300 // initialise zigzag controller -bool Copter::ModeZigZag::init(bool ignore_checks) +bool ModeZigZag::init(bool ignore_checks) { // initialize's loiter position and velocity on xy-axes from current pos and velocity loiter_nav->clear_pilot_desired_acceleration(); @@ -31,7 +31,7 @@ bool Copter::ModeZigZag::init(bool ignore_checks) // run the zigzag controller // should be called at 100hz or more -void Copter::ModeZigZag::run() +void ModeZigZag::run() { // initialize vertical speed and acceleration's range pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); @@ -62,7 +62,7 @@ void Copter::ModeZigZag::run() } // save current position as A (dest_num = 0) or B (dest_num = 1). If both A and B have been saved move to the one specified -void Copter::ModeZigZag::save_or_move_to_destination(uint8_t dest_num) +void ModeZigZag::save_or_move_to_destination(uint8_t dest_num) { // sanity check if (dest_num > 1) { @@ -117,7 +117,7 @@ void Copter::ModeZigZag::save_or_move_to_destination(uint8_t dest_num) } // return manual control to the pilot -void Copter::ModeZigZag::return_to_manual_control(bool maintain_target) +void ModeZigZag::return_to_manual_control(bool maintain_target) { if (stage == AUTO) { stage = MANUAL_REGAIN; @@ -132,7 +132,7 @@ void Copter::ModeZigZag::return_to_manual_control(bool maintain_target) } // fly the vehicle to closest point on line perpendicular to dest_A or dest_B -void Copter::ModeZigZag::auto_control() +void ModeZigZag::auto_control() { // process pilot's yaw input float target_yaw_rate = 0; @@ -161,7 +161,7 @@ void Copter::ModeZigZag::auto_control() } // manual_control - process manual control -void Copter::ModeZigZag::manual_control() +void ModeZigZag::manual_control() { float target_yaw_rate = 0.0f; float target_climb_rate = 0.0f; @@ -213,7 +213,7 @@ void Copter::ModeZigZag::manual_control() } // return true if vehicle is within a small area around the destination -bool Copter::ModeZigZag::reached_destination() +bool ModeZigZag::reached_destination() { // check if wp_nav believes it has reached the destination if (!wp_nav->reached_wp_destination()) { @@ -236,7 +236,7 @@ bool Copter::ModeZigZag::reached_destination() // calculate next destination according to vector A-B and current position // use_wpnav_alt should be true if waypoint controller's altitude target should be used, false for position control or current altitude target // terrain_alt is returned as true if the next_dest should be considered a terrain alt -bool Copter::ModeZigZag::calculate_next_dest(uint8_t dest_num, bool use_wpnav_alt, Vector3f& next_dest, bool& terrain_alt) const +bool ModeZigZag::calculate_next_dest(uint8_t dest_num, bool use_wpnav_alt, Vector3f& next_dest, bool& terrain_alt) const { // sanity check dest_num if (dest_num > 1) { diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 7f92d78136..4840530adf 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -1,25 +1,25 @@ #include "Copter.h" -Copter::Mode::_TakeOff Copter::Mode::takeoff; +Mode::_TakeOff Mode::takeoff; // This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes. // The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude // A safe takeoff speed is calculated and used to calculate a time_ms // the pos_control target is then slowly increased until time_ms expires -bool Copter::Mode::do_user_takeoff_start(float takeoff_alt_cm) +bool Mode::do_user_takeoff_start(float takeoff_alt_cm) { copter.flightmode->takeoff.start(takeoff_alt_cm); return true; } // initiate user takeoff - called when MAVLink TAKEOFF command is received -bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) +bool Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) { if (!copter.motors->armed()) { return false; } - if (!ap.land_complete) { + if (!copter.ap.land_complete) { // can't takeoff again! return false; } @@ -33,7 +33,7 @@ bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) } // Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning - if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED && ap.using_interlock) { + if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED && copter.ap.using_interlock) { return false; } @@ -46,7 +46,7 @@ bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) } // start takeoff to specified altitude above home in centimeters -void Copter::Mode::_TakeOff::start(float alt_cm) +void Mode::_TakeOff::start(float alt_cm) { // indicate we are taking off copter.set_land_complete(false); @@ -69,7 +69,7 @@ void Copter::Mode::_TakeOff::start(float alt_cm) } // stop takeoff -void Copter::Mode::_TakeOff::stop() +void Mode::_TakeOff::stop() { _running = false; start_ms = 0; @@ -79,7 +79,7 @@ void Copter::Mode::_TakeOff::stop() // pilot_climb_rate is both an input and an output // takeoff_climb_rate is only an output // has side-effect of turning takeoff off when timeout as expired -void Copter::Mode::_TakeOff::get_climb_rates(float& pilot_climb_rate, +void Mode::_TakeOff::get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate) { // return pilot_climb_rate if take-off inactive @@ -138,7 +138,7 @@ void Copter::Mode::_TakeOff::get_climb_rates(float& pilot_climb_rate, } } -void Copter::Mode::auto_takeoff_set_start_alt(void) +void Mode::auto_takeoff_set_start_alt(void) { // start with our current altitude auto_takeoff_no_nav_alt_cm = inertial_nav.get_altitude(); @@ -154,7 +154,7 @@ void Copter::Mode::auto_takeoff_set_start_alt(void) call attitude controller for automatic takeoff, limiting roll/pitch if below wp_navalt_min */ -void Copter::Mode::auto_takeoff_attitude_run(float target_yaw_rate) +void Mode::auto_takeoff_attitude_run(float target_yaw_rate) { float nav_roll, nav_pitch; @@ -173,12 +173,12 @@ void Copter::Mode::auto_takeoff_attitude_run(float target_yaw_rate) attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate); } -bool Copter::Mode::is_taking_off() const +bool Mode::is_taking_off() const { if (!has_user_takeoff(false)) { return false; } - if (ap.land_complete) { + if (copter.ap.land_complete) { return false; } if (takeoff.running()) {