diff options
author | Friedemann Ludwig <frieludwig@hotmail.com> | 2014-11-30 20:51:01 +0100 |
---|---|---|
committer | Friedemann Ludwig <frieludwig@hotmail.com> | 2014-11-30 20:51:01 +0100 |
commit | 6efc63d709f5afdbed29528647776d56e7f384a6 (patch) | |
tree | 833b5f13b49d951418e51d97be6bcb3d55275e9c /src/modules/fw_pos_control_l1 | |
parent | 8e8b622f4f5c4737e0f7dad33870f23742449fe9 (diff) | |
download | px4-firmware-6efc63d709f5afdbed29528647776d56e7f384a6.tar.gz px4-firmware-6efc63d709f5afdbed29528647776d56e7f384a6.tar.bz2 px4-firmware-6efc63d709f5afdbed29528647776d56e7f384a6.zip |
fixed somereview comments
Diffstat (limited to 'src/modules/fw_pos_control_l1')
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 13 |
1 files changed, 8 insertions, 5 deletions
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 4f84f088b..e7c95cc86 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -164,6 +164,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ float _hold_alt; /**< hold altitude for velocity mode */ + hrt_abstime _control_position_last_called; /**<last call of control_position */ /* land states */ bool land_noreturn_horizontal; @@ -448,6 +449,9 @@ FixedwingPositionControl::FixedwingPositionControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), + _hold_alt(0.0f), + _control_position_last_called(0), + land_noreturn_horizontal(false), land_noreturn_vertical(false), land_stayonground(false), @@ -877,12 +881,11 @@ bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - static hrt_abstime functionLastCalled = 0; - float dt = 0.0f; - if (functionLastCalled > 0) { - dt = (float)hrt_elapsed_time(&functionLastCalled) * 1e-6f; + float dt = FLT_MIN; // Using non zero value to a avoid division by zero + if (_control_position_last_called > 0) { + dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f; } - functionLastCalled = hrt_absolute_time(); + _control_position_last_called = hrt_absolute_time(); bool setpoint = true; |