aboutsummaryrefslogtreecommitdiff
path: root/src/modules/navigator/navigator_main.cpp
diff options
context:
space:
mode:
authorJulian Oes <julian@oes.ch>2014-02-18 14:04:11 +0100
committerJulian Oes <julian@oes.ch>2014-02-18 14:04:11 +0100
commit4b519e4d5daee4da8e619414d4b1cd697198b6a3 (patch)
tree915ed8f2a45523d1fdc404ceced5cb735c7e40a5 /src/modules/navigator/navigator_main.cpp
parent3508a42f6a308ad4dc53d6c3efc3b2f8013cc086 (diff)
downloadpx4-firmware-4b519e4d5daee4da8e619414d4b1cd697198b6a3.tar.gz
px4-firmware-4b519e4d5daee4da8e619414d4b1cd697198b6a3.tar.bz2
px4-firmware-4b519e4d5daee4da8e619414d4b1cd697198b6a3.zip
Navigator: Get rid of warnings first
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
-rw-r--r--src/modules/navigator/navigator_main.cpp46
1 files changed, 23 insertions, 23 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 577c767a8..f59663ca0 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);
}
}