Merged multirotor branch

1 files changed, 63 insertions, 26 deletions

diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index edb8761b8..53d86ec00 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -71,7 +71,8 @@ struct vehicle_status_s v_status;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
 struct actuator_armed_s armed;
-struct actuator_controls_effective_s actuators_0;
+struct actuator_controls_effective_s actuators_effective_0;
+struct actuator_controls_s actuators_0;
 struct vehicle_attitude_s att;
 
 struct mavlink_subscriptions mavlink_subs;
@@ -112,6 +113,7 @@ static void	l_attitude_setpoint(const struct listener *l);
 static void	l_actuator_outputs(const struct listener *l);
 static void	l_actuator_armed(const struct listener *l);
 static void	l_manual_control_setpoint(const struct listener *l);
+static void	l_vehicle_attitude_controls_effective(const struct listener *l);
 static void	l_vehicle_attitude_controls(const struct listener *l);
 static void	l_debug_key_value(const struct listener *l);
 static void	l_optical_flow(const struct listener *l);
@@ -138,6 +140,7 @@ static const struct listener listeners[] = {
 	{l_actuator_armed,		&mavlink_subs.armed_sub,	0},
 	{l_manual_control_setpoint,	&mavlink_subs.man_control_sp_sub, 0},
 	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_sub,	0},
+	{l_vehicle_attitude_controls_effective,	&mavlink_subs.actuators_effective_sub,	0},
 	{l_debug_key_value,		&mavlink_subs.debug_key_value,	0},
 	{l_optical_flow,		&mavlink_subs.optical_flow,	0},
 	{l_vehicle_rates_setpoint,	&mavlink_subs.rates_setpoint_sub,	0},
@@ -272,19 +275,23 @@ l_vehicle_status(const struct listener *l)
 	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
 
 	/* enable or disable HIL */
-	set_hil_on_off(v_status.flag_hil_enabled);
+	if (v_status.hil_state == HIL_STATE_ON)
+		set_hil_on_off(true);
+	else if (v_status.hil_state == HIL_STATE_OFF)
+		set_hil_on_off(false);
 
 	/* translate the current syste state to mavlink state and mode */
 	uint8_t mavlink_state = 0;
-	uint8_t mavlink_mode = 0;
-	get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+	uint8_t mavlink_base_mode = 0;
+	uint32_t mavlink_custom_mode = 0;
+	get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 	/* send heartbeat */
 	mavlink_msg_heartbeat_send(chan,
 				   mavlink_system.type,
 				   MAV_AUTOPILOT_PX4,
-				   mavlink_mode,
-				   v_status.state_machine,
+				   mavlink_base_mode,
+				   mavlink_custom_mode,
 				   mavlink_state);
 }
 
@@ -334,7 +341,7 @@ l_global_position(const struct listener *l)
 	int16_t vz = (int16_t)(global_pos.vz * 100.0f);
 
 	/* heading in degrees * 10, from 0 to 36.000) */
-	uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
+	uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
 
 	mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
 					     timestamp / 1000,
@@ -470,8 +477,9 @@ l_actuator_outputs(const struct listener *l)
 
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
-			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+			uint8_t mavlink_base_mode = 0;
+			uint32_t mavlink_custom_mode = 0;
+			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 			/* HIL message as per MAVLink spec */
 
@@ -488,7 +496,7 @@ l_actuator_outputs(const struct listener *l)
 							      -1,
 							      -1,
 							      -1,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
@@ -502,7 +510,7 @@ l_actuator_outputs(const struct listener *l)
 							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
 							      -1,
 							      -1,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
@@ -516,7 +524,7 @@ l_actuator_outputs(const struct listener *l)
 							      ((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,
+							      mavlink_base_mode,
 							      0);
 
 			} else {
@@ -530,7 +538,7 @@ l_actuator_outputs(const struct listener *l)
 							      (act_outputs.output[5] - 1500.0f) / 500.0f,
 							      (act_outputs.output[6] - 1500.0f) / 500.0f,
 							      (act_outputs.output[7] - 1500.0f) / 500.0f,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 			}
 		}
@@ -562,28 +570,54 @@ l_manual_control_setpoint(const struct listener *l)
 }
 
 void
-l_vehicle_attitude_controls(const struct listener *l)
+l_vehicle_attitude_controls_effective(const struct listener *l)
 {
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0);
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0);
 
 	if (gcs_link) {
 		/* send, add spaces so that string buffer is at least 10 chars long */
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl0    ",
-						   actuators_0.control_effective[0]);
+						   actuators_effective_0.control_effective[0]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl1    ",
-						   actuators_0.control_effective[1]);
+						   actuators_effective_0.control_effective[1]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl2     ",
-						   actuators_0.control_effective[2]);
+						   actuators_effective_0.control_effective[2]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl3     ",
-						   actuators_0.control_effective[3]);
+						   actuators_effective_0.control_effective[3]);
+	}
+}
+
+void
+l_vehicle_attitude_controls(const struct listener *l)
+{
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0);
+
+	if (gcs_link) {
+		/* send, add spaces so that string buffer is at least 10 chars long */
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl0    ",
+						   actuators_0.control[0]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl1    ",
+						   actuators_0.control[1]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl2     ",
+						   actuators_0.control[2]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl3     ",
+						   actuators_0.control[3]);
 	}
 }
 
@@ -632,12 +666,12 @@ l_airspeed(const struct listener *l)
 	orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
 
 	float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
-	float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1);
-	float alt = global_pos.alt;
-	float climb = global_pos.vz;
+	uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
+	float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
+	float alt = global_pos.relative_alt;
+	float climb = -global_pos.vz;
 
-	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed,
-		((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb);
+	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
 }
 
 static void *
@@ -673,7 +707,7 @@ uorb_receive_thread(void *arg)
 
 		/* handle the poll result */
 		if (poll_ret == 0) {
-			mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
+			/* silent */
 
 		} else if (poll_ret < 0) {
 			mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
@@ -768,7 +802,10 @@ uorb_receive_start(void)
 	orb_set_interval(mavlink_subs.man_control_sp_sub, 100);	/* 10Hz updates */
 
 	/* --- ACTUATOR CONTROL VALUE --- */
-	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	orb_set_interval(mavlink_subs.actuators_effective_sub, 100);	/* 10Hz updates */
+
+	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */
 
 	/* --- DEBUG VALUE OUTPUT --- */