merge origin master into fw_control

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)

apps/mavlink/orb_listener.c

@@ -441,21 +441,66 @@ l_actuator_outputs(struct listener *l)
 			uint8_t mavlink_mode = 0;
 			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
 
+			/* 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 {
 				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] - 1500.0f) / 600.0f) + 1.0f) / 2.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] - 1500.0f) / 600.0f) + 1.0f) / 2.0f;
+					throttle = (((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f);
 				}
 
-			/* HIL message as per MAVLink spec */
 				mavlink_msg_hil_controls_send(chan,
 					hrt_absolute_time(),
 					(act_outputs.output[0] - 1500.0f) / 600.0f,
@@ -470,6 +515,7 @@ l_actuator_outputs(struct listener *l)
 					0);
 			}
 		}
+	}
 }
 
 void