diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-29 19:10:42 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-29 19:10:42 +0200 |
commit | 94c69e11ae1ec44c80eec1c979201c7d7e51cdb0 (patch) | |
tree | 0d880776363cfad7deaca35546a2e2a83a8f72a5 /src/modules/fw_pos_control_l1 | |
parent | 9388b045579037371830207765d0fc38f92f38de (diff) | |
parent | 5db51bd704a4493fe3a53010b84bede19c980dfc (diff) | |
download | px4-firmware-94c69e11ae1ec44c80eec1c979201c7d7e51cdb0.tar.gz px4-firmware-94c69e11ae1ec44c80eec1c979201c7d7e51cdb0.tar.bz2 px4-firmware-94c69e11ae1ec44c80eec1c979201c7d7e51cdb0.zip |
Merge pull request #1089 from PX4/navigator_rewrite
Navigator rewrite
Diffstat (limited to 'src/modules/fw_pos_control_l1')
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 10 |
1 files changed, 3 insertions, 7 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 abab74c08..000c02e3d 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 @@ -153,8 +153,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - /** manual control states */ float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */ double _loiter_hold_lat; @@ -420,7 +418,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), /* states */ - _setpoint_valid(false), _loiter_hold(false), land_noreturn_horizontal(false), land_noreturn_vertical(false), @@ -691,7 +688,6 @@ FixedwingPositionControl::vehicle_setpoint_poll() if (pos_sp_triplet_updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - _setpoint_valid = true; } } @@ -736,7 +732,7 @@ void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { + if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); @@ -826,7 +822,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX this should only execute if auto AND safety off (actuators active), // else integrators should be constantly reset. - if (_control_mode.flag_control_position_enabled) { + if (pos_sp_triplet.current.valid) { if (!_was_pos_control_mode) { /* reset integrators */ @@ -869,7 +865,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) { + if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); |