mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Plane: move mode update_target_altitude to correct files
This commit is contained in:
Iampete1
Andrew Tridgell
parent
feece150c6
commit
4a21af22cb
@ -28,74 +28,6 @@ void Plane::adjust_altitude_target()
|
|||||||
control_mode->update_target_altitude();
|
control_mode->update_target_altitude();
|
||||||
}
|
}
|
||||||
|
|
||||||
// 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 (((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);
|
|
||||||
plane.guided_state.target_alt_time_ms = now;
|
|
||||||
// determine delta accurately as a float
|
|
||||||
float delta_amt_f = delta * plane.guided_state.target_alt_accel;
|
|
||||||
// then scale x100 to match last_target_alt and convert to a signed int32_t as it may be negative
|
|
||||||
int32_t delta_amt_i = (int32_t)(100.0 * delta_amt_f);
|
|
||||||
Location temp {};
|
|
||||||
temp.alt = plane.guided_state.last_target_alt + delta_amt_i; // ...to avoid floats here,
|
|
||||||
if (is_positive(plane.guided_state.target_alt_accel)) {
|
|
||||||
temp.alt = MIN(plane.guided_state.target_alt, temp.alt);
|
|
||||||
} else {
|
|
||||||
temp.alt = MAX(plane.guided_state.target_alt, temp.alt);
|
|
||||||
}
|
|
||||||
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
|
|
||||||
{
|
|
||||||
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);
|
|
||||||
} else if (plane.landing.is_on_approach()) {
|
|
||||||
plane.landing.setup_landing_glide_slope(plane.prev_WP_loc, plane.next_WP_loc, plane.current_loc, plane.target_altitude.offset_cm);
|
|
||||||
plane.landing.adjust_landing_slope_for_rangefinder_bump(plane.rangefinder_state, plane.prev_WP_loc, plane.next_WP_loc, plane.current_loc, plane.auto_state.wp_distance, plane.target_altitude.offset_cm);
|
|
||||||
} else if (plane.landing.get_target_altitude_location(target_location)) {
|
|
||||||
plane.set_target_altitude_location(target_location);
|
|
||||||
#if HAL_SOARING_ENABLED
|
|
||||||
} else if (plane.g2.soaring_controller.is_active() && plane.g2.soaring_controller.get_throttle_suppressed()) {
|
|
||||||
// Reset target alt to current alt, to prevent large altitude errors when gliding.
|
|
||||||
plane.set_target_altitude_location(plane.current_loc);
|
|
||||||
plane.reset_offset_altitude();
|
|
||||||
#endif
|
|
||||||
} else if (plane.reached_loiter_target()) {
|
|
||||||
// once we reach a loiter target then lock to the final
|
|
||||||
// altitude target
|
|
||||||
plane.set_target_altitude_location(plane.next_WP_loc);
|
|
||||||
} else if (plane.target_altitude.offset_cm != 0 &&
|
|
||||||
!plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc)) {
|
|
||||||
// control climb/descent rate
|
|
||||||
plane.set_target_altitude_proportion(plane.next_WP_loc, 1.0f-plane.auto_state.wp_proportion);
|
|
||||||
|
|
||||||
// stay within the range of the start and end locations in altitude
|
|
||||||
plane.constrain_target_altitude_location(plane.next_WP_loc, plane.prev_WP_loc);
|
|
||||||
} else {
|
|
||||||
plane.set_target_altitude_location(plane.next_WP_loc);
|
|
||||||
}
|
|
||||||
|
|
||||||
plane.altitude_error_cm = plane.calc_altitude_error_cm();
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
setup for a gradual glide slope to the next waypoint, if appropriate
|
setup for a gradual glide slope to the next waypoint, if appropriate
|
||||||
*/
|
*/
|
||||||
|
|||||||
@ -127,3 +127,40 @@ bool Mode::is_vtol_man_throttle() const
|
|||||||
#endif
|
#endif
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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);
|
||||||
|
} else if (plane.landing.is_on_approach()) {
|
||||||
|
plane.landing.setup_landing_glide_slope(plane.prev_WP_loc, plane.next_WP_loc, plane.current_loc, plane.target_altitude.offset_cm);
|
||||||
|
plane.landing.adjust_landing_slope_for_rangefinder_bump(plane.rangefinder_state, plane.prev_WP_loc, plane.next_WP_loc, plane.current_loc, plane.auto_state.wp_distance, plane.target_altitude.offset_cm);
|
||||||
|
} else if (plane.landing.get_target_altitude_location(target_location)) {
|
||||||
|
plane.set_target_altitude_location(target_location);
|
||||||
|
#if HAL_SOARING_ENABLED
|
||||||
|
} else if (plane.g2.soaring_controller.is_active() && plane.g2.soaring_controller.get_throttle_suppressed()) {
|
||||||
|
// Reset target alt to current alt, to prevent large altitude errors when gliding.
|
||||||
|
plane.set_target_altitude_location(plane.current_loc);
|
||||||
|
plane.reset_offset_altitude();
|
||||||
|
#endif
|
||||||
|
} else if (plane.reached_loiter_target()) {
|
||||||
|
// once we reach a loiter target then lock to the final
|
||||||
|
// altitude target
|
||||||
|
plane.set_target_altitude_location(plane.next_WP_loc);
|
||||||
|
} else if (plane.target_altitude.offset_cm != 0 &&
|
||||||
|
!plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc)) {
|
||||||
|
// control climb/descent rate
|
||||||
|
plane.set_target_altitude_proportion(plane.next_WP_loc, 1.0f-plane.auto_state.wp_proportion);
|
||||||
|
|
||||||
|
// stay within the range of the start and end locations in altitude
|
||||||
|
plane.constrain_target_altitude_location(plane.next_WP_loc, plane.prev_WP_loc);
|
||||||
|
} else {
|
||||||
|
plane.set_target_altitude_location(plane.next_WP_loc);
|
||||||
|
}
|
||||||
|
|
||||||
|
plane.altitude_error_cm = plane.calc_altitude_error_cm();
|
||||||
|
}
|
||||||
|
|||||||
@ -64,3 +64,32 @@ void ModeGuided::set_radius_and_direction(const float radius, const bool directi
|
|||||||
active_radius_m = constrain_int32(fabsf(radius), 0, UINT16_MAX);
|
active_radius_m = constrain_int32(fabsf(radius), 0, UINT16_MAX);
|
||||||
plane.loiter.direction = direction_is_ccw ? -1 : 1;
|
plane.loiter.direction = direction_is_ccw ? -1 : 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void ModeGuided::update_target_altitude()
|
||||||
|
{
|
||||||
|
#if OFFBOARD_GUIDED == ENABLED
|
||||||
|
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);
|
||||||
|
plane.guided_state.target_alt_time_ms = now;
|
||||||
|
// determine delta accurately as a float
|
||||||
|
float delta_amt_f = delta * plane.guided_state.target_alt_accel;
|
||||||
|
// then scale x100 to match last_target_alt and convert to a signed int32_t as it may be negative
|
||||||
|
int32_t delta_amt_i = (int32_t)(100.0 * delta_amt_f);
|
||||||
|
Location temp {};
|
||||||
|
temp.alt = plane.guided_state.last_target_alt + delta_amt_i; // ...to avoid floats here,
|
||||||
|
if (is_positive(plane.guided_state.target_alt_accel)) {
|
||||||
|
temp.alt = MIN(plane.guided_state.target_alt, temp.alt);
|
||||||
|
} else {
|
||||||
|
temp.alt = MAX(plane.guided_state.target_alt, temp.alt);
|
||||||
|
}
|
||||||
|
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
|
||||||
|
{
|
||||||
|
Mode::update_target_altitude();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user