Copter: FlightMode - convert ALT_HOLD flight mode
This commit is contained in:
parent
d74f0c72da
commit
79c06974b7
@ -796,8 +796,6 @@ private:
|
||||
MAV_RESULT mavlink_compassmot(mavlink_channel_t chan);
|
||||
void delay(uint32_t ms);
|
||||
void 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);
|
||||
bool althold_init(bool ignore_checks);
|
||||
void althold_run();
|
||||
bool auto_init(bool ignore_checks);
|
||||
void auto_run();
|
||||
void auto_takeoff_start(const Location& dest_loc);
|
||||
@ -1171,6 +1169,7 @@ private:
|
||||
Copter::FlightMode_ACRO flightmode_acro{*this};
|
||||
#endif
|
||||
|
||||
Copter::FlightMode_ALTHOLD flightmode_althold{*this};
|
||||
|
||||
public:
|
||||
void mavlink_delay_cb();
|
||||
|
@ -198,3 +198,29 @@ private:
|
||||
};
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
class FlightMode_ALTHOLD : public FlightMode {
|
||||
|
||||
public:
|
||||
|
||||
FlightMode_ALTHOLD(Copter &copter) :
|
||||
Copter::FlightMode(copter)
|
||||
{ }
|
||||
|
||||
bool init(bool ignore_checks) override;
|
||||
void run() override; // should be called at 100hz or more
|
||||
|
||||
bool requires_GPS() const override { return false; }
|
||||
bool has_manual_throttle() const override { return false; }
|
||||
bool allows_arming(bool from_gcs) const override { return true; };
|
||||
bool is_autopilot() const override { return false; }
|
||||
|
||||
protected:
|
||||
|
||||
const char *name() const override { return "ALT_HOLD"; }
|
||||
const char *name4() const override { return "ALTH"; }
|
||||
|
||||
private:
|
||||
|
||||
};
|
||||
|
@ -6,7 +6,7 @@
|
||||
*/
|
||||
|
||||
// althold_init - initialise althold controller
|
||||
bool Copter::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::althold_init(bool ignore_checks)
|
||||
|
||||
// althold_run - runs the althold controller
|
||||
// should be called at 100hz or more
|
||||
void Copter::althold_run()
|
||||
void Copter::FlightMode_ALTHOLD::run()
|
||||
{
|
||||
AltHoldModeState althold_state;
|
||||
float takeoff_climb_rate = 0.0f;
|
||||
@ -149,14 +149,14 @@ void Copter::althold_run()
|
||||
|
||||
#if AC_AVOID_ENABLED == ENABLED
|
||||
// apply avoidance
|
||||
avoid.adjust_roll_pitch(target_roll, target_pitch, aparm.angle_max);
|
||||
_copter.avoid.adjust_roll_pitch(target_roll, target_pitch, _copter.aparm.angle_max);
|
||||
#endif
|
||||
|
||||
// call attitude controller
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// adjust climb rate using rangefinder
|
||||
if (rangefinder_alt_ok()) {
|
||||
if (_copter.rangefinder_alt_ok()) {
|
||||
// if rangefinder is ok, use surface tracking
|
||||
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
|
||||
}
|
||||
|
@ -55,7 +55,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
|
||||
break;
|
||||
|
||||
case ALT_HOLD:
|
||||
success = althold_init(ignore_checks);
|
||||
success = flightmode_althold.init(ignore_checks);
|
||||
if (success) {
|
||||
flightmode = &flightmode_althold;
|
||||
}
|
||||
break;
|
||||
|
||||
case AUTO:
|
||||
@ -199,10 +202,6 @@ void Copter::update_flight_mode()
|
||||
#endif
|
||||
break;
|
||||
|
||||
case ALT_HOLD:
|
||||
althold_run();
|
||||
break;
|
||||
|
||||
case AUTO:
|
||||
auto_run();
|
||||
break;
|
||||
@ -413,9 +412,6 @@ void Copter::notify_flight_mode()
|
||||
case STABILIZE:
|
||||
notify.set_flight_mode_str("STAB");
|
||||
break;
|
||||
case ALT_HOLD:
|
||||
notify.set_flight_mode_str("ALTH");
|
||||
break;
|
||||
case AUTO:
|
||||
notify.set_flight_mode_str("AUTO");
|
||||
break;
|
||||
|
Loading…
Reference in New Issue
Block a user