merge origin master into fw_control

2 files changed, 78 insertions, 33 deletions

diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 460faf446..2527e0b0f 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -132,6 +132,7 @@ static struct mavlink_logbuffer lb;
 static void mavlink_update_system(void);
 static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
 static void usage(void);
+int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval);
 
 
 
@@ -161,15 +162,13 @@ set_hil_on_off(bool hil_enabled)
 		} else if (baudrate < 115200) {
 			/* 20 Hz */
 			hil_rate_interval = 50;
-		} else if (baudrate < 460800) {
-			/* 50 Hz */
-			hil_rate_interval = 20;
 		} else {
 			/* 100 Hz */
 			hil_rate_interval = 10;
 		}
 
 		orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
 	}
 
 	if (!hil_enabled && mavlink_hil_enabled) {
@@ -263,7 +262,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
 }
 
 
-static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
+int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
 {
 	int ret = OK;
 
@@ -453,19 +452,19 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
 /*
  * Internal function to give access to the channel status for each channel
  */
-mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
+mavlink_status_t* mavlink_get_channel_status(uint8_t channel)
 {
 	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
-	return &m_mavlink_status[chan];
+	return &m_mavlink_status[channel];
 }
 
 /*
  * Internal function to give access to the channel buffer for each channel
  */
-mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
+mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel)
 {
 	static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
-	return &m_mavlink_buffer[chan];
+	return &m_mavlink_buffer[channel];
 }
 
 void mavlink_update_system(void)
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index c49b47051..3ac229d73 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -441,33 +441,79 @@ l_actuator_outputs(struct listener *l)
 			uint8_t mavlink_mode = 0;
 			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
 
-			float rudder, throttle;
-
-			// XXX very ugly, needs rework
-			if (isfinite(act_outputs.output[3])
-				&& act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) {
-				/* throttle is fourth output */
-				rudder = (act_outputs.output[2] - 1500.0f) / 600.0f;
-				throttle = (((act_outputs.output[3] - 1500.0f) / 600.0f) + 1.0f) / 2.0f;
+			/* HIL message as per MAVLink spec */
+
+			/* scale / assign outputs depending on system type */
+
+			if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
+				mavlink_msg_hil_controls_send(chan,
+					hrt_absolute_time(),
+					((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
+					((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
+					((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
+					((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
+					-1,
+					-1,
+					-1,
+					-1,
+					mavlink_mode,
+					0);
+			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
+				mavlink_msg_hil_controls_send(chan,
+					hrt_absolute_time(),
+					((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
+					((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
+					((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
+					((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
+					((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
+					((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
+					-1,
+					-1,
+					mavlink_mode,
+					0);
+			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
+				mavlink_msg_hil_controls_send(chan,
+					hrt_absolute_time(),
+					((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
+					((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
+					((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
+					((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
+					((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
+					((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
+					((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
+					((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
+					mavlink_mode,
+					0);
 			} else {
-				/* only three outputs, put throttle on position 4 / index 3 */
-				rudder = 0;
-				throttle = (((act_outputs.output[2] - 1500.0f) / 600.0f) + 1.0f) / 2.0f;
+				float rudder, throttle;
+
+				/* SCALING: PWM min: 900, PWM max: 2100, center: 1500 */
+
+				// XXX very ugly, needs rework
+				if (isfinite(act_outputs.output[3])
+					&& act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) {
+					/* throttle is fourth output */
+					rudder = (act_outputs.output[2] - 1500.0f) / 600.0f;
+					throttle = (((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f);
+				} else {
+					/* only three outputs, put throttle on position 4 / index 3 */
+					rudder = 0;
+					throttle = (((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f);
+				}
+
+				mavlink_msg_hil_controls_send(chan,
+					hrt_absolute_time(),
+					(act_outputs.output[0] - 1500.0f) / 600.0f,
+					(act_outputs.output[1] - 1500.0f) / 600.0f,
+					rudder,
+					throttle,
+					(act_outputs.output[4] - 1500.0f) / 600.0f,
+					(act_outputs.output[5] - 1500.0f) / 600.0f,
+					(act_outputs.output[6] - 1500.0f) / 600.0f,
+					(act_outputs.output[7] - 1500.0f) / 600.0f,
+					mavlink_mode,
+					0);
 			}
-
-			/* HIL message as per MAVLink spec */
-			mavlink_msg_hil_controls_send(chan,
-				hrt_absolute_time(),
-				(act_outputs.output[0] - 1500.0f) / 600.0f,
-				(act_outputs.output[1] - 1500.0f) / 600.0f,
-				rudder,
-				throttle,
-				(act_outputs.output[4] - 1500.0f) / 600.0f,
-				(act_outputs.output[5] - 1500.0f) / 600.0f,
-				(act_outputs.output[6] - 1500.0f) / 600.0f,
-				(act_outputs.output[7] - 1500.0f) / 600.0f,
-				mavlink_mode,
-				0);
 		}
 	}
 }