mavlink_receiver: Fit in 'flash' region

PX4 · Oct 26, 2019 · 66d5786 · 66d5786
1 parent 84e6a4c
commit 66d5786
Show file tree

Hide file tree

Showing 2 changed files with 55 additions and 97 deletions.
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1289,6 +1289,57 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
 	}
 }
 
+void MavlinkReceiver::fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust)
+{
+	// Fill correct field by checking frametype
+	// TODO: add as needed
+	switch (_mavlink->get_system_type()) {
+	case MAV_TYPE_GENERIC:
+		break;
+
+	case MAV_TYPE_FIXED_WING:
+		thrust_body_array[0] = thrust;
+		break;
+
+	case MAV_TYPE_QUADROTOR:
+	case MAV_TYPE_HEXAROTOR:
+	case MAV_TYPE_OCTOROTOR:
+	case MAV_TYPE_TRICOPTER:
+	case MAV_TYPE_HELICOPTER:
+		thrust_body_array[2] = -thrust;
+		break;
+
+	case MAV_TYPE_GROUND_ROVER:
+		thrust_body_array[0] = thrust;
+		break;
+
+	case MAV_TYPE_VTOL_DUOROTOR:
+	case MAV_TYPE_VTOL_QUADROTOR:
+	case MAV_TYPE_VTOL_TILTROTOR:
+	case MAV_TYPE_VTOL_RESERVED2:
+	case MAV_TYPE_VTOL_RESERVED3:
+	case MAV_TYPE_VTOL_RESERVED4:
+	case MAV_TYPE_VTOL_RESERVED5:
+		switch (vehicle_type) {
+		case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
+			thrust_body_array[0] = thrust;
+
+			break;
+
+		case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
+			thrust_body_array[2] = -thrust;
+
+			break;
+
+		default:
+			// This should never happen
+			break;
+		}
+
+		break;
+	}
+}
+
 void
 MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 {
@@ -1389,57 +1440,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 					}
 
 					if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
-						// Fill correct field by checking frametype
-						// TODO: add as needed
-						switch (_mavlink->get_system_type()) {
-						case MAV_TYPE_GENERIC:
-							break;
-
-						case MAV_TYPE_FIXED_WING:
-							att_sp.thrust_body[0] = set_attitude_target.thrust;
-
-							break;
-
-						case MAV_TYPE_QUADROTOR:
-						case MAV_TYPE_HEXAROTOR:
-						case MAV_TYPE_OCTOROTOR:
-						case MAV_TYPE_TRICOPTER:
-						case MAV_TYPE_HELICOPTER:
-							att_sp.thrust_body[2] = -set_attitude_target.thrust;
-
-							break;
-
-						case MAV_TYPE_GROUND_ROVER:
-							att_sp.thrust_body[0] = set_attitude_target.thrust;
-
-							break;
-
-						case MAV_TYPE_VTOL_DUOROTOR:
-						case MAV_TYPE_VTOL_QUADROTOR:
-						case MAV_TYPE_VTOL_TILTROTOR:
-						case MAV_TYPE_VTOL_RESERVED2:
-						case MAV_TYPE_VTOL_RESERVED3:
-						case MAV_TYPE_VTOL_RESERVED4:
-						case MAV_TYPE_VTOL_RESERVED5:
-							switch (vehicle_status.vehicle_type) {
-							case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
-								att_sp.thrust_body[0] = set_attitude_target.thrust;
-
-								break;
-
-							case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
-								att_sp.thrust_body[2] = -set_attitude_target.thrust;
-
-								break;
-
-							default:
-								// This should never happen
-								break;
-							}
-
-							break;
-						}
-
+						fill_thrust(att_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust);
 					}
 
 					// Publish attitude setpoint
@@ -1477,52 +1478,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 					}
 
 					if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
-						switch (_mavlink->get_system_type()) {
-						case MAV_TYPE_GENERIC:
-							break;
-
-						case MAV_TYPE_FIXED_WING:
-							rates_sp.thrust_body[0] = set_attitude_target.thrust;
-							break;
-
-						case MAV_TYPE_QUADROTOR:
-						case MAV_TYPE_HEXAROTOR:
-						case MAV_TYPE_OCTOROTOR:
-						case MAV_TYPE_TRICOPTER:
-						case MAV_TYPE_HELICOPTER:
-							rates_sp.thrust_body[2] = -set_attitude_target.thrust;
-							break;
-
-						case MAV_TYPE_GROUND_ROVER:
-							rates_sp.thrust_body[0] = set_attitude_target.thrust;
-							break;
-
-						case MAV_TYPE_VTOL_DUOROTOR:
-						case MAV_TYPE_VTOL_QUADROTOR:
-						case MAV_TYPE_VTOL_TILTROTOR:
-						case MAV_TYPE_VTOL_RESERVED2:
-						case MAV_TYPE_VTOL_RESERVED3:
-						case MAV_TYPE_VTOL_RESERVED4:
-						case MAV_TYPE_VTOL_RESERVED5:
-							switch (vehicle_status.vehicle_type) {
-							case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
-								rates_sp.thrust_body[0] = set_attitude_target.thrust;
-
-								break;
-
-							case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
-								rates_sp.thrust_body[2] = -set_attitude_target.thrust;
-
-								break;
-
-							default:
-								// This should never happen
-								break;
-							}
-
-							break;
-						}
-
+						fill_thrust(rates_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust);
 					}
 
 					_rates_sp_pub.publish(rates_sp);

diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
@@ -197,6 +197,8 @@ class MavlinkReceiver : public ModuleParams
 	void send_flight_information();
 	void send_storage_information(int storage_id);
 
+	void fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust);
+
 	/**
 	 * @brief Updates the battery, optical flow, and flight ID subscribed parameters.
 	 */