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: update altitude setpoint if reference altitude changed

Browse Source
This commit is contained in:
Anton Babushkin 2013-10-11 11:58:21 +02:00
parent fbf4b6462d
commit c314f31068
1 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -70,11 +70,13 @@
#include "multirotor_pos_control_params.h"
#include "thrust_pid.h"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
static const float alt_ctl_dz = 0.2f;
static const float pos_ctl_dz = 0.05f;
__EXPORT int multirotor_pos_control_main(int argc, char *argv[]);
/**
@ -233,9 +235,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
float surface_offset = 0.0f; // Z of ground level
float z_sp_dist = 0.0f; // distance to ground setpoint (positive)
float ref_alt = 0.0f;
hrt_abstime ref_timestamp = 0;
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
uint64_t local_ref_timestamp = 0;
uint64_t surface_bottom_timestamp = 0;
@ -346,11 +349,11 @@ 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_prev_t) {
if (local_pos.ref_timestamp != ref_timestamp) {
/* reference alt changed, don't follow large ground level changes in manual flight */
//local_pos_sp.z += local_pos.ref_alt - ref_alt_prev;
local_pos_sp.z += local_pos.ref_alt - ref_alt;
// TODO also correct XY setpoint
//}
}
if (control_mode.flag_control_altitude_enabled) {
/* reset setpoints to current position if needed */
@ -704,6 +707,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
surface_bottom_timestamp = local_pos.surface_bottom_timestamp;
ref_alt = local_pos.ref_alt;
ref_timestamp = local_pos.ref_timestamp;
/* run at approximately 100 Hz */
usleep(10000);
}