fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

multirotor_pos_control: track reference position even if not active to handle reference changes properly

Browse Source
This commit is contained in:
Anton Babushkin 2013-10-08 19:27:20 +02:00
parent 52cf8836fe
commit ff92770305
1 changed files with 12 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -234,8 +234,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
float ref_alt = 0.0f;
hrt_abstime ref_alt_t = 0;
float ref_alt_prev = 0.0f;
hrt_abstime ref_alt_prev_t = 0;
uint64_t local_ref_timestamp = 0;
PID_t xy_pos_pids[2];
@ -331,11 +331,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
t_prev = t;
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) {
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
@ -344,14 +345,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
if (control_mode.flag_control_manual_enabled) {
/* manual control */
/* check for reference point updates and correct setpoint */
if (local_pos.ref_timestamp != ref_alt_t) {
if (ref_alt_t != 0) {
/* home alt changed, don't follow large ground level changes in manual flight */
local_pos_sp.z += local_pos.ref_alt - ref_alt;
if (local_pos.ref_timestamp != ref_alt_prev_t) {
if (ref_alt_prev_t != 0) {
/* reference alt changed, don't follow large ground level changes in manual flight */
local_pos_sp.z -= local_pos.ref_alt - ref_alt_prev;
}
ref_alt_t = local_pos.ref_timestamp;
ref_alt = local_pos.ref_alt;
// TODO also correct XY setpoint
}
@ -675,6 +674,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
reset_auto_sp_z = true;
}
/* track local position reference even when not controlling position */
ref_alt_prev_t = local_pos.ref_timestamp;
ref_alt_prev = local_pos.ref_alt;
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;