Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

replace _vehicle_status

Browse Source
This commit is contained in:
Thomas
2020-12-08 16:17:59 +01:00
committed by Silvan Fuhrer
parent db58269577
commit d025787334
2 changed files with 18 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+18 -9
View File
@@ -518,7 +518,10 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
_mavlink->request_stop_ulog_streaming(); _mavlink->request_stop_ulog_streaming();
} else if (cmd_mavlink.command == MAV_CMD_DO_CHANGE_SPEED) { } else if (cmd_mavlink.command == MAV_CMD_DO_CHANGE_SPEED) {
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { vehicle_control_mode_s control_mode{};
_control_mode_sub.copy(&control_mode);
if (control_mode.flag_control_offboard_enabled) {
// Not differentiating between airspeed and groundspeed yet // Not differentiating between airspeed and groundspeed yet
set_offb_cruising_speed(cmd_mavlink.param2); set_offb_cruising_speed(cmd_mavlink.param2);
} }
@@ -1524,6 +1527,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
_control_mode_sub.copy(&control_mode); _control_mode_sub.copy(&control_mode);
if (control_mode.flag_control_offboard_enabled) { 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 */ /* Publish attitude setpoint if attitude and thrust ignore bits are not set */
if (!(offboard_control_mode.ignore_attitude)) { if (!(offboard_control_mode.ignore_attitude)) {
@@ -1542,14 +1547,14 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
} }
if (!offboard_control_mode.ignore_thrust) { // don't overwrite thrust if it's invalid if (!offboard_control_mode.ignore_thrust) { // don't overwrite thrust if it's invalid
fill_thrust(att_sp.thrust_body, _vehicle_status.vehicle_type, set_attitude_target.thrust); fill_thrust(att_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust);
} }
// Publish attitude setpoint // 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); _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); _fw_virtual_att_sp_pub.publish(att_sp);
} else { } else {
@@ -1580,7 +1585,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
} }
if (!offboard_control_mode.ignore_thrust) { // don't overwrite thrust if it's invalid if (!offboard_control_mode.ignore_thrust) { // don't overwrite thrust if it's invalid
fill_thrust(rates_sp.thrust_body, _vehicle_status.vehicle_type, set_attitude_target.thrust); fill_thrust(rates_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust);
} }
_rates_sp_pub.publish(rates_sp); _rates_sp_pub.publish(rates_sp);
@@ -2974,8 +2979,6 @@ MavlinkReceiver::Run()
updateParams(); updateParams();
} }
_vehicle_status_sub.update(&_vehicle_status);
int ret = poll(&fds[0], 1, timeout); int ret = poll(&fds[0], 1, timeout);
if (ret > 0) { if (ret > 0) {
@@ -3128,7 +3131,10 @@ MavlinkReceiver::receive_start(pthread_t *thread, Mavlink *parent)
float float
MavlinkReceiver::get_offb_cruising_speed() MavlinkReceiver::get_offb_cruising_speed()
{ {
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
if (_offb_cruising_speed_mc > 0.0f) { if (_offb_cruising_speed_mc > 0.0f) {
return _offb_cruising_speed_mc; return _offb_cruising_speed_mc;
@@ -3149,7 +3155,10 @@ MavlinkReceiver::get_offb_cruising_speed()
void void
MavlinkReceiver::set_offb_cruising_speed(float speed) MavlinkReceiver::set_offb_cruising_speed(float speed)
{ {
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_offb_cruising_speed_mc = speed; _offb_cruising_speed_mc = speed;
} else { } else {
src/modules/mavlink/mavlink_receiver.h
-2
View File
@@ -334,8 +334,6 @@ private:
hrt_abstime _last_utm_global_pos_com{0}; hrt_abstime _last_utm_global_pos_com{0};
vehicle_status_s _vehicle_status{};
float _offb_cruising_speed_mc{-1.0f}; float _offb_cruising_speed_mc{-1.0f};
float _offb_cruising_speed_fw{-1.0f}; float _offb_cruising_speed_fw{-1.0f};