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

Merge pull request #11 from swissfang/fwintegratorlanded

Browse Source 
FW: in seatbelt/althold on ground reset integrators (swissfang)
This commit is contained in:
Lorenz Meier
2014-09-06 10:54:50 +02:00
parent 2fe50a6524 f315be5416
commit e57a758350
1 changed files with 33 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/fw_att_control_main.cpp
+33
View File
@@ -63,6 +63,7 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_status.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
@@ -124,6 +125,7 @@ private:
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
int _global_pos_sub; /**< global position subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */
@@ -139,6 +141,7 @@ private:
struct actuator_controls_s _actuators; /**< actuator control inputs */
struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */
struct vehicle_global_position_s _global_pos; /**< global position */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
@@ -275,6 +278,11 @@ private:
*/
void global_pos_poll();
/**
* Check for vehicle status updates.
*/
void vehicle_status_poll();
/**
* Shim for calling task_main from task_create.
*/
@@ -313,6 +321,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_params_sub(-1),
_manual_sub(-1),
_global_pos_sub(-1),
_vehicle_status_sub(-1),
/* publications */
_rate_sp_pub(-1),
@@ -338,6 +347,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_actuators = {};
_actuators_airframe = {};
_global_pos = {};
_vehicle_status = {};
_parameter_handles.tconst = param_find("FW_ATT_TC");
@@ -560,6 +570,18 @@ FixedwingAttitudeControl::global_pos_poll()
}
}
void
FixedwingAttitudeControl::vehicle_status_poll()
{
/* check if there is new status information */
bool vehicle_status_updated;
orb_check(_vehicle_status_sub, &vehicle_status_updated);
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}
}
void
FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
@@ -585,6 +607,7 @@ FixedwingAttitudeControl::task_main()
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vcontrol_mode_sub, 200);
@@ -599,6 +622,7 @@ FixedwingAttitudeControl::task_main()
vehicle_accel_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_status_poll();
/* wakeup source(s) */
struct pollfd fds[2];
@@ -667,6 +691,8 @@ FixedwingAttitudeControl::task_main()
global_pos_poll();
vehicle_status_poll();
/* lock integrator until control is started */
bool lock_integrator;
@@ -779,6 +805,13 @@ FixedwingAttitudeControl::task_main()
}
}
/* If the aircraft is on ground reset the integrators */
if (_vehicle_status.condition_landed) {
_roll_ctrl.reset_integrator();
_pitch_ctrl.reset_integrator();
_yaw_ctrl.reset_integrator();
}
/* Prepare speed_body_u and speed_body_w */
float speed_body_u = 0.0f;
float speed_body_v = 0.0f;