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