Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: move adjust_altitude_target functianlity to per mode functions

Browse Source
This commit is contained in:
Iampete1 2022-12-14 12:39:29 +00:00 committed by Andrew Tridgell
parent ab43dadb57
commit f28d7a3574
4 changed files with 40 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
ArduPlane/altitude.cpp
View File

@ -25,17 +25,14 @@
*/
void Plane::adjust_altitude_target()
{
Location target_location;
control_mode->update_target_altitude();
}
if (control_mode == &mode_fbwb ||
control_mode == &mode_cruise) {
return;
}
if ((control_mode == &mode_loiter) && plane.stick_mixing_enabled() && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) {
return;
}
// to be moved to mode_guided.cpp in a future PR, kept here to minimise diff
void ModeGuided::update_target_altitude()
{
#if OFFBOARD_GUIDED == ENABLED
if (control_mode == &mode_guided && ((plane.guided_state.target_alt_time_ms != 0) || plane.guided_state.target_alt > -0.001 )) { // target_alt now defaults to -1, and _time_ms defaults to zero.
if (((plane.guided_state.target_alt_time_ms != 0) || plane.guided_state.target_alt > -0.001 )) { // target_alt now defaults to -1, and _time_ms defaults to zero.
// offboard altitude demanded
uint32_t now = AP_HAL::millis();
float delta = 1e-3f * (now - plane.guided_state.target_alt_time_ms);
@ -53,11 +50,20 @@ void Plane::adjust_altitude_target()
}
plane.guided_state.last_target_alt = temp.alt;
plane.set_target_altitude_location(temp);
plane.altitude_error_cm = plane.calc_altitude_error_cm();
} else
#endif // OFFBOARD_GUIDED == ENABLED
if (control_mode->update_target_altitude()) {
// handled in mode specific code
} else if (plane.landing.is_flaring()) {
{
Mode::update_target_altitude();
}
}
// to be moved to mode.cpp in a future PR, kept here to minimise diff
void Mode::update_target_altitude()
{
Location target_location;
if (plane.landing.is_flaring()) {
// during a landing flare, use TECS_LAND_SINK as a target sink
// rate, and ignores the target altitude
plane.set_target_altitude_location(plane.next_WP_loc);

12
ArduPlane/mode.h
View File

@ -115,7 +115,7 @@ public:
virtual bool does_auto_throttle() const { return false; }
// method for mode specific target altitude profiles
virtual bool update_target_altitude() { return false; }
virtual void update_target_altitude();
// handle a guided target request from GCS
virtual bool handle_guided_request(Location target_loc) { return false; }
@ -226,6 +226,8 @@ public:
void set_radius_and_direction(const float radius, const bool direction_is_ccw);
void update_target_altitude() override;
protected:
bool _enter() override;
@ -278,6 +280,8 @@ public:
bool allows_terrain_disable() const override { return true; }
void update_target_altitude() override;
protected:
bool _enter() override;
@ -422,6 +426,8 @@ public:
bool does_auto_throttle() const override { return true; }
void update_target_altitude() override {};
protected:
bool _enter() override;
@ -448,6 +454,8 @@ public:
bool does_auto_throttle() const override { return true; }
void update_target_altitude() override {};
protected:
bool _enter() override;
@ -596,7 +604,7 @@ public:
bool does_auto_throttle() const override { return true; }
bool update_target_altitude() override;
void update_target_altitude() override;
bool allows_throttle_nudging() const override;

7
ArduPlane/mode_loiter.cpp
View File

@ -110,3 +110,10 @@ void ModeLoiter::navigate()
plane.update_loiter(0);
}
void ModeLoiter::update_target_altitude()
{
if (plane.stick_mixing_enabled() && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) {
return;
}
Mode::update_target_altitude();
}

7
ArduPlane/mode_qrtl.cpp
View File

@ -132,13 +132,14 @@ void ModeQRTL::run()
/*
update target altitude for QRTL profile
*/
bool ModeQRTL::update_target_altitude()
void ModeQRTL::update_target_altitude()
{
/*
update height target in approach
*/
if ((submode != SubMode::RTL) || (plane.quadplane.poscontrol.get_state() != QuadPlane::QPOS_APPROACH)) {
return false;
Mode::update_target_altitude();
return;
}
/*
@ -158,7 +159,7 @@ bool ModeQRTL::update_target_altitude()
Location loc = plane.next_WP_loc;
loc.alt += alt*100;
plane.set_target_altitude_location(loc);
return true;
plane.altitude_error_cm = plane.calc_altitude_error_cm();
}
// only nudge during approach