Merged rates setpoints

apps/mavlink/mavlink_parameters.c

@@ -72,20 +72,6 @@ static unsigned int mavlink_param_queue_index = 0;
  */
 void mavlink_pm_callback(void *arg, param_t param);
 
-/**
- * Save parameters to EEPROM.
- *
- * Stores the parameters to /eeprom/parameters
- */
-static int mavlink_pm_save_eeprom(void);
-
-/**
- * Load parameters from EEPROM.
- *
- * Loads the parameters from /eeprom/parameters
- */
-static int mavlink_pm_load_eeprom(void);
-
 void mavlink_pm_callback(void *arg, param_t param)
 {
 	mavlink_pm_send_param(param);
@@ -232,69 +218,5 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
 				}
 
 		} break;
-
-		// case MAVLINK_MSG_ID_COMMAND_LONG: {
-		// 	mavlink_command_long_t cmd_mavlink;
-		// 	mavlink_msg_command_long_decode(msg, &cmd_mavlink);
-
-		// 	uint8_t result = MAV_RESULT_UNSUPPORTED;
-
-		// 	if (cmd_mavlink.target_system == mavlink_system.sysid &&
-		// 		((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
-
-		// 		// XXX move this to LOW PRIO THREAD of commander app
-
-		// 		/* preflight parameter load / store */
-		// 		if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) {
-		// 			/* Read all parameters from EEPROM to RAM */
-
-		// 			if (((int)(cmd_mavlink.param1)) == 0)	{
-
-		// 				/* read all parameters from EEPROM to RAM */
-		// 				int read_ret = param_load_default();
-		// 				if (read_ret == OK) {
-		// 					//printf("[mavlink pm] Loaded EEPROM params in RAM\n");
-		// 					mavlink_missionlib_send_gcs_string("[pm] OK loading %s", param_get_default_file());
-		// 					result = MAV_RESULT_ACCEPTED;
-		// 				} else if (read_ret == 1) {
-		// 					mavlink_missionlib_send_gcs_string("[pm] OK no changes %s", param_get_default_file());
-		// 					result = MAV_RESULT_ACCEPTED;
-		// 				} else {
-		// 					if (read_ret < -1) {
-		// 						mavlink_missionlib_send_gcs_string("[pm] ERR loading %s", param_get_default_file());
-		// 					} else {
-		// 						mavlink_missionlib_send_gcs_string("[pm] ERR no file %s", param_get_default_file());
-		// 					}
-		// 					result = MAV_RESULT_FAILED;
-		// 				}
-
-		// 			} else if (((int)(cmd_mavlink.param1)) == 1)	{
-
-		// 				/* write all parameters from RAM to EEPROM */
-		// 				int write_ret = param_save_default();
-		// 				if (write_ret == OK) {
-		// 					mavlink_missionlib_send_gcs_string("[pm] OK saved %s", param_get_default_file());
-		// 					result = MAV_RESULT_ACCEPTED;
-
-		// 				} else {
-		// 					if (write_ret < -1) {
-		// 						mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file());
-		// 					} else {
-		// 						mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file());
-		// 					}
-		// 					result = MAV_RESULT_FAILED;
-		// 				}
-
-		// 			} else {
-		// 				//fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n");
-		// 				mavlink_missionlib_send_gcs_string("[pm] refusing unsupp. STOR request");
-		// 				result = MAV_RESULT_UNSUPPORTED;
-		// 			}
-		// 		}
-		// 	}
-
-		// 	/* send back command result */
-		// 	//mavlink_msg_command_ack_send(chan, cmd.command, result);
-		// } break;
 	}
 }

apps/mavlink/orb_listener.c

@@ -114,6 +114,7 @@ static void	l_manual_control_setpoint(struct listener *l);
 static void	l_vehicle_attitude_controls(struct listener *l);
 static void	l_debug_key_value(struct listener *l);
 static void	l_optical_flow(struct listener *l);
+static void	l_vehicle_rates_setpoint(struct listener *l);
 
 struct listener listeners[] = {
 	{l_sensor_combined,		&mavlink_subs.sensor_sub,	0},
@@ -136,6 +137,7 @@ struct listener listeners[] = {
 	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_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},
 };
 
 static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@@ -408,6 +410,23 @@ l_attitude_setpoint(struct listener *l)
 							att_sp.thrust);
 }
 
+void
+l_vehicle_rates_setpoint(struct listener *l)
+{
+	struct vehicle_rates_setpoint_s rates_sp;
+
+	/* copy local position data into local buffer */
+	orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp);
+
+	if (gcs_link)
+		mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0,
+							rates_sp.timestamp/1000,
+							rates_sp.roll,
+							rates_sp.pitch,
+							rates_sp.yaw,
+							rates_sp.thrust);
+}
+
 void
 l_actuator_outputs(struct listener *l)
 {
@@ -695,6 +714,10 @@ uorb_receive_start(void)
 	mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	orb_set_interval(mavlink_subs.spa_sub, 2000);	/* 0.5 Hz updates */
 
+	/* --- RATES SETPOINT VALUE --- */
+	mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+	orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000);  /* 0.5 Hz updates */
+
 	/* --- ACTUATOR OUTPUTS --- */
 	mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
 	mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));

apps/mavlink/orb_topics.h

@@ -52,6 +52,7 @@
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls_effective.h>
@@ -79,6 +80,7 @@ struct mavlink_subscriptions {
 	int debug_key_value;
 	int input_rc_sub;
 	int optical_flow;
+	int rates_setpoint_sub;
 };
 
 extern struct mavlink_subscriptions mavlink_subs;