drivers/modules: changes after mavlink_log change

The mavlink_log API changes lead to changes in all drivers/modules using
it.

src/modules/navigator/datalinkloss.cpp

@@ -42,7 +42,7 @@
 #include <math.h>
 #include <fcntl.h>
 
-#include <mavlink/mavlink_log.h>
+#include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
 #include <navigator/navigation.h>
@@ -187,7 +187,7 @@ DataLinkLoss::advance_dll()
 		if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
 			warnx("%d data link losses, limit is %d, fly to airfield home",
 					_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "too many DL losses, fly to airfield home");
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "too many DL losses, fly to airfield home");
 			_navigator->get_mission_result()->stay_in_failsafe = true;
 			_navigator->set_mission_result_updated();
 			reset_mission_item_reached();
@@ -195,19 +195,19 @@ DataLinkLoss::advance_dll()
 		} else {
 			if (!_param_skipcommshold.get()) {
 				warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
-				mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to comms hold");
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to comms hold");
 				_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
 			} else {
 				/* comms hold wp not active, fly to airfield home directly */
 				warnx("Skipping comms hold wp. Flying directly to airfield home");
-				mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to airfield home, comms hold skipped");
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to airfield home, comms hold skipped");
 				_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 			}
 		}
 		break;
 	case DLL_STATE_FLYTOCOMMSHOLDWP:
 		warnx("fly to airfield home");
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to airfield home");
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to airfield home");
 		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->set_mission_result_updated();
@@ -216,7 +216,7 @@ DataLinkLoss::advance_dll()
 	case DLL_STATE_FLYTOAIRFIELDHOMEWP:
 		_dll_state = DLL_STATE_TERMINATE;
 		warnx("time is up, state should have been changed manually by now");
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "no manual control, terminating");
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no manual control, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();

src/modules/navigator/enginefailure.cpp

@@ -41,7 +41,7 @@
 #include <math.h>
 #include <fcntl.h>
 
-#include <mavlink/mavlink_log.h>
+#include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
 #include <navigator/navigation.h>
@@ -141,7 +141,7 @@ EngineFailure::advance_ef()
 {
 	switch (_ef_state) {
 	case EF_STATE_NONE:
-		mavlink_log_emergency(_navigator->get_mavlink_fd(), "Engine failure. Loitering down");
+		mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "Engine failure. Loitering down");
 		_ef_state = EF_STATE_LOITERDOWN;
 		break;
 	default:

src/modules/navigator/geofence.cpp

@@ -44,14 +44,15 @@
 #include <string.h>
 #include <dataman/dataman.h>
 #include <systemlib/err.h>
+#include <systemlib/mavlink_log.h>
 #include <stdlib.h>
 #include <stdio.h>
 #include <ctype.h>
 #include <px4_config.h>
 #include <unistd.h>
-#include <mavlink/mavlink_log.h>
 #include <geo/geo.h>
 #include <drivers/drv_hrt.h>
+#include "navigator.h"
 
 #define GEOFENCE_RANGE_WARNING_LIMIT 3000000
 
@@ -61,8 +62,10 @@
 #endif
 static const int ERROR = -1;
 
-Geofence::Geofence() :
+
+Geofence::Geofence(Navigator *navigator) :
 	SuperBlock(NULL, "GF"),
+	_navigator(navigator),
 	_fence_pub(nullptr),
 	_home_pos{},
 	_home_pos_set(false),
@@ -77,8 +80,7 @@ Geofence::Geofence() :
 	_param_counter_threshold(this, "COUNT"),
 	_param_max_hor_distance(this, "MAX_HOR_DIST"),
 	_param_max_ver_distance(this, "MAX_VER_DIST"),
-	_outside_counter(0),
-	_mavlinkFd(-1)
+	_outside_counter(0)
 {
 	/* Load initial params */
 	updateParams();
@@ -145,8 +147,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
 
 				if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) {
 					if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
-						mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m",
-								     (double)(dist_z - max_vertical_distance));
+						mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(),
+										 "Geofence exceeded max vertical distance by %.1f m",
+									         (double)(dist_z - max_vertical_distance));
 						_last_vertical_range_warning = hrt_absolute_time();
 					}
 
@@ -155,8 +158,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
 
 				if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) {
 					if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
-						mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m",
-								     (double)(dist_xy - max_horizontal_distance));
+						mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(),
+										 "Geofence exceeded max horizontal distance by %.1f m",
+									         (double)(dist_xy - max_horizontal_distance));
 						_last_horizontal_range_warning = hrt_absolute_time();
 					}
 
@@ -403,12 +407,12 @@ Geofence::loadFromFile(const char *filename)
 	if (gotVertical && pointCounter > 0) {
 		_vertices_count = pointCounter;
 		warnx("Geofence: imported successfully");
-		mavlink_log_info(_mavlinkFd, "Geofence imported");
+		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported");
 		rc = OK;
 
 	} else {
 		warnx("Geofence: import error");
-		mavlink_log_critical(_mavlinkFd, "Geofence import error");
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence import error");
 	}
 
 error:

src/modules/navigator/geofence.h

@@ -53,10 +53,12 @@
 
 #define GEOFENCE_FILENAME PX4_ROOTFSDIR"/fs/microsd/etc/geofence.txt"
 
+class Navigator;
+
 class Geofence : public control::SuperBlock
 {
 public:
-	Geofence();
+	Geofence(Navigator *navigator);
 	~Geofence();
 
 	/* Altitude mode, corresponding to the param GF_ALTMODE */
@@ -105,9 +107,9 @@ public:
 
 	int getGeofenceAction() { return _param_action.get(); }
 
-	void setMavlinkFd(int value) { _mavlinkFd = value; }
-
 private:
+	Navigator	*_navigator;
+
 	orb_advert_t	_fence_pub;			/**< publish fence topic */
 
 	home_position_s _home_pos;
@@ -131,8 +133,6 @@ private:
 
 	unsigned _outside_counter;
 
-	int _mavlinkFd;
-
 	bool inside(double lat, double lon, float altitude);
 	bool inside(const struct vehicle_global_position_s &global_position);
 	bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl);

src/modules/navigator/gpsfailure.cpp

@@ -42,7 +42,7 @@
 #include <math.h>
 #include <fcntl.h>
 
-#include <mavlink/mavlink_log.h>
+#include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
 #include <navigator/navigation.h>
@@ -162,12 +162,12 @@ GpsFailure::advance_gpsf()
 	case GPSF_STATE_NONE:
 		_gpsf_state = GPSF_STATE_LOITER;
 		warnx("gpsf loiter");
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "GPS failed: open loop loiter");
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "GPS failed: open loop loiter");
 		break;
 	case GPSF_STATE_LOITER:
 		_gpsf_state = GPSF_STATE_TERMINATE;
 		warnx("gpsf terminate");
-		mavlink_log_emergency(_navigator->get_mavlink_fd(), "no gps recovery, termination");
+		mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no gps recovery, termination");
 		warnx("mavlink sent");
 		break;
 	case GPSF_STATE_TERMINATE:

src/modules/navigator/land.cpp

@@ -44,8 +44,8 @@
 #include <math.h>
 #include <fcntl.h>
 
-#include <mavlink/mavlink_log.h>
 #include <systemlib/err.h>
+#include <systemlib/mavlink_log.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/position_setpoint_triplet.h>

src/modules/navigator/loiter.cpp

@@ -45,7 +45,7 @@
 #include <math.h>
 #include <fcntl.h>
 
-#include <mavlink/mavlink_log.h>
+#include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>
 
 #include <uORB/uORB.h>

src/modules/navigator/mission.cpp

@@ -52,7 +52,7 @@
 #include <drivers/drv_hrt.h>
 
 #include <dataman/dataman.h>
-#include <mavlink/mavlink_log.h>
+#include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
 #include <lib/mathlib/mathlib.h>
@@ -257,7 +257,7 @@ Mission::update_offboard_mission()
 		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
 		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
-		failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
+		failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
 				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
 				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
 				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
@@ -351,7 +351,7 @@ Mission::set_mission_items()
 	if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_ONBOARD) {
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "onboard mission now running");
 			user_feedback_done = true;
 		}
 		_mission_type = MISSION_TYPE_ONBOARD;
@@ -360,7 +360,7 @@ Mission::set_mission_items()
 	} else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
-			mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
+			mavlink_log_info(_navigator->get_mavlink_log_pub(), "offboard mission now running");
 			user_feedback_done = true;
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
@@ -368,7 +368,7 @@ Mission::set_mission_items()
 		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
 			/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "mission finished, loitering");
 			user_feedback_done = true;
 
 			/* use last setpoint for loiter */
@@ -400,9 +400,9 @@ Mission::set_mission_items()
 			if (_navigator->get_vstatus()->condition_landed) {
 				/* landed, refusing to take off without a mission */
 
-				mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff");
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, refusing takeoff");
 			} else {
-				mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, loitering");
 			}
 
 			user_feedback_done = true;
@@ -432,7 +432,7 @@ Mission::set_mission_items()
 
 			float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
 
-			mavlink_log_info(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
+			mavlink_log_info(_navigator->get_mavlink_log_pub(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
 
 			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
 			_mission_item.lat = _navigator->get_global_position()->lat;
@@ -821,7 +821,7 @@ Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item
 	int offset = 1;
 
 	if (read_mission_item(onboard, 0, mission_item)) {
-		
+
 		first_res = true;
 
 		/* trying to find next position mission item */
@@ -873,7 +873,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
 		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
 			/* mission item index out of bounds - if they are equal, we just reached the end */
 			if (*mission_index_ptr != (int)mission->count) {
-				mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
+				mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
 			}
 			return false;
 		}
@@ -886,8 +886,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
 		/* read mission item from datamanager */
 		if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			mavlink_and_console_log_critical(_navigator->get_mavlink_fd(),
-			                     "ERROR waypoint could not be read");
+			mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(), "ERROR waypoint could not be read");
 			return false;
 		}
 
@@ -906,8 +905,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
 					    &mission_item_tmp, len) != len) {
 						/* not supposed to happen unless the datamanager can't access the
 						 * dataman */
-						mavlink_log_critical(_navigator->get_mavlink_fd(),
-								     "ERROR DO JUMP waypoint could not be written");
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR DO JUMP waypoint could not be written");
 						return false;
 					}
 					report_do_jump_mission_changed(*mission_index_ptr,
@@ -919,8 +917,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
 
 			} else {
 				if (offset == 0) {
-					mavlink_log_info(_navigator->get_mavlink_fd(),
-							     "DO JUMP repetitions completed");
+					mavlink_log_info(_navigator->get_mavlink_log_pub(), "DO JUMP repetitions completed");
 				}
 				/* no more DO_JUMPS, therefore just try to continue with next mission item */
 				(*mission_index_ptr)++;
@@ -934,8 +931,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
 	}
 
 	/* we have given up, we don't want to cycle forever */
-	mavlink_log_critical(_navigator->get_mavlink_fd(),
-			     "ERROR DO JUMP is cycling, giving up");
+	mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR DO JUMP is cycling, giving up");
 	return false;
 }
 
@@ -957,7 +953,7 @@ Mission::save_offboard_mission_state()
 			if (mission_state.current_seq != _current_offboard_mission_index) {
 				if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
 					warnx("ERROR: can't save mission state");
-					mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
+					mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: can't save mission state");
 				}
 			}
 		}
@@ -969,12 +965,12 @@ Mission::save_offboard_mission_state()
 		mission_state.current_seq = _current_offboard_mission_index;
 
 		warnx("ERROR: invalid mission state");
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: invalid mission state");
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: invalid mission state");
 
 		/* write modified state only if changed */
 		if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
 			warnx("ERROR: can't save mission state");
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: can't save mission state");
 		}
 	}
 
@@ -1027,7 +1023,7 @@ Mission::check_mission_valid()
 
 		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
-		_navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
+		_navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
 				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
 				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
 				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,

src/modules/navigator/mission_block.cpp

@@ -49,7 +49,7 @@
 
 #include <systemlib/err.h>
 #include <geo/geo.h>
-#include <mavlink/mavlink_log.h>
+#include <systemlib/mavlink_log.h>
 #include <mathlib/mathlib.h>
 
 #include <uORB/uORB.h>
@@ -233,7 +233,7 @@ MissionBlock::is_mission_item_reached()
 			_time_first_inside_orbit = now;
 
 			// if (_mission_item.time_inside > 0.01f) {
-			// 	mavlink_log_critical(_mavlink_fd, "waypoint reached, wait for %.1fs",
+			// 	mavlink_log_critical(_mavlink_log_pub, "waypoint reached, wait for %.1fs",
 			// 		(double)_mission_item.time_inside);
 			// }
 		}
@@ -486,7 +486,7 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
 	if (at_current_location) {
 		item->lat = _navigator->get_global_position()->lat;
 		item->lon = _navigator->get_global_position()->lon;
-	
+
 	/* use home position */
 	} else {
 		item->lat = _navigator->get_home_position()->lat;

src/modules/navigator/mission_feasibility_checker.cpp

@@ -45,7 +45,7 @@
 #include <geo/geo.h>
 #include <math.h>
 #include <mathlib/mathlib.h>
-#include <mavlink/mavlink_log.h>
+#include <systemlib/mavlink_log.h>
 #include <fw_pos_control_l1/landingslope.h>
 #include <systemlib/err.h>
 #include <stdio.h>
@@ -54,7 +54,7 @@
 #include <uORB/topics/fence.h>
 
 MissionFeasibilityChecker::MissionFeasibilityChecker() :
-	_mavlink_fd(-1),
+	_mavlink_log_pub(nullptr),
 	_capabilities_sub(-1),
 	_initDone(false),
 	_dist_1wp_ok(false)
@@ -63,7 +63,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() :
 }
 
 
-bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing,
+bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_pub, bool isRotarywing,
 	dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
 	float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued,
 	float default_acceptance_rad,
@@ -74,13 +74,13 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota
 	/* Init if not done yet */
 	init();
 
-	_mavlink_fd = mavlink_fd;
+	_mavlink_log_pub = mavlink_log_pub;
 
 	// first check if we have a valid position
 	if (!home_valid /* can later use global / local pos for finer granularity */) {
 		failed = true;
 		warned = true;
-		mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock.");
+		mavlink_log_info(_mavlink_log_pub, "Not yet ready for mission, no position lock.");
 	} else {
 		failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued);
 	}
@@ -127,7 +127,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr
 			}
 
 			if (takeoff_alt - 1.0f < acceptance_radius) {
-				mavlink_log_critical(_mavlink_fd, "Mission rejected: Takeoff altitude too low!");
+				mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Takeoff altitude too low!");
 				return false;
 			}
 		}
@@ -165,7 +165,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
 			if (MissionBlock::item_contains_position(&missionitem) &&
 				!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) {
 
-				mavlink_log_critical(_mavlink_fd, "Geofence violation for waypoint %d", i);
+				mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i);
 				return false;
 			}
 		}
@@ -194,10 +194,10 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 			warning_issued = true;
 
 			if (throw_error) {
-				mavlink_log_critical(_mavlink_fd, "Rejecting mission: No home pos, WP %d uses rel alt", i+1);
+				mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: No home pos, WP %d uses rel alt", i+1);
 				return false;
 			} else	{
-				mavlink_log_critical(_mavlink_fd, "Warning: No home pos, WP %d uses rel alt", i+1);
+				mavlink_log_critical(_mavlink_log_pub, "Warning: No home pos, WP %d uses rel alt", i+1);
 				return true;
 			}
 		}
@@ -210,10 +210,10 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 			warning_issued = true;
 
 			if (throw_error) {
-				mavlink_log_critical(_mavlink_fd, "Rejecting mission: Waypoint %d below home", i+1);
+				mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: Waypoint %d below home", i+1);
 				return false;
 			} else	{
-				mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i+1);
+				mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home", i+1);
 				return true;
 			}
 		}
@@ -230,7 +230,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 
 		if (dm_read(dm_current, i, &missionitem, len) != len) {
 			// not supposed to happen unless the datamanager can't access the SD card, etc.
-			mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access SD card");
+			mavlink_log_critical(_mavlink_log_pub, "Rejecting Mission: Cannot access SD card");
 			return false;
 		}
 
@@ -250,7 +250,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 			missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
 			missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
 
-			mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported cmd: %d", (int)(i+1), (int)missionitem.nav_cmd);
+			mavlink_log_critical(_mavlink_log_pub, "Rejecting mission item %i: unsupported cmd: %d", (int)(i+1), (int)missionitem.nav_cmd);
 			return false;
 		}
 
@@ -259,7 +259,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 			i == 0 &&
 			condition_landed) {
 
-			mavlink_log_critical(_mavlink_fd, "Rejecting mission that starts with LAND command while vehicle is landed.");
+			mavlink_log_critical(_mavlink_log_pub, "Rejecting mission that starts with LAND command while vehicle is landed.");
 			return false;
 		}
 
@@ -308,25 +308,25 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
 							return true;
 						} else {
 							/* Landing waypoint is above altitude of slope at the given waypoint distance */
-							mavlink_log_critical(_mavlink_fd, "Landing: last waypoint too high/too close");
-							mavlink_log_critical(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm",
+							mavlink_log_critical(_mavlink_log_pub, "Landing: last waypoint too high/too close");
+							mavlink_log_critical(_mavlink_log_pub, "Move down to %.1fm or move further away by %.1fm",
 									(double)(slope_alt_req),
 									(double)(wp_distance_req - wp_distance));
 							return false;
 						}
 					} else {
 						/* Landing waypoint is above last waypoint */
-						mavlink_log_critical(_mavlink_fd, "Landing waypoint above last nav waypoint");
+						mavlink_log_critical(_mavlink_log_pub, "Landing waypoint above last nav waypoint");
 						return false;
 					}
 				} else {
 					/* Last wp is in flare region */
 					//xxx give recommendations
-					mavlink_log_critical(_mavlink_fd, "Warning: Landing: last waypoint in flare region");
+					mavlink_log_critical(_mavlink_log_pub, "Warning: Landing: last waypoint in flare region");
 					return false;
 				}
 			} else {
-				mavlink_log_critical(_mavlink_fd, "Warning: starting with land waypoint");
+				mavlink_log_critical(_mavlink_log_pub, "Warning: starting with land waypoint");
 				return false;
 			}
 		}
@@ -353,13 +353,13 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
 
 					/* check actuator number */
 					if (mission_item.params[0] < 0 || mission_item.params[0] > 5) {
-						mavlink_log_critical(_mavlink_fd, "Actuator number %d is out of bounds 0..5", (int)mission_item.params[0]);
+						mavlink_log_critical(_mavlink_log_pub, "Actuator number %d is out of bounds 0..5", (int)mission_item.params[0]);
 						warning_issued = true;
 						return false;
 					}
 					/* check actuator value */
 					if (mission_item.params[1] < -2000 || mission_item.params[1] > 2000) {
-						mavlink_log_critical(_mavlink_fd, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.params[1]);
+						mavlink_log_critical(_mavlink_log_pub, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.params[1]);
 						warning_issued = true;
 						return false;
 					}
@@ -375,14 +375,14 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
 						_dist_1wp_ok = true;
 						if (dist_to_1wp > ((dist_first_wp * 3) / 2)) {
 							/* allow at 2/3 distance, but warn */
-							mavlink_log_critical(_mavlink_fd, "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
+							mavlink_log_critical(_mavlink_log_pub, "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
 							warning_issued = true;
 						}
 						return true;
 
 					} else {
 						/* item is too far from home */
-						mavlink_log_critical(_mavlink_fd, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp);
+						mavlink_log_critical(_mavlink_log_pub, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp);
 						warning_issued = true;
 						return false;
 					}
@@ -390,7 +390,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
 
 			} else {
 				/* error reading, mission is invalid */
-				mavlink_log_info(_mavlink_fd, "error reading offboard mission");
+				mavlink_log_info(_mavlink_log_pub, "error reading offboard mission");
 				return false;
 			}
 		}

src/modules/navigator/mission_feasibility_checker.h

@@ -52,7 +52,7 @@
 class MissionFeasibilityChecker
 {
 private:
-	int		_mavlink_fd;
+	orb_advert_t		*_mavlink_log_pub;
 
 	int _capabilities_sub;
 	struct navigation_capabilities_s _nav_caps;
@@ -83,7 +83,7 @@ public:
 	/*
 	 * Returns true if mission is feasible and false otherwise
 	 */
-	bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current,
+	bool checkMissionFeasible(orb_advert_t *mavlink_log_pub, bool isRotarywing, dm_item_t dm_current,
 		size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid,
 		double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad,
 		bool condition_landed);

src/modules/navigator/navigator.h

@@ -176,7 +176,7 @@ public:
 	 * @return the distance at which the next waypoint should be used
 	 */
 	float		get_acceptance_radius(float mission_item_radius);
-	int		get_mavlink_fd() { return _mavlink_fd; }
+	orb_advert_t	*get_mavlink_log_pub() { return &_mavlink_log_pub; }
 
 	void		increment_mission_instance_count() { _mission_instance_count++; }
 
@@ -187,7 +187,7 @@ private:
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
 	int		_navigator_task;		/**< task handle for sensor task */
 
-	int		_mavlink_fd;			/**< the file descriptor to send messages over mavlink */
+	orb_advert_t	_mavlink_log_pub;		/**< the uORB advert to send messages over mavlink */
 
 	int		_global_pos_sub;		/**< global position subscription */
 	int		_gps_pos_sub;		/**< gps position subscription */

src/modules/navigator/navigator_main.cpp

@@ -79,7 +79,7 @@
 #include <geo/geo.h>
 #include <dataman/dataman.h>
 #include <mathlib/mathlib.h>
-#include <mavlink/mavlink_log.h>
+#include <systemlib/mavlink_log.h>
 
 #include "navigator.h"
 
@@ -102,7 +102,7 @@ Navigator::Navigator() :
 	SuperBlock(NULL, "NAV"),
 	_task_should_exit(false),
 	_navigator_task(-1),
-	_mavlink_fd(-1),
+	_mavlink_log_pub(nullptr),
 	_global_pos_sub(-1),
 	_gps_pos_sub(-1),
 	_home_pos_sub(-1),
@@ -131,7 +131,7 @@ Navigator::Navigator() :
 	_mission_item_valid(false),
 	_mission_instance_count(0),
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
-	_geofence{},
+	_geofence(this),
 	_geofence_violation_warning_sent(false),
 	_inside_fence(true),
 	_can_loiter_at_sp(false),
@@ -265,9 +265,6 @@ Navigator::task_main_trampoline(int argc, char *argv[])
 void
 Navigator::task_main()
 {
-	_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
-	_geofence.setMavlinkFd(_mavlink_fd);
-
 	bool have_geofence_position_data = false;
 
 	/* Try to load the geofence:
@@ -281,7 +278,7 @@ Navigator::task_main()
 
 	} else {
 		if (_geofence.clearDm() != OK) {
-			mavlink_log_critical(_mavlink_fd, "failed clearing geofence");
+			mavlink_log_critical(&_mavlink_log_pub, "failed clearing geofence");
 		}
 	}
 
@@ -308,9 +305,6 @@ Navigator::task_main()
 	navigation_capabilities_update();
 	params_update();
 
-	hrt_abstime mavlink_open_time = 0;
-	const hrt_abstime mavlink_open_interval = 500000;
-
 	/* wakeup source(s) */
 	px4_pollfd_struct_t fds[1] = {};
 
@@ -342,12 +336,6 @@ Navigator::task_main()
 
 		perf_begin(_loop_perf);
 
-		if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) {
-			/* try to reopen the mavlink log device with specified interval */
-			mavlink_open_time = hrt_abstime() + mavlink_open_interval;
-			_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
-		}
-
 		bool updated;
 
 		/* gps updated */
@@ -435,7 +423,7 @@ Navigator::task_main()
 
 				/* Issue a warning about the geofence violation once */
 				if (!_geofence_violation_warning_sent) {
-					mavlink_log_critical(_mavlink_fd, "Geofence violation");
+					mavlink_log_critical(&_mavlink_log_pub, "Geofence violation");
 					_geofence_violation_warning_sent = true;
 				}
 			} else {
@@ -777,6 +765,6 @@ Navigator::set_mission_failure(const char* reason)
 	if (!_mission_result.mission_failure) {
 		_mission_result.mission_failure = true;
 		set_mission_result_updated();
-		mavlink_log_critical(_mavlink_fd, "%s", reason);
+		mavlink_log_critical(&_mavlink_log_pub, "%s", reason);
 	}
 }

src/modules/navigator/rcloss.cpp

@@ -42,7 +42,7 @@
 #include <math.h>
 #include <fcntl.h>
 
-#include <mavlink/mavlink_log.h>
+#include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
 
@@ -155,11 +155,11 @@ RCLoss::advance_rcl()
 	case RCL_STATE_NONE:
 		if (_param_loitertime.get() > 0.0f) {
 			warnx("RC loss, OBC mode, loiter");
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "rc loss, loitering");
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "rc loss, loitering");
 			_rcl_state = RCL_STATE_LOITER;
 		} else {
 			warnx("RC loss, OBC mode, slip loiter, terminate");
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "rc loss, terminating");
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "rc loss, terminating");
 			_rcl_state = RCL_STATE_TERMINATE;
 			_navigator->get_mission_result()->stay_in_failsafe = true;
 			_navigator->set_mission_result_updated();
@@ -169,7 +169,7 @@ RCLoss::advance_rcl()
 	case RCL_STATE_LOITER:
 		_rcl_state = RCL_STATE_TERMINATE;
 		warnx("time is up, no RC regain, terminating");
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "RC not regained, terminating");
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RC not regained, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();

src/modules/navigator/rtl.cpp

@@ -42,7 +42,7 @@
 #include <math.h>
 #include <fcntl.h>
 
-#include <mavlink/mavlink_log.h>
+#include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
 
@@ -96,7 +96,7 @@ RTL::on_activation()
 		/* for safety reasons don't go into RTL if landed */
 		if (_navigator->get_vstatus()->condition_landed) {
 			_rtl_state = RTL_STATE_LANDED;
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "no RTL when landed");
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no RTL when landed");
 
 		/* if lower than return altitude, climb up first */
 		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
@@ -157,7 +157,7 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: climb to %d m (%d m above home)",
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
 			(int)(climb_alt),
 			(int)(climb_alt - _navigator->get_home_position()->alt));
 		break;
@@ -189,7 +189,7 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d m (%d m above home)",
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
 			(int)(_mission_item.altitude),
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 
@@ -212,7 +212,7 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = false;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: descend to %d m (%d m above home)",
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
 			(int)(_mission_item.altitude),
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
@@ -238,10 +238,10 @@ RTL::set_rtl_item()
 		_navigator->set_can_loiter_at_sp(true);
 
 		if (autoland) {
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: loiter %.1fs", (double)_mission_item.time_inside);
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)_mission_item.time_inside);
 
 		} else {
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, loiter");
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
 		}
 		break;
 	}
@@ -249,14 +249,14 @@ RTL::set_rtl_item()
 	case RTL_STATE_LAND: {
 		set_land_item(&_mission_item, false);
 
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: land at home");
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: land at home");
 		break;
 	}
 
 	case RTL_STATE_LANDED: {
 		set_idle_item(&_mission_item);
 
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, landed");
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
 		break;
 	}
 

src/modules/navigator/takeoff.cpp

@@ -44,7 +44,7 @@
 #include <math.h>
 #include <fcntl.h>
 
-#include <mavlink/mavlink_log.h>
+#include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>
 
 #include <uORB/uORB.h>