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

Weathervane: pass quaterionon as constant reference

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2018-09-18 14:02:39 +02:00
committed by Lorenz Meier
parent ccaeb58708
commit 0e835cb498
3 changed files with 16 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/WeatherVane/WeatherVane.cpp
+1 -1
View File
@@ -47,7 +47,7 @@ WeatherVane::WeatherVane() :
_R_sp_prev = matrix::Dcmf(); _R_sp_prev = matrix::Dcmf();
} }
void WeatherVane::update(matrix::Quatf q_sp_prev, float yaw) void WeatherVane::update(const matrix::Quatf &q_sp_prev, float yaw)
{ {
_R_sp_prev = matrix::Dcmf(q_sp_prev); _R_sp_prev = matrix::Dcmf(q_sp_prev);
_yaw = yaw; _yaw = yaw;
src/lib/WeatherVane/WeatherVane.hpp
+1 -1
View File
@@ -62,7 +62,7 @@ public:
bool auto_enabled() { return _wv_auto_enabled.get(); } bool auto_enabled() { return _wv_auto_enabled.get(); }
void update(matrix::Quatf q_sp_prev, float yaw); void update(const matrix::Quatf &q_sp_prev, float yaw);
float get_weathervane_yawrate(); float get_weathervane_yawrate();
src/modules/mc_pos_control/mc_pos_control_main.cpp
+6 -2
View File
@@ -379,8 +379,10 @@ MulticopterPositionControl::parameters_update(bool force)
// set trigger time for arm hysteresis // set trigger time for arm hysteresis
_arm_hysteresis.set_hysteresis_time_from(false, (int)(MPC_IDLE_TKO.get() * 1000000.0f)); _arm_hysteresis.set_hysteresis_time_from(false, (int)(MPC_IDLE_TKO.get() * 1000000.0f));
if (_wv_controller != nullptr) {
_wv_controller->update_parameters(); _wv_controller->update_parameters();
} }
}
return OK; return OK;
} }
@@ -603,6 +605,7 @@ MulticopterPositionControl::task_main()
// activate the weathervane controller if required. If activated a flighttask can use it to implement a yaw-rate control strategy // 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 // that turns the nose of the vehicle into the wind
if (_wv_controller != nullptr) {
if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled
&& _wv_controller->manual_enabled()) { && _wv_controller->manual_enabled()) {
_wv_controller->activate(); _wv_controller->activate();
@@ -614,6 +617,9 @@ MulticopterPositionControl::task_main()
_wv_controller->deactivate(); _wv_controller->deactivate();
} }
_wv_controller->update(matrix::Quatf(_att_sp.q_d), _local_pos.yaw);
}
if (_control_mode.flag_armed) { if (_control_mode.flag_armed) {
// as soon vehicle is armed check for flighttask // as soon vehicle is armed check for flighttask
start_flight_task(); start_flight_task();
@@ -750,8 +756,6 @@ MulticopterPositionControl::task_main()
_att_sp.disable_mc_yaw_control = false; _att_sp.disable_mc_yaw_control = false;
_att_sp.apply_flaps = false; _att_sp.apply_flaps = false;
_wv_controller->update(matrix::Quatf(_att_sp.q_d), _local_pos.yaw);
if (!constraints.landing_gear) { if (!constraints.landing_gear) {
if (constraints.landing_gear == vehicle_constraints_s::GEAR_UP) { if (constraints.landing_gear == vehicle_constraints_s::GEAR_UP) {
_att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP; _att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP;