Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Peter Barker 2019-05-10 12:18:49 +10:00 committed by Randy Mackay
parent a025794bae
commit 676d75c391
33 changed files with 411 additions and 376 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.cpp
View File

@ -119,7 +119,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(run_nav_updates, 50, 100), SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(update_throttle_hover,100, 90), SCHED_TASK(update_throttle_hover,100, 90),
#if MODE_SMARTRTL_ENABLED == ENABLED #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 #endif
#if SPRAYER_ENABLED == ENABLED #if SPRAYER_ENABLED == ENABLED
SCHED_TASK_CLASS(AC_Sprayer, &copter.sprayer, update, 3, 90), SCHED_TASK_CLASS(AC_Sprayer, &copter.sprayer, update, 3, 90),

53
ArduCopter/Copter.h
View File

@ -176,6 +176,19 @@
#include <SITL/SITL.h> #include <SITL/SITL.h>
#endif #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 { class Copter : public AP_HAL::HAL::Callbacks {
public: public:
@ -194,6 +207,33 @@ public:
friend class RC_Channel_Copter; friend class RC_Channel_Copter;
friend class RC_Channels_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); Copter(void);
// HAL::Callbacks implementation. // HAL::Callbacks implementation.
@ -374,12 +414,6 @@ private:
} sensor_health; } sensor_health;
// Motor Output // Motor Output
#if FRAME_CONFIG == HELI_FRAME
#define MOTOR_CLASS AP_MotorsHeli
#else
#define MOTOR_CLASS AP_MotorsMulticopter
#endif
MOTOR_CLASS *motors; MOTOR_CLASS *motors;
const struct AP_Param::GroupInfo *motors_var_info; const struct AP_Param::GroupInfo *motors_var_info;
@ -428,11 +462,6 @@ private:
// Attitude, Position and Waypoint navigation objects // Attitude, Position and Waypoint navigation objects
// To-Do: move inertial nav up or other navigation variables down here // 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_AttitudeControl_t *attitude_control;
AC_PosControl *pos_control; AC_PosControl *pos_control;
AC_WPNav *wp_nav; AC_WPNav *wp_nav;
@ -847,8 +876,6 @@ private:
void publish_osd_info(); void publish_osd_info();
#endif #endif
#include "mode.h"
Mode *flightmode; Mode *flightmode;
#if MODE_ACRO_ENABLED == ENABLED #if MODE_ACRO_ENABLED == ENABLED
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME

2
ArduCopter/GCS_Mavlink.cpp
View File

@ -164,7 +164,7 @@ void GCS_MAVLINK_Copter::send_nav_controller_output() const
return; return;
} }
const Vector3f &targets = copter.attitude_control->get_att_target_euler_cd(); 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( mavlink_msg_nav_controller_output_send(
chan, chan,
targets.x * 1.0e-2f, targets.x * 1.0e-2f,

6
ArduCopter/Parameters.cpp
View File

@ -742,7 +742,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Description: Used by THROW mode. Specifies whether Copter is thrown upward or dropped. // @Description: Used by THROW mode. Specifies whether Copter is thrown upward or dropped.
// @Values: 0:Upward Throw,1:Drop // @Values: 0:Upward Throw,1:Drop
// @User: Standard // @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 #endif
// @Param: GND_EFFECT_COMP // @Param: GND_EFFECT_COMP
@ -882,7 +882,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED #if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED
// @Group: FHLD // @Group: FHLD
// @Path: mode_flowhold.cpp // @Path: mode_flowhold.cpp
AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, Copter::ModeFlowHold), AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, ModeFlowHold),
#endif #endif
#if MODE_FOLLOW_ENABLED == ENABLED #if MODE_FOLLOW_ENABLED == ENABLED
@ -898,7 +898,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
#if AUTOTUNE_ENABLED == ENABLED #if AUTOTUNE_ENABLED == ENABLED
// @Group: AUTOTUNE_ // @Group: AUTOTUNE_
// @Path: ../libraries/AC_AutoTune/AC_AutoTune.cpp // @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 #endif
#ifdef ENABLE_SCRIPTING #ifdef ENABLE_SCRIPTING

10
ArduCopter/Parameters.h
View File

@ -2,6 +2,16 @@
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#if GRIPPER_ENABLED == ENABLED
# include <AP_Gripper/AP_Gripper.h>
#endif
#if MODE_FOLLOW_ENABLED == ENABLED
# include <AP_Follow/AP_Follow.h>
#endif
#if VISUAL_ODOMETRY_ENABLED == ENABLED
# include <AP_VisualOdom/AP_VisualOdom.h>
#endif
// Global parameter class. // Global parameter class.
// //
class Parameters { class Parameters {

22
ArduCopter/autoyaw.cpp
View File

@ -1,9 +1,9 @@
#include "Copter.h" #include "Copter.h"
Copter::Mode::AutoYaw Copter::Mode::auto_yaw; Mode::AutoYaw Mode::auto_yaw;
// roi_yaw - returns heading towards location held in roi // roi_yaw - returns heading towards location held in roi
float Copter::Mode::AutoYaw::roi_yaw() float Mode::AutoYaw::roi_yaw()
{ {
roi_yaw_counter++; roi_yaw_counter++;
if (roi_yaw_counter >= 4) { if (roi_yaw_counter >= 4) {
@ -14,7 +14,7 @@ float Copter::Mode::AutoYaw::roi_yaw()
return _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(); const Vector3f& vel = copter.inertial_nav.get_velocity();
float speed = norm(vel.x,vel.y); float speed = norm(vel.x,vel.y);
@ -25,14 +25,14 @@ float Copter::Mode::AutoYaw::look_ahead_yaw()
return _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)); set_mode(default_mode(rtl));
} }
// default_mode - returns auto_yaw.mode() based on WP_YAW_BEHAVIOR parameter // default_mode - returns auto_yaw.mode() based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL // 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) { 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 // 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 // return immediately if no change
if (_mode == yaw_mode) { 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 // 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; 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 // 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 location is zero lat, lon and altitude turn off ROI
if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) { 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 // 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); set_mode(AUTO_YAW_RATE);
_rate_cds = turn_rate_cds; _rate_cds = turn_rate_cds;
} }
// yaw - returns target heading depending upon auto_yaw.mode() // yaw - returns target heading depending upon auto_yaw.mode()
float Copter::Mode::AutoYaw::yaw() float Mode::AutoYaw::yaw()
{ {
switch (_mode) { switch (_mode) {
@ -207,7 +207,7 @@ float Copter::Mode::AutoYaw::yaw()
// returns yaw rate normally set by SET_POSITION_TARGET mavlink // returns yaw rate normally set by SET_POSITION_TARGET mavlink
// messages (positive is clockwise, negative is counter clockwise) // 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) { if (_mode == AUTO_YAW_RATE) {
return _rate_cds; return _rate_cds;

83
ArduCopter/mode.cpp
View File

@ -8,7 +8,7 @@
/* /*
constructor for Mode object constructor for Mode object
*/ */
Copter::Mode::Mode(void) : Mode::Mode(void) :
g(copter.g), g(copter.g),
g2(copter.g2), g2(copter.g2),
wp_nav(copter.wp_nav), wp_nav(copter.wp_nav),
@ -22,16 +22,15 @@ Copter::Mode::Mode(void) :
channel_pitch(copter.channel_pitch), channel_pitch(copter.channel_pitch),
channel_throttle(copter.channel_throttle), channel_throttle(copter.channel_throttle),
channel_yaw(copter.channel_yaw), channel_yaw(copter.channel_yaw),
G_Dt(copter.G_Dt), G_Dt(copter.G_Dt)
ap(copter.ap)
{ }; { };
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 // 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) { switch (mode) {
#if MODE_ACRO_ENABLED == ENABLED #if MODE_ACRO_ENABLED == ENABLED
@ -144,7 +143,7 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode)
#if OPTFLOW == ENABLED #if OPTFLOW == ENABLED
case FLOWHOLD: case FLOWHOLD:
ret = (Copter::Mode *)g2.mode_flowhold_ptr; ret = (Mode *)g2.mode_flowhold_ptr;
break; break;
#endif #endif
@ -181,7 +180,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
return true; return true;
} }
Copter::Mode *new_flightmode = mode_from_mode_num(mode); Mode *new_flightmode = mode_from_mode_num(mode);
if (new_flightmode == nullptr) { if (new_flightmode == nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING,"No such mode"); gcs().send_text(MAV_SEVERITY_WARNING,"No such mode");
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(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 // exit_mode - high level call to organise cleanup as a flight mode is exited
void Copter::exit_mode(Copter::Mode *&old_flightmode, void Copter::exit_mode(Mode *&old_flightmode,
Copter::Mode *&new_flightmode) Mode *&new_flightmode)
{ {
#if AUTOTUNE_ENABLED == ENABLED #if AUTOTUNE_ENABLED == ENABLED
if (old_flightmode == &mode_autotune) { if (old_flightmode == &mode_autotune) {
@ -335,7 +334,7 @@ void Copter::notify_flight_mode() {
notify.set_flight_mode_str(flightmode->name4()); 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 to make high level decisions about control modes
run_autopilot(); 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 // get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees // 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 // fetch roll and pitch inputs
roll_out = channel_roll->get_control_in(); 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 // 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) { if (!copter.ap.land_complete) {
// can't take off if we're already flying // 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; 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 true;
} }
return false; 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) { if (spool_up) {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); 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); 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 // run attitude controller
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); 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); 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 // command aircraft to initiate the shutdown process
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); 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 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; int32_t alt_above_ground;
if (copter.rangefinder_alt_ok()) { if (copter.rangefinder_alt_ok()) {
@ -459,11 +458,11 @@ int32_t Copter::Mode::get_alt_above_ground_cm(void)
return alt_above_ground; 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 #if PRECISION_LANDING == ENABLED
const bool navigating = pos_control->is_active_xy(); 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 #else
bool doing_precision_landing = false; bool doing_precision_landing = false;
#endif #endif
@ -502,14 +501,14 @@ void Copter::Mode::land_run_vertical_control(bool pause_descent)
pos_control->update_z_controller(); 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_roll = 0.0f;
float target_pitch = 0.0f; float target_pitch = 0.0f;
float target_yaw_rate = 0; float target_yaw_rate = 0;
// relax loiter target if we might be landed // relax loiter target if we might be landed
if (ap.land_complete_maybe) { if (copter.ap.land_complete_maybe) {
loiter_nav->soften_for_landing(); loiter_nav->soften_for_landing();
} }
@ -532,10 +531,10 @@ void Copter::Mode::land_run_horizontal_control()
// record if pilot has overridden roll or pitch // record if pilot has overridden roll or pitch
if (!is_zero(target_roll) || !is_zero(target_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); 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 #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 // run precision landing
if (doing_precision_landing) { if (doing_precision_landing) {
Vector2f target_pos, target_vel_rel; 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(); return motors->get_throttle_hover();
} }
@ -612,7 +611,7 @@ float Copter::Mode::throttle_hover() const
// used only for manual throttle modes // used only for manual throttle modes
// thr_mid should be in the range 0 to 1 // thr_mid should be in the range 0 to 1
// returns throttle output 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(); const float thr_mid = throttle_hover();
int16_t throttle_control = channel_throttle->get_control_in(); int16_t throttle_control = channel_throttle->get_control_in();
@ -640,7 +639,7 @@ float Copter::Mode::get_pilot_desired_throttle() const
return throttle_out; 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 // Alt Hold State Machine Determination
if (!motors->armed()) { 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 // the aircraft should progress through the take off procedure
return AltHold_Takeoff; 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 // 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 // the aircraft should move to a ground idle state
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); 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; // pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base // these are candidates for moving into the Mode base
// class. // 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); 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); 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(); return copter.get_non_takeoff_throttle();
} }
void Copter::Mode::update_simple_mode(void) { void Mode::update_simple_mode(void) {
copter.update_simple_mode(); 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); 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); return copter.set_land_complete(b);
} }
GCS_Copter &Copter::Mode::gcs() GCS_Copter &Mode::gcs()
{ {
return copter.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); return copter.logger.Write_Event(id);
} }
void Copter::Mode::set_throttle_takeoff() void Mode::set_throttle_takeoff()
{ {
return copter.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); 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(); return copter.get_pilot_speed_dn();
} }

57
ArduCopter/mode.h
View File

@ -1,6 +1,6 @@
#pragma once #pragma once
// this class is #included into the Copter:: namespace #include "Copter.h"
class Mode { class Mode {
@ -99,7 +99,6 @@ protected:
RC_Channel *&channel_throttle; RC_Channel *&channel_throttle;
RC_Channel *&channel_yaw; RC_Channel *&channel_yaw;
float &G_Dt; float &G_Dt;
ap_t &ap;
// note that we support two entirely different automatic takeoffs: // note that we support two entirely different automatic takeoffs:
@ -221,7 +220,7 @@ class ModeAcro : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
virtual void run() override; virtual void run() override;
@ -241,7 +240,7 @@ protected:
if (g2.acro_thr_mid > 0) { if (g2.acro_thr_mid > 0) {
return g2.acro_thr_mid; return g2.acro_thr_mid;
} }
return Copter::Mode::throttle_hover(); return Mode::throttle_hover();
} }
private: private:
@ -254,7 +253,7 @@ class ModeAcro_Heli : public ModeAcro {
public: public:
// inherit constructor // inherit constructor
using Copter::ModeAcro::Mode; using ModeAcro::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -270,7 +269,7 @@ class ModeAltHold : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -297,7 +296,7 @@ class ModeAuto : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -339,9 +338,9 @@ public:
bool do_guided(const AP_Mission::Mission_Command& cmd); bool do_guided(const AP_Mission::Mission_Command& cmd);
AP_Mission mission{ AP_Mission mission{
FUNCTOR_BIND_MEMBER(&Copter::ModeAuto::start_command, bool, const AP_Mission::Mission_Command &), FUNCTOR_BIND_MEMBER(&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(&ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Copter::ModeAuto::exit_mission, void)}; FUNCTOR_BIND_MEMBER(&ModeAuto::exit_mission, void)};
protected: protected:
@ -488,7 +487,7 @@ class ModeAutoTune : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -514,7 +513,7 @@ class ModeBrake : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -543,7 +542,7 @@ class ModeCircle : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -572,7 +571,7 @@ class ModeDrift : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -598,7 +597,7 @@ class ModeFlip : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -725,7 +724,7 @@ class ModeGuided : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -789,7 +788,7 @@ class ModeGuidedNoGPS : public ModeGuided {
public: public:
// inherit constructor // inherit constructor
using Copter::ModeGuided::Mode; using ModeGuided::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -813,7 +812,7 @@ class ModeLand : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -844,7 +843,7 @@ class ModeLoiter : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -885,7 +884,7 @@ class ModePosHold : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -964,7 +963,7 @@ class ModeRTL : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override { void run() override {
@ -1037,7 +1036,7 @@ class ModeSmartRTL : public ModeRTL {
public: public:
// inherit constructor // inherit constructor
using Copter::ModeRTL::Mode; using ModeRTL::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -1074,7 +1073,7 @@ class ModeSport : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -1101,7 +1100,7 @@ class ModeStabilize : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
virtual void run() override; virtual void run() override;
@ -1124,7 +1123,7 @@ class ModeStabilize_Heli : public ModeStabilize {
public: public:
// inherit constructor // inherit constructor
using Copter::ModeStabilize::Mode; using ModeStabilize::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -1141,7 +1140,7 @@ class ModeThrow : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -1192,7 +1191,7 @@ class ModeAvoidADSB : public ModeGuided {
public: public:
// inherit constructor // inherit constructor
using Copter::ModeGuided::Mode; using ModeGuided::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -1218,7 +1217,7 @@ class ModeFollow : public ModeGuided {
public: public:
// inherit constructor // inherit constructor
using Copter::ModeGuided::Mode; using ModeGuided::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;
@ -1244,7 +1243,7 @@ class ModeZigZag : public Mode {
public: public:
// inherit constructor // inherit constructor
using Copter::Mode::Mode; using Mode::Mode;
bool init(bool ignore_checks) override; bool init(bool ignore_checks) override;
void run() override; void run() override;

6
ArduCopter/mode_acro.cpp
View File

@ -8,7 +8,7 @@
* Init and run calls for acro flight mode * Init and run calls for acro flight mode
*/ */
void Copter::ModeAcro::run() void ModeAcro::run()
{ {
// convert the input to the desired body frame rate // convert the input to the desired body frame rate
float target_roll, target_pitch, target_yaw; float target_roll, target_pitch, target_yaw;
@ -17,7 +17,7 @@ void Copter::ModeAcro::run()
if (!motors->armed()) { if (!motors->armed()) {
// Motors should be Stopped // Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
} else if (ap.throttle_zero) { } else if (copter.ap.throttle_zero) {
// Attempting to Land // Attempting to Land
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else { } 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 // 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 // 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; float rate_limit;
Vector3f rate_ef_level, rate_bf_level, rate_bf_request; Vector3f rate_ef_level, rate_bf_level, rate_bf_request;

8
ArduCopter/mode_acro_heli.cpp
View File

@ -8,7 +8,7 @@
*/ */
// heli_acro_init - initialise acro controller // 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 // 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()); 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 // heli_acro_run - runs the acro controller
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModeAcro_Heli::run() void ModeAcro_Heli::run()
{ {
float target_roll, target_pitch, target_yaw; float target_roll, target_pitch, target_yaw;
float pilot_throttle_scaled; float pilot_throttle_scaled;
@ -73,7 +73,7 @@ void Copter::ModeAcro_Heli::run()
// only mimic flybar response when trainer mode is disabled // only mimic flybar response when trainer mode is disabled
if (g.acro_trainer == ACRO_TRAINER_DISABLED) { if (g.acro_trainer == ACRO_TRAINER_DISABLED) {
// while landed always leak off target attitude to current attitude // 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); virtual_flybar(target_roll, target_pitch, target_yaw, 3.0f, 3.0f);
// while flying use acro balance parameters for leak rate // while flying use acro balance parameters for leak rate
} else { } else {
@ -123,7 +123,7 @@ void Copter::ModeAcro_Heli::run()
// virtual_flybar - acts like a flybar by leaking target atttitude back to current attitude // 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; Vector3f rate_ef_level, rate_bf_level;

4
ArduCopter/mode_althold.cpp
View File

@ -6,7 +6,7 @@
*/ */
// althold_init - initialise althold controller // althold_init - initialise althold controller
bool Copter::ModeAltHold::init(bool ignore_checks) bool ModeAltHold::init(bool ignore_checks)
{ {
// initialise position and desired velocity // initialise position and desired velocity
if (!pos_control->is_active_z()) { if (!pos_control->is_active_z()) {
@ -19,7 +19,7 @@ bool Copter::ModeAltHold::init(bool ignore_checks)
// althold_run - runs the althold controller // althold_run - runs the althold controller
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModeAltHold::run() void ModeAltHold::run()
{ {
float takeoff_climb_rate = 0.0f; float takeoff_climb_rate = 0.0f;

166
ArduCopter/mode_auto.cpp
View File

@ -20,13 +20,13 @@
*/ */
// auto_init - initialise auto controller // 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) { if (mission.num_commands() > 1 || ignore_checks) {
_mode = Auto_Loiter; _mode = Auto_Loiter;
// reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips) // 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"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd");
return false; return false;
} }
@ -54,7 +54,7 @@ bool Copter::ModeAuto::init(bool ignore_checks)
// auto_run - runs the auto controller // auto_run - runs the auto controller
// should be called at 100hz or more // should be called at 100hz or more
// relies on run_autopilot being called at 10hz which handles decision making and non-navigation related commands // 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 // call the correct auto controller
switch (_mode) { switch (_mode) {
@ -106,7 +106,7 @@ void Copter::ModeAuto::run()
// auto_loiter_start - initialises loitering in auto mode // auto_loiter_start - initialises loitering in auto mode
// returns success/failure because this can be called by exit_mission // 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 // return failure if GPS is bad
if (!copter.position_ok()) { if (!copter.position_ok()) {
@ -128,7 +128,7 @@ bool Copter::ModeAuto::loiter_start()
} }
// auto_rtl_start - initialises RTL in AUTO flight mode // auto_rtl_start - initialises RTL in AUTO flight mode
void Copter::ModeAuto::rtl_start() void ModeAuto::rtl_start()
{ {
_mode = Auto_RTL; _mode = Auto_RTL;
@ -137,7 +137,7 @@ void Copter::ModeAuto::rtl_start()
} }
// auto_takeoff_start - initialises waypoint controller to implement take-off // 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; _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 // 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; _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 // 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; _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 // auto_land_start - initialises controller to implement a landing
void Copter::ModeAuto::land_start() void ModeAuto::land_start()
{ {
// set target to stopping point // set target to stopping point
Vector3f stopping_point; Vector3f stopping_point;
@ -228,7 +228,7 @@ void Copter::ModeAuto::land_start()
} }
// auto_land_start - initialises controller to implement a landing // 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; _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 // 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 // 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 // convert location to vector from ekf origin
Vector3f circle_center_neu; 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 // 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 // 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; _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 // 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 // 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, AC_WPNav::spline_segment_end_type seg_end_type,
const Location& next_destination) const Location& next_destination)
{ {
@ -337,7 +337,7 @@ void Copter::ModeAuto::spline_start(const Location& destination, bool stopped_at
#if NAV_GUIDED == ENABLED #if NAV_GUIDED == ENABLED
// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode // 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; _mode = Auto_NavGuided;
@ -349,7 +349,7 @@ void Copter::ModeAuto::nav_guided_start()
} }
#endif //NAV_GUIDED #endif //NAV_GUIDED
bool Copter::ModeAuto::is_landing() const bool ModeAuto::is_landing() const
{ {
switch(_mode) { switch(_mode) {
case Auto_Land: case Auto_Land:
@ -362,12 +362,12 @@ bool Copter::ModeAuto::is_landing() const
return false; return false;
} }
bool Copter::ModeAuto::is_taking_off() const bool ModeAuto::is_taking_off() const
{ {
return _mode == Auto_TakeOff; return _mode == Auto_TakeOff;
} }
bool Copter::ModeAuto::landing_gear_should_be_deployed() const bool ModeAuto::landing_gear_should_be_deployed() const
{ {
switch(_mode) { switch(_mode) {
case Auto_Land: 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 // 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 // set target to stopping point
Vector3f 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 // 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 // To-Do: logging when new commands start/end
if (copter.should_log(MASK_LOG_CMD)) { 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 // exit_mission - function that is called once the mission completes
void Copter::ModeAuto::exit_mission() void ModeAuto::exit_mission()
{ {
// play a tone // play a tone
AP_Notify::events.mission_complete = 1; AP_Notify::events.mission_complete = 1;
// if we are not on the ground switch to loiter or land // 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 // try to enter loiter but if that fails land
if (!loiter_start()) { if (!loiter_start()) {
set_mode(LAND, MODE_REASON_MISSION_END); set_mode(LAND, MODE_REASON_MISSION_END);
@ -542,7 +542,7 @@ void Copter::ModeAuto::exit_mission()
} }
// do_guided - start guided mode // 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 // only process guided waypoint if we are in guided mode
if (copter.control_mode != GUIDED && !(copter.control_mode == AUTO && mode() == Auto_NavGuided)) { 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; return true;
} }
uint32_t Copter::ModeAuto::wp_distance() const uint32_t ModeAuto::wp_distance() const
{ {
switch (_mode) { switch (_mode) {
case Auto_Circle: 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) { switch (_mode) {
case Auto_Circle: 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) { switch (_mode) {
case Auto_NavGuided: case Auto_NavGuided:
@ -608,7 +608,7 @@ bool Copter::ModeAuto::get_wp(Location& destination)
} }
// update mission // update mission
void Copter::ModeAuto::run_autopilot() void ModeAuto::run_autopilot()
{ {
mission.update(); 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 // 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 // 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) { if (copter.flightmode != &copter.mode_auto) {
return false; return false;
@ -730,7 +730,7 @@ bool Copter::ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
// auto_takeoff_run - takeoff in auto mode // auto_takeoff_run - takeoff in auto mode
// called by auto_run at 100hz or more // called by auto_run at 100hz or more
void Copter::ModeAuto::takeoff_run() void ModeAuto::takeoff_run()
{ {
auto_takeoff_run(); auto_takeoff_run();
if (wp_nav->reached_wp_destination()) { if (wp_nav->reached_wp_destination()) {
@ -741,7 +741,7 @@ void Copter::ModeAuto::takeoff_run()
// auto_wp_run - runs the auto waypoint controller // auto_wp_run - runs the auto waypoint controller
// called by auto_run at 100hz or more // called by auto_run at 100hz or more
void Copter::ModeAuto::wp_run() void ModeAuto::wp_run()
{ {
// process pilot's yaw input // process pilot's yaw input
float target_yaw_rate = 0; float target_yaw_rate = 0;
@ -781,7 +781,7 @@ void Copter::ModeAuto::wp_run()
// auto_spline_run - runs the auto spline controller // auto_spline_run - runs the auto spline controller
// called by auto_run at 100hz or more // 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 not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) { if (is_disarmed_or_landed()) {
@ -821,7 +821,7 @@ void Copter::ModeAuto::spline_run()
// auto_land_run - lands in auto mode // auto_land_run - lands in auto mode
// called by auto_run at 100hz or more // 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 // 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 // auto_rtl_run - rtl in AUTO flight mode
// called by auto_run at 100hz or more // called by auto_run at 100hz or more
void Copter::ModeAuto::rtl_run() void ModeAuto::rtl_run()
{ {
// call regular rtl flight mode run function // call regular rtl flight mode run function
copter.mode_rtl.run(false); copter.mode_rtl.run(false);
@ -848,7 +848,7 @@ void Copter::ModeAuto::rtl_run()
// auto_circle_run - circle in AUTO flight mode // auto_circle_run - circle in AUTO flight mode
// called by auto_run at 100hz or more // called by auto_run at 100hz or more
void Copter::ModeAuto::circle_run() void ModeAuto::circle_run()
{ {
// call circle controller // call circle controller
copter.circle_nav->update(); copter.circle_nav->update();
@ -868,7 +868,7 @@ void Copter::ModeAuto::circle_run()
#if NAV_GUIDED == ENABLED #if NAV_GUIDED == ENABLED
// auto_nav_guided_run - allows control by external navigation controller // auto_nav_guided_run - allows control by external navigation controller
// called by auto_run at 100hz or more // 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 // call regular guided flight mode run function
copter.mode_guided.run(); copter.mode_guided.run();
@ -877,7 +877,7 @@ void Copter::ModeAuto::nav_guided_run()
// auto_loiter_run - loiter in AUTO flight mode // auto_loiter_run - loiter in AUTO flight mode
// called by auto_run at 100hz or more // 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 not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) { if (is_disarmed_or_landed()) {
@ -904,7 +904,7 @@ void Copter::ModeAuto::loiter_run()
// auto_loiter_run - loiter to altitude in AUTO flight mode // auto_loiter_run - loiter to altitude in AUTO flight mode
// called by auto_run at 100hz or more // 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 not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (is_disarmed_or_landed() || !motors->get_interlock()) { 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 // 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; _mode = Auto_NavPayloadPlace;
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; 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 // auto_payload_place_run - places an object in auto mode
// called by auto_run at 100hz or more // 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()) { if (!payload_place_run_should_run()) {
zero_throttle_and_relax_ac(); 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 // must be armed
if (!motors->armed()) { if (!motors->armed()) {
return false; return false;
} }
// must be auto-armed // must be auto-armed
if (!ap.auto_armed) { if (!copter.ap.auto_armed) {
return false; return false;
} }
// must not be landed // must not be landed
if (ap.land_complete) { if (copter.ap.land_complete) {
return false; return false;
} }
// interlock must be enabled (i.e. unsafe) // interlock must be enabled (i.e. unsafe)
@ -1029,7 +1029,7 @@ bool Copter::ModeAuto::payload_place_run_should_run()
return true; return true;
} }
void Copter::ModeAuto::payload_place_run_loiter() void ModeAuto::payload_place_run_loiter()
{ {
// loiter... // loiter...
land_run_horizontal_control(); land_run_horizontal_control();
@ -1045,7 +1045,7 @@ void Copter::ModeAuto::payload_place_run_loiter()
pos_control->update_z_controller(); pos_control->update_z_controller();
} }
void Copter::ModeAuto::payload_place_run_descend() void ModeAuto::payload_place_run_descend()
{ {
land_run_horizontal_control(); land_run_horizontal_control();
land_run_vertical_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 // terrain_adjusted_location: returns a Location with lat/lon from cmd
// and altitude from our current altitude adjusted for location // 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 // convert to location class
Location target_loc(cmd.content.location); 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 // 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 // Set wp navigation target to safe altitude above current position
takeoff_start(cmd.content.location); 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); 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 // 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); 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 // 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 // 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 // do_loiter_unlimited - start loitering with no end conditions
// note: caller should set yaw_mode // 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 // convert back to location
Location target_loc(cmd.content.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 // 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); 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 // do_loiter_time - initiate loitering at a point for a given time period
// note: caller should set yaw_mode // 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 // re-use loiter unlimited
do_loiter_unlimited(cmd); 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 // do_loiter_alt - initiate loitering at a point until a given altitude is reached
// note: caller should set yaw_mode // 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 // re-use loiter unlimited
do_loiter_unlimited(cmd); 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 // 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); 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 #if NAV_GUIDED == ENABLED
// do_nav_guided_enable - initiate accepting commands from external nav computer // 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) { if (cmd.p1 > 0) {
// start guided within auto // 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 // 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( copter.mode_guided.limit_set(
cmd.p1 * 1000, // convert seconds to ms 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 #endif // NAV_GUIDED
// do_nav_delay - Delay the next navigation command // 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(); nav_delay_time_start = millis();
@ -1366,18 +1366,18 @@ void Copter::ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd)
// Condition (May) commands // 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_start = millis();
condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds 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; 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( auto_yaw.set_fixed_yaw(
cmd.content.yaw.angle_deg, 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.target_ms > 0) {
if (cmd.content.speed.speed_type == 2) { 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 (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)) { 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) // 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 // 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 // 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); auto_yaw.set_roi(cmd.content.location);
} }
// point the camera to a specified angle // 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 MOUNT == ENABLED
if (!copter.camera_mount.has_pan_control()) { 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 #if WINCH_ENABLED == ENABLED
// control winch based on mission command // 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 // Note: we ignore the gripper num parameter because we only support one gripper
switch (cmd.content.winch.action) { switch (cmd.content.winch.action) {
@ -1464,7 +1464,7 @@ void Copter::ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd)
#endif #endif
// do_payload_place - initiate placing procedure // 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 location provided we fly to that location at current altitude
if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) { 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 // do_RTL - start Return-to-Launch
void Copter::ModeAuto::do_RTL(void) void ModeAuto::do_RTL(void)
{ {
// start rtl in auto flight mode // start rtl in auto flight mode
rtl_start(); rtl_start();
@ -1495,14 +1495,14 @@ void Copter::ModeAuto::do_RTL(void)
/********************************************************************************/ /********************************************************************************/
// verify_takeoff - check if we have completed the takeoff // verify_takeoff - check if we have completed the takeoff
bool Copter::ModeAuto::verify_takeoff() bool ModeAuto::verify_takeoff()
{ {
// have we reached our target altitude? // have we reached our target altitude?
return copter.wp_nav->reached_wp_destination(); return copter.wp_nav->reached_wp_destination();
} }
// verify_land - returns true if landing has been completed // verify_land - returns true if landing has been completed
bool Copter::ModeAuto::verify_land() bool ModeAuto::verify_land()
{ {
bool retval = false; bool retval = false;
@ -1523,7 +1523,7 @@ bool Copter::ModeAuto::verify_land()
case LandStateType_Descending: case LandStateType_Descending:
// rely on THROTTLE_LAND mode to correctly update landing status // 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; break;
default: default:
@ -1547,7 +1547,7 @@ bool Copter::ModeAuto::verify_land()
#endif #endif
// verify_payload_place - returns true if placing has been completed // 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 hover_throttle_calibrate_time = 2000; // milliseconds
const uint16_t descend_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(); const uint32_t now = AP_HAL::millis();
// if we discover we've landed then immediately release the load: // 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) { switch (nav_payload_place.state) {
case PayloadPlaceStateType_FlyToLocation: case PayloadPlaceStateType_FlyToLocation:
case PayloadPlaceStateType_Calibrating_Hover_Start: case PayloadPlaceStateType_Calibrating_Hover_Start:
@ -1698,13 +1698,13 @@ bool Copter::ModeAuto::verify_payload_place()
} }
#undef debug #undef debug
bool Copter::ModeAuto::verify_loiter_unlimited() bool ModeAuto::verify_loiter_unlimited()
{ {
return false; return false;
} }
// verify_loiter_time - check if we have loitered long enough // 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 // return immediately if we haven't reached our destination
if (!copter.wp_nav->reached_wp_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 // verify_loiter_to_alt - check if we have reached both destination
// (roughly) and altitude (precisely) // (roughly) and altitude (precisely)
bool Copter::ModeAuto::verify_loiter_to_alt() bool ModeAuto::verify_loiter_to_alt()
{ {
if (loiter_to_alt.reached_destination_xy && if (loiter_to_alt.reached_destination_xy &&
loiter_to_alt.reached_alt) { 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 // verify_RTL - handles any state changes required to implement RTL
// do_RTL should have been called once first to initialise all variables // do_RTL should have been called once first to initialise all variables
// returns true with RTL has completed successfully // returns true with RTL has completed successfully
bool Copter::ModeAuto::verify_RTL() bool ModeAuto::verify_RTL()
{ {
return (copter.mode_rtl.state_complete() && return (copter.mode_rtl.state_complete() &&
(copter.mode_rtl.state() == RTL_FinalDescent || copter.mode_rtl.state() == RTL_Land) && (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 // 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)) { if (millis() - condition_start > (uint32_t)MAX(condition_value,0)) {
condition_value = 0; condition_value = 0;
@ -1759,7 +1759,7 @@ bool Copter::ModeAuto::verify_wait_delay()
return false; return false;
} }
bool Copter::ModeAuto::verify_within_distance() bool ModeAuto::verify_within_distance()
{ {
if (wp_distance() < (uint32_t)MAX(condition_value,0)) { if (wp_distance() < (uint32_t)MAX(condition_value,0)) {
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 // 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) // 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) { 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 // 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 // check if we have reached the waypoint
if ( !copter.wp_nav->reached_wp_destination() ) { 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 // 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 // check if we've reached the edge
if (mode() == Auto_CircleMoveToEdge) { 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 // 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 // check if we have reached the waypoint
if ( !copter.wp_nav->reached_wp_destination() ) { 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 #if NAV_GUIDED == ENABLED
// verify_nav_guided - check if we have breached any limits // 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 disabling guided mode then immediately return true so we move to next command
if (cmd.p1 == 0) { if (cmd.p1 == 0) {
@ -1878,7 +1878,7 @@ bool Copter::ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Comman
#endif // NAV_GUIDED #endif // NAV_GUIDED
// verify_nav_delay - check if we have waited long enough // 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)) { if (millis() - nav_delay_time_start > (uint32_t)MAX(nav_delay_time_max,0)) {
nav_delay_time_max = 0; nav_delay_time_max = 0;

28
ArduCopter/mode_autotune.cpp
View File

@ -6,7 +6,7 @@
#if AUTOTUNE_ENABLED == ENABLED #if AUTOTUNE_ENABLED == ENABLED
bool Copter::AutoTune::init() bool AutoTune::init()
{ {
// use position hold while tuning if we were in QLOITER // use position hold while tuning if we were in QLOITER
bool position_hold = (copter.control_mode == LOITER || copter.control_mode == POSHOLD); bool position_hold = (copter.control_mode == LOITER || copter.control_mode == POSHOLD);
@ -21,7 +21,7 @@ bool Copter::AutoTune::init()
/* /*
start autotune mode start autotune mode
*/ */
bool Copter::AutoTune::start() bool AutoTune::start()
{ {
// only allow flip from Stabilize, AltHold, PosHold or Loiter modes // only allow flip from Stabilize, AltHold, PosHold or Loiter modes
if (copter.control_mode != STABILIZE && copter.control_mode != ALT_HOLD && if (copter.control_mode != STABILIZE && copter.control_mode != ALT_HOLD &&
@ -42,7 +42,7 @@ bool Copter::AutoTune::start()
return AC_AutoTune::start(); return AC_AutoTune::start();
} }
void Copter::AutoTune::run() void AutoTune::run()
{ {
// apply SIMPLE mode transform to pilot inputs // apply SIMPLE mode transform to pilot inputs
copter.update_simple_mode(); copter.update_simple_mode();
@ -77,7 +77,7 @@ void Copter::AutoTune::run()
/* /*
get stick input climb rate 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()); 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 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.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()); 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 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_speed_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up);
copter.pos_control->set_max_accel_z(copter.g.pilot_accel_z); 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_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()); 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 Write an event packet. This maps from AC_AutoTune event IDs to
copter event IDs copter event IDs
*/ */
void Copter::AutoTune::Log_Write_Event(enum at_event id) void AutoTune::Log_Write_Event(enum at_event id)
{ {
const struct { const struct {
enum at_event eid; 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 check if we have a good position estimate
*/ */
bool Copter::AutoTune::position_ok() bool AutoTune::position_ok()
{ {
return copter.position_ok(); return copter.position_ok();
} }
@ -152,28 +152,28 @@ bool Copter::AutoTune::position_ok()
/* /*
initialise autotune mode initialise autotune mode
*/ */
bool Copter::ModeAutoTune::init(bool ignore_checks) bool ModeAutoTune::init(bool ignore_checks)
{ {
return copter.autotune.init(); return copter.autotune.init();
} }
void Copter::ModeAutoTune::run() void ModeAutoTune::run()
{ {
copter.autotune.run(); copter.autotune.run();
} }
void Copter::ModeAutoTune::save_tuning_gains() void ModeAutoTune::save_tuning_gains()
{ {
copter.autotune.save_tuning_gains(); copter.autotune.save_tuning_gains();
} }
void Copter::ModeAutoTune::stop() void ModeAutoTune::stop()
{ {
copter.autotune.stop(); copter.autotune.stop();
} }
void Copter::ModeAutoTune::reset() void ModeAutoTune::reset()
{ {
copter.autotune.reset(); copter.autotune.reset();
} }

12
ArduCopter/mode_avoid_adsb.cpp
View File

@ -10,13 +10,13 @@
*/ */
// initialise avoid_adsb controller // initialise avoid_adsb controller
bool Copter::ModeAvoidADSB::init(const bool ignore_checks) bool ModeAvoidADSB::init(const bool ignore_checks)
{ {
// re-use guided mode // 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 // check flight mode
if (copter.control_mode != AVOID_ADSB) { 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 // re-use guided mode's velocity controller
Copter::ModeGuided::set_velocity(velocity_neu); ModeGuided::set_velocity(velocity_neu);
return true; return true;
} }
// runs the AVOID_ADSB controller // runs the AVOID_ADSB controller
void Copter::ModeAvoidADSB::run() void ModeAvoidADSB::run()
{ {
// re-use guided mode's velocity controller // re-use guided mode's velocity controller
// Note: this is safe from interference from GCSs and companion computer's whose guided mode // 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 // position and velocity requests will be ignored while the vehicle is not in guided mode
Copter::ModeGuided::run(); ModeGuided::run();
} }

10
ArduCopter/mode_brake.cpp
View File

@ -7,7 +7,7 @@
*/ */
// brake_init - initialise brake controller // brake_init - initialise brake controller
bool Copter::ModeBrake::init(bool ignore_checks) bool ModeBrake::init(bool ignore_checks)
{ {
// set target to current position // set target to current position
wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE); 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 // brake_run - runs the brake controller
// should be called at 100hz or more // 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 not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) { if (is_disarmed_or_landed()) {
@ -42,7 +42,7 @@ void Copter::ModeBrake::run()
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// relax stop target if we might be landed // relax stop target if we might be landed
if (ap.land_complete_maybe) { if (copter.ap.land_complete_maybe) {
loiter_nav->soften_for_landing(); loiter_nav->soften_for_landing();
} }
@ -54,7 +54,7 @@ void Copter::ModeBrake::run()
// update altitude target and call position controller // update altitude target and call position controller
// protects heli's from inflight motor interlock disable // 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); pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
} else { } else {
pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); 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_start = millis();
_timeout_ms = timeout_ms; _timeout_ms = timeout_ms;

10
ArduCopter/mode_circle.cpp
View File

@ -7,7 +7,7 @@
*/ */
// circle_init - initialise circle controller flight mode // circle_init - initialise circle controller flight mode
bool Copter::ModeCircle::init(bool ignore_checks) bool ModeCircle::init(bool ignore_checks)
{ {
pilot_yaw_override = false; pilot_yaw_override = false;
@ -25,7 +25,7 @@ bool Copter::ModeCircle::init(bool ignore_checks)
// circle_run - runs the circle flight mode // circle_run - runs the circle flight mode
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModeCircle::run() void ModeCircle::run()
{ {
// initialize speeds and accelerations // initialize speeds and accelerations
pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy()); 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 // update altitude target and call position controller
// protects heli's from inflight motor interlock disable // 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); pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
} else { } else {
pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); 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(); 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(); 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(); return copter.circle_nav->get_bearing_to_target();
} }

8
ArduCopter/mode_drift.cpp
View File

@ -29,14 +29,14 @@
#endif #endif
// drift_init - initialise drift controller // drift_init - initialise drift controller
bool Copter::ModeDrift::init(bool ignore_checks) bool ModeDrift::init(bool ignore_checks)
{ {
return true; return true;
} }
// drift_run - runs the drift controller // drift_run - runs the drift controller
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModeDrift::run() void ModeDrift::run()
{ {
static float braker = 0.0f; static float braker = 0.0f;
static float roll_input = 0.0f; static float roll_input = 0.0f;
@ -81,7 +81,7 @@ void Copter::ModeDrift::run()
if (!motors->armed()) { if (!motors->armed()) {
// Motors should be Stopped // Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
} else if (ap.throttle_zero) { } else if (copter.ap.throttle_zero) {
// Attempting to Land // Attempting to Land
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else { } 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 // 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 // throttle assist - adjusts throttle to slow the vehicle's vertical velocity
// Only active when pilot's throttle is between 213 ~ 787 // Only active when pilot's throttle is between 213 ~ 787

8
ArduCopter/mode_flip.cpp
View File

@ -33,7 +33,7 @@
#define FLIP_PITCH_FORWARD -1 // used to set flip_dir #define FLIP_PITCH_FORWARD -1 // used to set flip_dir
// flip_init - initialise flip controller // 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 // only allow flip from ACRO, Stabilize, AltHold or Drift flight modes
if (copter.control_mode != ACRO && 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 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; return false;
} }
@ -54,7 +54,7 @@ bool Copter::ModeFlip::init(bool ignore_checks)
} }
// only allow flip when flying // only allow flip when flying
if (!motors->armed() || ap.land_complete) { if (!motors->armed() || copter.ap.land_complete) {
return false; return false;
} }
@ -92,7 +92,7 @@ bool Copter::ModeFlip::init(bool ignore_checks)
// run - runs the flip controller // run - runs the flip controller
// should be called at 100hz or more // 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 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)) { if (!motors->armed() || (abs(channel_roll->get_control_in()) >= 4000) || (abs(channel_pitch->get_control_in()) >= 4000) || ((millis() - start_time_ms) > FLIP_TIMEOUT_MS)) {

22
ArduCopter/mode_flowhold.cpp
View File

@ -8,7 +8,7 @@
without rangefinder without rangefinder
*/ */
const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = { const AP_Param::GroupInfo ModeFlowHold::var_info[] = {
// @Param: _XY_P // @Param: _XY_P
// @DisplayName: FlowHold P gain // @DisplayName: FlowHold P gain
// @Description: FlowHold (horizontal) P gain. // @Description: FlowHold (horizontal) P gain.
@ -37,14 +37,14 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = {
// @Range: 0 100 // @Range: 0 100
// @Units: Hz // @Units: Hz
// @User: Advanced // @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 // @Param: _FLOW_MAX
// @DisplayName: FlowHold Flow Rate Max // @DisplayName: FlowHold Flow Rate Max
// @Description: Controls maximum apparent flow rate in flowhold // @Description: Controls maximum apparent flow rate in flowhold
// @Range: 0.1 2.5 // @Range: 0.1 2.5
// @User: Standard // @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 // @Param: _FILT_HZ
// @DisplayName: FlowHold Filter Frequency // @DisplayName: FlowHold Filter Frequency
@ -52,14 +52,14 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = {
// @Range: 1 100 // @Range: 1 100
// @Units: Hz // @Units: Hz
// @User: Standard // @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 // @Param: _QUAL_MIN
// @DisplayName: FlowHold Flow quality minimum // @DisplayName: FlowHold Flow quality minimum
// @Description: Minimum flow quality to use flow position hold // @Description: Minimum flow quality to use flow position hold
// @Range: 0 255 // @Range: 0 255
// @User: Standard // @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 // 5 was FLOW_SPEED
@ -69,12 +69,12 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = {
// @Range: 1 30 // @Range: 1 30
// @User: Standard // @User: Standard
// @Units: deg/s // @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 AP_GROUPEND
}; };
Copter::ModeFlowHold::ModeFlowHold(void) : Mode() ModeFlowHold::ModeFlowHold(void) : Mode()
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
@ -82,7 +82,7 @@ Copter::ModeFlowHold::ModeFlowHold(void) : Mode()
#define CONTROL_FLOWHOLD_EARTH_FRAME 0 #define CONTROL_FLOWHOLD_EARTH_FRAME 0
// flowhold_init - initialise flowhold controller // 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()) { if (!copter.optflow.enabled() || !copter.optflow.healthy()) {
return false; 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 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(); 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 // flowhold_run - runs the flowhold controller
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModeFlowHold::run() void ModeFlowHold::run()
{ {
float takeoff_climb_rate = 0.0f; float takeoff_climb_rate = 0.0f;
@ -342,7 +342,7 @@ void Copter::ModeFlowHold::run()
/* /*
update height estimate using integrated accelerometer ratio with optical flow 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; float ins_height = copter.inertial_nav.get_altitude() * 0.01;

16
ArduCopter/mode_follow.cpp
View File

@ -14,17 +14,17 @@
*/ */
// initialise follow mode // initialise follow mode
bool Copter::ModeFollow::init(const bool ignore_checks) bool ModeFollow::init(const bool ignore_checks)
{ {
if (!g2.follow.enabled()) { if (!g2.follow.enabled()) {
gcs().send_text(MAV_SEVERITY_WARNING, "Set FOLL_ENABLE = 1"); gcs().send_text(MAV_SEVERITY_WARNING, "Set FOLL_ENABLE = 1");
return false; return false;
} }
// re-use guided mode // 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 not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) { if (is_disarmed_or_landed()) {
@ -148,17 +148,17 @@ void Copter::ModeFollow::run()
last_log_ms = now; last_log_ms = now;
} }
// re-use guided mode's velocity controller (takes NEU) // 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; 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; 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 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 dist = g2.follow.get_distance_to_target();
float bearing = g2.follow.get_bearing_to_target(); float bearing = g2.follow.get_bearing_to_target();

66
ArduCopter/mode_guided.cpp
View File

@ -38,7 +38,7 @@ struct Guided_Limit {
} guided_limit; } guided_limit;
// guided_init - initialise guided controller // guided_init - initialise guided controller
bool Copter::ModeGuided::init(bool ignore_checks) bool ModeGuided::init(bool ignore_checks)
{ {
// start in position control mode // start in position control mode
pos_control_start(); 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 // 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; 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 // initialise guided mode's position controller
void Copter::ModeGuided::pos_control_start() void ModeGuided::pos_control_start()
{ {
// set to position control mode // set to position control mode
guided_mode = Guided_WP; guided_mode = Guided_WP;
@ -95,7 +95,7 @@ void Copter::ModeGuided::pos_control_start()
} }
// initialise guided mode's velocity controller // initialise guided mode's velocity controller
void Copter::ModeGuided::vel_control_start() void ModeGuided::vel_control_start()
{ {
// set guided_mode to velocity controller // set guided_mode to velocity controller
guided_mode = Guided_Velocity; guided_mode = Guided_Velocity;
@ -113,7 +113,7 @@ void Copter::ModeGuided::vel_control_start()
} }
// initialise guided mode's posvel controller // initialise guided mode's posvel controller
void Copter::ModeGuided::posvel_control_start() void ModeGuided::posvel_control_start()
{ {
// set guided_mode to velocity controller // set guided_mode to velocity controller
guided_mode = Guided_PosVel; guided_mode = Guided_PosVel;
@ -139,13 +139,13 @@ void Copter::ModeGuided::posvel_control_start()
auto_yaw.set_mode(AUTO_YAW_HOLD); 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; return guided_mode == Guided_TakeOff;
} }
// initialise guided mode's angle controller // initialise guided mode's angle controller
void Copter::ModeGuided::angle_control_start() void ModeGuided::angle_control_start()
{ {
// set guided_mode to velocity controller // set guided_mode to velocity controller
guided_mode = Guided_Angle; guided_mode = Guided_Angle;
@ -176,7 +176,7 @@ void Copter::ModeGuided::angle_control_start()
// guided_set_destination - sets guided mode's target destination // guided_set_destination - sets guided mode's target destination
// Returns true if the fence is enabled and guided waypoint is within the fence // Returns true if the fence is enabled and guided waypoint is within the fence
// else return false if the waypoint is outside 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 // ensure we are in position control mode
if (guided_mode != Guided_WP) { if (guided_mode != Guided_WP) {
@ -204,7 +204,7 @@ bool Copter::ModeGuided::set_destination(const Vector3f& destination, bool use_y
return true; return true;
} }
bool Copter::ModeGuided::get_wp(Location& destination) bool ModeGuided::get_wp(Location& destination)
{ {
if (guided_mode != Guided_WP) { if (guided_mode != Guided_WP) {
return false; return false;
@ -215,7 +215,7 @@ bool Copter::ModeGuided::get_wp(Location& destination)
// sets guided mode's target from a Location object // sets guided mode's target from a Location object
// returns false if destination could not be set (probably caused by missing terrain data) // 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 // 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 // ensure we are in position control mode
if (guided_mode != Guided_WP) { 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 // 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 // check we are in velocity control mode
if (guided_mode != Guided_Velocity) { 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 // 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 // check we are in velocity control mode
if (guided_mode != Guided_PosVel) { if (guided_mode != Guided_PosVel) {
@ -301,7 +301,7 @@ bool Copter::ModeGuided::set_destination_posvel(const Vector3f& destination, con
} }
// set guided mode angle target // 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 // check we are in velocity control mode
if (guided_mode != Guided_Angle) { 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(); guided_angle_state.update_time_ms = millis();
// interpret positive climb rate as triggering take-off // 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); 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 // guided_run - runs the guided controller
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModeGuided::run() void ModeGuided::run()
{ {
// call the correct auto controller // call the correct auto controller
switch (guided_mode) { switch (guided_mode) {
@ -366,7 +366,7 @@ void Copter::ModeGuided::run()
// guided_takeoff_run - takeoff in guided mode // guided_takeoff_run - takeoff in guided mode
// called by guided_run at 100hz or more // called by guided_run at 100hz or more
void Copter::ModeGuided::takeoff_run() void ModeGuided::takeoff_run()
{ {
auto_takeoff_run(); auto_takeoff_run();
if (wp_nav->reached_wp_destination()) { 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 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(); make_safe_spool_down();
wp_nav->shift_wp_origin_to_current_pos(); wp_nav->shift_wp_origin_to_current_pos();
return; return;
@ -413,7 +413,7 @@ void Copter::Mode::auto_takeoff_run()
// guided_pos_control_run - runs the guided position controller // guided_pos_control_run - runs the guided position controller
// called from guided_run // called from guided_run
void Copter::ModeGuided::pos_control_run() void ModeGuided::pos_control_run()
{ {
// process pilot's yaw input // process pilot's yaw input
float target_yaw_rate = 0; float target_yaw_rate = 0;
@ -455,7 +455,7 @@ void Copter::ModeGuided::pos_control_run()
// guided_vel_control_run - runs the guided velocity controller // guided_vel_control_run - runs the guided velocity controller
// called from guided_run // called from guided_run
void Copter::ModeGuided::vel_control_run() void ModeGuided::vel_control_run()
{ {
// process pilot's yaw input // process pilot's yaw input
float target_yaw_rate = 0; float target_yaw_rate = 0;
@ -507,7 +507,7 @@ void Copter::ModeGuided::vel_control_run()
// guided_posvel_control_run - runs the guided spline controller // guided_posvel_control_run - runs the guided spline controller
// called from guided_run // called from guided_run
void Copter::ModeGuided::posvel_control_run() void ModeGuided::posvel_control_run()
{ {
// process pilot's yaw input // process pilot's yaw input
float target_yaw_rate = 0; float target_yaw_rate = 0;
@ -572,7 +572,7 @@ void Copter::ModeGuided::posvel_control_run()
// guided_angle_control_run - runs the guided angle controller // guided_angle_control_run - runs the guided angle controller
// called from guided_run // called from guided_run
void Copter::ModeGuided::angle_control_run() void ModeGuided::angle_control_run()
{ {
// constrain desired lean angles // constrain desired lean angles
float roll_in = guided_angle_state.roll_cd; 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 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(); make_safe_spool_down();
return; return;
} }
// TODO: use get_alt_hold_state // TODO: use get_alt_hold_state
// landed with positive desired climb rate, takeoff // 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(); zero_throttle_and_relax_ac();
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
if (motors->get_spool_state() == AP_Motors::SpoolState::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 // 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 // get current desired velocity
Vector3f curr_vel_des = pos_control->get_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 // 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) { if (use_yaw) {
auto_yaw.set_fixed_yaw(yaw_cd * 0.01f, 0.0f, 0, relative_angle); 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 code
// guided_limit_clear - clear/turn off guided limits // guided_limit_clear - clear/turn off guided limits
void Copter::ModeGuided::limit_clear() void ModeGuided::limit_clear()
{ {
guided_limit.timeout_ms = 0; guided_limit.timeout_ms = 0;
guided_limit.alt_min_cm = 0.0f; 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 // 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.timeout_ms = timeout_ms;
guided_limit.alt_min_cm = alt_min_cm; 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 // 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 // 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 // initialise start time
guided_limit.start_time = AP_HAL::millis(); 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 // guided_limit_check - returns true if guided mode has breached a limit
// used when guided is invoked from the NAV_GUIDED_ENABLE mission command // 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 // check if we have passed the timeout
if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) { 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()) { switch(mode()) {
case Guided_WP: 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()) { switch(mode()) {
case Guided_WP: 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) { if (mode() == Guided_WP) {
return wp_nav->crosstrack_error(); return wp_nav->crosstrack_error();

8
ArduCopter/mode_guided_nogps.cpp
View File

@ -7,19 +7,19 @@
*/ */
// initialise guided_nogps controller // initialise guided_nogps controller
bool Copter::ModeGuidedNoGPS::init(bool ignore_checks) bool ModeGuidedNoGPS::init(bool ignore_checks)
{ {
// start in angle control mode // start in angle control mode
Copter::ModeGuided::angle_control_start(); ModeGuided::angle_control_start();
return true; return true;
} }
// guided_run - runs the guided controller // guided_run - runs the guided controller
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModeGuidedNoGPS::run() void ModeGuidedNoGPS::run()
{ {
// run angle controller // run angle controller
Copter::ModeGuided::angle_control_run(); ModeGuided::angle_control_run();
} }
#endif #endif

16
ArduCopter/mode_land.cpp
View File

@ -7,7 +7,7 @@ static uint32_t land_start_time;
static bool land_pause; static bool land_pause;
// land_init - initialise land controller // 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 // check if we have GPS and decide which LAND we're going to do
land_with_gps = copter.position_ok(); land_with_gps = copter.position_ok();
@ -32,14 +32,14 @@ bool Copter::ModeLand::init(bool ignore_checks)
land_pause = false; land_pause = false;
// reset flag indicating if pilot has applied roll or pitch inputs during landing // 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; return true;
} }
// land_run - runs the land controller // land_run - runs the land controller
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModeLand::run() void ModeLand::run()
{ {
if (land_with_gps) { if (land_with_gps) {
gps_run(); gps_run();
@ -51,10 +51,10 @@ void Copter::ModeLand::run()
// land_gps_run - runs the land controller // land_gps_run - runs the land controller
// horizontal position controlled with loiter controller // horizontal position controlled with loiter controller
// should be called at 100hz or more // 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 // 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(); copter.arming.disarm();
} }
@ -79,7 +79,7 @@ void Copter::ModeLand::gps_run()
// land_nogps_run - runs the land controller // land_nogps_run - runs the land controller
// pilot controls roll and pitch angles // pilot controls roll and pitch angles
// should be called at 100hz or more // 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_roll = 0.0f, target_pitch = 0.0f;
float target_yaw_rate = 0; float target_yaw_rate = 0;
@ -108,7 +108,7 @@ void Copter::ModeLand::nogps_run()
} }
// disarm when the landing detector says we've landed // 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(); 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 // 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 // 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 // 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; land_with_gps = false;
} }

16
ArduCopter/mode_loiter.cpp
View File

@ -7,7 +7,7 @@
*/ */
// loiter_init - initialise loiter controller // loiter_init - initialise loiter controller
bool Copter::ModeLoiter::init(bool ignore_checks) bool ModeLoiter::init(bool ignore_checks)
{ {
if (!copter.failsafe.radio) { if (!copter.failsafe.radio) {
float target_roll, target_pitch; float target_roll, target_pitch;
@ -35,12 +35,12 @@ bool Copter::ModeLoiter::init(bool ignore_checks)
} }
#if PRECISION_LANDING == ENABLED #if PRECISION_LANDING == ENABLED
bool Copter::ModeLoiter::do_precision_loiter() bool ModeLoiter::do_precision_loiter()
{ {
if (!_precision_loiter_enabled) { if (!_precision_loiter_enabled) {
return false; return false;
} }
if (ap.land_complete_maybe) { if (copter.ap.land_complete_maybe) {
return false; // don't move on the ground return false; // don't move on the ground
} }
// if the pilot *really* wants to move the vehicle, let them.... // if the pilot *really* wants to move the vehicle, let them....
@ -53,7 +53,7 @@ bool Copter::ModeLoiter::do_precision_loiter()
return true; return true;
} }
void Copter::ModeLoiter::precision_loiter_xy() void ModeLoiter::precision_loiter_xy()
{ {
loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->clear_pilot_desired_acceleration();
Vector2f target_pos, target_vel_rel; Vector2f target_pos, target_vel_rel;
@ -72,7 +72,7 @@ void Copter::ModeLoiter::precision_loiter_xy()
// loiter_run - runs the loiter controller // loiter_run - runs the loiter controller
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModeLoiter::run() void ModeLoiter::run()
{ {
float target_roll, target_pitch; float target_roll, target_pitch;
float target_yaw_rate = 0.0f; float target_yaw_rate = 0.0f;
@ -106,7 +106,7 @@ void Copter::ModeLoiter::run()
} }
// relax loiter target if we might be landed // relax loiter target if we might be landed
if (ap.land_complete_maybe) { if (copter.ap.land_complete_maybe) {
loiter_nav->soften_for_landing(); 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(); 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(); return loiter_nav->get_bearing_to_target();
} }

22
ArduCopter/mode_poshold.cpp
View File

@ -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 #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 // poshold_init - initialise PosHold controller
bool Copter::ModePosHold::init(bool ignore_checks) bool ModePosHold::init(bool ignore_checks)
{ {
// initialize vertical speeds and acceleration // initialize vertical speeds and acceleration
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); 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 // compute brake_gain
brake_gain = (15.0f * (float)g.poshold_brake_rate + 95.0f) / 100.0f; 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 // if landed begin in loiter mode
roll_mode = RPMode::LOITER; roll_mode = RPMode::LOITER;
pitch_mode = RPMode::LOITER; pitch_mode = RPMode::LOITER;
@ -68,7 +68,7 @@ bool Copter::ModePosHold::init(bool ignore_checks)
// poshold_run - runs the PosHold controller // poshold_run - runs the PosHold controller
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModePosHold::run() void ModePosHold::run()
{ {
float takeoff_climb_rate = 0.0f; 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 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); target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
// relax loiter target if we might be landed // relax loiter target if we might be landed
if (ap.land_complete_maybe) { if (copter.ap.land_complete_maybe) {
loiter_nav->soften_for_landing(); 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 // 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 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)) { 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_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 // 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); mix_ratio = constrain_float(mix_ratio, 0.0f, 1.0f);
return (int16_t)((mix_ratio * first_control) + ((1.0f-mix_ratio)*second_control)); 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 // 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 // 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) // 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; float lean_angle;
int16_t brake_rate = g.poshold_brake_rate; 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 // update_wind_comp_estimate - updates wind compensation estimate
// should be called at the maximum loop rate when loiter is engaged // 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 // check wind estimate start has not been delayed
if (wind_comp_start_timer > 0) { 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 // get_wind_comp_lean_angles - retrieve wind compensation angles in body frame roll and pitch angles
// should be called at the maximum loop rate // 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 // reduce rate to 10hz
wind_comp_timer++; 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 // 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; roll_mode = RPMode::CONTROLLER_TO_PILOT_OVERRIDE;
controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; 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 // 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; pitch_mode = RPMode::CONTROLLER_TO_PILOT_OVERRIDE;
controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;

44
ArduCopter/mode_rtl.cpp
View File

@ -10,7 +10,7 @@
*/ */
// rtl_init - initialise rtl controller // rtl_init - initialise rtl controller
bool Copter::ModeRTL::init(bool ignore_checks) bool ModeRTL::init(bool ignore_checks)
{ {
if (!ignore_checks) { if (!ignore_checks) {
if (!AP::ahrs().home_is_set()) { if (!AP::ahrs().home_is_set()) {
@ -26,7 +26,7 @@ bool Copter::ModeRTL::init(bool ignore_checks)
} }
// re-start RTL with terrain following disabled // 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); AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RESTARTED_RTL);
if (rtl_path.terrain_used) { if (rtl_path.terrain_used) {
@ -39,7 +39,7 @@ void Copter::ModeRTL::restart_without_terrain()
// rtl_run - runs the return-to-launch controller // rtl_run - runs the return-to-launch controller
// should be called at 100hz or more // 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()) { if (!motors->armed()) {
return; return;
@ -105,7 +105,7 @@ void Copter::ModeRTL::run(bool disarm_on_land)
} }
// rtl_climb_start - initialise climb to RTL altitude // rtl_climb_start - initialise climb to RTL altitude
void Copter::ModeRTL::climb_start() void ModeRTL::climb_start()
{ {
_state = RTL_InitialClimb; _state = RTL_InitialClimb;
_state_complete = false; _state_complete = false;
@ -129,7 +129,7 @@ void Copter::ModeRTL::climb_start()
} }
// rtl_return_start - initialise return to home // rtl_return_start - initialise return to home
void Copter::ModeRTL::return_start() void ModeRTL::return_start()
{ {
_state = RTL_ReturnHome; _state = RTL_ReturnHome;
_state_complete = false; _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 // 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 // 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 not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) { if (is_disarmed_or_landed()) {
@ -186,7 +186,7 @@ void Copter::ModeRTL::climb_return_run()
} }
// rtl_loiterathome_start - initialise return to home // rtl_loiterathome_start - initialise return to home
void Copter::ModeRTL::loiterathome_start() void ModeRTL::loiterathome_start()
{ {
_state = RTL_LoiterAtHome; _state = RTL_LoiterAtHome;
_state_complete = false; _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 // 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 // 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 not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) { if (is_disarmed_or_landed()) {
@ -253,7 +253,7 @@ void Copter::ModeRTL::loiterathome_run()
} }
// rtl_descent_start - initialise descent to final alt // rtl_descent_start - initialise descent to final alt
void Copter::ModeRTL::descent_start() void ModeRTL::descent_start()
{ {
_state = RTL_FinalDescent; _state = RTL_FinalDescent;
_state_complete = false; _state_complete = false;
@ -270,7 +270,7 @@ void Copter::ModeRTL::descent_start()
// rtl_descent_run - implements the final descent to the RTL_ALT // rtl_descent_run - implements the final descent to the RTL_ALT
// called by rtl_run at 100hz or more // called by rtl_run at 100hz or more
void Copter::ModeRTL::descent_run() void ModeRTL::descent_run()
{ {
float target_roll = 0.0f; float target_roll = 0.0f;
float target_pitch = 0.0f; float target_pitch = 0.0f;
@ -301,10 +301,10 @@ void Copter::ModeRTL::descent_run()
// record if pilot has overridden roll or pitch // record if pilot has overridden roll or pitch
if (!is_zero(target_roll) || !is_zero(target_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); 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 // rtl_loiterathome_start - initialise controllers to loiter over home
void Copter::ModeRTL::land_start() void ModeRTL::land_start()
{ {
_state = RTL_Land; _state = RTL_Land;
_state_complete = false; _state_complete = false;
@ -351,12 +351,12 @@ void Copter::ModeRTL::land_start()
auto_yaw.set_mode(AUTO_YAW_HOLD); auto_yaw.set_mode(AUTO_YAW_HOLD);
} }
bool Copter::ModeRTL::is_landing() const bool ModeRTL::is_landing() const
{ {
return _state == RTL_Land; return _state == RTL_Land;
} }
bool Copter::ModeRTL::landing_gear_should_be_deployed() const bool ModeRTL::landing_gear_should_be_deployed() const
{ {
switch(_state) { switch(_state) {
case RTL_LoiterAtHome: case RTL_LoiterAtHome:
@ -371,13 +371,13 @@ bool Copter::ModeRTL::landing_gear_should_be_deployed() const
// rtl_returnhome_run - return home // rtl_returnhome_run - return home
// called by rtl_run at 100hz or more // 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 // 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 // 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(); copter.arming.disarm();
} }
@ -395,7 +395,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
land_run_vertical_control(); land_run_vertical_control();
} }
void Copter::ModeRTL::build_path() void ModeRTL::build_path()
{ {
// origin point is our stopping point // origin point is our stopping point
Vector3f stopping_point; Vector3f stopping_point;
@ -420,7 +420,7 @@ void Copter::ModeRTL::build_path()
// compute the return target - home or rally point // compute the return target - home or rally point
// return altitude in cm above home at which vehicle should return home // 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) // 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) // set return target to nearest rally point or home position (Note: alt is absolute)
#if AC_RALLY == ENABLED #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); 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(); 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(); return wp_nav->get_wp_bearing_to_destination();
} }

18
ArduCopter/mode_smart_rtl.cpp
View File

@ -9,7 +9,7 @@
* Once the copter is close to home, it will run a standard land controller. * 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()) { if (g2.smart_rtl.is_active()) {
// initialise waypoint and spline controller // initialise waypoint and spline controller
@ -33,12 +33,12 @@ bool Copter::ModeSmartRTL::init(bool ignore_checks)
} }
// perform cleanup required when leaving smart_rtl // perform cleanup required when leaving smart_rtl
void Copter::ModeSmartRTL::exit() void ModeSmartRTL::exit()
{ {
g2.smart_rtl.cancel_request_for_thorough_cleanup(); g2.smart_rtl.cancel_request_for_thorough_cleanup();
} }
void Copter::ModeSmartRTL::run() void ModeSmartRTL::run()
{ {
switch (smart_rtl_state) { switch (smart_rtl_state) {
case SmartRTL_WaitForPathCleanup: 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 // hover at current target position
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); 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; float target_yaw_rate = 0.0f;
if (!copter.failsafe.radio) { 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 we are close to 2m above start point, we are ready to land.
if (wp_nav->reached_wp_destination()) { 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 // 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); const bool should_save_position = motors->armed() && (copter.control_mode != SMART_RTL);
copter.g2.smart_rtl.update(copter.position_ok(), should_save_position); 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(); 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(); return wp_nav->get_wp_bearing_to_destination();
} }

4
ArduCopter/mode_sport.cpp
View File

@ -7,7 +7,7 @@
*/ */
// sport_init - initialise sport controller // sport_init - initialise sport controller
bool Copter::ModeSport::init(bool ignore_checks) bool ModeSport::init(bool ignore_checks)
{ {
// initialize vertical speed and acceleration // initialize vertical speed and acceleration
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); 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 // sport_run - runs the sport controller
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModeSport::run() void ModeSport::run()
{ {
float takeoff_climb_rate = 0.0f; float takeoff_climb_rate = 0.0f;

4
ArduCopter/mode_stabilize.cpp
View File

@ -6,7 +6,7 @@
// stabilize_run - runs the main stabilize controller // stabilize_run - runs the main stabilize controller
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModeStabilize::run() void ModeStabilize::run()
{ {
// apply simple mode transform to pilot inputs // apply simple mode transform to pilot inputs
update_simple_mode(); update_simple_mode();
@ -21,7 +21,7 @@ void Copter::ModeStabilize::run()
if (!motors->armed()) { if (!motors->armed()) {
// Motors should be Stopped // Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
} else if (ap.throttle_zero) { } else if (copter.ap.throttle_zero) {
// Attempting to Land // Attempting to Land
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else { } else {

4
ArduCopter/mode_stabilize_heli.cpp
View File

@ -6,7 +6,7 @@
*/ */
// stabilize_init - initialise stabilize controller // 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 // set stab collective true to use stabilize scaled collective pitch range
copter.input_manager.set_use_stab_col(true); 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 // stabilize_run - runs the main stabilize controller
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModeStabilize_Heli::run() void ModeStabilize_Heli::run()
{ {
float target_roll, target_pitch; float target_roll, target_pitch;
float target_yaw_rate; float target_yaw_rate;

12
ArduCopter/mode_throw.cpp
View File

@ -3,7 +3,7 @@
#if MODE_THROW_ENABLED == ENABLED #if MODE_THROW_ENABLED == ENABLED
// throw_init - initialise throw controller // throw_init - initialise throw controller
bool Copter::ModeThrow::init(bool ignore_checks) bool ModeThrow::init(bool ignore_checks)
{ {
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// do not allow helis to use throw to start // 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 // runs the throw to start controller
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModeThrow::run() void ModeThrow::run()
{ {
/* Throw State Machine /* Throw State Machine
Throw_Disarmed - motors are off 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 // Check that we have a valid navigation solution
nav_filter_status filt_status = inertial_nav.get_filter_status(); 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 // Check that we have uprighted the copter
const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned(); const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned();
return (rotMat.c.z > 0.866f); // is_upright 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 // Check that we are within 0.5m of the demanded height
return (pos_control->get_alt_error() < 50.0f); 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 // check that our horizontal position error is within 50cm
return (pos_control->get_horizontal_error() < 50.0f); return (pos_control->get_horizontal_error() < 50.0f);

16
ArduCopter/mode_zigzag.cpp
View File

@ -9,7 +9,7 @@
#define ZIGZAG_WP_RADIUS_CM 300 #define ZIGZAG_WP_RADIUS_CM 300
// initialise zigzag controller // 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 // initialize's loiter position and velocity on xy-axes from current pos and velocity
loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->clear_pilot_desired_acceleration();
@ -31,7 +31,7 @@ bool Copter::ModeZigZag::init(bool ignore_checks)
// run the zigzag controller // run the zigzag controller
// should be called at 100hz or more // should be called at 100hz or more
void Copter::ModeZigZag::run() void ModeZigZag::run()
{ {
// initialize vertical speed and acceleration's range // initialize vertical speed and acceleration's range
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); 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 // 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 // sanity check
if (dest_num > 1) { 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 // 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) { if (stage == AUTO) {
stage = MANUAL_REGAIN; 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 // 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 // process pilot's yaw input
float target_yaw_rate = 0; float target_yaw_rate = 0;
@ -161,7 +161,7 @@ void Copter::ModeZigZag::auto_control()
} }
// manual_control - process manual control // manual_control - process manual control
void Copter::ModeZigZag::manual_control() void ModeZigZag::manual_control()
{ {
float target_yaw_rate = 0.0f; float target_yaw_rate = 0.0f;
float target_climb_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 // 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 // check if wp_nav believes it has reached the destination
if (!wp_nav->reached_wp_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 // 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 // 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 // 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 // sanity check dest_num
if (dest_num > 1) { if (dest_num > 1) {

24
ArduCopter/takeoff.cpp
View File

@ -1,25 +1,25 @@
#include "Copter.h" #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. // 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 // 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 // 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 // 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); copter.flightmode->takeoff.start(takeoff_alt_cm);
return true; return true;
} }
// initiate user takeoff - called when MAVLink TAKEOFF command is received // 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()) { if (!copter.motors->armed()) {
return false; return false;
} }
if (!ap.land_complete) { if (!copter.ap.land_complete) {
// can't takeoff again! // can't takeoff again!
return false; 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 // 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; 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 // 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 // indicate we are taking off
copter.set_land_complete(false); copter.set_land_complete(false);
@ -69,7 +69,7 @@ void Copter::Mode::_TakeOff::start(float alt_cm)
} }
// stop takeoff // stop takeoff
void Copter::Mode::_TakeOff::stop() void Mode::_TakeOff::stop()
{ {
_running = false; _running = false;
start_ms = 0; start_ms = 0;
@ -79,7 +79,7 @@ void Copter::Mode::_TakeOff::stop()
// pilot_climb_rate is both an input and an output // pilot_climb_rate is both an input and an output
// takeoff_climb_rate is only an output // takeoff_climb_rate is only an output
// has side-effect of turning takeoff off when timeout as expired // 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) float& takeoff_climb_rate)
{ {
// return pilot_climb_rate if take-off inactive // 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 // start with our current altitude
auto_takeoff_no_nav_alt_cm = inertial_nav.get_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 call attitude controller for automatic takeoff, limiting roll/pitch
if below wp_navalt_min 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; 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); 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)) { if (!has_user_takeoff(false)) {
return false; return false;
} }
if (ap.land_complete) { if (copter.ap.land_complete) {
return false; return false;
} }
if (takeoff.running()) { if (takeoff.running()) {