merged release_v1.0.0 into master

src/modules/mavlink/mavlink.c

@@ -33,9 +33,9 @@
 
 /**
  * @file mavlink.c
- * Adapter functions expected by the protocol library
+ * Define MAVLink specific parameters
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 #include <px4_config.h>
@@ -46,7 +46,6 @@
 #include "mavlink_bridge_header.h"
 #include <systemlib/param/param.h>
 
-/* define MAVLink specific parameters */
 /**
  * MAVLink system ID
  * @group MAVLink
@@ -59,20 +58,36 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
  * MAVLink component ID
  * @group MAVLink
  * @min 1
- * @max 50
+ * @max 250
  */
 PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
 
 /**
- * MAVLink type
+ * MAVLink Radio ID
+ *
+ * When non-zero the MAVLink app will attempt to configure the
+ * radio to this ID and re-set the parameter to 0. If the value
+ * is negative it will reset the complete radio config to
+ * factory defaults.
+ *
  * @group MAVLink
+ * @min -1
+ * @max 240
  */
-PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
+PARAM_DEFINE_INT32(MAV_RADIO_ID, 0);
 
 /**
- * Use/Accept HIL GPS message (even if not in HIL mode)
+ * MAVLink airframe type
  *
- * If set to 1 incomming HIL GPS messages are parsed.
+ * @min 1
+ * @group MAVLink
+ */
+PARAM_DEFINE_INT32(MAV_TYPE, 1);
+
+/**
+ * Use/Accept HIL GPS message even if not in HIL mode
+ *
+ * If set to 1 incoming HIL GPS messages are parsed.
  *
  * @group MAVLink
  */
@@ -81,8 +96,8 @@ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
 /**
  * Forward external setpoint messages
  *
- * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard
- * control mode
+ * If set to 1 incoming external setpoint messages will be directly forwarded
+ * to the controllers if in offboard control mode
  *
  * @group MAVLink
  */

src/modules/mavlink/mavlink_main.cpp

@@ -145,6 +145,7 @@ Mavlink::Mavlink() :
 	_mavlink_ftp(nullptr),
 	_mode(MAVLINK_MODE_NORMAL),
 	_channel(MAVLINK_COMM_0),
+	_radio_id(0),
 	_logbuffer {},
 	_total_counter(0),
 	_receive_thread {},
@@ -184,6 +185,7 @@ Mavlink::Mavlink() :
 	_param_initialized(false),
 	_param_system_id(0),
 	_param_component_id(0),
+	_param_radio_id(0),
 	_param_system_type(MAV_TYPE_FIXED_WING),
 	_param_use_hil_gps(0),
 	_param_forward_externalsp(0),
@@ -524,6 +526,7 @@ void Mavlink::mavlink_update_system(void)
 	if (!_param_initialized) {
 		_param_system_id = param_find("MAV_SYS_ID");
 		_param_component_id = param_find("MAV_COMP_ID");
+		_param_radio_id = param_find("MAV_RADIO_ID");
 		_param_system_type = param_find("MAV_TYPE");
 		_param_use_hil_gps = param_find("MAV_USEHILGPS");
 		_param_forward_externalsp = param_find("MAV_FWDEXTSP");
@@ -539,6 +542,7 @@ void Mavlink::mavlink_update_system(void)
 	int32_t component_id;
 	param_get(_param_component_id, &component_id);
 
+	param_get(_param_radio_id, &_radio_id);
 
 	/* only allow system ID and component ID updates
 	 * after reboot - not during operation */
@@ -1662,6 +1666,50 @@ Mavlink::task_main(int argc, char *argv[])
 			mavlink_update_system();
 		}
 
+		/* radio config check */
+		if (_radio_id != 0 && _rstatus.type == TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
+			/* request to configure radio and radio is present */
+			FILE *fs = fdopen(_uart_fd, "w");
+			
+			if (fs) {
+				/* switch to AT command mode */
+				usleep(1200000);
+				fprintf(fs, "+++\n");
+				usleep(1200000);
+
+				if (_radio_id > 0) {
+					/* set channel */
+					fprintf(fs, "ATS3=%u\n", _radio_id);
+					usleep(200000);
+				} else {
+					/* reset to factory defaults */
+					fprintf(fs, "AT&F\n");
+					usleep(200000);
+				}
+
+				/* write config */
+				fprintf(fs, "AT&W");
+				usleep(200000);
+
+				/* reboot */
+				fprintf(fs, "ATZ");
+				usleep(200000);
+
+				// XXX NuttX suffers from a bug where
+				// fclose() also closes the fd, not just
+				// the file stream. Since this is a one-time
+				// config thing, we leave the file struct
+				// allocated.
+				//fclose(fs);
+			} else {
+				warnx("open fd %d failed", _uart_fd);
+			}
+
+			/* reset param and save */
+			_radio_id = 0;
+			param_set(_param_radio_id, &_radio_id);
+		}
+
 		if (status_sub->update(&status_time, &status)) {
 			/* switch HIL mode if required */
 			set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);

src/modules/mavlink/mavlink_main.h

@@ -355,6 +355,7 @@ private:
 	MAVLINK_MODE 		_mode;
 
 	mavlink_channel_t	_channel;
+	int32_t			_radio_id;
 
 	struct mavlink_logbuffer _logbuffer;
 	unsigned int		_total_counter;
@@ -424,6 +425,7 @@ private:
 	bool			_param_initialized;
 	param_t			_param_system_id;
 	param_t			_param_component_id;
+	param_t			_param_radio_id;
 	param_t			_param_system_type;
 	param_t			_param_use_hil_gps;
 	param_t			_param_forward_externalsp;

src/modules/mavlink/mavlink_messages.cpp

@@ -1776,6 +1776,10 @@ protected:
 			msg.x = pos_sp.x;
 			msg.y = pos_sp.y;
 			msg.z = pos_sp.z;
+			msg.yaw = pos_sp.yaw;
+			msg.vx = pos_sp.vx;
+			msg.vy = pos_sp.vy;
+			msg.vz = pos_sp.vz;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg);
 		}