diff options
Diffstat (limited to 'src/modules/fw_pos_control_l1')
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 31 |
1 files changed, 30 insertions, 1 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 3d5bce134..cd4a0d58e 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 @@ -74,6 +74,7 @@ #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/navigation_capabilities.h> #include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> #include <systemlib/err.h> @@ -129,9 +130,11 @@ private: int _accel_sub; /**< body frame accelerations */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ + orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< vehicle status */ @@ -312,6 +315,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* publications */ _attitude_sp_pub(-1), + _nav_capabilities_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), @@ -323,6 +327,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _groundspeed_undershoot(0.0f), _global_pos_valid(false) { + _nav_capabilities.turn_distance = 0.0f; + _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); _parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); _parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R"); @@ -625,6 +631,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // else integrators should be constantly reset. if (_control_mode.flag_control_position_enabled) { + /* get circle mode */ + bool was_circle_mode = _l1_control.circle_mode(); + /* execute navigation once we have a setpoint */ if (_setpoint_valid) { @@ -642,7 +651,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { /* - * No valid next waypoint, go for heading hold. + * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ prev_wp.setX(global_triplet.current.lat / 1e7f); @@ -810,6 +819,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } } + if (was_circle_mode && !_l1_control.circle_mode()) { + /* just kicked out of loiter, reset roll integrals */ + _att_sp.roll_reset_integral = true; + } + } else if (0/* seatbelt mode enabled */) { /** SEATBELT FLIGHT **/ @@ -968,6 +982,21 @@ FixedwingPositionControl::task_main() _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } + float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy); + + /* lazily publish navigation capabilities */ + if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { + + /* set new turn distance */ + _nav_capabilities.turn_distance = turn_distance; + + if (_nav_capabilities_pub > 0) { + orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + } else { + _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); + } + } + } } |