From 4b519e4d5daee4da8e619414d4b1cd697198b6a3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Feb 2014 14:04:11 +0100 Subject: [PATCH 01/10] Navigator: Get rid of warnings first --- src/modules/navigator/navigator_main.cpp | 46 ++++++++++++------------ 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 577c767a86..f59663ca00 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -154,17 +154,16 @@ private: int _capabilities_sub; /**< notification of vehicle capabilities updates */ int _control_mode_sub; /**< vehicle control mode subscription */ - orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ + orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ struct vehicle_status_s _vstatus; /**< vehicle status */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct home_position_s _home_pos; /**< home position for RTL */ - struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ - struct mission_item_s _mission_item; /**< current mission item */ - bool _mission_item_valid; /**< current mission item valid */ + struct mission_item_s _mission_item; /**< current mission item */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -178,21 +177,22 @@ private: class Mission _mission; - bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ - bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ + bool _mission_item_valid; /**< current mission item valid */ + bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ + bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; - bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ - bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ + bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ + bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ MissionFeasibilityChecker missionFeasiblityChecker; - uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ bool _pos_sp_triplet_updated; - char *nav_states_str[NAV_STATE_MAX]; + const char *nav_states_str[NAV_STATE_MAX]; struct { float min_altitude; @@ -382,11 +382,11 @@ Navigator::Navigator() : _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), - _control_mode_sub(-1), _params_sub(-1), _offboard_mission_sub(-1), _onboard_mission_sub(-1), _capabilities_sub(-1), + _control_mode_sub(-1), /* publications */ _pos_sp_triplet_pub(-1), @@ -395,22 +395,22 @@ Navigator::Navigator() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), -/* states */ - _rtl_state(RTL_STATE_NONE), + _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), _mission(), + _mission_item_valid(false), _global_pos_valid(false), _reset_loiter_pos(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _set_nav_state_timestamp(0), - _mission_item_valid(false), _need_takeoff(true), _do_takeoff(false), + _set_nav_state_timestamp(0), _pos_sp_triplet_updated(false), - _geofence_violation_warning_sent(false) +/* states */ + _rtl_state(RTL_STATE_NONE) { _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); @@ -1162,7 +1162,7 @@ Navigator::set_mission_item() } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt)); } else { if (onboard) { @@ -1318,7 +1318,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt)); break; } @@ -1344,7 +1344,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); break; } @@ -1362,7 +1362,7 @@ Navigator::set_rtl_item() _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = _parameters.rtl_land_delay < 0.0 ? 0.0f : _parameters.rtl_land_delay; + _mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; _mission_item.origin = ORIGIN_ONBOARD; @@ -1371,7 +1371,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); break; } @@ -1525,7 +1525,7 @@ Navigator::check_mission_item_reached() _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); + mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", (double)_mission_item.time_inside); } } From 3eaa892ee42f7de84698d46ef8cf3b29b96a2b34 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Feb 2014 14:35:41 +0100 Subject: [PATCH 02/10] fw_pos_control_l1: indentation only --- .../fw_pos_control_l1_main.cpp | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 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 45fdaa355e..10aa890885 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 @@ -130,23 +130,23 @@ private: int _att_sub; /**< vehicle attitude subscription */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ - int _control_mode_sub; /**< vehicle status subscription */ + int _control_mode_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ - int _sensor_combined_sub; /**< for body frame accelerations */ + int _sensor_combined_sub; /**< for 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 */ - struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ - struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ + 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 */ + struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ + struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -192,7 +192,7 @@ private: uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s bool _global_pos_valid; ///< global position is valid - math::Matrix<3, 3> _R_nb; ///< current attitude + math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; TECS _tecs; @@ -406,8 +406,8 @@ FixedwingPositionControl::FixedwingPositionControl() : airspeed_s _airspeed = {0}; vehicle_control_mode_s _control_mode = {0}; vehicle_global_position_s _global_pos = {0}; - position_setpoint_triplet_s _pos_sp_triplet = {0}; - sensor_combined_s _sensor_combined = {0}; + position_setpoint_triplet_s _pos_sp_triplet = {0}; + sensor_combined_s _sensor_combined = {0}; @@ -587,8 +587,8 @@ FixedwingPositionControl::vehicle_control_mode_poll() orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); if (!was_armed && _control_mode.flag_armed) { - _launch_lat = _global_pos.lat / 1e7f; - _launch_lon = _global_pos.lon / 1e7f; + _launch_lat = _global_pos.lat / 1e7d; + _launch_lon = _global_pos.lon / 1e7d; _launch_alt = _global_pos.alt; _launch_valid = true; } From 83f66e45999c7db8734534ba1cfb9f9e023f47a7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Feb 2014 14:40:11 +0100 Subject: [PATCH 03/10] fw_pos_control_l1: use double everywhere for lat and lon --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 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 10aa890885..c1aa8d39c8 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 @@ -154,13 +154,13 @@ private: /** manual control states */ float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ - float _loiter_hold_lat; - float _loiter_hold_lon; + double _loiter_hold_lat; + double _loiter_hold_lon; float _loiter_hold_alt; bool _loiter_hold; - float _launch_lat; - float _launch_lon; + double _launch_lat; + double _launch_lon; float _launch_alt; bool _launch_valid; @@ -587,8 +587,8 @@ FixedwingPositionControl::vehicle_control_mode_poll() orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); if (!was_armed && _control_mode.flag_armed) { - _launch_lat = _global_pos.lat / 1e7d; - _launch_lon = _global_pos.lon / 1e7d; + _launch_lat = _global_pos.lat; + _launch_lon = _global_pos.lon; _launch_alt = _global_pos.alt; _launch_valid = true; } From 79d2e5de6cf00186359e5b436c602055e4110249 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Feb 2014 10:48:31 +0100 Subject: [PATCH 04/10] TECS: use constant from geo --- src/lib/ecl/ecl.h | 3 +-- src/lib/external_lgpl/tecs/tecs.cpp | 5 +---- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index e0f207696a..aa3c5000a4 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -38,7 +38,6 @@ */ #include -#include #define ecl_absolute_time hrt_absolute_time -#define ecl_elapsed_time hrt_elapsed_time \ No newline at end of file +#define ecl_elapsed_time hrt_elapsed_time diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 0f28bccadd..3730b19204 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -3,13 +3,10 @@ #include "tecs.h" #include #include +#include using namespace math; -#ifndef CONSTANTS_ONE_G -#define CONSTANTS_ONE_G GRAVITY -#endif - /** * @file tecs.cpp * From 84df8ee78d28c51fc0f6272771d3b7c12b281cfe Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Feb 2014 10:48:46 +0100 Subject: [PATCH 05/10] TECS: warning fixes --- src/lib/external_lgpl/tecs/tecs.h | 27 +++++++++++++-------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index d1ebacda16..5cafb1c791 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -28,16 +28,7 @@ class __EXPORT TECS { public: TECS() : - - _airspeed_enabled(false), - _throttle_slewrate(0.0f), - _climbOutDem(false), - _hgt_dem_prev(0.0f), - _hgt_dem_adj_last(0.0f), - _hgt_dem_in_old(0.0f), - _TAS_dem_last(0.0f), - _TAS_dem_adj(0.0f), - _TAS_dem(0.0f), + _pitch_dem(0.0f), _integ1_state(0.0f), _integ2_state(0.0f), _integ3_state(0.0f), @@ -45,8 +36,16 @@ public: _integ5_state(0.0f), _integ6_state(0.0f), _integ7_state(0.0f), - _pitch_dem(0.0f), _last_pitch_dem(0.0f), + _vel_dot(0.0f), + _TAS_dem(0.0f), + _TAS_dem_last(0.0f), + _hgt_dem_in_old(0.0f), + _hgt_dem_adj_last(0.0f), + _hgt_dem_prev(0.0f), + _TAS_dem_adj(0.0f), + _STEdotErrLast(0.0f), + _climbOutDem(false), _SPE_dem(0.0f), _SKE_dem(0.0f), _SPEdot_dem(0.0f), @@ -55,9 +54,9 @@ public: _SKE_est(0.0f), _SPEdot(0.0f), _SKEdot(0.0f), - _vel_dot(0.0f), - _STEdotErrLast(0.0f) { - + _airspeed_enabled(false), + _throttle_slewrate(0.0f) + { } bool airspeed_sensor_enabled() { From 18f71e6bc41e9823f8c82c49c03ee8c7261b8053 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Feb 2014 10:49:03 +0100 Subject: [PATCH 06/10] fw_pos_control_l1: warning fixes --- .../fw_pos_control_l1_main.cpp | 55 ++++++++++--------- 1 file changed, 28 insertions(+), 27 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 c1aa8d39c8..62c69d3ebe 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 @@ -362,6 +362,7 @@ FixedwingPositionControl *g_control; FixedwingPositionControl::FixedwingPositionControl() : + _mavlink_fd(-1), _task_should_exit(false), _control_task(-1), @@ -380,36 +381,34 @@ FixedwingPositionControl::FixedwingPositionControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), + /* states */ _setpoint_valid(false), _loiter_hold(false), - _airspeed_error(0.0f), - _airspeed_valid(false), - _groundspeed_undershoot(0.0f), - _global_pos_valid(false), land_noreturn_horizontal(false), land_noreturn_vertical(false), land_stayonground(false), land_motor_lim(false), land_onslope(false), - flare_curve_alt_last(0.0f), - _mavlink_fd(-1), - launchDetector(), launch_detected(false), - usePreTakeoffThrust(false) + usePreTakeoffThrust(false), + flare_curve_alt_last(0.0f), + launchDetector(), + _airspeed_error(0.0f), + _airspeed_valid(false), + _groundspeed_undershoot(0.0f), + _global_pos_valid(false) { /* safely initialize structs */ - vehicle_attitude_s _att = {0}; - vehicle_attitude_setpoint_s _att_sp = {0}; - navigation_capabilities_s _nav_capabilities = {0}; - manual_control_setpoint_s _manual = {0}; - airspeed_s _airspeed = {0}; - vehicle_control_mode_s _control_mode = {0}; - vehicle_global_position_s _global_pos = {0}; - position_setpoint_triplet_s _pos_sp_triplet = {0}; - sensor_combined_s _sensor_combined = {0}; - - + memset(&_att, 0, sizeof(vehicle_attitude_s)); + memset(&_att_sp, 0, sizeof(vehicle_attitude_setpoint_s)); + memset(&_nav_capabilities, 0, sizeof(navigation_capabilities_s)); + memset(&_manual, 0, sizeof(manual_control_setpoint_s)); + memset(&_airspeed, 0, sizeof(airspeed_s)); + memset(&_control_mode, 0, sizeof(vehicle_control_mode_s)); + memset(&_global_pos, 0, sizeof(vehicle_global_position_s)); + memset(&_pos_sp_triplet, 0, sizeof(position_setpoint_triplet_s)); + memset(&_sensor_combined, 0, sizeof(sensor_combined_s)); _nav_capabilities.turn_distance = 0.0f; @@ -916,7 +915,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); /* avoid climbout */ - if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground) { flare_curve_alt = pos_sp_triplet.current.alt; land_stayonground = true; @@ -1074,13 +1073,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _seatbelt_hold_heading = _att.yaw + _manual.yaw; } - /* climb out control */ - bool climb_out = false; + //XXX not used - /* user wants to climb out */ - if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { - climb_out = true; - } + /* climb out control */ +// bool climb_out = false; +// +// /* user wants to climb out */ +// if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { +// climb_out = true; +// } /* if in seatbelt mode, set airspeed based on manual control */ @@ -1287,7 +1288,7 @@ FixedwingPositionControl::task_main() float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { + if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) { /* set new turn distance */ _nav_capabilities.turn_distance = turn_distance; From ae3e625ce8cec56527dc6809c0b48081773b47a3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 23:20:17 +0100 Subject: [PATCH 07/10] fw pos ctrl: initialize uorb structs in initialization list --- .../fw_pos_control_l1_main.cpp | 23 ++++++++----------- 1 file changed, 10 insertions(+), 13 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 62c69d3ebe..1feb539b98 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 @@ -397,20 +397,17 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_error(0.0f), _airspeed_valid(false), _groundspeed_undershoot(0.0f), - _global_pos_valid(false) + _global_pos_valid(false), + _att(), + _att_sp(), + _nav_capabilities(), + _manual(), + _airspeed(), + _control_mode(), + _global_pos(), + _pos_sp_triplet(), + _sensor_combined() { - /* safely initialize structs */ - memset(&_att, 0, sizeof(vehicle_attitude_s)); - memset(&_att_sp, 0, sizeof(vehicle_attitude_setpoint_s)); - memset(&_nav_capabilities, 0, sizeof(navigation_capabilities_s)); - memset(&_manual, 0, sizeof(manual_control_setpoint_s)); - memset(&_airspeed, 0, sizeof(airspeed_s)); - memset(&_control_mode, 0, sizeof(vehicle_control_mode_s)); - memset(&_global_pos, 0, sizeof(vehicle_global_position_s)); - memset(&_pos_sp_triplet, 0, sizeof(position_setpoint_triplet_s)); - memset(&_sensor_combined, 0, sizeof(sensor_combined_s)); - - _nav_capabilities.turn_distance = 0.0f; _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); From ab8ece3961f50348cf4bf464cb7d953b182a0a44 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 12:42:48 +0100 Subject: [PATCH 08/10] l1_pos_control: prevent NaNs for roll setpoint --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 3b68a0a4e8..d1c864d782 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -38,6 +38,8 @@ * */ +#include + #include "ecl_l1_pos_controller.h" float ECL_L1_Pos_Controller::nav_roll() @@ -231,8 +233,15 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con /* calculate the vector from waypoint A to current position */ math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); - /* store the normalized vector from waypoint A to current position */ - math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + math::Vector<2> vector_A_to_airplane_unit; + + /* prevent NaN when normalizing */ + if (vector_A_to_airplane.length() > FLT_EPSILON) { + /* store the normalized vector from waypoint A to current position */ + vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + } else { + vector_A_to_airplane_unit = vector_A_to_airplane; + } /* calculate eta angle towards the loiter center */ From 75c6706278119e50ce56ecbb1620025a9b9b936d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 14:19:57 +0100 Subject: [PATCH 09/10] l1_pos_control: set gndspeed_undershoot to 0 in loiter --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 ed57e0d349..2e8d038d0d 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 @@ -695,7 +695,7 @@ void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (_global_pos_valid) { + if (_global_pos_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)); From 909e138182000350df19d47367d6f68d0a6b7fd4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 14:27:24 +0100 Subject: [PATCH 10/10] l1_pos_control: fix stupid 'if not' mistake --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2e8d038d0d..ed6d8792ce 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 @@ -695,7 +695,7 @@ void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (_global_pos_valid && !pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { + if (_global_pos_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));