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

View File

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

View File

@ -176,6 +176,19 @@
#include <SITL/SITL.h>
#endif
#if FRAME_CONFIG == HELI_FRAME
#define AC_AttitudeControl_t AC_AttitudeControl_Heli
#else
#define AC_AttitudeControl_t AC_AttitudeControl_Multi
#endif
#if FRAME_CONFIG == HELI_FRAME
#define MOTOR_CLASS AP_MotorsHeli
#else
#define MOTOR_CLASS AP_MotorsMulticopter
#endif
#include "mode.h"
class Copter : public AP_HAL::HAL::Callbacks {
public:
@ -194,6 +207,33 @@ public:
friend class RC_Channel_Copter;
friend class RC_Channels_Copter;
friend class AutoTune;
friend class Mode;
friend class ModeAcro;
friend class ModeAcro_Heli;
friend class ModeAltHold;
friend class ModeAuto;
friend class ModeAutoTune;
friend class ModeAvoidADSB;
friend class ModeBrake;
friend class ModeCircle;
friend class ModeDrift;
friend class ModeFlip;
friend class ModeFlowHold;
friend class ModeFollow;
friend class ModeGuided;
friend class ModeLand;
friend class ModeLoiter;
friend class ModePosHold;
friend class ModeRTL;
friend class ModeSmartRTL;
friend class ModeSport;
friend class ModeStabilize;
friend class ModeStabilize_Heli;
friend class ModeThrow;
friend class ModeZigZag;
Copter(void);
// HAL::Callbacks implementation.
@ -374,12 +414,6 @@ private:
} sensor_health;
// Motor Output
#if FRAME_CONFIG == HELI_FRAME
#define MOTOR_CLASS AP_MotorsHeli
#else
#define MOTOR_CLASS AP_MotorsMulticopter
#endif
MOTOR_CLASS *motors;
const struct AP_Param::GroupInfo *motors_var_info;
@ -428,11 +462,6 @@ private:
// Attitude, Position and Waypoint navigation objects
// To-Do: move inertial nav up or other navigation variables down here
#if FRAME_CONFIG == HELI_FRAME
#define AC_AttitudeControl_t AC_AttitudeControl_Heli
#else
#define AC_AttitudeControl_t AC_AttitudeControl_Multi
#endif
AC_AttitudeControl_t *attitude_control;
AC_PosControl *pos_control;
AC_WPNav *wp_nav;
@ -847,8 +876,6 @@ private:
void publish_osd_info();
#endif
#include "mode.h"
Mode *flightmode;
#if MODE_ACRO_ENABLED == ENABLED
#if FRAME_CONFIG == HELI_FRAME

View File

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

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.
// @Values: 0:Upward Throw,1:Drop
// @User: Standard
AP_GROUPINFO("THROW_TYPE", 4, ParametersG2, throw_type, Copter::ModeThrow::ThrowType_Upward),
AP_GROUPINFO("THROW_TYPE", 4, ParametersG2, throw_type, ModeThrow::ThrowType_Upward),
#endif
// @Param: GND_EFFECT_COMP
@ -882,7 +882,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED
// @Group: FHLD
// @Path: mode_flowhold.cpp
AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, Copter::ModeFlowHold),
AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, ModeFlowHold),
#endif
#if MODE_FOLLOW_ENABLED == ENABLED
@ -898,7 +898,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
#if AUTOTUNE_ENABLED == ENABLED
// @Group: AUTOTUNE_
// @Path: ../libraries/AC_AutoTune/AC_AutoTune.cpp
AP_SUBGROUPPTR(autotune_ptr, "AUTOTUNE_", 29, ParametersG2, Copter::AutoTune),
AP_SUBGROUPPTR(autotune_ptr, "AUTOTUNE_", 29, ParametersG2, AutoTune),
#endif
#ifdef ENABLE_SCRIPTING

View File

@ -2,6 +2,16 @@
#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.
//
class Parameters {

View File

@ -1,9 +1,9 @@
#include "Copter.h"
Copter::Mode::AutoYaw Copter::Mode::auto_yaw;
Mode::AutoYaw Mode::auto_yaw;
// roi_yaw - returns heading towards location held in roi
float Copter::Mode::AutoYaw::roi_yaw()
float Mode::AutoYaw::roi_yaw()
{
roi_yaw_counter++;
if (roi_yaw_counter >= 4) {
@ -14,7 +14,7 @@ float Copter::Mode::AutoYaw::roi_yaw()
return _roi_yaw;
}
float Copter::Mode::AutoYaw::look_ahead_yaw()
float Mode::AutoYaw::look_ahead_yaw()
{
const Vector3f& vel = copter.inertial_nav.get_velocity();
float speed = norm(vel.x,vel.y);
@ -25,14 +25,14 @@ float Copter::Mode::AutoYaw::look_ahead_yaw()
return _look_ahead_yaw;
}
void Copter::Mode::AutoYaw::set_mode_to_default(bool rtl)
void Mode::AutoYaw::set_mode_to_default(bool rtl)
{
set_mode(default_mode(rtl));
}
// default_mode - returns auto_yaw.mode() based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
autopilot_yaw_mode Copter::Mode::AutoYaw::default_mode(bool rtl) const
autopilot_yaw_mode Mode::AutoYaw::default_mode(bool rtl) const
{
switch (copter.g.wp_yaw_behavior) {
@ -56,7 +56,7 @@ autopilot_yaw_mode Copter::Mode::AutoYaw::default_mode(bool rtl) const
}
// set_mode - sets the yaw mode for auto
void Copter::Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
{
// return immediately if no change
if (_mode == yaw_mode) {
@ -98,7 +98,7 @@ void Copter::Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
}
// set_fixed_yaw - sets the yaw look at heading for auto mode
void Copter::Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle)
void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle)
{
const int32_t curr_yaw_target = copter.attitude_control->get_att_target_euler_cd().z;
@ -130,7 +130,7 @@ void Copter::Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_dps,
}
// set_roi - sets the yaw to look at roi for auto mode
void Copter::Mode::AutoYaw::set_roi(const Location &roi_location)
void Mode::AutoYaw::set_roi(const Location &roi_location)
{
// if location is zero lat, lon and altitude turn off ROI
if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) {
@ -169,14 +169,14 @@ void Copter::Mode::AutoYaw::set_roi(const Location &roi_location)
}
// set auto yaw rate in centi-degrees per second
void Copter::Mode::AutoYaw::set_rate(float turn_rate_cds)
void Mode::AutoYaw::set_rate(float turn_rate_cds)
{
set_mode(AUTO_YAW_RATE);
_rate_cds = turn_rate_cds;
}
// yaw - returns target heading depending upon auto_yaw.mode()
float Copter::Mode::AutoYaw::yaw()
float Mode::AutoYaw::yaw()
{
switch (_mode) {
@ -207,7 +207,7 @@ float Copter::Mode::AutoYaw::yaw()
// returns yaw rate normally set by SET_POSITION_TARGET mavlink
// messages (positive is clockwise, negative is counter clockwise)
float Copter::Mode::AutoYaw::rate_cds() const
float Mode::AutoYaw::rate_cds() const
{
if (_mode == AUTO_YAW_RATE) {
return _rate_cds;

View File

@ -8,7 +8,7 @@
/*
constructor for Mode object
*/
Copter::Mode::Mode(void) :
Mode::Mode(void) :
g(copter.g),
g2(copter.g2),
wp_nav(copter.wp_nav),
@ -22,16 +22,15 @@ Copter::Mode::Mode(void) :
channel_pitch(copter.channel_pitch),
channel_throttle(copter.channel_throttle),
channel_yaw(copter.channel_yaw),
G_Dt(copter.G_Dt),
ap(copter.ap)
G_Dt(copter.G_Dt)
{ };
float Copter::Mode::auto_takeoff_no_nav_alt_cm = 0;
float Mode::auto_takeoff_no_nav_alt_cm = 0;
// return the static controller object corresponding to supplied mode
Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode)
Mode *Copter::mode_from_mode_num(const uint8_t mode)
{
Copter::Mode *ret = nullptr;
Mode *ret = nullptr;
switch (mode) {
#if MODE_ACRO_ENABLED == ENABLED
@ -144,7 +143,7 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode)
#if OPTFLOW == ENABLED
case FLOWHOLD:
ret = (Copter::Mode *)g2.mode_flowhold_ptr;
ret = (Mode *)g2.mode_flowhold_ptr;
break;
#endif
@ -181,7 +180,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
return true;
}
Copter::Mode *new_flightmode = mode_from_mode_num(mode);
Mode *new_flightmode = mode_from_mode_num(mode);
if (new_flightmode == nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING,"No such mode");
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
@ -271,8 +270,8 @@ void Copter::update_flight_mode()
}
// exit_mode - high level call to organise cleanup as a flight mode is exited
void Copter::exit_mode(Copter::Mode *&old_flightmode,
Copter::Mode *&new_flightmode)
void Copter::exit_mode(Mode *&old_flightmode,
Mode *&new_flightmode)
{
#if AUTOTUNE_ENABLED == ENABLED
if (old_flightmode == &mode_autotune) {
@ -335,7 +334,7 @@ void Copter::notify_flight_mode() {
notify.set_flight_mode_str(flightmode->name4());
}
void Copter::Mode::update_navigation()
void Mode::update_navigation()
{
// run autopilot to make high level decisions about control modes
run_autopilot();
@ -343,7 +342,7 @@ void Copter::Mode::update_navigation()
// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
void Copter::Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
void Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
{
// fetch roll and pitch inputs
roll_out = channel_roll->get_control_in();
@ -371,7 +370,7 @@ void Copter::Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_o
// roll_out and pitch_out are returned
}
bool Copter::Mode::_TakeOff::triggered(const float target_climb_rate) const
bool Mode::_TakeOff::triggered(const float target_climb_rate) const
{
if (!copter.ap.land_complete) {
// can't take off if we're already flying
@ -390,15 +389,15 @@ bool Copter::Mode::_TakeOff::triggered(const float target_climb_rate) const
return true;
}
bool Copter::Mode::is_disarmed_or_landed() const
bool Mode::is_disarmed_or_landed() const
{
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
if (!motors->armed() || !copter.ap.auto_armed || copter.ap.land_complete) {
return true;
}
return false;
}
void Copter::Mode::zero_throttle_and_relax_ac(bool spool_up)
void Mode::zero_throttle_and_relax_ac(bool spool_up)
{
if (spool_up) {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
@ -409,14 +408,14 @@ void Copter::Mode::zero_throttle_and_relax_ac(bool spool_up)
attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt);
}
void Copter::Mode::zero_throttle_and_hold_attitude()
void Mode::zero_throttle_and_hold_attitude()
{
// run attitude controller
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt);
}
void Copter::Mode::make_safe_spool_down()
void Mode::make_safe_spool_down()
{
// command aircraft to initiate the shutdown process
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
@ -445,7 +444,7 @@ void Copter::Mode::make_safe_spool_down()
/*
get a height above ground estimate for landing
*/
int32_t Copter::Mode::get_alt_above_ground_cm(void)
int32_t Mode::get_alt_above_ground_cm(void)
{
int32_t alt_above_ground;
if (copter.rangefinder_alt_ok()) {
@ -459,11 +458,11 @@ int32_t Copter::Mode::get_alt_above_ground_cm(void)
return alt_above_ground;
}
void Copter::Mode::land_run_vertical_control(bool pause_descent)
void Mode::land_run_vertical_control(bool pause_descent)
{
#if PRECISION_LANDING == ENABLED
const bool navigating = pos_control->is_active_xy();
bool doing_precision_landing = !ap.land_repo_active && copter.precland.target_acquired() && navigating;
bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired() && navigating;
#else
bool doing_precision_landing = false;
#endif
@ -502,14 +501,14 @@ void Copter::Mode::land_run_vertical_control(bool pause_descent)
pos_control->update_z_controller();
}
void Copter::Mode::land_run_horizontal_control()
void Mode::land_run_horizontal_control()
{
float target_roll = 0.0f;
float target_pitch = 0.0f;
float target_yaw_rate = 0;
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
if (copter.ap.land_complete_maybe) {
loiter_nav->soften_for_landing();
}
@ -532,10 +531,10 @@ void Copter::Mode::land_run_horizontal_control()
// record if pilot has overridden roll or pitch
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
if (!ap.land_repo_active) {
if (!copter.ap.land_repo_active) {
copter.Log_Write_Event(DATA_LAND_REPO_ACTIVE);
}
ap.land_repo_active = true;
copter.ap.land_repo_active = true;
}
}
@ -547,7 +546,7 @@ void Copter::Mode::land_run_horizontal_control()
}
#if PRECISION_LANDING == ENABLED
bool doing_precision_landing = !ap.land_repo_active && copter.precland.target_acquired();
bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired();
// run precision landing
if (doing_precision_landing) {
Vector2f target_pos, target_vel_rel;
@ -603,7 +602,7 @@ void Copter::Mode::land_run_horizontal_control()
}
}
float Copter::Mode::throttle_hover() const
float Mode::throttle_hover() const
{
return motors->get_throttle_hover();
}
@ -612,7 +611,7 @@ float Copter::Mode::throttle_hover() const
// used only for manual throttle modes
// thr_mid should be in the range 0 to 1
// returns throttle output 0 to 1
float Copter::Mode::get_pilot_desired_throttle() const
float Mode::get_pilot_desired_throttle() const
{
const float thr_mid = throttle_hover();
int16_t throttle_control = channel_throttle->get_control_in();
@ -640,7 +639,7 @@ float Copter::Mode::get_pilot_desired_throttle() const
return throttle_out;
}
Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_climb_rate_cms)
Mode::AltHoldModeState Mode::get_alt_hold_state(float target_climb_rate_cms)
{
// Alt Hold State Machine Determination
if (!motors->armed()) {
@ -665,9 +664,9 @@ Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_cli
// the aircraft should progress through the take off procedure
return AltHold_Takeoff;
} else if (!ap.auto_armed || ap.land_complete) {
} else if (!copter.ap.auto_armed || copter.ap.land_complete) {
// the aircraft is armed and landed
if (target_climb_rate_cms < 0.0f && !ap.using_interlock) {
if (target_climb_rate_cms < 0.0f && !copter.ap.using_interlock) {
// the aircraft should move to a ground idle state
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
@ -695,56 +694,56 @@ Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_cli
// pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base
// class.
float Copter::Mode::get_pilot_desired_yaw_rate(int16_t stick_angle)
float Mode::get_pilot_desired_yaw_rate(int16_t stick_angle)
{
return copter.get_pilot_desired_yaw_rate(stick_angle);
}
float Copter::Mode::get_pilot_desired_climb_rate(float throttle_control)
float Mode::get_pilot_desired_climb_rate(float throttle_control)
{
return copter.get_pilot_desired_climb_rate(throttle_control);
}
float Copter::Mode::get_non_takeoff_throttle()
float Mode::get_non_takeoff_throttle()
{
return copter.get_non_takeoff_throttle();
}
void Copter::Mode::update_simple_mode(void) {
void Mode::update_simple_mode(void) {
copter.update_simple_mode();
}
bool Copter::Mode::set_mode(control_mode_t mode, mode_reason_t reason)
bool Mode::set_mode(control_mode_t mode, mode_reason_t reason)
{
return copter.set_mode(mode, reason);
}
void Copter::Mode::set_land_complete(bool b)
void Mode::set_land_complete(bool b)
{
return copter.set_land_complete(b);
}
GCS_Copter &Copter::Mode::gcs()
GCS_Copter &Mode::gcs()
{
return copter.gcs();
}
void Copter::Mode::Log_Write_Event(Log_Event id)
void Mode::Log_Write_Event(Log_Event id)
{
return copter.logger.Write_Event(id);
}
void Copter::Mode::set_throttle_takeoff()
void Mode::set_throttle_takeoff()
{
return copter.set_throttle_takeoff();
}
float Copter::Mode::get_avoidance_adjusted_climbrate(float target_rate)
float Mode::get_avoidance_adjusted_climbrate(float target_rate)
{
return copter.get_avoidance_adjusted_climbrate(target_rate);
}
uint16_t Copter::Mode::get_pilot_speed_dn()
uint16_t Mode::get_pilot_speed_dn()
{
return copter.get_pilot_speed_dn();
}

View File

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

View File

@ -8,7 +8,7 @@
* Init and run calls for acro flight mode
*/
void Copter::ModeAcro::run()
void ModeAcro::run()
{
// convert the input to the desired body frame rate
float target_roll, target_pitch, target_yaw;
@ -17,7 +17,7 @@ void Copter::ModeAcro::run()
if (!motors->armed()) {
// Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
} else if (ap.throttle_zero) {
} else if (copter.ap.throttle_zero) {
// Attempting to Land
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
@ -59,7 +59,7 @@ void Copter::ModeAcro::run()
// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates
// returns desired angle rates in centi-degrees-per-second
void Copter::ModeAcro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
void ModeAcro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
{
float rate_limit;
Vector3f rate_ef_level, rate_bf_level, rate_bf_request;

View File

@ -8,7 +8,7 @@
*/
// heli_acro_init - initialise acro controller
bool Copter::ModeAcro_Heli::init(bool ignore_checks)
bool ModeAcro_Heli::init(bool ignore_checks)
{
// if heli is equipped with a flybar, then tell the attitude controller to pass through controls directly to servos
attitude_control->use_flybar_passthrough(motors->has_flybar(), motors->supports_yaw_passthrough());
@ -24,7 +24,7 @@ bool Copter::ModeAcro_Heli::init(bool ignore_checks)
// heli_acro_run - runs the acro controller
// should be called at 100hz or more
void Copter::ModeAcro_Heli::run()
void ModeAcro_Heli::run()
{
float target_roll, target_pitch, target_yaw;
float pilot_throttle_scaled;
@ -73,7 +73,7 @@ void Copter::ModeAcro_Heli::run()
// only mimic flybar response when trainer mode is disabled
if (g.acro_trainer == ACRO_TRAINER_DISABLED) {
// while landed always leak off target attitude to current attitude
if (ap.land_complete) {
if (copter.ap.land_complete) {
virtual_flybar(target_roll, target_pitch, target_yaw, 3.0f, 3.0f);
// while flying use acro balance parameters for leak rate
} else {
@ -123,7 +123,7 @@ void Copter::ModeAcro_Heli::run()
// virtual_flybar - acts like a flybar by leaking target atttitude back to current attitude
void Copter::ModeAcro_Heli::virtual_flybar( float &roll_out, float &pitch_out, float &yaw_out, float pitch_leak, float roll_leak)
void ModeAcro_Heli::virtual_flybar( float &roll_out, float &pitch_out, float &yaw_out, float pitch_leak, float roll_leak)
{
Vector3f rate_ef_level, rate_bf_level;

View File

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

View File

@ -20,13 +20,13 @@
*/
// auto_init - initialise auto controller
bool Copter::ModeAuto::init(bool ignore_checks)
bool ModeAuto::init(bool ignore_checks)
{
if (mission.num_commands() > 1 || ignore_checks) {
_mode = Auto_Loiter;
// reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips)
if (motors->armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) {
if (motors->armed() && copter.ap.land_complete && !mission.starts_with_takeoff_cmd()) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd");
return false;
}
@ -54,7 +54,7 @@ bool Copter::ModeAuto::init(bool ignore_checks)
// auto_run - runs the auto controller
// should be called at 100hz or more
// relies on run_autopilot being called at 10hz which handles decision making and non-navigation related commands
void Copter::ModeAuto::run()
void ModeAuto::run()
{
// call the correct auto controller
switch (_mode) {
@ -106,7 +106,7 @@ void Copter::ModeAuto::run()
// auto_loiter_start - initialises loitering in auto mode
// returns success/failure because this can be called by exit_mission
bool Copter::ModeAuto::loiter_start()
bool ModeAuto::loiter_start()
{
// return failure if GPS is bad
if (!copter.position_ok()) {
@ -128,7 +128,7 @@ bool Copter::ModeAuto::loiter_start()
}
// auto_rtl_start - initialises RTL in AUTO flight mode
void Copter::ModeAuto::rtl_start()
void ModeAuto::rtl_start()
{
_mode = Auto_RTL;
@ -137,7 +137,7 @@ void Copter::ModeAuto::rtl_start()
}
// auto_takeoff_start - initialises waypoint controller to implement take-off
void Copter::ModeAuto::takeoff_start(const Location& dest_loc)
void ModeAuto::takeoff_start(const Location& dest_loc)
{
_mode = Auto_TakeOff;
@ -183,7 +183,7 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc)
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
void Copter::ModeAuto::wp_start(const Vector3f& destination)
void ModeAuto::wp_start(const Vector3f& destination)
{
_mode = Auto_WP;
@ -198,7 +198,7 @@ void Copter::ModeAuto::wp_start(const Vector3f& destination)
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
void Copter::ModeAuto::wp_start(const Location& dest_loc)
void ModeAuto::wp_start(const Location& dest_loc)
{
_mode = Auto_WP;
@ -217,7 +217,7 @@ void Copter::ModeAuto::wp_start(const Location& dest_loc)
}
// auto_land_start - initialises controller to implement a landing
void Copter::ModeAuto::land_start()
void ModeAuto::land_start()
{
// set target to stopping point
Vector3f stopping_point;
@ -228,7 +228,7 @@ void Copter::ModeAuto::land_start()
}
// auto_land_start - initialises controller to implement a landing
void Copter::ModeAuto::land_start(const Vector3f& destination)
void ModeAuto::land_start(const Vector3f& destination)
{
_mode = Auto_Land;
@ -247,7 +247,7 @@ void Copter::ModeAuto::land_start(const Vector3f& destination)
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
// we assume the caller has performed all required GPS_ok checks
void Copter::ModeAuto::circle_movetoedge_start(const Location &circle_center, float radius_m)
void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radius_m)
{
// convert location to vector from ekf origin
Vector3f circle_center_neu;
@ -305,7 +305,7 @@ void Copter::ModeAuto::circle_movetoedge_start(const Location &circle_center, fl
// auto_circle_start - initialises controller to fly a circle in AUTO flight mode
// assumes that circle_nav object has already been initialised with circle center and radius
void Copter::ModeAuto::circle_start()
void ModeAuto::circle_start()
{
_mode = Auto_Circle;
@ -315,7 +315,7 @@ void Copter::ModeAuto::circle_start()
// auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller
// seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE. If Straight or Spline the next_destination should be provided
void Copter::ModeAuto::spline_start(const Location& destination, bool stopped_at_start,
void ModeAuto::spline_start(const Location& destination, bool stopped_at_start,
AC_WPNav::spline_segment_end_type seg_end_type,
const Location& next_destination)
{
@ -337,7 +337,7 @@ void Copter::ModeAuto::spline_start(const Location& destination, bool stopped_at
#if NAV_GUIDED == ENABLED
// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
void Copter::ModeAuto::nav_guided_start()
void ModeAuto::nav_guided_start()
{
_mode = Auto_NavGuided;
@ -349,7 +349,7 @@ void Copter::ModeAuto::nav_guided_start()
}
#endif //NAV_GUIDED
bool Copter::ModeAuto::is_landing() const
bool ModeAuto::is_landing() const
{
switch(_mode) {
case Auto_Land:
@ -362,12 +362,12 @@ bool Copter::ModeAuto::is_landing() const
return false;
}
bool Copter::ModeAuto::is_taking_off() const
bool ModeAuto::is_taking_off() const
{
return _mode == Auto_TakeOff;
}
bool Copter::ModeAuto::landing_gear_should_be_deployed() const
bool ModeAuto::landing_gear_should_be_deployed() const
{
switch(_mode) {
case Auto_Land:
@ -381,7 +381,7 @@ bool Copter::ModeAuto::landing_gear_should_be_deployed() const
}
// auto_payload_place_start - initialises controller to implement a placing
void Copter::ModeAuto::payload_place_start()
void ModeAuto::payload_place_start()
{
// set target to stopping point
Vector3f stopping_point;
@ -393,7 +393,7 @@ void Copter::ModeAuto::payload_place_start()
}
// start_command - this function will be called when the ap_mission lib wishes to start a new command
bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
{
// To-Do: logging when new commands start/end
if (copter.should_log(MASK_LOG_CMD)) {
@ -525,12 +525,12 @@ bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
}
// exit_mission - function that is called once the mission completes
void Copter::ModeAuto::exit_mission()
void ModeAuto::exit_mission()
{
// play a tone
AP_Notify::events.mission_complete = 1;
// if we are not on the ground switch to loiter or land
if (!ap.land_complete) {
if (!copter.ap.land_complete) {
// try to enter loiter but if that fails land
if (!loiter_start()) {
set_mode(LAND, MODE_REASON_MISSION_END);
@ -542,7 +542,7 @@ void Copter::ModeAuto::exit_mission()
}
// do_guided - start guided mode
bool Copter::ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
bool ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
{
// only process guided waypoint if we are in guided mode
if (copter.control_mode != GUIDED && !(copter.control_mode == AUTO && mode() == Auto_NavGuided)) {
@ -571,7 +571,7 @@ bool Copter::ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
return true;
}
uint32_t Copter::ModeAuto::wp_distance() const
uint32_t ModeAuto::wp_distance() const
{
switch (_mode) {
case Auto_Circle:
@ -583,7 +583,7 @@ uint32_t Copter::ModeAuto::wp_distance() const
}
}
int32_t Copter::ModeAuto::wp_bearing() const
int32_t ModeAuto::wp_bearing() const
{
switch (_mode) {
case Auto_Circle:
@ -595,7 +595,7 @@ int32_t Copter::ModeAuto::wp_bearing() const
}
}
bool Copter::ModeAuto::get_wp(Location& destination)
bool ModeAuto::get_wp(Location& destination)
{
switch (_mode) {
case Auto_NavGuided:
@ -608,7 +608,7 @@ bool Copter::ModeAuto::get_wp(Location& destination)
}
// update mission
void Copter::ModeAuto::run_autopilot()
void ModeAuto::run_autopilot()
{
mission.update();
}
@ -624,7 +624,7 @@ Return true if we do not recognize the command so that we move on to the next co
// verify_command - callback function called from ap-mission at 10hz or higher when a command is being run
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
bool Copter::ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
{
if (copter.flightmode != &copter.mode_auto) {
return false;
@ -730,7 +730,7 @@ bool Copter::ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
// auto_takeoff_run - takeoff in auto mode
// called by auto_run at 100hz or more
void Copter::ModeAuto::takeoff_run()
void ModeAuto::takeoff_run()
{
auto_takeoff_run();
if (wp_nav->reached_wp_destination()) {
@ -741,7 +741,7 @@ void Copter::ModeAuto::takeoff_run()
// auto_wp_run - runs the auto waypoint controller
// called by auto_run at 100hz or more
void Copter::ModeAuto::wp_run()
void ModeAuto::wp_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
@ -781,7 +781,7 @@ void Copter::ModeAuto::wp_run()
// auto_spline_run - runs the auto spline controller
// called by auto_run at 100hz or more
void Copter::ModeAuto::spline_run()
void ModeAuto::spline_run()
{
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
@ -821,7 +821,7 @@ void Copter::ModeAuto::spline_run()
// auto_land_run - lands in auto mode
// called by auto_run at 100hz or more
void Copter::ModeAuto::land_run()
void ModeAuto::land_run()
{
// if not armed set throttle to zero and exit immediately
@ -840,7 +840,7 @@ void Copter::ModeAuto::land_run()
// auto_rtl_run - rtl in AUTO flight mode
// called by auto_run at 100hz or more
void Copter::ModeAuto::rtl_run()
void ModeAuto::rtl_run()
{
// call regular rtl flight mode run function
copter.mode_rtl.run(false);
@ -848,7 +848,7 @@ void Copter::ModeAuto::rtl_run()
// auto_circle_run - circle in AUTO flight mode
// called by auto_run at 100hz or more
void Copter::ModeAuto::circle_run()
void ModeAuto::circle_run()
{
// call circle controller
copter.circle_nav->update();
@ -868,7 +868,7 @@ void Copter::ModeAuto::circle_run()
#if NAV_GUIDED == ENABLED
// auto_nav_guided_run - allows control by external navigation controller
// called by auto_run at 100hz or more
void Copter::ModeAuto::nav_guided_run()
void ModeAuto::nav_guided_run()
{
// call regular guided flight mode run function
copter.mode_guided.run();
@ -877,7 +877,7 @@ void Copter::ModeAuto::nav_guided_run()
// auto_loiter_run - loiter in AUTO flight mode
// called by auto_run at 100hz or more
void Copter::ModeAuto::loiter_run()
void ModeAuto::loiter_run()
{
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
@ -904,7 +904,7 @@ void Copter::ModeAuto::loiter_run()
// auto_loiter_run - loiter to altitude in AUTO flight mode
// called by auto_run at 100hz or more
void Copter::ModeAuto::loiter_to_alt_run()
void ModeAuto::loiter_to_alt_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (is_disarmed_or_landed() || !motors->get_interlock()) {
@ -955,7 +955,7 @@ void Copter::ModeAuto::loiter_to_alt_run()
}
// auto_payload_place_start - initialises controller to implement placement of a load
void Copter::ModeAuto::payload_place_start(const Vector3f& destination)
void ModeAuto::payload_place_start(const Vector3f& destination)
{
_mode = Auto_NavPayloadPlace;
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
@ -975,7 +975,7 @@ void Copter::ModeAuto::payload_place_start(const Vector3f& destination)
// auto_payload_place_run - places an object in auto mode
// called by auto_run at 100hz or more
void Copter::ModeAuto::payload_place_run()
void ModeAuto::payload_place_run()
{
if (!payload_place_run_should_run()) {
zero_throttle_and_relax_ac();
@ -1007,18 +1007,18 @@ void Copter::ModeAuto::payload_place_run()
}
}
bool Copter::ModeAuto::payload_place_run_should_run()
bool ModeAuto::payload_place_run_should_run()
{
// must be armed
if (!motors->armed()) {
return false;
}
// must be auto-armed
if (!ap.auto_armed) {
if (!copter.ap.auto_armed) {
return false;
}
// must not be landed
if (ap.land_complete) {
if (copter.ap.land_complete) {
return false;
}
// interlock must be enabled (i.e. unsafe)
@ -1029,7 +1029,7 @@ bool Copter::ModeAuto::payload_place_run_should_run()
return true;
}
void Copter::ModeAuto::payload_place_run_loiter()
void ModeAuto::payload_place_run_loiter()
{
// loiter...
land_run_horizontal_control();
@ -1045,7 +1045,7 @@ void Copter::ModeAuto::payload_place_run_loiter()
pos_control->update_z_controller();
}
void Copter::ModeAuto::payload_place_run_descend()
void ModeAuto::payload_place_run_descend()
{
land_run_horizontal_control();
land_run_vertical_control();
@ -1053,7 +1053,7 @@ void Copter::ModeAuto::payload_place_run_descend()
// terrain_adjusted_location: returns a Location with lat/lon from cmd
// and altitude from our current altitude adjusted for location
Location Copter::ModeAuto::terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const
Location ModeAuto::terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const
{
// convert to location class
Location target_loc(cmd.content.location);
@ -1077,13 +1077,13 @@ Location Copter::ModeAuto::terrain_adjusted_location(const AP_Mission::Mission_C
/********************************************************************************/
// do_takeoff - initiate takeoff navigation command
void Copter::ModeAuto::do_takeoff(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_takeoff(const AP_Mission::Mission_Command& cmd)
{
// Set wp navigation target to safe altitude above current position
takeoff_start(cmd.content.location);
}
Location Copter::ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd) const
Location ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd) const
{
Location ret(cmd.content.location);
@ -1108,7 +1108,7 @@ Location Copter::ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd)
}
// do_nav_wp - initiate move to next waypoint
void Copter::ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
Location target_loc = loc_from_cmd(cmd);
@ -1153,7 +1153,7 @@ void Copter::ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
}
// do_land - initiate landing procedure
void Copter::ModeAuto::do_land(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_land(const AP_Mission::Mission_Command& cmd)
{
// To-Do: check if we have already landed
@ -1176,7 +1176,7 @@ void Copter::ModeAuto::do_land(const AP_Mission::Mission_Command& cmd)
// do_loiter_unlimited - start loitering with no end conditions
// note: caller should set yaw_mode
void Copter::ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
// convert back to location
Location target_loc(cmd.content.location);
@ -1210,7 +1210,7 @@ void Copter::ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cm
}
// do_circle - initiate moving in a circle
void Copter::ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
{
const Location circle_center = loc_from_cmd(cmd);
@ -1223,7 +1223,7 @@ void Copter::ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
// do_loiter_time - initiate loitering at a point for a given time period
// note: caller should set yaw_mode
void Copter::ModeAuto::do_loiter_time(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_loiter_time(const AP_Mission::Mission_Command& cmd)
{
// re-use loiter unlimited
do_loiter_unlimited(cmd);
@ -1235,7 +1235,7 @@ void Copter::ModeAuto::do_loiter_time(const AP_Mission::Mission_Command& cmd)
// do_loiter_alt - initiate loitering at a point until a given altitude is reached
// note: caller should set yaw_mode
void Copter::ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
{
// re-use loiter unlimited
do_loiter_unlimited(cmd);
@ -1270,7 +1270,7 @@ void Copter::ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
}
// do_spline_wp - initiate move to next waypoint
void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
{
const Location target_loc = loc_from_cmd(cmd);
@ -1328,7 +1328,7 @@ void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
#if NAV_GUIDED == ENABLED
// do_nav_guided_enable - initiate accepting commands from external nav computer
void Copter::ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
{
if (cmd.p1 > 0) {
// start guided within auto
@ -1337,7 +1337,7 @@ void Copter::ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& c
}
// do_guided_limits - pass guided limits to guided controller
void Copter::ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd)
{
copter.mode_guided.limit_set(
cmd.p1 * 1000, // convert seconds to ms
@ -1348,7 +1348,7 @@ void Copter::ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd)
#endif // NAV_GUIDED
// do_nav_delay - Delay the next navigation command
void Copter::ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd)
{
nav_delay_time_start = millis();
@ -1366,18 +1366,18 @@ void Copter::ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd)
// Condition (May) commands
/********************************************************************************/
void Copter::ModeAuto::do_wait_delay(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_wait_delay(const AP_Mission::Mission_Command& cmd)
{
condition_start = millis();
condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
}
void Copter::ModeAuto::do_within_distance(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_within_distance(const AP_Mission::Mission_Command& cmd)
{
condition_value = cmd.content.distance.meters * 100;
}
void Copter::ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd)
{
auto_yaw.set_fixed_yaw(
cmd.content.yaw.angle_deg,
@ -1392,7 +1392,7 @@ void Copter::ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd)
void Copter::ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
{
if (cmd.content.speed.target_ms > 0) {
if (cmd.content.speed.speed_type == 2) {
@ -1405,7 +1405,7 @@ void Copter::ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
}
}
void Copter::ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd)
{
if (cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) {
if (!copter.set_home_to_current_location(false)) {
@ -1422,13 +1422,13 @@ void Copter::ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd)
// this involves either moving the camera to point at the ROI (region of interest)
// and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature
// TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
void Copter::ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd)
{
auto_yaw.set_roi(cmd.content.location);
}
// point the camera to a specified angle
void Copter::ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
{
#if MOUNT == ENABLED
if (!copter.camera_mount.has_pan_control()) {
@ -1440,7 +1440,7 @@ void Copter::ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
#if WINCH_ENABLED == ENABLED
// control winch based on mission command
void Copter::ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd)
{
// Note: we ignore the gripper num parameter because we only support one gripper
switch (cmd.content.winch.action) {
@ -1464,7 +1464,7 @@ void Copter::ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd)
#endif
// do_payload_place - initiate placing procedure
void Copter::ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd)
void ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd)
{
// if location provided we fly to that location at current altitude
if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
@ -1484,7 +1484,7 @@ void Copter::ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd)
}
// do_RTL - start Return-to-Launch
void Copter::ModeAuto::do_RTL(void)
void ModeAuto::do_RTL(void)
{
// start rtl in auto flight mode
rtl_start();
@ -1495,14 +1495,14 @@ void Copter::ModeAuto::do_RTL(void)
/********************************************************************************/
// verify_takeoff - check if we have completed the takeoff
bool Copter::ModeAuto::verify_takeoff()
bool ModeAuto::verify_takeoff()
{
// have we reached our target altitude?
return copter.wp_nav->reached_wp_destination();
}
// verify_land - returns true if landing has been completed
bool Copter::ModeAuto::verify_land()
bool ModeAuto::verify_land()
{
bool retval = false;
@ -1523,7 +1523,7 @@ bool Copter::ModeAuto::verify_land()
case LandStateType_Descending:
// rely on THROTTLE_LAND mode to correctly update landing status
retval = ap.land_complete && (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE);
retval = copter.ap.land_complete && (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE);
break;
default:
@ -1547,7 +1547,7 @@ bool Copter::ModeAuto::verify_land()
#endif
// verify_payload_place - returns true if placing has been completed
bool Copter::ModeAuto::verify_payload_place()
bool ModeAuto::verify_payload_place()
{
const uint16_t hover_throttle_calibrate_time = 2000; // milliseconds
const uint16_t descend_throttle_calibrate_time = 2000; // milliseconds
@ -1559,7 +1559,7 @@ bool Copter::ModeAuto::verify_payload_place()
const uint32_t now = AP_HAL::millis();
// if we discover we've landed then immediately release the load:
if (ap.land_complete) {
if (copter.ap.land_complete) {
switch (nav_payload_place.state) {
case PayloadPlaceStateType_FlyToLocation:
case PayloadPlaceStateType_Calibrating_Hover_Start:
@ -1698,13 +1698,13 @@ bool Copter::ModeAuto::verify_payload_place()
}
#undef debug
bool Copter::ModeAuto::verify_loiter_unlimited()
bool ModeAuto::verify_loiter_unlimited()
{
return false;
}
// verify_loiter_time - check if we have loitered long enough
bool Copter::ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
{
// return immediately if we haven't reached our destination
if (!copter.wp_nav->reached_wp_destination()) {
@ -1727,7 +1727,7 @@ bool Copter::ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd
// verify_loiter_to_alt - check if we have reached both destination
// (roughly) and altitude (precisely)
bool Copter::ModeAuto::verify_loiter_to_alt()
bool ModeAuto::verify_loiter_to_alt()
{
if (loiter_to_alt.reached_destination_xy &&
loiter_to_alt.reached_alt) {
@ -1739,7 +1739,7 @@ bool Copter::ModeAuto::verify_loiter_to_alt()
// verify_RTL - handles any state changes required to implement RTL
// do_RTL should have been called once first to initialise all variables
// returns true with RTL has completed successfully
bool Copter::ModeAuto::verify_RTL()
bool ModeAuto::verify_RTL()
{
return (copter.mode_rtl.state_complete() &&
(copter.mode_rtl.state() == RTL_FinalDescent || copter.mode_rtl.state() == RTL_Land) &&
@ -1750,7 +1750,7 @@ bool Copter::ModeAuto::verify_RTL()
// Verify Condition (May) commands
/********************************************************************************/
bool Copter::ModeAuto::verify_wait_delay()
bool ModeAuto::verify_wait_delay()
{
if (millis() - condition_start > (uint32_t)MAX(condition_value,0)) {
condition_value = 0;
@ -1759,7 +1759,7 @@ bool Copter::ModeAuto::verify_wait_delay()
return false;
}
bool Copter::ModeAuto::verify_within_distance()
bool ModeAuto::verify_within_distance()
{
if (wp_distance() < (uint32_t)MAX(condition_value,0)) {
condition_value = 0;
@ -1769,7 +1769,7 @@ bool Copter::ModeAuto::verify_within_distance()
}
// verify_yaw - return true if we have reached the desired heading
bool Copter::ModeAuto::verify_yaw()
bool ModeAuto::verify_yaw()
{
// set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command)
if (auto_yaw.mode() != AUTO_YAW_FIXED) {
@ -1781,7 +1781,7 @@ bool Copter::ModeAuto::verify_yaw()
}
// verify_nav_wp - check if we have reached the next way point
bool Copter::ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{
// check if we have reached the waypoint
if ( !copter.wp_nav->reached_wp_destination() ) {
@ -1810,7 +1810,7 @@ bool Copter::ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
}
// verify_circle - check if we have circled the point enough
bool Copter::ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
{
// check if we've reached the edge
if (mode() == Auto_CircleMoveToEdge) {
@ -1843,7 +1843,7 @@ bool Copter::ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
}
// verify_spline_wp - check if we have reached the next way point using spline
bool Copter::ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
bool ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
{
// check if we have reached the waypoint
if ( !copter.wp_nav->reached_wp_destination() ) {
@ -1865,7 +1865,7 @@ bool Copter::ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
#if NAV_GUIDED == ENABLED
// verify_nav_guided - check if we have breached any limits
bool Copter::ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
{
// if disabling guided mode then immediately return true so we move to next command
if (cmd.p1 == 0) {
@ -1878,7 +1878,7 @@ bool Copter::ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Comman
#endif // NAV_GUIDED
// verify_nav_delay - check if we have waited long enough
bool Copter::ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
{
if (millis() - nav_delay_time_start > (uint32_t)MAX(nav_delay_time_max,0)) {
nav_delay_time_max = 0;

View File

@ -6,7 +6,7 @@
#if AUTOTUNE_ENABLED == ENABLED
bool Copter::AutoTune::init()
bool AutoTune::init()
{
// use position hold while tuning if we were in QLOITER
bool position_hold = (copter.control_mode == LOITER || copter.control_mode == POSHOLD);
@ -21,7 +21,7 @@ bool Copter::AutoTune::init()
/*
start autotune mode
*/
bool Copter::AutoTune::start()
bool AutoTune::start()
{
// only allow flip from Stabilize, AltHold, PosHold or Loiter modes
if (copter.control_mode != STABILIZE && copter.control_mode != ALT_HOLD &&
@ -42,7 +42,7 @@ bool Copter::AutoTune::start()
return AC_AutoTune::start();
}
void Copter::AutoTune::run()
void AutoTune::run()
{
// apply SIMPLE mode transform to pilot inputs
copter.update_simple_mode();
@ -77,7 +77,7 @@ void Copter::AutoTune::run()
/*
get stick input climb rate
*/
float Copter::AutoTune::get_pilot_desired_climb_rate_cms(void) const
float AutoTune::get_pilot_desired_climb_rate_cms(void) const
{
float target_climb_rate = copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in());
@ -90,7 +90,7 @@ float Copter::AutoTune::get_pilot_desired_climb_rate_cms(void) const
/*
get stick roll, pitch and yaw rate
*/
void Copter::AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pitch_cd, float &yaw_rate_cds)
void AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pitch_cd, float &yaw_rate_cds)
{
copter.mode_autotune.get_pilot_desired_lean_angles(des_roll_cd, des_pitch_cd, copter.aparm.angle_max,
copter.attitude_control->get_althold_lean_angle_max());
@ -100,13 +100,13 @@ void Copter::AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &
/*
setup z controller velocity and accel limits
*/
void Copter::AutoTune::init_z_limits()
void AutoTune::init_z_limits()
{
copter.pos_control->set_max_speed_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up);
copter.pos_control->set_max_accel_z(copter.g.pilot_accel_z);
}
void Copter::AutoTune::log_pids()
void AutoTune::log_pids()
{
copter.logger.Write_PID(LOG_PIDR_MSG, copter.attitude_control->get_rate_roll_pid().get_pid_info());
copter.logger.Write_PID(LOG_PIDP_MSG, copter.attitude_control->get_rate_pitch_pid().get_pid_info());
@ -118,7 +118,7 @@ void Copter::AutoTune::log_pids()
Write an event packet. This maps from AC_AutoTune event IDs to
copter event IDs
*/
void Copter::AutoTune::Log_Write_Event(enum at_event id)
void AutoTune::Log_Write_Event(enum at_event id)
{
const struct {
enum at_event eid;
@ -144,7 +144,7 @@ void Copter::AutoTune::Log_Write_Event(enum at_event id)
/*
check if we have a good position estimate
*/
bool Copter::AutoTune::position_ok()
bool AutoTune::position_ok()
{
return copter.position_ok();
}
@ -152,28 +152,28 @@ bool Copter::AutoTune::position_ok()
/*
initialise autotune mode
*/
bool Copter::ModeAutoTune::init(bool ignore_checks)
bool ModeAutoTune::init(bool ignore_checks)
{
return copter.autotune.init();
}
void Copter::ModeAutoTune::run()
void ModeAutoTune::run()
{
copter.autotune.run();
}
void Copter::ModeAutoTune::save_tuning_gains()
void ModeAutoTune::save_tuning_gains()
{
copter.autotune.save_tuning_gains();
}
void Copter::ModeAutoTune::stop()
void ModeAutoTune::stop()
{
copter.autotune.stop();
}
void Copter::ModeAutoTune::reset()
void ModeAutoTune::reset()
{
copter.autotune.reset();
}

View File

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

View File

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

View File

@ -7,7 +7,7 @@
*/
// circle_init - initialise circle controller flight mode
bool Copter::ModeCircle::init(bool ignore_checks)
bool ModeCircle::init(bool ignore_checks)
{
pilot_yaw_override = false;
@ -25,7 +25,7 @@ bool Copter::ModeCircle::init(bool ignore_checks)
// circle_run - runs the circle flight mode
// should be called at 100hz or more
void Copter::ModeCircle::run()
void ModeCircle::run()
{
// initialize speeds and accelerations
pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy());
@ -72,7 +72,7 @@ void Copter::ModeCircle::run()
// update altitude target and call position controller
// protects heli's from inflight motor interlock disable
if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::GROUND_IDLE && !ap.land_complete) {
if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::GROUND_IDLE && !copter.ap.land_complete) {
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
} else {
pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
@ -80,12 +80,12 @@ void Copter::ModeCircle::run()
pos_control->update_z_controller();
}
uint32_t Copter::ModeCircle::wp_distance() const
uint32_t ModeCircle::wp_distance() const
{
return copter.circle_nav->get_distance_to_target();
}
int32_t Copter::ModeCircle::wp_bearing() const
int32_t ModeCircle::wp_bearing() const
{
return copter.circle_nav->get_bearing_to_target();
}

View File

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

View File

@ -33,7 +33,7 @@
#define FLIP_PITCH_FORWARD -1 // used to set flip_dir
// flip_init - initialise flip controller
bool Copter::ModeFlip::init(bool ignore_checks)
bool ModeFlip::init(bool ignore_checks)
{
// only allow flip from ACRO, Stabilize, AltHold or Drift flight modes
if (copter.control_mode != ACRO &&
@ -44,7 +44,7 @@ bool Copter::ModeFlip::init(bool ignore_checks)
}
// if in acro or stabilize ensure throttle is above zero
if (ap.throttle_zero && (copter.control_mode == ACRO || copter.control_mode == STABILIZE)) {
if (copter.ap.throttle_zero && (copter.control_mode == ACRO || copter.control_mode == STABILIZE)) {
return false;
}
@ -54,7 +54,7 @@ bool Copter::ModeFlip::init(bool ignore_checks)
}
// only allow flip when flying
if (!motors->armed() || ap.land_complete) {
if (!motors->armed() || copter.ap.land_complete) {
return false;
}
@ -92,7 +92,7 @@ bool Copter::ModeFlip::init(bool ignore_checks)
// run - runs the flip controller
// should be called at 100hz or more
void Copter::ModeFlip::run()
void ModeFlip::run()
{
// if pilot inputs roll > 40deg or timeout occurs abandon flip
if (!motors->armed() || (abs(channel_roll->get_control_in()) >= 4000) || (abs(channel_pitch->get_control_in()) >= 4000) || ((millis() - start_time_ms) > FLIP_TIMEOUT_MS)) {

View File

@ -8,7 +8,7 @@
without rangefinder
*/
const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = {
const AP_Param::GroupInfo ModeFlowHold::var_info[] = {
// @Param: _XY_P
// @DisplayName: FlowHold P gain
// @Description: FlowHold (horizontal) P gain.
@ -37,14 +37,14 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = {
// @Range: 0 100
// @Units: Hz
// @User: Advanced
AP_SUBGROUPINFO(flow_pi_xy, "_XY_", 1, Copter::ModeFlowHold, AC_PI_2D),
AP_SUBGROUPINFO(flow_pi_xy, "_XY_", 1, ModeFlowHold, AC_PI_2D),
// @Param: _FLOW_MAX
// @DisplayName: FlowHold Flow Rate Max
// @Description: Controls maximum apparent flow rate in flowhold
// @Range: 0.1 2.5
// @User: Standard
AP_GROUPINFO("_FLOW_MAX", 2, Copter::ModeFlowHold, flow_max, 0.6),
AP_GROUPINFO("_FLOW_MAX", 2, ModeFlowHold, flow_max, 0.6),
// @Param: _FILT_HZ
// @DisplayName: FlowHold Filter Frequency
@ -52,14 +52,14 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = {
// @Range: 1 100
// @Units: Hz
// @User: Standard
AP_GROUPINFO("_FILT_HZ", 3, Copter::ModeFlowHold, flow_filter_hz, 5),
AP_GROUPINFO("_FILT_HZ", 3, ModeFlowHold, flow_filter_hz, 5),
// @Param: _QUAL_MIN
// @DisplayName: FlowHold Flow quality minimum
// @Description: Minimum flow quality to use flow position hold
// @Range: 0 255
// @User: Standard
AP_GROUPINFO("_QUAL_MIN", 4, Copter::ModeFlowHold, flow_min_quality, 10),
AP_GROUPINFO("_QUAL_MIN", 4, ModeFlowHold, flow_min_quality, 10),
// 5 was FLOW_SPEED
@ -69,12 +69,12 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = {
// @Range: 1 30
// @User: Standard
// @Units: deg/s
AP_GROUPINFO("_BRAKE_RATE", 6, Copter::ModeFlowHold, brake_rate_dps, 8),
AP_GROUPINFO("_BRAKE_RATE", 6, ModeFlowHold, brake_rate_dps, 8),
AP_GROUPEND
};
Copter::ModeFlowHold::ModeFlowHold(void) : Mode()
ModeFlowHold::ModeFlowHold(void) : Mode()
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -82,7 +82,7 @@ Copter::ModeFlowHold::ModeFlowHold(void) : Mode()
#define CONTROL_FLOWHOLD_EARTH_FRAME 0
// flowhold_init - initialise flowhold controller
bool Copter::ModeFlowHold::init(bool ignore_checks)
bool ModeFlowHold::init(bool ignore_checks)
{
if (!copter.optflow.enabled() || !copter.optflow.healthy()) {
return false;
@ -116,7 +116,7 @@ bool Copter::ModeFlowHold::init(bool ignore_checks)
/*
calculate desired attitude from flow sensor. Called when flow sensor is healthy
*/
void Copter::ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stick_input)
void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stick_input)
{
uint32_t now = AP_HAL::millis();
@ -215,7 +215,7 @@ void Copter::ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stic
// flowhold_run - runs the flowhold controller
// should be called at 100hz or more
void Copter::ModeFlowHold::run()
void ModeFlowHold::run()
{
float takeoff_climb_rate = 0.0f;
@ -342,7 +342,7 @@ void Copter::ModeFlowHold::run()
/*
update height estimate using integrated accelerometer ratio with optical flow
*/
void Copter::ModeFlowHold::update_height_estimate(void)
void ModeFlowHold::update_height_estimate(void)
{
float ins_height = copter.inertial_nav.get_altitude() * 0.01;

View File

@ -14,17 +14,17 @@
*/
// initialise follow mode
bool Copter::ModeFollow::init(const bool ignore_checks)
bool ModeFollow::init(const bool ignore_checks)
{
if (!g2.follow.enabled()) {
gcs().send_text(MAV_SEVERITY_WARNING, "Set FOLL_ENABLE = 1");
return false;
}
// re-use guided mode
return Copter::ModeGuided::init(ignore_checks);
return ModeGuided::init(ignore_checks);
}
void Copter::ModeFollow::run()
void ModeFollow::run()
{
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
@ -148,17 +148,17 @@ void Copter::ModeFollow::run()
last_log_ms = now;
}
// re-use guided mode's velocity controller (takes NEU)
Copter::ModeGuided::set_velocity(desired_velocity_neu_cms, use_yaw, yaw_cd, false, 0.0f, false, log_request);
ModeGuided::set_velocity(desired_velocity_neu_cms, use_yaw, yaw_cd, false, 0.0f, false, log_request);
Copter::ModeGuided::run();
ModeGuided::run();
}
uint32_t Copter::ModeFollow::wp_distance() const
uint32_t ModeFollow::wp_distance() const
{
return g2.follow.get_distance_to_target() * 100;
}
int32_t Copter::ModeFollow::wp_bearing() const
int32_t ModeFollow::wp_bearing() const
{
return g2.follow.get_bearing_to_target() * 100;
}
@ -166,7 +166,7 @@ int32_t Copter::ModeFollow::wp_bearing() const
/*
get target position for mavlink reporting
*/
bool Copter::ModeFollow::get_wp(Location &loc)
bool ModeFollow::get_wp(Location &loc)
{
float dist = g2.follow.get_distance_to_target();
float bearing = g2.follow.get_bearing_to_target();

View File

@ -38,7 +38,7 @@ struct Guided_Limit {
} guided_limit;
// guided_init - initialise guided controller
bool Copter::ModeGuided::init(bool ignore_checks)
bool ModeGuided::init(bool ignore_checks)
{
// start in position control mode
pos_control_start();
@ -47,7 +47,7 @@ bool Copter::ModeGuided::init(bool ignore_checks)
// do_user_takeoff_start - initialises waypoint controller to implement take-off
bool Copter::ModeGuided::do_user_takeoff_start(float final_alt_above_home)
bool ModeGuided::do_user_takeoff_start(float final_alt_above_home)
{
guided_mode = Guided_TakeOff;
@ -75,7 +75,7 @@ bool Copter::ModeGuided::do_user_takeoff_start(float final_alt_above_home)
}
// initialise guided mode's position controller
void Copter::ModeGuided::pos_control_start()
void ModeGuided::pos_control_start()
{
// set to position control mode
guided_mode = Guided_WP;
@ -95,7 +95,7 @@ void Copter::ModeGuided::pos_control_start()
}
// initialise guided mode's velocity controller
void Copter::ModeGuided::vel_control_start()
void ModeGuided::vel_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_Velocity;
@ -113,7 +113,7 @@ void Copter::ModeGuided::vel_control_start()
}
// initialise guided mode's posvel controller
void Copter::ModeGuided::posvel_control_start()
void ModeGuided::posvel_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_PosVel;
@ -139,13 +139,13 @@ void Copter::ModeGuided::posvel_control_start()
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
bool Copter::ModeGuided::is_taking_off() const
bool ModeGuided::is_taking_off() const
{
return guided_mode == Guided_TakeOff;
}
// initialise guided mode's angle controller
void Copter::ModeGuided::angle_control_start()
void ModeGuided::angle_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_Angle;
@ -176,7 +176,7 @@ void Copter::ModeGuided::angle_control_start()
// guided_set_destination - sets guided mode's target destination
// Returns true if the fence is enabled and guided waypoint is within the fence
// else return false if the waypoint is outside the fence
bool Copter::ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
@ -204,7 +204,7 @@ bool Copter::ModeGuided::set_destination(const Vector3f& destination, bool use_y
return true;
}
bool Copter::ModeGuided::get_wp(Location& destination)
bool ModeGuided::get_wp(Location& destination)
{
if (guided_mode != Guided_WP) {
return false;
@ -215,7 +215,7 @@ bool Copter::ModeGuided::get_wp(Location& destination)
// sets guided mode's target from a Location object
// returns false if destination could not be set (probably caused by missing terrain data)
// or if the fence is enabled and guided waypoint is outside the fence
bool Copter::ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
@ -248,7 +248,7 @@ bool Copter::ModeGuided::set_destination(const Location& dest_loc, bool use_yaw,
}
// guided_set_velocity - sets guided mode's target velocity
void Copter::ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
void ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
{
// check we are in velocity control mode
if (guided_mode != Guided_Velocity) {
@ -269,7 +269,7 @@ void Copter::ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, fl
}
// set guided mode posvel target
bool Copter::ModeGuided::set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
// check we are in velocity control mode
if (guided_mode != Guided_PosVel) {
@ -301,7 +301,7 @@ bool Copter::ModeGuided::set_destination_posvel(const Vector3f& destination, con
}
// set guided mode angle target
void Copter::ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads)
void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads)
{
// check we are in velocity control mode
if (guided_mode != Guided_Angle) {
@ -320,7 +320,7 @@ void Copter::ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bo
guided_angle_state.update_time_ms = millis();
// interpret positive climb rate as triggering take-off
if (motors->armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) {
if (motors->armed() && !copter.ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) {
copter.set_auto_armed(true);
}
@ -332,7 +332,7 @@ void Copter::ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bo
// guided_run - runs the guided controller
// should be called at 100hz or more
void Copter::ModeGuided::run()
void ModeGuided::run()
{
// call the correct auto controller
switch (guided_mode) {
@ -366,7 +366,7 @@ void Copter::ModeGuided::run()
// guided_takeoff_run - takeoff in guided mode
// called by guided_run at 100hz or more
void Copter::ModeGuided::takeoff_run()
void ModeGuided::takeoff_run()
{
auto_takeoff_run();
if (wp_nav->reached_wp_destination()) {
@ -375,10 +375,10 @@ void Copter::ModeGuided::takeoff_run()
}
}
void Copter::Mode::auto_takeoff_run()
void Mode::auto_takeoff_run()
{
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed) {
if (!motors->armed() || !copter.ap.auto_armed) {
make_safe_spool_down();
wp_nav->shift_wp_origin_to_current_pos();
return;
@ -413,7 +413,7 @@ void Copter::Mode::auto_takeoff_run()
// guided_pos_control_run - runs the guided position controller
// called from guided_run
void Copter::ModeGuided::pos_control_run()
void ModeGuided::pos_control_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
@ -455,7 +455,7 @@ void Copter::ModeGuided::pos_control_run()
// guided_vel_control_run - runs the guided velocity controller
// called from guided_run
void Copter::ModeGuided::vel_control_run()
void ModeGuided::vel_control_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
@ -507,7 +507,7 @@ void Copter::ModeGuided::vel_control_run()
// guided_posvel_control_run - runs the guided spline controller
// called from guided_run
void Copter::ModeGuided::posvel_control_run()
void ModeGuided::posvel_control_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
@ -572,7 +572,7 @@ void Copter::ModeGuided::posvel_control_run()
// guided_angle_control_run - runs the guided angle controller
// called from guided_run
void Copter::ModeGuided::angle_control_run()
void ModeGuided::angle_control_run()
{
// constrain desired lean angles
float roll_in = guided_angle_state.roll_cd;
@ -605,14 +605,14 @@ void Copter::ModeGuided::angle_control_run()
}
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || (ap.land_complete && (guided_angle_state.climb_rate_cms <= 0.0f))) {
if (!motors->armed() || !copter.ap.auto_armed || (copter.ap.land_complete && (guided_angle_state.climb_rate_cms <= 0.0f))) {
make_safe_spool_down();
return;
}
// TODO: use get_alt_hold_state
// landed with positive desired climb rate, takeoff
if (ap.land_complete && (guided_angle_state.climb_rate_cms > 0.0f)) {
if (copter.ap.land_complete && (guided_angle_state.climb_rate_cms > 0.0f)) {
zero_throttle_and_relax_ac();
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
@ -638,7 +638,7 @@ void Copter::ModeGuided::angle_control_run()
}
// helper function to update position controller's desired velocity while respecting acceleration limits
void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des)
void ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des)
{
// get current desired velocity
Vector3f curr_vel_des = pos_control->get_desired_velocity();
@ -672,7 +672,7 @@ void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const
}
// helper function to set yaw state and targets
void Copter::ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
void ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
{
if (use_yaw) {
auto_yaw.set_fixed_yaw(yaw_cd * 0.01f, 0.0f, 0, relative_angle);
@ -684,7 +684,7 @@ void Copter::ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_
// Guided Limit code
// guided_limit_clear - clear/turn off guided limits
void Copter::ModeGuided::limit_clear()
void ModeGuided::limit_clear()
{
guided_limit.timeout_ms = 0;
guided_limit.alt_min_cm = 0.0f;
@ -693,7 +693,7 @@ void Copter::ModeGuided::limit_clear()
}
// guided_limit_set - set guided timeout and movement limits
void Copter::ModeGuided::limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
void ModeGuided::limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
{
guided_limit.timeout_ms = timeout_ms;
guided_limit.alt_min_cm = alt_min_cm;
@ -703,7 +703,7 @@ void Copter::ModeGuided::limit_set(uint32_t timeout_ms, float alt_min_cm, float
// guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
// only called from AUTO mode's auto_nav_guided_start function
void Copter::ModeGuided::limit_init_time_and_pos()
void ModeGuided::limit_init_time_and_pos()
{
// initialise start time
guided_limit.start_time = AP_HAL::millis();
@ -714,7 +714,7 @@ void Copter::ModeGuided::limit_init_time_and_pos()
// guided_limit_check - returns true if guided mode has breached a limit
// used when guided is invoked from the NAV_GUIDED_ENABLE mission command
bool Copter::ModeGuided::limit_check()
bool ModeGuided::limit_check()
{
// check if we have passed the timeout
if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) {
@ -747,7 +747,7 @@ bool Copter::ModeGuided::limit_check()
}
uint32_t Copter::ModeGuided::wp_distance() const
uint32_t ModeGuided::wp_distance() const
{
switch(mode()) {
case Guided_WP:
@ -761,7 +761,7 @@ uint32_t Copter::ModeGuided::wp_distance() const
}
}
int32_t Copter::ModeGuided::wp_bearing() const
int32_t ModeGuided::wp_bearing() const
{
switch(mode()) {
case Guided_WP:
@ -775,7 +775,7 @@ int32_t Copter::ModeGuided::wp_bearing() const
}
}
float Copter::ModeGuided::crosstrack_error() const
float ModeGuided::crosstrack_error() const
{
if (mode() == Guided_WP) {
return wp_nav->crosstrack_error();

View File

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

View File

@ -7,7 +7,7 @@ static uint32_t land_start_time;
static bool land_pause;
// land_init - initialise land controller
bool Copter::ModeLand::init(bool ignore_checks)
bool ModeLand::init(bool ignore_checks)
{
// check if we have GPS and decide which LAND we're going to do
land_with_gps = copter.position_ok();
@ -32,14 +32,14 @@ bool Copter::ModeLand::init(bool ignore_checks)
land_pause = false;
// reset flag indicating if pilot has applied roll or pitch inputs during landing
ap.land_repo_active = false;
copter.ap.land_repo_active = false;
return true;
}
// land_run - runs the land controller
// should be called at 100hz or more
void Copter::ModeLand::run()
void ModeLand::run()
{
if (land_with_gps) {
gps_run();
@ -51,10 +51,10 @@ void Copter::ModeLand::run()
// land_gps_run - runs the land controller
// horizontal position controlled with loiter controller
// should be called at 100hz or more
void Copter::ModeLand::gps_run()
void ModeLand::gps_run()
{
// disarm when the landing detector says we've landed
if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
copter.arming.disarm();
}
@ -79,7 +79,7 @@ void Copter::ModeLand::gps_run()
// land_nogps_run - runs the land controller
// pilot controls roll and pitch angles
// should be called at 100hz or more
void Copter::ModeLand::nogps_run()
void ModeLand::nogps_run()
{
float target_roll = 0.0f, target_pitch = 0.0f;
float target_yaw_rate = 0;
@ -108,7 +108,7 @@ void Copter::ModeLand::nogps_run()
}
// disarm when the landing detector says we've landed
if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
copter.arming.disarm();
}
@ -134,7 +134,7 @@ void Copter::ModeLand::nogps_run()
// do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
// called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS
// has no effect if we are not already in LAND mode
void Copter::ModeLand::do_not_use_GPS()
void ModeLand::do_not_use_GPS()
{
land_with_gps = false;
}

View File

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

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
// poshold_init - initialise PosHold controller
bool Copter::ModePosHold::init(bool ignore_checks)
bool ModePosHold::init(bool ignore_checks)
{
// initialize vertical speeds and acceleration
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
@ -43,7 +43,7 @@ bool Copter::ModePosHold::init(bool ignore_checks)
// compute brake_gain
brake_gain = (15.0f * (float)g.poshold_brake_rate + 95.0f) / 100.0f;
if (ap.land_complete) {
if (copter.ap.land_complete) {
// if landed begin in loiter mode
roll_mode = RPMode::LOITER;
pitch_mode = RPMode::LOITER;
@ -68,7 +68,7 @@ bool Copter::ModePosHold::init(bool ignore_checks)
// poshold_run - runs the PosHold controller
// should be called at 100hz or more
void Copter::ModePosHold::run()
void ModePosHold::run()
{
float takeoff_climb_rate = 0.0f;
float brake_to_loiter_mix; // mix of brake and loiter controls. 0 = fully brake controls, 1 = fully loiter controls
@ -96,7 +96,7 @@ void Copter::ModePosHold::run()
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
if (copter.ap.land_complete_maybe) {
loiter_nav->soften_for_landing();
}
@ -508,7 +508,7 @@ void Copter::ModePosHold::run()
}
// poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received
void Copter::ModePosHold::update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw)
void ModePosHold::update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw)
{
// if raw input is large or reversing the vehicle's lean angle immediately set the fitlered angle to the new raw angle
if ((lean_angle_filtered > 0 && lean_angle_raw < 0) || (lean_angle_filtered < 0 && lean_angle_raw > 0) || (fabsf(lean_angle_raw) > POSHOLD_STICK_RELEASE_SMOOTH_ANGLE)) {
@ -530,7 +530,7 @@ void Copter::ModePosHold::update_pilot_lean_angle(float &lean_angle_filtered, fl
// mix_controls - mixes two controls based on the mix_ratio
// mix_ratio of 1 = use first_control completely, 0 = use second_control completely, 0.5 = mix evenly
int16_t Copter::ModePosHold::mix_controls(float mix_ratio, int16_t first_control, int16_t second_control)
int16_t ModePosHold::mix_controls(float mix_ratio, int16_t first_control, int16_t second_control)
{
mix_ratio = constrain_float(mix_ratio, 0.0f, 1.0f);
return (int16_t)((mix_ratio * first_control) + ((1.0f-mix_ratio)*second_control));
@ -539,7 +539,7 @@ int16_t Copter::ModePosHold::mix_controls(float mix_ratio, int16_t first_control
// update_brake_angle_from_velocity - updates the brake_angle based on the vehicle's velocity and brake_gain
// brake_angle is slewed with the wpnav.poshold_brake_rate and constrained by the wpnav.poshold_braking_angle_max
// velocity is assumed to be in the same direction as lean angle so for pitch you should provide the velocity backwards (i.e. -ve forward velocity)
void Copter::ModePosHold::update_brake_angle_from_velocity(int16_t &brake_angle, float velocity)
void ModePosHold::update_brake_angle_from_velocity(int16_t &brake_angle, float velocity)
{
float lean_angle;
int16_t brake_rate = g.poshold_brake_rate;
@ -565,7 +565,7 @@ void Copter::ModePosHold::update_brake_angle_from_velocity(int16_t &brake_angle,
// update_wind_comp_estimate - updates wind compensation estimate
// should be called at the maximum loop rate when loiter is engaged
void Copter::ModePosHold::update_wind_comp_estimate()
void ModePosHold::update_wind_comp_estimate()
{
// check wind estimate start has not been delayed
if (wind_comp_start_timer > 0) {
@ -601,7 +601,7 @@ void Copter::ModePosHold::update_wind_comp_estimate()
// get_wind_comp_lean_angles - retrieve wind compensation angles in body frame roll and pitch angles
// should be called at the maximum loop rate
void Copter::ModePosHold::get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle)
void ModePosHold::get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle)
{
// reduce rate to 10hz
wind_comp_timer++;
@ -616,7 +616,7 @@ void Copter::ModePosHold::get_wind_comp_lean_angles(int16_t &roll_angle, int16_t
}
// roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis
void Copter::ModePosHold::roll_controller_to_pilot_override()
void ModePosHold::roll_controller_to_pilot_override()
{
roll_mode = RPMode::CONTROLLER_TO_PILOT_OVERRIDE;
controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
@ -627,7 +627,7 @@ void Copter::ModePosHold::roll_controller_to_pilot_override()
}
// pitch_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis
void Copter::ModePosHold::pitch_controller_to_pilot_override()
void ModePosHold::pitch_controller_to_pilot_override()
{
pitch_mode = RPMode::CONTROLLER_TO_PILOT_OVERRIDE;
controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;

View File

@ -10,7 +10,7 @@
*/
// rtl_init - initialise rtl controller
bool Copter::ModeRTL::init(bool ignore_checks)
bool ModeRTL::init(bool ignore_checks)
{
if (!ignore_checks) {
if (!AP::ahrs().home_is_set()) {
@ -26,7 +26,7 @@ bool Copter::ModeRTL::init(bool ignore_checks)
}
// re-start RTL with terrain following disabled
void Copter::ModeRTL::restart_without_terrain()
void ModeRTL::restart_without_terrain()
{
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RESTARTED_RTL);
if (rtl_path.terrain_used) {
@ -39,7 +39,7 @@ void Copter::ModeRTL::restart_without_terrain()
// rtl_run - runs the return-to-launch controller
// should be called at 100hz or more
void Copter::ModeRTL::run(bool disarm_on_land)
void ModeRTL::run(bool disarm_on_land)
{
if (!motors->armed()) {
return;
@ -105,7 +105,7 @@ void Copter::ModeRTL::run(bool disarm_on_land)
}
// rtl_climb_start - initialise climb to RTL altitude
void Copter::ModeRTL::climb_start()
void ModeRTL::climb_start()
{
_state = RTL_InitialClimb;
_state_complete = false;
@ -129,7 +129,7 @@ void Copter::ModeRTL::climb_start()
}
// rtl_return_start - initialise return to home
void Copter::ModeRTL::return_start()
void ModeRTL::return_start()
{
_state = RTL_ReturnHome;
_state_complete = false;
@ -145,7 +145,7 @@ void Copter::ModeRTL::return_start()
// rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
// called by rtl_run at 100hz or more
void Copter::ModeRTL::climb_return_run()
void ModeRTL::climb_return_run()
{
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
@ -186,7 +186,7 @@ void Copter::ModeRTL::climb_return_run()
}
// rtl_loiterathome_start - initialise return to home
void Copter::ModeRTL::loiterathome_start()
void ModeRTL::loiterathome_start()
{
_state = RTL_LoiterAtHome;
_state_complete = false;
@ -202,7 +202,7 @@ void Copter::ModeRTL::loiterathome_start()
// rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
// called by rtl_run at 100hz or more
void Copter::ModeRTL::loiterathome_run()
void ModeRTL::loiterathome_run()
{
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
@ -253,7 +253,7 @@ void Copter::ModeRTL::loiterathome_run()
}
// rtl_descent_start - initialise descent to final alt
void Copter::ModeRTL::descent_start()
void ModeRTL::descent_start()
{
_state = RTL_FinalDescent;
_state_complete = false;
@ -270,7 +270,7 @@ void Copter::ModeRTL::descent_start()
// rtl_descent_run - implements the final descent to the RTL_ALT
// called by rtl_run at 100hz or more
void Copter::ModeRTL::descent_run()
void ModeRTL::descent_run()
{
float target_roll = 0.0f;
float target_pitch = 0.0f;
@ -301,10 +301,10 @@ void Copter::ModeRTL::descent_run()
// record if pilot has overridden roll or pitch
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
if (!ap.land_repo_active) {
if (!copter.ap.land_repo_active) {
copter.Log_Write_Event(DATA_LAND_REPO_ACTIVE);
}
ap.land_repo_active = true;
copter.ap.land_repo_active = true;
}
}
@ -333,7 +333,7 @@ void Copter::ModeRTL::descent_run()
}
// rtl_loiterathome_start - initialise controllers to loiter over home
void Copter::ModeRTL::land_start()
void ModeRTL::land_start()
{
_state = RTL_Land;
_state_complete = false;
@ -351,12 +351,12 @@ void Copter::ModeRTL::land_start()
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
bool Copter::ModeRTL::is_landing() const
bool ModeRTL::is_landing() const
{
return _state == RTL_Land;
}
bool Copter::ModeRTL::landing_gear_should_be_deployed() const
bool ModeRTL::landing_gear_should_be_deployed() const
{
switch(_state) {
case RTL_LoiterAtHome:
@ -371,13 +371,13 @@ bool Copter::ModeRTL::landing_gear_should_be_deployed() const
// rtl_returnhome_run - return home
// called by rtl_run at 100hz or more
void Copter::ModeRTL::land_run(bool disarm_on_land)
void ModeRTL::land_run(bool disarm_on_land)
{
// check if we've completed this stage of RTL
_state_complete = ap.land_complete;
_state_complete = copter.ap.land_complete;
// disarm when the landing detector says we've landed
if (disarm_on_land && ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
if (disarm_on_land && copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
copter.arming.disarm();
}
@ -395,7 +395,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
land_run_vertical_control();
}
void Copter::ModeRTL::build_path()
void ModeRTL::build_path()
{
// origin point is our stopping point
Vector3f stopping_point;
@ -420,7 +420,7 @@ void Copter::ModeRTL::build_path()
// compute the return target - home or rally point
// return altitude in cm above home at which vehicle should return home
// return target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set)
void Copter::ModeRTL::compute_return_target()
void ModeRTL::compute_return_target()
{
// set return target to nearest rally point or home position (Note: alt is absolute)
#if AC_RALLY == ENABLED
@ -495,12 +495,12 @@ void Copter::ModeRTL::compute_return_target()
rtl_path.return_target.alt = MAX(rtl_path.return_target.alt, curr_alt);
}
uint32_t Copter::ModeRTL::wp_distance() const
uint32_t ModeRTL::wp_distance() const
{
return wp_nav->get_wp_distance_to_destination();
}
int32_t Copter::ModeRTL::wp_bearing() const
int32_t ModeRTL::wp_bearing() const
{
return wp_nav->get_wp_bearing_to_destination();
}

View File

@ -9,7 +9,7 @@
* Once the copter is close to home, it will run a standard land controller.
*/
bool Copter::ModeSmartRTL::init(bool ignore_checks)
bool ModeSmartRTL::init(bool ignore_checks)
{
if (g2.smart_rtl.is_active()) {
// initialise waypoint and spline controller
@ -33,12 +33,12 @@ bool Copter::ModeSmartRTL::init(bool ignore_checks)
}
// perform cleanup required when leaving smart_rtl
void Copter::ModeSmartRTL::exit()
void ModeSmartRTL::exit()
{
g2.smart_rtl.cancel_request_for_thorough_cleanup();
}
void Copter::ModeSmartRTL::run()
void ModeSmartRTL::run()
{
switch (smart_rtl_state) {
case SmartRTL_WaitForPathCleanup:
@ -59,7 +59,7 @@ void Copter::ModeSmartRTL::run()
}
}
void Copter::ModeSmartRTL::wait_cleanup_run()
void ModeSmartRTL::wait_cleanup_run()
{
// hover at current target position
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
@ -73,7 +73,7 @@ void Copter::ModeSmartRTL::wait_cleanup_run()
}
}
void Copter::ModeSmartRTL::path_follow_run()
void ModeSmartRTL::path_follow_run()
{
float target_yaw_rate = 0.0f;
if (!copter.failsafe.radio) {
@ -119,7 +119,7 @@ void Copter::ModeSmartRTL::path_follow_run()
}
}
void Copter::ModeSmartRTL::pre_land_position_run()
void ModeSmartRTL::pre_land_position_run()
{
// if we are close to 2m above start point, we are ready to land.
if (wp_nav->reached_wp_destination()) {
@ -142,19 +142,19 @@ void Copter::ModeSmartRTL::pre_land_position_run()
}
// save current position for use by the smart_rtl flight mode
void Copter::ModeSmartRTL::save_position()
void ModeSmartRTL::save_position()
{
const bool should_save_position = motors->armed() && (copter.control_mode != SMART_RTL);
copter.g2.smart_rtl.update(copter.position_ok(), should_save_position);
}
uint32_t Copter::ModeSmartRTL::wp_distance() const
uint32_t ModeSmartRTL::wp_distance() const
{
return wp_nav->get_wp_distance_to_destination();
}
int32_t Copter::ModeSmartRTL::wp_bearing() const
int32_t ModeSmartRTL::wp_bearing() const
{
return wp_nav->get_wp_bearing_to_destination();
}

View File

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

View File

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

View File

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

View File

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

View File

@ -9,7 +9,7 @@
#define ZIGZAG_WP_RADIUS_CM 300
// initialise zigzag controller
bool Copter::ModeZigZag::init(bool ignore_checks)
bool ModeZigZag::init(bool ignore_checks)
{
// initialize's loiter position and velocity on xy-axes from current pos and velocity
loiter_nav->clear_pilot_desired_acceleration();
@ -31,7 +31,7 @@ bool Copter::ModeZigZag::init(bool ignore_checks)
// run the zigzag controller
// should be called at 100hz or more
void Copter::ModeZigZag::run()
void ModeZigZag::run()
{
// initialize vertical speed and acceleration's range
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
@ -62,7 +62,7 @@ void Copter::ModeZigZag::run()
}
// save current position as A (dest_num = 0) or B (dest_num = 1). If both A and B have been saved move to the one specified
void Copter::ModeZigZag::save_or_move_to_destination(uint8_t dest_num)
void ModeZigZag::save_or_move_to_destination(uint8_t dest_num)
{
// sanity check
if (dest_num > 1) {
@ -117,7 +117,7 @@ void Copter::ModeZigZag::save_or_move_to_destination(uint8_t dest_num)
}
// return manual control to the pilot
void Copter::ModeZigZag::return_to_manual_control(bool maintain_target)
void ModeZigZag::return_to_manual_control(bool maintain_target)
{
if (stage == AUTO) {
stage = MANUAL_REGAIN;
@ -132,7 +132,7 @@ void Copter::ModeZigZag::return_to_manual_control(bool maintain_target)
}
// fly the vehicle to closest point on line perpendicular to dest_A or dest_B
void Copter::ModeZigZag::auto_control()
void ModeZigZag::auto_control()
{
// process pilot's yaw input
float target_yaw_rate = 0;
@ -161,7 +161,7 @@ void Copter::ModeZigZag::auto_control()
}
// manual_control - process manual control
void Copter::ModeZigZag::manual_control()
void ModeZigZag::manual_control()
{
float target_yaw_rate = 0.0f;
float target_climb_rate = 0.0f;
@ -213,7 +213,7 @@ void Copter::ModeZigZag::manual_control()
}
// return true if vehicle is within a small area around the destination
bool Copter::ModeZigZag::reached_destination()
bool ModeZigZag::reached_destination()
{
// check if wp_nav believes it has reached the destination
if (!wp_nav->reached_wp_destination()) {
@ -236,7 +236,7 @@ bool Copter::ModeZigZag::reached_destination()
// calculate next destination according to vector A-B and current position
// use_wpnav_alt should be true if waypoint controller's altitude target should be used, false for position control or current altitude target
// terrain_alt is returned as true if the next_dest should be considered a terrain alt
bool Copter::ModeZigZag::calculate_next_dest(uint8_t dest_num, bool use_wpnav_alt, Vector3f& next_dest, bool& terrain_alt) const
bool ModeZigZag::calculate_next_dest(uint8_t dest_num, bool use_wpnav_alt, Vector3f& next_dest, bool& terrain_alt) const
{
// sanity check dest_num
if (dest_num > 1) {

View File

@ -1,25 +1,25 @@
#include "Copter.h"
Copter::Mode::_TakeOff Copter::Mode::takeoff;
Mode::_TakeOff Mode::takeoff;
// This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes.
// The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude
// A safe takeoff speed is calculated and used to calculate a time_ms
// the pos_control target is then slowly increased until time_ms expires
bool Copter::Mode::do_user_takeoff_start(float takeoff_alt_cm)
bool Mode::do_user_takeoff_start(float takeoff_alt_cm)
{
copter.flightmode->takeoff.start(takeoff_alt_cm);
return true;
}
// initiate user takeoff - called when MAVLink TAKEOFF command is received
bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
bool Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
{
if (!copter.motors->armed()) {
return false;
}
if (!ap.land_complete) {
if (!copter.ap.land_complete) {
// can't takeoff again!
return false;
}
@ -33,7 +33,7 @@ bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
}
// Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning
if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED && ap.using_interlock) {
if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED && copter.ap.using_interlock) {
return false;
}
@ -46,7 +46,7 @@ bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
}
// start takeoff to specified altitude above home in centimeters
void Copter::Mode::_TakeOff::start(float alt_cm)
void Mode::_TakeOff::start(float alt_cm)
{
// indicate we are taking off
copter.set_land_complete(false);
@ -69,7 +69,7 @@ void Copter::Mode::_TakeOff::start(float alt_cm)
}
// stop takeoff
void Copter::Mode::_TakeOff::stop()
void Mode::_TakeOff::stop()
{
_running = false;
start_ms = 0;
@ -79,7 +79,7 @@ void Copter::Mode::_TakeOff::stop()
// pilot_climb_rate is both an input and an output
// takeoff_climb_rate is only an output
// has side-effect of turning takeoff off when timeout as expired
void Copter::Mode::_TakeOff::get_climb_rates(float& pilot_climb_rate,
void Mode::_TakeOff::get_climb_rates(float& pilot_climb_rate,
float& takeoff_climb_rate)
{
// return pilot_climb_rate if take-off inactive
@ -138,7 +138,7 @@ void Copter::Mode::_TakeOff::get_climb_rates(float& pilot_climb_rate,
}
}
void Copter::Mode::auto_takeoff_set_start_alt(void)
void Mode::auto_takeoff_set_start_alt(void)
{
// start with our current altitude
auto_takeoff_no_nav_alt_cm = inertial_nav.get_altitude();
@ -154,7 +154,7 @@ void Copter::Mode::auto_takeoff_set_start_alt(void)
call attitude controller for automatic takeoff, limiting roll/pitch
if below wp_navalt_min
*/
void Copter::Mode::auto_takeoff_attitude_run(float target_yaw_rate)
void Mode::auto_takeoff_attitude_run(float target_yaw_rate)
{
float nav_roll, nav_pitch;
@ -173,12 +173,12 @@ void Copter::Mode::auto_takeoff_attitude_run(float target_yaw_rate)
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate);
}
bool Copter::Mode::is_taking_off() const
bool Mode::is_taking_off() const
{
if (!has_user_takeoff(false)) {
return false;
}
if (ap.land_complete) {
if (copter.ap.land_complete) {
return false;
}
if (takeoff.running()) {