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