From 0326a8ec768006cb6f2929ede00b5abaf8f29ddf Mon Sep 17 00:00:00 2001 From: Ildar Sadykov Date: Sat, 26 Oct 2019 15:53:27 +0300 Subject: [PATCH] mavlink_receiver: Fit in 'flash' region --- src/modules/mavlink/mavlink_receiver.cpp | 150 ++++++++--------------- src/modules/mavlink/mavlink_receiver.h | 2 + 2 files changed, 55 insertions(+), 97 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7c3640740f..0b0ada8080 100644 --- 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 index 20b0f3fc7c..95e94adf2e 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -197,6 +197,8 @@ private: 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. */