diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 2c79c775cda4..0b0ada808032 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) { @@ -1368,6 +1419,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) _control_mode_sub.copy(&control_mode); if (control_mode.flag_control_offboard_enabled) { + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ if (!(offboard_control_mode.ignore_attitude)) { @@ -1387,31 +1440,19 @@ 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; - } + fill_thrust(att_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust); } - _att_sp_pub.publish(att_sp); + // Publish attitude setpoint + if (vehicle_status.is_vtol && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) { + _mc_virtual_att_sp_pub.publish(att_sp); + + } else if (vehicle_status.is_vtol && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { + _fw_virtual_att_sp_pub.publish(att_sp); + + } else { + _att_sp_pub.publish(att_sp); + } } /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ @@ -1437,27 +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; - } - + 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 1b9051724a15..95e94adf2e6c 100644 --- 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. */ @@ -229,6 +231,8 @@ class MavlinkReceiver : public ModuleParams uORB::Publication _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)}; uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)}; + uORB::Publication _fw_virtual_att_sp_pub{ORB_ID(fw_virtual_attitude_setpoint)}; uORB::Publication _global_pos_pub{ORB_ID(vehicle_global_position)}; uORB::Publication _gps_pub{ORB_ID(vehicle_gps_position)}; uORB::Publication _land_detector_pub{ORB_ID(vehicle_land_detected)}; @@ -262,6 +266,7 @@ class MavlinkReceiver : public ModuleParams uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; static constexpr unsigned int MOM_SWITCH_COUNT{8}; uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {};