fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Apply suggestions from code review

Browse Source 
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
This commit is contained in:
Ted 2024-11-19 09:57:35 +00:00 committed by Matthias Grob
parent f13718d482
commit 49fbcb2153
3 changed files with 13 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
View File

@ -106,11 +106,11 @@ void ActuatorEffectivenessHelicopter::updateParams()
int32_t linearize_servos = 0;
param_get(_param_handles.linearize_servos, &linearize_servos);
_geometry.linearize_servos = (linearize_servos != 0);
float max_sevo_throw = 0.f;
param_get(_param_handles.max_sevo_throw, &max_sevo_throw);
max_sevo_throw *= M_PI_F / 180.0f; //converting deg to rad
_geometry.max_sevo_height = sinf(max_sevo_throw);
_geometry.inverse_max_servo_throw = 1/max_sevo_throw;
float max_servo_throw_deg = 0.f;
param_get(_param_handles.max_servo_throw, &max_servo_throw_deg);
const float max_servo_throw = math::radians(max_servo_throw)
_geometry.max_servo_height = sinf(max_servo_throw);
_geometry.inverse_max_servo_throw = 1.f / max_servo_throw;
}
bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration,
@ -197,13 +197,12 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
float ActuatorEffectivenessHelicopter::getLinearServoOutput(float input) const
{
input = math::constrain(input, -1.0f, 1.0f);
//servo output is calculated by normalizing input to arm rotation of CA_MAX_SVO_THROW degrees as full input for a linear throw
float svo_height = _geometry.max_sevo_height * input;
if (std::isnan(svo_height)) {
if (!PX4_ISFINITE(svo_height)) {
svo_height = 0.0f;
}

4
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
View File

@ -67,7 +67,7 @@ public:
float yaw_sign;
float spoolup_time;
int linearize_servos;
float max_sevo_height;
float max_servo_height;
float inverse_max_servo_throw;
};
@ -121,7 +121,7 @@ private:
param_t yaw_ccw;
param_t spoolup_time;
param_t linearize_servos;
param_t max_sevo_throw;
param_t max_servo_throw;
param_t inverse_max_servo_throw;
};
ParamHandles _param_handles{};

10
src/modules/control_allocator/module.yaml
View File

@ -575,7 +575,7 @@ parameters:
description:
short: Defines max/min throw of servo.
long: |
Defines max/min throw of servo when linearizing servo output. Only used when CA_LIN_SERVO is enabled.
Defines min/max throw of servo when linearizing servo output. Only used when CA_LIN_SERVO is enabled.
type: float
decimal: 3
unit: deg
@ -1147,9 +1147,9 @@ mixer:
name: CA_HELI_YAW_CCW
- label: 'Throttle spoolup time'
name: COM_SPOOLUP_TIME
- label: 'linearize servos'
- label: 'Linearize servos'
name: CA_LIN_SERVO
- label: 'max/min throw of servo in degrees'
- label: 'Min/max servo throw in degrees'
name: CA_MAX_SVO_THROW
11: # Helicopter (tail Servo)
@ -1180,9 +1180,9 @@ mixer:
name: CA_HELI_YAW_CCW
- label: 'Throttle spoolup time'
name: COM_SPOOLUP_TIME
- label: 'linearize servos'
- label: 'Linearize servos'
name: CA_LIN_SERVO
- label: 'max/min throw of servo in degrees'
- label: 'Min/max servo throw in degrees'
name: CA_MAX_SVO_THROW