fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 19:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Helicopter: add offset for yaw compensation based on collective pitch

Browse Source
This commit is contained in:
Matthias Grob
2023-03-24 07:16:36 +01:00
committed by Beat Küng
parent eeb90ac70a
commit 937d27f8ee
3 changed files with 23 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
+3 -1
View File
@@ -61,6 +61,7 @@ ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *p
}
_param_handles.yaw_collective_pitch_scale = param_find("CA_HELI_YAW_CP_S");
_param_handles.yaw_collective_pitch_offset = param_find("CA_HELI_YAW_CP_O");
_param_handles.yaw_throttle_scale = param_find("CA_HELI_YAW_TH_S");
_param_handles.yaw_ccw = param_find("CA_HELI_YAW_CCW");
_param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME");
@@ -95,6 +96,7 @@ void ActuatorEffectivenessHelicopter::updateParams()
}
param_get(_param_handles.yaw_collective_pitch_scale, &_geometry.yaw_collective_pitch_scale);
param_get(_param_handles.yaw_collective_pitch_offset, &_geometry.yaw_collective_pitch_offset);
param_get(_param_handles.yaw_throttle_scale, &_geometry.yaw_throttle_scale);
param_get(_param_handles.spoolup_time, &_geometry.spoolup_time);
int32_t yaw_ccw = 0;
@@ -142,7 +144,7 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
actuator_sp(0) = mainMotorEnaged() ? throttle : NAN;
actuator_sp(1) = control_sp(ControlAxis::YAW) * _geometry.yaw_sign
+ fabsf(collective_pitch) * _geometry.yaw_collective_pitch_scale
+ fabsf(collective_pitch - _geometry.yaw_collective_pitch_offset) * _geometry.yaw_collective_pitch_scale
+ throttle * _geometry.yaw_throttle_scale;
// Saturation check for yaw
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
+2
View File
@@ -60,6 +60,7 @@ public:
float throttle_curve[NUM_CURVE_POINTS];
float pitch_curve[NUM_CURVE_POINTS];
float yaw_collective_pitch_scale;
float yaw_collective_pitch_offset;
float yaw_throttle_scale;
float yaw_sign;
float spoolup_time;
@@ -109,6 +110,7 @@ private:
param_t throttle_curve[NUM_CURVE_POINTS];
param_t pitch_curve[NUM_CURVE_POINTS];
param_t yaw_collective_pitch_scale;
param_t yaw_collective_pitch_offset;
param_t yaw_throttle_scale;
param_t yaw_ccw;
param_t spoolup_time;