Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

FlightModeManager: restore weathervane calls

Browse Source 
I had to do an extra subscription to the vehicle attitude.
I don't know how to test this.
This commit is contained in:
Matthias Grob
2020-10-24 15:05:03 +02:00
committed by Daniel Agar
parent aa888223f0
commit 62ada2e2dc
2 changed files with 19 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/FlightModeManager.cpp
+17 -15
View File
@@ -34,6 +34,7 @@
#include "FlightModeManager.hpp"
#include <lib/mathlib/mathlib.h>
#include <lib/matrix/matrix/math.hpp>
using namespace time_literals;
@@ -106,24 +107,25 @@ void FlightModeManager::Run()
_takeoff.updateTakeoffState(_vehicle_control_mode_sub.get().flag_armed, _vehicle_land_detected_sub.get().landed, false,
10.f, !_vehicle_control_mode_sub.get().flag_control_climb_rate_enabled, time_stamp_now);
// // activate the weathervane controller if required. If activated a flighttask can use it to implement a yaw-rate control strategy
// // that turns the nose of the vehicle into the wind
// if (_wv_controller != nullptr) {
// activate the weathervane controller if required. If activated a flighttask can use it to implement a yaw-rate control strategy
// that turns the nose of the vehicle into the wind
if (_wv_controller != nullptr) {
// // in manual mode we just want to use weathervane if position is controlled as well TODO
// // in mission, enabling wv is done in flight task
// if (_control_mode.flag_control_manual_enabled) {
// if (_control_mode.flag_control_position_enabled && _wv_controller->weathervane_enabled()) {
// _wv_controller->activate();
// in manual mode we just want to use weathervane if position is controlled as well
// in mission, enabling wv is done in flight task
if (_vehicle_control_mode_sub.get().flag_control_manual_enabled) {
if (_vehicle_control_mode_sub.get().flag_control_position_enabled && _wv_controller->weathervane_enabled()) {
_wv_controller->activate();
// } else {
// _wv_controller->deactivate();
// }
// }
} else {
_wv_controller->deactivate();
}
}
// _wv_dcm_z_sp_prev = Quatf(attitude_setpoint.q_d).dcm_z(); // TODO
// _wv_controller->update(_wv_dcm_z_sp_prev, _states.yaw);
// }
vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint);
_wv_controller->update(matrix::Quatf(vehicle_attitude_setpoint.q_d).dcm_z(), vehicle_local_position.heading);
}
start_flight_task();
src/modules/flight_mode_manager/FlightModeManager.hpp
+2
View File
@@ -42,6 +42,7 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
@@ -92,6 +93,7 @@ private:
uORB::SubscriptionData<home_position_s> _home_position_sub{ORB_ID(home_position)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::SubscriptionData<vehicle_control_mode_s> _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::SubscriptionData<vehicle_land_detected_s> _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};