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

Merge pull request #1089 from PX4/navigator_rewrite

Browse Source 
Navigator rewrite
This commit is contained in:
Lorenz Meier
2014-06-29 19:10:42 +02:00
parent 9388b04557 5db51bd704
commit 94c69e11ae
47 changed files with 2986 additions and 2591 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+3 -7
View File
@@ -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> &current_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> &current_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> &current_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();