WIP - Live mixer tuning through mavlink including px4io

.gitmodules

@@ -1,6 +1,6 @@
 [submodule "modules/PX4Firmware"]
 	path = modules/PX4Firmware
-	url = git://github.com/ArduPilot/PX4Firmware.git
+	url = https://github.com/crashmatt/Firmware.git
 [submodule "modules/PX4NuttX"]
 	path = modules/PX4NuttX
 	url = git://github.com/ArduPilot/PX4NuttX.git
@@ -15,7 +15,7 @@
 	url = git://github.com/google/benchmark.git
 [submodule "modules/mavlink"]
 	path = modules/mavlink
-	url = git://github.com/ArduPilot/mavlink
+	url = https://github.com/crashmatt/mavlink.git
 [submodule "gtest"]
 	path = modules/gtest
 	url = git://github.com/ArduPilot/googletest

ArduPlane/GCS_Mavlink.cpp

@@ -659,6 +659,12 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
         CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
         plane.adsb.send_adsb_vehicle(chan);
         break;
+    case MSG_MIXER_DATA:
+#if defined(MIXER_CONFIGURATION)
+        CHECK_PAYLOAD_SIZE(MIXER_DATA);
+        plane.mixer.send_mixer_data(chan);
+#endif 	//MIXER_CONFIGURATION
+        break;
     }
     return true;
 }
@@ -778,6 +784,12 @@ GCS_MAVLINK_Plane::data_stream_send(void)
         }
     }
 
+#if defined(MIXER_CONFIGURATION)
+    if(plane.mixer.send_queued()){
+    	send_message(MSG_MIXER_DATA);
+    }
+#endif //MIXER_CONFIGURATION
+
     if (plane.gcs_out_of_time) return;
 
     if (plane.in_mavlink_delay) {
@@ -1716,6 +1728,15 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
         break;
     }
 
+#if defined(MIXER_CONFIGURATION)
+    case MAVLINK_MSG_ID_MIXER_DATA_REQUEST:
+    case MAVLINK_MSG_ID_MIXER_PARAMETER_SET:
+    {
+    	plane.mixer.handle_msg(msg);
+    	break;
+    }
+#endif 	//MIXER_CONFIGURATION
+
     case MAVLINK_MSG_ID_GIMBAL_REPORT:
     {
 #if MOUNT == ENABLED

ArduPlane/Plane.h

@@ -32,7 +32,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Common/AP_Common.h>
 #include <AP_Menu/AP_Menu.h>
-#include <AP_Param/AP_Param.h>
+#include <AP_RCMixer/AP_RCMixer.h>
 #include <StorageManager/StorageManager.h>
 #include <AP_GPS/AP_GPS.h>         // ArduPilot GPS library
 #include <AP_Baro/AP_Baro.h>        // ArduPilot barometer library
@@ -43,6 +43,7 @@
 #include <AP_AccelCal/AP_AccelCal.h>                // interface and maths for accelerometer calibration
 #include <AP_AHRS/AP_AHRS.h>         // ArduPilot Mega DCM Library
 #include <RC_Channel/RC_Channel.h>     // RC Channel Library
+#include <AP_RCMixer/AP_RCMixer.h>     // APM Mixer manager library
 #include <SRV_Channel/SRV_Channel.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>     // Range finder library
 #include <Filter/Filter.h>                     // Filter library
@@ -750,6 +751,10 @@ class Plane : public AP_HAL::HAL::Callbacks {
 
     AP_Param param_loader {var_info};
 
+#if defined(MIXER_CONFIGURATION)
+    AP_RCMixer mixer;
+#endif 	//MIXER_CONFIGURATION
+
     static const AP_Scheduler::Task scheduler_tasks[];
     static const AP_Param::Info var_info[];
 

ArduPlane/make.inc

@@ -62,3 +62,5 @@ LIBRARIES += AP_Tuning
 LIBRARIES += AP_Stats
 LIBRARIES += AP_Landing
 LIBRARIES += AP_Beacon
+LIBRARIES += AP_RCMixer
+

libraries/AP_HAL/RCOutput.h

@@ -112,6 +112,31 @@ class AP_HAL::RCOutput {
       enable SBUS out at the given rate
      */
     virtual bool     enable_sbus_out(uint16_t rate_gz) { return false; }
+
+    /*
+      get the number of mixers
+     */
+    virtual int     get_mixer_count(void) { return -1; }
+
+    /*
+      get the number of submixers for the mixer at given index
+     */
+    virtual int     get_submixer_count(uint16_t mixer_index) { return -1; }
+
+    /*
+      get the mixer type for the mixer/submixer at given indices
+     */
+    virtual int     get_mixer_type(uint16_t mixer_index, uint16_t submixer_index) { return -1; }
+
+    /*
+      get the mixer parameter for the mixer/submixer at given indices
+     */
+    virtual bool    get_mixer_parameter(uint16_t mixer_index, uint16_t submixer_index, uint16_t parameter_index, float *param_value) { return false; }
+
+    /*
+      set the mixer parameter for the mixer/submixer at given indices to param_value
+     */
+    virtual bool    set_mixer_parameter(uint16_t mixer_index, uint16_t submixer_index, uint16_t parameter_index, float param_value) { return false; }
 
     /*
       output modes. Allows for support of oneshot

libraries/AP_HAL_PX4/RCOutput.cpp

@@ -13,6 +13,7 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_sbus.h>
+#include <drivers/drv_mixer.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -645,5 +646,121 @@ void PX4RCOutput::set_output_mode(enum output_mode mode)
     }
 }
 
+#if defined(MIXER_CONFIGURATION)
+/*
+  get the number of mixers
+ */
+int PX4RCOutput::get_mixer_count(void) {
+	int mixers = -1;
+
+	//Presume px4io
+    int fd = open("/dev/px4io", 0);
+    if (fd == -1) {
+        return -200;
+    }
+
+	if( ioctl(fd, MIXERIOCGETMIXERCOUNT, (unsigned long) &mixers) != 0) {
+		mixers = -201;
+	}
+	close(fd);
+	return mixers;
+}
+
+/*
+  get the number of submixers for the mixer at given index
+ */
+int PX4RCOutput::get_submixer_count(uint16_t mixer_index) {
+	int val = mixer_index;
+
+	//Presume px4io
+    int fd = open("/dev/px4io", 0);
+    if (fd == -1) {
+        return -200;
+    }
+
+	if( ioctl(fd, MIXERIOCGETSUBMIXERCOUNT, (unsigned long) &val) != 0) {
+		val = -202;
+	}
+	close(fd);
+	return val;
+}
+
+/*
+  get the mixer type for the mixer/submixer at given indices
+ */
+int PX4RCOutput::get_mixer_type(uint16_t mixer_index, uint16_t submixer_index) {
+	int mix_type = -1;
+	mixer_type_s type;
+
+	type.mix_index = mixer_index;
+	type.mix_sub_index = submixer_index;
+
+	//Presume px4io
+    int fd = open("/dev/px4io", 0);
+    if (fd == -1) {
+        return -200;
+    }
+
+	if( ioctl(fd, MIXERIOCGETTYPE, (unsigned long) &type) == 0) {
+		mix_type = type.mix_type;
+	} else {
+		mix_type = -203;
+	}
+	close(fd);
+	return mix_type;
+}
+
+/*
+  get the mixer parameter for the mixer/submixer at given indices
+ */
+bool PX4RCOutput::get_mixer_parameter(uint16_t mixer_index, uint16_t submixer_index, uint16_t parameter_index, float *param_value) {
+	mixer_param_s param;
+	bool ret = false;
+
+	//Presume px4io
+    int fd = open("/dev/px4io", 0);
+    if (fd == -1) {
+        return false;
+    }
+
+	param.mix_index = mixer_index;
+	param.param_index = parameter_index;
+	param.mix_sub_index = submixer_index;
+
+	if(ioctl(fd, MIXERIOCGETPARAM, (unsigned long) &param) == 0) {
+		*param_value = param.value;
+		ret = true;
+	}
+	close(fd);
+	return ret;
+}
+
+/*
+  set the mixer parameter for the mixer/submixer at given indices to param_value
+  WARNING - THIS WILL BLOCK UNTIL REMOTE MIXER WRITE IS DONE.  THIS MAY TAKE A WHILE.
+ */
+bool PX4RCOutput::set_mixer_parameter(uint16_t mixer_index, uint16_t submixer_index, uint16_t parameter_index, float param_value) {
+	mixer_param_s param;
+	bool ret = false;
+
+	//Presume px4io
+    int fd = open("/dev/px4io", 0);
+    if (fd == -1) {
+        return false;
+    }
+
+	param.mix_index = mixer_index;
+	param.param_index = parameter_index;
+	param.mix_sub_index = submixer_index;
+	param.value = param_value;
+
+	if(ioctl(fd, MIXERIOCSETPARAM, (unsigned long) &param) == 0) {
+		ret = true;
+	}
+	close(fd);
+	return ret;
+}
+#endif 	//MIXER_CONFIGURATION
+
 
 #endif // CONFIG_HAL_BOARD

libraries/AP_HAL_PX4/RCOutput.h

@@ -36,6 +36,13 @@ class PX4::PX4RCOutput : public AP_HAL::RCOutput
 
     void _timer_tick(void);
     bool enable_sbus_out(uint16_t rate_hz) override;
+#if defined(MIXER_CONFIGURATION)
+    int     get_mixer_count(void) override;
+    int     get_submixer_count(uint16_t mixer_index) override;
+    int     get_mixer_type(uint16_t mixer_index, uint16_t submixer_index) override;
+    bool    get_mixer_parameter(uint16_t mixer_index, uint16_t submixer_index, uint16_t parameter_index, float *param_value) override;
+    bool    set_mixer_parameter(uint16_t mixer_index, uint16_t submixer_index, uint16_t parameter_index, float param_value) override;
+#endif 	//MIXER_CONFIGURATION
 
 private:
     int _pwm_fd;

libraries/AP_RCMixer/AP_RCMixer.cpp

@@ -0,0 +1,119 @@
+#include <AP_HAL/AP_HAL.h>
+#include "AP_RCMixer.h"
+#include <GCS_MAVLink/GCS.h>
+
+extern AP_HAL::HAL& hal;
+
+// object constructor.
+AP_RCMixer::AP_RCMixer(void)
+{
+    _mixer_data.group = 0;;
+    _mixer_data.mixer = 0;
+    _mixer_data.submixer = 0;
+    _mixer_data.parameter = 0;
+    _mixer_data.type = 0;
+	_mixer_data.int_value = -1;
+	_mixer_data.param_value = 0.0;
+	_mixer_data.param_type = 0.0;
+
+	_status = AP_RCMIXER_STATUS_WAITING;
+}
+
+void AP_RCMixer::handle_msg(const mavlink_message_t *msg){
+#if defined(MIXER_CONFIGURATION)
+    switch (msg->msgid) {
+		case MAVLINK_MSG_ID_MIXER_DATA_REQUEST: {
+		    mavlink_mixer_data_request_t packet;
+		    mavlink_msg_mixer_data_request_decode(msg, &packet);
+
+		    _mixer_data.group = packet.mixer_group;
+		    _mixer_data.mixer = packet.mixer_index;
+		    _mixer_data.submixer = packet.mixer_sub_index;
+		    _mixer_data.parameter = packet.parameter_index;
+		    _mixer_data.type = packet.data_type;
+
+		    // Make sure we are talking to px4io only
+		    if(packet.mixer_group != 1){
+		    	_mixer_data.int_value = -100;
+				_mixer_data.param_value = 0.0;
+				_status = AP_RCMIXER_STATUS_SEND_DATA;
+				return;
+	    	}
+
+			switch(packet.data_type){
+			case MIXER_DATA_TYPE_MIXER_COUNT:
+				_mixer_data.int_value = hal.rcout->get_mixer_count();
+				_mixer_data.param_value = 0.0;
+				break;
+			case MIXER_DATA_TYPE_SUBMIXER_COUNT:
+				_mixer_data.int_value = hal.rcout->get_submixer_count(_mixer_data.mixer);
+				_mixer_data.param_value = 0.0;
+				break;
+			case MIXER_DATA_TYPE_MIXTYPE:
+				_mixer_data.int_value = hal.rcout->get_mixer_type(_mixer_data.mixer, _mixer_data.submixer);
+				_mixer_data.param_value = 0.0;
+				break;
+			case MIXER_DATA_TYPE_PARAMETER:
+				_mixer_data.int_value = 0;
+				hal.rcout->get_mixer_parameter(_mixer_data.mixer, _mixer_data.submixer, _mixer_data.parameter, &_mixer_data.param_value);
+				_mixer_data.param_type = MAVLINK_TYPE_FLOAT;
+				break;
+			default:
+				break;
+			}
+			_status = AP_RCMIXER_STATUS_SEND_DATA;
+			break;
+		}
+		case MAVLINK_MSG_ID_MIXER_PARAMETER_SET: {
+		    mavlink_mixer_parameter_set_t packet;
+		    mavlink_msg_mixer_parameter_set_decode(msg, &packet);
+
+		    if( packet.mixer_group != 1){
+		    	break;
+		    }
+
+		    _mixer_data.group = packet.mixer_group;
+		    _mixer_data.mixer = packet.mixer_index;
+		    _mixer_data.submixer = packet.mixer_sub_index;
+		    _mixer_data.parameter = packet.parameter_index;
+		    _mixer_data.type = MIXER_DATA_TYPE_PARAMETER;
+		    _mixer_data.param_value = packet.param_value;
+		    _mixer_data.param_type = MAVLINK_TYPE_FLOAT;
+
+			if (hal.rcout->set_mixer_parameter(packet.mixer_index, packet.mixer_sub_index, _mixer_data.parameter, _mixer_data.param_value)) {
+				_mixer_data.int_value = 0;
+			} else {
+				_mixer_data.int_value = -1;
+			}
+			_status = AP_RCMIXER_STATUS_SEND_DATA;
+			break;
+		}
+		default:
+			break;
+	}
+#endif 	//MIXER_CONFIGURATION
+}
+
+//MAVLink mixer data send
+void AP_RCMixer::send_mixer_data(mavlink_channel_t chan){
+//	GCS_MAVLINK::send_statustext_chan(MAV_SEVERITY_INFO, chan, "send_mixer_data");
+	switch(int(_status)){
+	case AP_RCMIXER_STATUS_SEND_DATA:
+		mavlink_msg_mixer_data_send(chan,
+						(uint8_t) _mixer_data.group,
+						(uint8_t) _mixer_data.mixer,
+						(uint8_t) _mixer_data.submixer,
+						(uint8_t) _mixer_data.parameter,
+						(uint8_t) _mixer_data.type,
+						_mixer_data.int_value,
+						_mixer_data.param_value,
+						_mixer_data.param_type
+						);
+		_status = AP_RCMIXER_STATUS_WAITING;
+		break;
+	default:
+		break;
+	}
+}
+
+