Accomodating for offboard control setups

4 files changed, 61 insertions, 41 deletions

diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 2ac803ce0..698e43f96 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -119,6 +119,7 @@ bool mavlink_hil_enabled = false;
 /* protocol interface */
 static int uart;
 static int baudrate;
+bool gcs_link = true;
 
 /* interface mode */
 static enum {
@@ -600,14 +601,14 @@ int mavlink_thread_main(int argc, char *argv[])
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
 	} else if (baudrate >= 57600) {
 		/* 10 Hz / 100 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 200);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300);
 		/* 10 Hz / 100 ms ATTITUDE */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
 		/* 5 Hz / 200 ms */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
 		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
 		/* 2 Hz */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
 	} else {
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index 15ed70dbd..550746794 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -93,6 +93,8 @@ static orb_advert_t flow_pub = -1;
 static orb_advert_t offboard_control_sp_pub = -1;
 static orb_advert_t vicon_position_pub = -1;
 
+extern bool gcs_link;
+
 static void
 handle_message(mavlink_message_t *msg)
 {
@@ -218,6 +220,10 @@ handle_message(mavlink_message_t *msg)
 		//printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", mavlink_system.sysid, quad_motors_setpoint.mode);
 
 		if (mavlink_system.sysid < 4) {
+
+			/* switch to a receiving link mode */
+			gcs_link = false;
+
 			/* 
 			 * rate control mode - defined by MAVLink
 			 */
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index c2428874f..0b073cc65 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -61,6 +61,8 @@
 #include "mavlink_hil.h"
 #include "util.h"
 
+extern bool gcs_link;
+
 struct vehicle_global_position_s global_pos;
 struct vehicle_local_position_s local_pos;
 struct vehicle_status_s v_status;
@@ -172,15 +174,16 @@ l_sensor_combined(struct listener *l)
 		baro_counter = raw.baro_counter;
 	}
 
-	mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp,
-		raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
-		raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
-		raw.gyro_rad_s[1], raw.gyro_rad_s[2],
-		raw.magnetometer_ga[0],
-		raw.magnetometer_ga[1],raw.magnetometer_ga[2],
-		raw.baro_pres_mbar, 0 /* no diff pressure yet */,
-		raw.baro_alt_meter, raw.baro_temp_celcius,
-		fields_updated);
+	if (gcs_link)
+		mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp,
+			raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
+			raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
+			raw.gyro_rad_s[1], raw.gyro_rad_s[2],
+			raw.magnetometer_ga[0],
+			raw.magnetometer_ga[1],raw.magnetometer_ga[2],
+			raw.baro_pres_mbar, 0 /* no diff pressure yet */,
+			raw.baro_alt_meter, raw.baro_temp_celcius,
+			fields_updated);
 
 	sensors_raw_counter++;
 }
@@ -194,8 +197,9 @@ l_vehicle_attitude(struct listener *l)
 	/* copy attitude data into local buffer */
 	orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
 
-	/* send sensor values */
-	mavlink_msg_attitude_send(MAVLINK_COMM_0,
+	if (gcs_link)
+		/* send sensor values */
+		mavlink_msg_attitude_send(MAVLINK_COMM_0,
 				  last_sensor_timestamp / 1000,
 				  att.roll,
 				  att.pitch,
@@ -271,8 +275,9 @@ l_rc_channels(struct listener *l)
 	/* copy rc channels into local buffer */
 	orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
 	
-	/* Channels are sent in MAVLink main loop at a fixed interval */
-	mavlink_msg_rc_channels_raw_send(chan,
+	if (gcs_link)
+		/* Channels are sent in MAVLink main loop at a fixed interval */
+		mavlink_msg_rc_channels_raw_send(chan,
 					 rc.timestamp / 1000,
 					 0,
 					 rc.chan[0].raw,
@@ -322,7 +327,8 @@ l_local_position(struct listener *l)
 	/* copy local position data into local buffer */
 	orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos);
 	
-	mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
+	if (gcs_link)
+		mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
 					    local_pos.timestamp / 1000,
 					    local_pos.x,
 					    local_pos.y,
@@ -344,7 +350,8 @@ l_global_position_setpoint(struct listener *l)
 	if (global_sp.altitude_is_relative)
 		coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
 
-	mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
+	if (gcs_link)
+		mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
 						      coordinate_frame,
 						      global_sp.lat,
 						      global_sp.lon,
@@ -360,7 +367,8 @@ l_local_position_setpoint(struct listener *l)
 	/* copy local position data into local buffer */
 	orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp);
 
-	mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0,
+	if (gcs_link)
+		mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0,
 						 MAV_FRAME_LOCAL_NED,
 						 local_sp.x,
 						 local_sp.y,
@@ -376,7 +384,8 @@ l_attitude_setpoint(struct listener *l)
 	/* copy local position data into local buffer */
 	orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp);
 
-	mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0,
+	if (gcs_link)
+		mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0,
 							att_sp.timestamp/1000,
 							att_sp.roll_body,
 							att_sp.pitch_body,
@@ -399,7 +408,8 @@ l_actuator_outputs(struct listener *l)
 	/* copy actuator data into local buffer */
 	orb_copy(ids[l->arg], *l->subp, &act_outputs);
 
-	mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
+	if (gcs_link)
+		mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
 					  l->arg /* port number */,
 					  act_outputs.output[0],
 					  act_outputs.output[1],
@@ -425,7 +435,8 @@ l_manual_control_setpoint(struct listener *l)
 	/* copy manual control data into local buffer */
 	orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control);
 
-	mavlink_msg_manual_control_send(MAVLINK_COMM_0,
+	if (gcs_link)
+		mavlink_msg_manual_control_send(MAVLINK_COMM_0,
 					mavlink_system.sysid,
 					man_control.roll * 1000,
 					man_control.pitch * 1000,
@@ -441,23 +452,25 @@ l_vehicle_attitude_controls(struct listener *l)
 
 	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators);
 
-	/* 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.control[0]);
-	mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-					   last_sensor_timestamp / 1000,
-					   "ctrl1       ",
-					   actuators.control[1]);
-	mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-					   last_sensor_timestamp / 1000,
-					   "ctrl2       ",
-					   actuators.control[2]);
-	mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-					   last_sensor_timestamp / 1000,
-					   "ctrl3       ",
-					   actuators.control[3]);
+	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.control[0]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl1       ",
+						   actuators.control[1]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl2       ",
+						   actuators.control[2]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl3       ",
+						   actuators.control[3]);
+	}
 
 	/* Only send in HIL mode */
 	if (mavlink_hil_enabled) {
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c
index 75f457fb7..42aa0ac63 100644
--- a/apps/multirotor_att_control/multirotor_rate_control.c
+++ b/apps/multirotor_att_control/multirotor_rate_control.c
@@ -181,8 +181,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	if (motor_skip_counter % 2500 == 0) {
 		/* update parameters from storage */
 		parameters_update(&h, &p);
-		warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz",
-			(double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
+		// warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz",
+		// 	(double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
 	}
 
 	/* calculate current control outputs */