mavlink_receiver: split message handlers to separate methods

src/modules/mavlink/mavlink_receiver.cpp

@@ -36,6 +36,7 @@
  * MAVLink protocol message receive and dispatch
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 /* XXX trim includes */
@@ -120,9 +121,69 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 void
 MavlinkReceiver::handle_message(mavlink_message_t *msg)
 {
-	uint64_t timestamp = hrt_absolute_time();
+	switch (msg->msgid) {
+	case MAVLINK_MSG_ID_COMMAND_LONG:
+		handle_message_command_long(msg);
+		break;
+
+	case MAVLINK_MSG_ID_OPTICAL_FLOW:
+		handle_message_optical_flow(msg);
+		break;
+
+	case MAVLINK_MSG_ID_SET_MODE:
+		handle_message_set_mode(msg);
+		break;
+
+	case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
+		handle_message_vicon_position_estimate(msg);
+		break;
+
+	case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST:
+		handle_message_quad_swarm_roll_pitch_yaw_thrust(msg);
+		break;
+
+	case MAVLINK_MSG_ID_RADIO_STATUS:
+		handle_message_radio_status(msg);
+		break;
+
+	case MAVLINK_MSG_ID_MANUAL_CONTROL:
+		handle_message_manual_control(msg);
+		break;
+
+	default:
+		break;
+	}
+
+	/*
+	 * Only decode hil messages in HIL mode.
+	 *
+	 * The HIL mode is enabled by the HIL bit flag
+	 * in the system mode. Either send a set mode
+	 * COMMAND_LONG message or a SET_MODE message
+	 */
+	if (_mavlink->get_hil_enabled()) {
+		switch (msg->msgid) {
+		case MAVLINK_MSG_ID_HIL_SENSOR:
+			handle_message_hil_sensor(msg);
+			break;
+
+		case MAVLINK_MSG_ID_HIL_GPS:
+			handle_message_hil_gps(msg);
+			break;
 
-	if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
+		case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
+			handle_message_hil_state_quaternion(msg);
+			break;
+
+		default:
+			break;
+		}
+	}
+}
+
+void
+MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
+{
 	/* command */
 	mavlink_command_long_t cmd_mavlink;
 	mavlink_msg_command_long_decode(msg, &cmd_mavlink);
@@ -132,7 +193,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		//check for MAVLINK terminate command
 		if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
 			/* This is the link shutdown command, terminate mavlink */
-				printf("[mavlink] Terminating .. \n");
+			warnx("terminated by remote command");
 			fflush(stdout);
 			usleep(50000);
 
@@ -169,8 +230,11 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 			}
 		}
 	}
+}
 
-	} else if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) {
+void
+MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
+{
 	/* optical flow */
 	mavlink_optical_flow_t flow;
 	mavlink_msg_optical_flow_decode(msg, &flow);
@@ -178,7 +242,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	struct optical_flow_s f;
 	memset(&f, 0, sizeof(f));
 
-		f.timestamp = timestamp;
+	f.timestamp = hrt_absolute_time();
 	f.flow_raw_x = flow.flow_x;
 	f.flow_raw_y = flow.flow_y;
 	f.flow_comp_x_m = flow.flow_comp_m_x;
@@ -193,9 +257,11 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	} else {
 		orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
 	}
+}
 
-	} else if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) {
-		/* set mode on request */
+void
+MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
+{
 	mavlink_set_mode_t new_mode;
 	mavlink_msg_set_mode_decode(msg, &new_mode);
 
@@ -204,7 +270,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 
 	union px4_custom_mode custom_mode;
 	custom_mode.data = new_mode.custom_mode;
-		/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
+	/* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
 	vcmd.param1 = new_mode.base_mode;
 	vcmd.param2 = custom_mode.main_mode;
 	vcmd.param3 = 0;
@@ -219,28 +285,27 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	vcmd.source_component = msg->compid;
 	vcmd.confirmation = 1;
 
-		/* check if topic is advertised */
 	if (_cmd_pub <= 0) {
 		_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
 
 	} else {
-			/* create command */
 		orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
 	}
+}
 
-	} else if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
-		/* vicon */
+void
+MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
+{
 	mavlink_vicon_position_estimate_t pos;
 	mavlink_msg_vicon_position_estimate_decode(msg, &pos);
 
 	struct vehicle_vicon_position_s vicon_position;
 	memset(&vicon_position, 0, sizeof(vicon_position));
-		vicon_position.timestamp = timestamp;
 
+	vicon_position.timestamp = hrt_absolute_time();
 	vicon_position.x = pos.x;
 	vicon_position.y = pos.y;
 	vicon_position.z = pos.z;
-
 	vicon_position.roll = pos.roll;
 	vicon_position.pitch = pos.pitch;
 	vicon_position.yaw = pos.yaw;
@@ -251,9 +316,11 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	} else {
 		orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position);
 	}
+}
 
-	} else if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
-		/* offboard control */
+void
+MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg)
+{
 	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
 	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
 
@@ -261,14 +328,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		struct offboard_control_setpoint_s offboard_control_sp;
 		memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
 
-			/* switch to a receiving link mode */
-			//TODO why do we need this?
-			//_mavlink->set_mode(Mavlink::MODE_TX_HEARTBEAT_ONLY);
-
-			/*
-			 * rate control mode - defined by MAVLink
-			 */
-
 		uint8_t ml_mode = 0;
 		bool ml_armed = false;
 
@@ -310,7 +369,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		offboard_control_sp.armed = ml_armed;
 		offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode);
 
-			offboard_control_sp.timestamp = timestamp;
+		offboard_control_sp.timestamp = hrt_absolute_time();
 
 		if (_offboard_control_sp_pub <= 0) {
 			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
@@ -319,16 +378,18 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
 		}
 	}
+}
 
-	} else if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
-		/* telemetry status */
+void
+MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
+{
 	mavlink_radio_status_t rstatus;
 	mavlink_msg_radio_status_decode(msg, &rstatus);
 
 	struct telemetry_status_s tstatus;
 	memset(&tstatus, 0, sizeof(tstatus));
 
-		tstatus.timestamp = timestamp;
+	tstatus.timestamp = hrt_absolute_time();
 	tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
 	tstatus.rssi = rstatus.rssi;
 	tstatus.remote_rssi = rstatus.remrssi;
@@ -344,9 +405,11 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	} else {
 		orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
 	}
+}
 
-	} else if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
-		/* manual control */
+void
+MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
+{
 	mavlink_manual_control_t man;
 	mavlink_msg_manual_control_decode(msg, &man);
 
@@ -355,7 +418,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		struct rc_channels_s rc;
 		memset(&rc, 0, sizeof(rc));
 
-			rc.timestamp = timestamp;
+		rc.timestamp = hrt_absolute_time();
 		rc.chan_count = 4;
 
 		rc.chan[0].scaled = man.x / 1000.0f;
@@ -379,7 +442,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		/* get a copy first, to prevent altering values that are not sent by the mavlink command */
 		orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual);
 
-			manual.timestamp = timestamp;
+		manual.timestamp = hrt_absolute_time();
 		manual.roll = man.x / 1000.0f;
 		manual.pitch = man.y / 1000.0f;
 		manual.yaw = man.r / 1000.0f;
@@ -392,22 +455,16 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 			orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
 		}
 	}
-	}
-
-	/*
-	 * Only decode hil messages in HIL mode.
-	 *
-	 * The HIL mode is enabled by the HIL bit flag
-	 * in the system mode. Either send a set mode
-	 * COMMAND_LONG message or a SET_MODE message
-	 */
+}
 
-	if (_mavlink->get_hil_enabled()) {
-		if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) {
-			/* HIL sensors */
+void
+MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
+{
 	mavlink_hil_sensor_t imu;
 	mavlink_msg_hil_sensor_decode(msg, &imu);
 
+	uint64_t timestamp = hrt_absolute_time();
+
 	/* airspeed */
 	{
 		struct airspeed_s airspeed;
@@ -596,12 +653,16 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		_old_timestamp = timestamp;
 		_hil_frames = 0;
 	}
+}
 
-		} else if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) {
-			/* HIL GPS */
+void
+MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
+{
 	mavlink_hil_gps_t gps;
 	mavlink_msg_hil_gps_decode(msg, &gps);
 
+	uint64_t timestamp = hrt_absolute_time();
+
 	struct vehicle_gps_position_s hil_gps;
 	memset(&hil_gps, 0, sizeof(hil_gps));
 
@@ -637,12 +698,16 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	} else {
 		_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
 	}
+}
 
-		} else if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) {
-			/* HIL state quaternion */
+void
+MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
+{
 	mavlink_hil_state_quaternion_t hil_state;
 	mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
 
+	uint64_t timestamp = hrt_absolute_time();
+
 	/* airspeed */
 	{
 		struct airspeed_s airspeed;
@@ -795,8 +860,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 		}
 	}
-		}
-	}
 }
 
 

src/modules/mavlink/mavlink_receiver.h

@@ -105,6 +105,17 @@ private:
 	Mavlink	*_mavlink;
 
 	void handle_message(mavlink_message_t *msg);
+	void handle_message_command_long(mavlink_message_t *msg);
+	void handle_message_optical_flow(mavlink_message_t *msg);
+	void handle_message_set_mode(mavlink_message_t *msg);
+	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
+	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
+	void handle_message_radio_status(mavlink_message_t *msg);
+	void handle_message_manual_control(mavlink_message_t *msg);
+	void handle_message_hil_sensor(mavlink_message_t *msg);
+	void handle_message_hil_gps(mavlink_message_t *msg);
+	void handle_message_hil_state_quaternion(mavlink_message_t *msg);
+
 	void *receive_thread(void *arg);
 
 	mavlink_status_t status;