dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Weathervane: pass quaterionon as constant reference 

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/WeatherVane/WeatherVane.cpp
View File

@ -47,7 +47,7 @@ WeatherVane::WeatherVane() :
_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);
_yaw = yaw;

2
src/lib/WeatherVane/WeatherVane.hpp
View File

@ -62,7 +62,7 @@ public:
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();

24
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -379,7 +379,9 @@ MulticopterPositionControl::parameters_update(bool force)
// set trigger time for arm hysteresis
_arm_hysteresis.set_hysteresis_time_from(false, (int)(MPC_IDLE_TKO.get() * 1000000.0f));
_wv_controller->update_parameters();
if (_wv_controller != nullptr) {
_wv_controller->update_parameters();
}
}
return OK;
@ -603,15 +605,19 @@ MulticopterPositionControl::task_main()
// 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 (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled
&& _wv_controller->manual_enabled()) {
_wv_controller->activate();
if (_wv_controller != nullptr) {
if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled
&& _wv_controller->manual_enabled()) {
_wv_controller->activate();
} else if (_control_mode.flag_control_auto_enabled && _wv_controller->auto_enabled()) {
_wv_controller->activate();
} else if (_control_mode.flag_control_auto_enabled && _wv_controller->auto_enabled()) {
_wv_controller->activate();
} else {
_wv_controller->deactivate();
} else {
_wv_controller->deactivate();
}
_wv_controller->update(matrix::Quatf(_att_sp.q_d), _local_pos.yaw);
}
if (_control_mode.flag_armed) {
@ -750,8 +756,6 @@ MulticopterPositionControl::task_main()
_att_sp.disable_mc_yaw_control = 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 == vehicle_constraints_s::GEAR_UP) {
_att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP;