From 3e819601589658680bc377ed0009e6c4104347ee Mon Sep 17 00:00:00 2001 From: Ildar Sadykov Date: Wed, 9 Oct 2019 15:20:19 +0300 Subject: [PATCH 1/8] Check system type for SET_ATTITUDE_TARGET attitude publising Check if system type is VTOL and publish attitude to proper uORB topic --- src/modules/mavlink/mavlink_receiver.cpp | 21 ++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 3 +++ 2 files changed, 23 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 2c79c775cda4..511b011d2e76 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -85,6 +85,20 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _parameters_manager(parent), _mavlink_timesync(parent) { + unsigned int system_type = _mavlink->get_system_type(); + + if (system_type == MAV_TYPE_VTOL_DUOROTOR || + system_type == MAV_TYPE_VTOL_QUADROTOR || + system_type == MAV_TYPE_VTOL_TILTROTOR || + system_type == MAV_TYPE_VTOL_RESERVED2 || + system_type == MAV_TYPE_VTOL_RESERVED3 || + system_type == MAV_TYPE_VTOL_RESERVED4 || + system_type == MAV_TYPE_VTOL_RESERVED5) { + _is_vtol = true; + + } else { + _is_vtol = false; + } } void @@ -1411,7 +1425,12 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } } - _att_sp_pub.publish(att_sp); + if (!_is_vtol) { + _att_sp_pub.publish(att_sp); + + } else { + _mc_virtual_att_sp_pub.publish(att_sp); + } } /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 1b9051724a15..14cfde2055ce 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -229,6 +229,7 @@ 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 _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)}; @@ -275,6 +276,8 @@ class MavlinkReceiver : public ModuleParams hrt_abstime _last_utm_global_pos_com{0}; + bool _is_vtol{false}; + DEFINE_PARAMETERS( (ParamFloat) _param_bat_crit_thr, (ParamFloat) _param_bat_emergen_thr, From dc7728be46556f2509cd495742e99130afab3987 Mon Sep 17 00:00:00 2001 From: Ildar Sadykov Date: Wed, 9 Oct 2019 16:09:33 +0300 Subject: [PATCH 2/8] VTOL state check for attitude setpoint publish --- src/modules/mavlink/mavlink_receiver.cpp | 66 ++++++++++++++---------- src/modules/mavlink/mavlink_receiver.h | 5 ++ 2 files changed, 44 insertions(+), 27 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 511b011d2e76..36ddb2ba6828 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -99,6 +99,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : } else { _is_vtol = false; } + } void @@ -1401,36 +1402,47 @@ 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; + if (!_is_vtol) { + // 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; + } + + _att_sp_pub.publish(att_sp); - 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; + } else { + if (_vtol_vehicle_status_sub.updated()) { + _vtol_vehicle_status_sub.copy(&_vtol_vehicle_status); + } + + if (_vtol_vehicle_status.vtol_in_rw_mode) { + att_sp.thrust_body[2] = -set_attitude_target.thrust; + _mc_virtual_att_sp_pub.publish(att_sp); + + } else { + att_sp.thrust_body[0] = set_attitude_target.thrust; + _fw_virtual_att_sp_pub.publish(att_sp); + } } } - - if (!_is_vtol) { - _att_sp_pub.publish(att_sp); - - } else { - _mc_virtual_att_sp_pub.publish(att_sp); - } } /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 14cfde2055ce..c0e60eaae204 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -97,6 +97,7 @@ #include #include #include +#include class Mavlink; @@ -230,6 +231,7 @@ class MavlinkReceiver : public ModuleParams 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)}; @@ -263,6 +265,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 _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; static constexpr unsigned int MOM_SWITCH_COUNT{8}; uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {}; @@ -278,6 +281,8 @@ class MavlinkReceiver : public ModuleParams bool _is_vtol{false}; + vtol_vehicle_status_s _vtol_vehicle_status{}; + DEFINE_PARAMETERS( (ParamFloat) _param_bat_crit_thr, (ParamFloat) _param_bat_emergen_thr, From 535bcf1b30fa9d9e67363bf1120366ff7f2736a5 Mon Sep 17 00:00:00 2001 From: Ildar Sadykov Date: Wed, 9 Oct 2019 17:12:19 +0300 Subject: [PATCH 3/8] Removed _is_vtol in MavlinkReceiver class --- src/modules/mavlink/mavlink_receiver.cpp | 71 ++++++++++++------------ src/modules/mavlink/mavlink_receiver.h | 2 - 2 files changed, 34 insertions(+), 39 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 36ddb2ba6828..d8321715be07 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -85,21 +85,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _parameters_manager(parent), _mavlink_timesync(parent) { - unsigned int system_type = _mavlink->get_system_type(); - - if (system_type == MAV_TYPE_VTOL_DUOROTOR || - system_type == MAV_TYPE_VTOL_QUADROTOR || - system_type == MAV_TYPE_VTOL_TILTROTOR || - system_type == MAV_TYPE_VTOL_RESERVED2 || - system_type == MAV_TYPE_VTOL_RESERVED3 || - system_type == MAV_TYPE_VTOL_RESERVED4 || - system_type == MAV_TYPE_VTOL_RESERVED5) { - _is_vtol = true; - - } else { - _is_vtol = false; - } - } void @@ -1402,33 +1387,42 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid - if (!_is_vtol) { - // 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; + // Fill correct field by checking frametype + // TODO: add as needed + switch (_mavlink->get_system_type()) { + case MAV_TYPE_GENERIC: + break; - case MAV_TYPE_GROUND_ROVER: - att_sp.thrust_body[0] = set_attitude_target.thrust; - break; - } + case MAV_TYPE_FIXED_WING: + att_sp.thrust_body[0] = set_attitude_target.thrust; _att_sp_pub.publish(att_sp); + break; - } else { + 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; + + _att_sp_pub.publish(att_sp); + break; + + case MAV_TYPE_GROUND_ROVER: + att_sp.thrust_body[0] = set_attitude_target.thrust; + + _att_sp_pub.publish(att_sp); + 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: if (_vtol_vehicle_status_sub.updated()) { _vtol_vehicle_status_sub.copy(&_vtol_vehicle_status); } @@ -1441,7 +1435,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) att_sp.thrust_body[0] = set_attitude_target.thrust; _fw_virtual_att_sp_pub.publish(att_sp); } + + break; } + } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index c0e60eaae204..f253d1da58ed 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -279,8 +279,6 @@ class MavlinkReceiver : public ModuleParams hrt_abstime _last_utm_global_pos_com{0}; - bool _is_vtol{false}; - vtol_vehicle_status_s _vtol_vehicle_status{}; DEFINE_PARAMETERS( From 0f6264e5b044c14b990c4c293bbfc2225639b766 Mon Sep 17 00:00:00 2001 From: Ildar Sadykov Date: Wed, 9 Oct 2019 18:00:09 +0300 Subject: [PATCH 4/8] mavlink_receiver: vtol_vehicle_status changed to vehicle_status subscription --- src/modules/mavlink/mavlink_receiver.cpp | 23 +++++++++++++++++------ src/modules/mavlink/mavlink_receiver.h | 5 ++--- 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d8321715be07..21843d1cab04 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1423,17 +1423,28 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) case MAV_TYPE_VTOL_RESERVED3: case MAV_TYPE_VTOL_RESERVED4: case MAV_TYPE_VTOL_RESERVED5: - if (_vtol_vehicle_status_sub.updated()) { - _vtol_vehicle_status_sub.copy(&_vtol_vehicle_status); + if (_vehicle_status_sub.updated()) { + _vehicle_status_sub.copy(&_vehicle_status); } - if (_vtol_vehicle_status.vtol_in_rw_mode) { + switch (_vehicle_status.vehicle_type) { + case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: + att_sp.thrust_body[0] = set_attitude_target.thrust; + + _fw_virtual_att_sp_pub.publish(att_sp); + + break; + + case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING: att_sp.thrust_body[2] = -set_attitude_target.thrust; + _mc_virtual_att_sp_pub.publish(att_sp); - } else { - att_sp.thrust_body[0] = set_attitude_target.thrust; - _fw_virtual_att_sp_pub.publish(att_sp); + break; + + default: + // This should never happen + break; } break; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index f253d1da58ed..4254a0220143 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -97,7 +97,6 @@ #include #include #include -#include class Mavlink; @@ -265,7 +264,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 _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; + 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] {}; @@ -279,7 +278,7 @@ class MavlinkReceiver : public ModuleParams hrt_abstime _last_utm_global_pos_com{0}; - vtol_vehicle_status_s _vtol_vehicle_status{}; + vehicle_status_s _vehicle_status{}; DEFINE_PARAMETERS( (ParamFloat) _param_bat_crit_thr, From ab5ee982acde2544ef7fdedc03beb74a0ac79ef5 Mon Sep 17 00:00:00 2001 From: Ildar Sadykov Date: Thu, 10 Oct 2019 10:59:35 +0300 Subject: [PATCH 5/8] Apply suggestions from code review Co-Authored-By: Julien Lecoeur --- src/modules/mavlink/mavlink_receiver.cpp | 25 +++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 21843d1cab04..e462e4c86f64 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1387,6 +1387,9 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + if (_vehicle_status_sub.updated()) { + _vehicle_status_sub.copy(&_vehicle_status); + } // Fill correct field by checking frametype // TODO: add as needed @@ -1397,7 +1400,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) case MAV_TYPE_FIXED_WING: att_sp.thrust_body[0] = set_attitude_target.thrust; - _att_sp_pub.publish(att_sp); break; case MAV_TYPE_QUADROTOR: @@ -1407,13 +1409,11 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) case MAV_TYPE_HELICOPTER: att_sp.thrust_body[2] = -set_attitude_target.thrust; - _att_sp_pub.publish(att_sp); break; case MAV_TYPE_GROUND_ROVER: att_sp.thrust_body[0] = set_attitude_target.thrust; - _att_sp_pub.publish(att_sp); break; case MAV_TYPE_VTOL_DUOROTOR: @@ -1423,23 +1423,15 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) case MAV_TYPE_VTOL_RESERVED3: case MAV_TYPE_VTOL_RESERVED4: case MAV_TYPE_VTOL_RESERVED5: - if (_vehicle_status_sub.updated()) { - _vehicle_status_sub.copy(&_vehicle_status); - } - switch (_vehicle_status.vehicle_type) { case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: att_sp.thrust_body[0] = set_attitude_target.thrust; - _fw_virtual_att_sp_pub.publish(att_sp); - break; case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING: att_sp.thrust_body[2] = -set_attitude_target.thrust; - _mc_virtual_att_sp_pub.publish(att_sp); - break; default: @@ -1451,6 +1443,17 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } } + + // 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 */ From bc80b794af1a64754f99ec945b013893a7f58501 Mon Sep 17 00:00:00 2001 From: Ildar Sadykov Date: Mon, 14 Oct 2019 16:22:59 +0300 Subject: [PATCH 6/8] set_attitude_target: set rate thrust according to VTOL mode --- src/modules/mavlink/mavlink_receiver.cpp | 32 +++++++++++++++++++++--- 1 file changed, 28 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e462e4c86f64..7e9ea4f583a0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1368,6 +1368,9 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) _control_mode_sub.copy(&control_mode); if (control_mode.flag_control_offboard_enabled) { + if (_vehicle_status_sub.updated()) { + _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,10 +1390,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid - if (_vehicle_status_sub.updated()) { - _vehicle_status_sub.copy(&_vehicle_status); - } - // Fill correct field by checking frametype // TODO: add as needed switch (_mavlink->get_system_type()) { @@ -1498,6 +1497,31 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) 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; } } From 4dda73a884e7cae6b35bb7463e99e09dcd34257e Mon Sep 17 00:00:00 2001 From: Ildar Sadykov Date: Mon, 14 Oct 2019 18:00:03 +0300 Subject: [PATCH 7/8] set_attitude_target: get vehicle_status every time. --- src/modules/mavlink/mavlink_receiver.cpp | 13 ++++++------- src/modules/mavlink/mavlink_receiver.h | 2 -- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7e9ea4f583a0..7c3640740f07 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1368,9 +1368,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) _control_mode_sub.copy(&control_mode); if (control_mode.flag_control_offboard_enabled) { - if (_vehicle_status_sub.updated()) { - _vehicle_status_sub.copy(&_vehicle_status); - } + 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)) { @@ -1422,7 +1421,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) case MAV_TYPE_VTOL_RESERVED3: case MAV_TYPE_VTOL_RESERVED4: case MAV_TYPE_VTOL_RESERVED5: - switch (_vehicle_status.vehicle_type) { + switch (vehicle_status.vehicle_type) { case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: att_sp.thrust_body[0] = set_attitude_target.thrust; @@ -1444,10 +1443,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } // Publish attitude setpoint - if (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) { + 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)) { + } 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 { @@ -1505,7 +1504,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) case MAV_TYPE_VTOL_RESERVED3: case MAV_TYPE_VTOL_RESERVED4: case MAV_TYPE_VTOL_RESERVED5: - switch (_vehicle_status.vehicle_type) { + switch (vehicle_status.vehicle_type) { case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: rates_sp.thrust_body[0] = set_attitude_target.thrust; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4254a0220143..20b0f3fc7c81 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -278,8 +278,6 @@ class MavlinkReceiver : public ModuleParams hrt_abstime _last_utm_global_pos_com{0}; - vehicle_status_s _vehicle_status{}; - DEFINE_PARAMETERS( (ParamFloat) _param_bat_crit_thr, (ParamFloat) _param_bat_emergen_thr, From 66d57862c0211869cd0da77d6f722499a0059f50 Mon Sep 17 00:00:00 2001 From: Ildar Sadykov Date: Sat, 26 Oct 2019 15:53:27 +0300 Subject: [PATCH 8/8] 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 7c3640740f07..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) { @@ -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 20b0f3fc7c81..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. */