Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: Fix truncation of a time interval

Browse Source 
Truncating the a millisecond time delta to uint16_t causes it to wrap
every 65 seconds, which could momentarily reset the desired climb rate
This commit is contained in:
Michael du Breuil 2018-01-24 17:38:07 -07:00 committed by Andrew Tridgell
parent ebf405e53d
commit aec7521da4
1 changed files with 12 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduPlane/quadplane.cpp
View File

@ -718,7 +718,7 @@ void QuadPlane::control_stabilize(void)
// run the multicopter Z controller
void QuadPlane::run_z_controller(void)
{
uint32_t now = AP_HAL::millis();
const uint32_t now = AP_HAL::millis();
if (now - last_pidz_active_ms > 2000) {
// set alt target to current height on transition. This
// starts the Z controller off with the right values
@ -960,7 +960,7 @@ void QuadPlane::control_loiter()
plane.nav_roll_cd = wp_nav->get_roll();
plane.nav_pitch_cd = wp_nav->get_pitch();
uint32_t now = AP_HAL::millis();
const uint32_t now = AP_HAL::millis();
if (now - last_pidz_init_ms < (uint32_t)transition_time_ms*2 && !is_tailsitter()) {
// we limit pitch during initial transition
float pitch_limit_cd = linear_interpolate(loiter_initial_pitch_cd, aparm.angle_max,
@ -1090,7 +1090,7 @@ float QuadPlane::assist_climb_rate_cms(void)
climb_rate = constrain_float(climb_rate, -wp_nav->get_speed_down(), wp_nav->get_speed_up());
// bring in the demanded climb rate over 2 seconds
uint16_t dt_since_start = last_pidz_active_ms - last_pidz_init_ms;
const uint32_t dt_since_start = last_pidz_active_ms - last_pidz_init_ms;
if (dt_since_start < 2000) {
climb_rate = linear_interpolate(0, climb_rate, dt_since_start, 0, 2000);
}
@ -1160,10 +1160,11 @@ bool QuadPlane::assistance_needed(float aspeed)
in_angle_assist = false;
return false;
}
const uint32_t now = AP_HAL::millis();
if (angle_error_start_ms == 0) {
angle_error_start_ms = AP_HAL::millis();
angle_error_start_ms = now;
}
bool ret = (AP_HAL::millis() - angle_error_start_ms) >= 1000U;
bool ret = (now - angle_error_start_ms) >= 1000U;
if (ret && !in_angle_assist) {
in_angle_assist = true;
gcs().send_text(MAV_SEVERITY_INFO, "Angle assist r=%d p=%d",
@ -1386,7 +1387,7 @@ void QuadPlane::update(void)
if (!in_vtol_mode()) {
update_transition();
} else {
uint32_t now = AP_HAL::millis();
const uint32_t now = AP_HAL::millis();
assisted_flight = false;
@ -1538,7 +1539,7 @@ void QuadPlane::motors_output(void)
if (motors->armed()) {
plane.DataFlash.Log_Write_Rate(plane.ahrs, *motors, *attitude_control, *pos_control);
Log_Write_QControl_Tuning();
uint32_t now = AP_HAL::millis();
const uint32_t now = AP_HAL::millis();
if (now - last_ctrl_log_ms > 100) {
attitude_control->control_monitor_log();
}
@ -1936,13 +1937,14 @@ void QuadPlane::setup_target_position(void)
poscontrol.target.y = diff2d.y * 100;
poscontrol.target.z = plane.next_WP_loc.alt - origin.alt;
const uint32_t now = AP_HAL::millis();
if (!locations_are_same(loc, last_auto_target) ||
plane.next_WP_loc.alt != last_auto_target.alt ||
millis() - last_loiter_ms > 500) {
now - last_loiter_ms > 500) {
wp_nav->set_wp_destination(poscontrol.target);
last_auto_target = loc;
}
last_loiter_ms = millis();
last_loiter_ms = now;
// setup vertical speed and acceleration
pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
@ -2159,7 +2161,7 @@ void QuadPlane::check_land_complete(void)
// only apply to final landing phase
return;
}
uint32_t now = AP_HAL::millis();
const uint32_t now = AP_HAL::millis();
bool might_be_landed = (landing_detect.lower_limit_start_ms != 0 &&
now - landing_detect.lower_limit_start_ms > 1000);
if (!might_be_landed) {