control_allocator: add coaxial helicopter effectiveness

It's now just a copy of the helicopter such that changes get well
visible in the history.
This commit is contained in:
Matthias Grob
2023-09-28 08:55:06 +02:00
committed by Beat Küng
parent b56e7a036c
commit b3b373e074
6 changed files with 413 additions and 0 deletions
@@ -0,0 +1,252 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "ActuatorEffectivenessHelicopterCoaxial.hpp"
#include <lib/mathlib/mathlib.h>
using namespace matrix;
using namespace time_literals;
ActuatorEffectivenessHelicopterCoaxial::ActuatorEffectivenessHelicopterCoaxial(ModuleParams *parent)
: ModuleParams(parent)
{
for (int i = 0; i < NUM_SWASH_PLATE_SERVOS_MAX; ++i) {
char buffer[17];
snprintf(buffer, sizeof(buffer), "CA_SP0_ANG%u", i);
_param_handles.swash_plate_servos[i].angle = param_find(buffer);
snprintf(buffer, sizeof(buffer), "CA_SP0_ARM_L%u", i);
_param_handles.swash_plate_servos[i].arm_length = param_find(buffer);
snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i);
_param_handles.swash_plate_servos[i].trim = param_find(buffer);
}
_param_handles.num_swash_plate_servos = param_find("CA_SP0_COUNT");
for (int i = 0; i < NUM_CURVE_POINTS; ++i) {
char buffer[17];
snprintf(buffer, sizeof(buffer), "CA_HELI_THR_C%u", i);
_param_handles.throttle_curve[i] = param_find(buffer);
snprintf(buffer, sizeof(buffer), "CA_HELI_PITCH_C%u", i);
_param_handles.pitch_curve[i] = param_find(buffer);
}
_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");
updateParams();
}
void ActuatorEffectivenessHelicopterCoaxial::updateParams()
{
ModuleParams::updateParams();
int32_t count = 0;
if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) {
PX4_ERR("param_get failed");
return;
}
_geometry.num_swash_plate_servos = math::constrain((int)count, 3, NUM_SWASH_PLATE_SERVOS_MAX);
for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) {
float angle_deg{};
param_get(_param_handles.swash_plate_servos[i].angle, &angle_deg);
_geometry.swash_plate_servos[i].angle = math::radians(angle_deg);
param_get(_param_handles.swash_plate_servos[i].arm_length, &_geometry.swash_plate_servos[i].arm_length);
param_get(_param_handles.swash_plate_servos[i].trim, &_geometry.swash_plate_servos[i].trim);
}
for (int i = 0; i < NUM_CURVE_POINTS; ++i) {
param_get(_param_handles.throttle_curve[i], &_geometry.throttle_curve[i]);
param_get(_param_handles.pitch_curve[i], &_geometry.pitch_curve[i]);
}
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;
param_get(_param_handles.yaw_ccw, &yaw_ccw);
_geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f;
}
bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuration &configuration,
EffectivenessUpdateReason external_update)
{
if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
return false;
}
// As the allocation is non-linear, we use updateSetpoint() instead of the matrix
configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{});
// Tail (yaw) motor
configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{});
// N swash plate servos
_first_swash_plate_servo_index = configuration.num_actuators_matrix[0];
for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) {
configuration.addActuator(ActuatorType::SERVOS, Vector3f{}, Vector3f{});
configuration.trim[configuration.selected_matrix](i) = _geometry.swash_plate_servos[i].trim;
}
return true;
}
void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
{
_saturation_flags = {};
// throttle/collective pitch curve
const float throttle = math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
_geometry.throttle_curve) * throttleSpoolupProgress();
const float collective_pitch = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.pitch_curve);
// actuator mapping
actuator_sp(0) = mainMotorEnaged() ? throttle : NAN;
actuator_sp(1) = control_sp(ControlAxis::YAW) * _geometry.yaw_sign
+ fabsf(collective_pitch - _geometry.yaw_collective_pitch_offset) * _geometry.yaw_collective_pitch_scale
+ throttle * _geometry.yaw_throttle_scale;
// Saturation check for yaw
if (actuator_sp(1) < actuator_min(1)) {
setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos);
} else if (actuator_sp(1) > actuator_max(1)) {
setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg);
}
for (int i = 0; i < _geometry.num_swash_plate_servos; i++) {
float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length;
float pitch_coeff = cosf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length;
actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch
+ control_sp(ControlAxis::PITCH) * pitch_coeff
- control_sp(ControlAxis::ROLL) * roll_coeff
+ _geometry.swash_plate_servos[i].trim;
// Saturation check for roll & pitch
if (actuator_sp(_first_swash_plate_servo_index + i) < actuator_min(_first_swash_plate_servo_index + i)) {
setSaturationFlag(roll_coeff, _saturation_flags.roll_pos, _saturation_flags.roll_neg);
setSaturationFlag(pitch_coeff, _saturation_flags.pitch_neg, _saturation_flags.pitch_pos);
} else if (actuator_sp(_first_swash_plate_servo_index + i) > actuator_max(_first_swash_plate_servo_index + i)) {
setSaturationFlag(roll_coeff, _saturation_flags.roll_neg, _saturation_flags.roll_pos);
setSaturationFlag(pitch_coeff, _saturation_flags.pitch_pos, _saturation_flags.pitch_neg);
}
}
}
bool ActuatorEffectivenessHelicopterCoaxial::mainMotorEnaged()
{
manual_control_switches_s manual_control_switches;
if (_manual_control_switches_sub.update(&manual_control_switches)) {
_main_motor_engaged = manual_control_switches.engage_main_motor_switch == manual_control_switches_s::SWITCH_POS_NONE
|| manual_control_switches.engage_main_motor_switch == manual_control_switches_s::SWITCH_POS_ON;
}
return _main_motor_engaged;
}
float ActuatorEffectivenessHelicopterCoaxial::throttleSpoolupProgress()
{
vehicle_status_s vehicle_status;
if (_vehicle_status_sub.update(&vehicle_status)) {
_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
_armed_time = vehicle_status.armed_time;
}
const float time_since_arming = (hrt_absolute_time() - _armed_time) / 1e6f;
const float spoolup_progress = time_since_arming / _geometry.spoolup_time;
if (_armed && spoolup_progress < 1.f) {
return spoolup_progress;
}
return 1.f;
}
void ActuatorEffectivenessHelicopterCoaxial::setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag)
{
if (coeff > 0.f) {
// A positive change in given axis will increase saturation
positive_flag = true;
} else if (coeff < 0.f) {
// A negative change in given axis will increase saturation
negative_flag = true;
}
}
void ActuatorEffectivenessHelicopterCoaxial::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
{
// Note: the values '-1', '1' and '0' are just to indicate a negative,
// positive or no saturation to the rate controller. The actual magnitude is not used.
if (_saturation_flags.roll_pos) {
status.unallocated_torque[0] = 1.f;
} else if (_saturation_flags.roll_neg) {
status.unallocated_torque[0] = -1.f;
}
if (_saturation_flags.pitch_pos) {
status.unallocated_torque[1] = 1.f;
} else if (_saturation_flags.pitch_neg) {
status.unallocated_torque[1] = -1.f;
}
if (_saturation_flags.yaw_pos) {
status.unallocated_torque[2] = 1.f;
} else if (_saturation_flags.yaw_neg) {
status.unallocated_torque[2] = -1.f;
}
if (_saturation_flags.thrust_pos) {
status.unallocated_thrust[2] = 1.f;
} else if (_saturation_flags.thrust_neg) {
status.unallocated_thrust[2] = -1.f;
}
}
@@ -0,0 +1,132 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "ActuatorEffectiveness.hpp"
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/manual_control_switches.h>
class ActuatorEffectivenessHelicopterCoaxial : public ModuleParams, public ActuatorEffectiveness
{
public:
static constexpr int NUM_SWASH_PLATE_SERVOS_MAX = 4;
static constexpr int NUM_CURVE_POINTS = 5;
struct SwashPlateGeometry {
float angle;
float arm_length;
float trim;
};
struct Geometry {
SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX];
int num_swash_plate_servos{0};
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;
};
ActuatorEffectivenessHelicopterCoaxial(ModuleParams *parent);
virtual ~ActuatorEffectivenessHelicopterCoaxial() = default;
bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
const char *name() const override { return "Helicopter"; }
const Geometry &geometry() const { return _geometry; }
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
private:
float throttleSpoolupProgress();
bool mainMotorEnaged();
void updateParams() override;
struct SaturationFlags {
bool roll_pos;
bool roll_neg;
bool pitch_pos;
bool pitch_neg;
bool yaw_pos;
bool yaw_neg;
bool thrust_pos;
bool thrust_neg;
};
static void setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag);
struct ParamHandlesSwashPlate {
param_t angle;
param_t arm_length;
param_t trim;
};
struct ParamHandles {
ParamHandlesSwashPlate swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX];
param_t num_swash_plate_servos;
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;
};
ParamHandles _param_handles{};
Geometry _geometry{};
int _first_swash_plate_servo_index{};
SaturationFlags _saturation_flags;
// Throttle spoolup state
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
bool _armed{false};
uint64_t _armed_time{0};
uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
bool _main_motor_engaged{true};
};
@@ -44,6 +44,8 @@ px4_add_library(ActuatorEffectiveness
ActuatorEffectivenessFixedWing.hpp
ActuatorEffectivenessHelicopter.cpp
ActuatorEffectivenessHelicopter.hpp
ActuatorEffectivenessHelicopterCoaxial.cpp
ActuatorEffectivenessHelicopterCoaxial.hpp
ActuatorEffectivenessMCTilt.cpp
ActuatorEffectivenessMCTilt.hpp
ActuatorEffectivenessMultirotor.cpp
@@ -266,6 +266,10 @@ ControlAllocator::update_effectiveness_source()
tmp = new ActuatorEffectivenessHelicopter(this, ActuatorType::SERVOS);
break;
case EffectivenessSource::HELICOPTER_COAXIAL:
tmp = new ActuatorEffectivenessHelicopterCoaxial(this);
break;
default:
PX4_ERR("Unknown airframe");
break;
@@ -53,6 +53,7 @@
#include <ActuatorEffectivenessCustom.hpp>
#include <ActuatorEffectivenessUUV.hpp>
#include <ActuatorEffectivenessHelicopter.hpp>
#include <ActuatorEffectivenessHelicopterCoaxial.hpp>
#include <ControlAllocation.hpp>
#include <ControlAllocationPseudoInverse.hpp>
@@ -157,6 +158,7 @@ private:
CUSTOM = 9,
HELICOPTER_TAIL_ESC = 10,
HELICOPTER_TAIL_SERVO = 11,
HELICOPTER_COAXIAL = 12,
};
enum class FailureMode {
+21
View File
@@ -29,6 +29,7 @@ parameters:
9: Custom
10: Helicopter (tail ESC)
11: Helicopter (tail Servo)
12: Helicopter (Coaxial)
default: 0
CA_METHOD:
@@ -1119,3 +1120,23 @@ mixer:
name: CA_HELI_YAW_CCW
- label: 'Throttle spoolup time'
name: COM_SPOOLUP_TIME
12: # Helicopter (Coaxial)
actuators:
- actuator_type: 'motor'
count: 2
item_label_prefix: ['Clockwise Rotor', 'Counter-clockwise Rotor']
- actuator_type: 'servo'
group_label: 'Swash plate servos'
count: 'CA_SP0_COUNT'
per_item_parameters:
extra:
- name: 'CA_SP0_ANG${i}'
label: 'Angle'
- name: 'CA_SP0_ARM_L${i}'
label: 'Arm Length (relative)'
- name: 'CA_SV_CS${i}_TRIM'
label: 'Trim'
parameters:
- label: 'Throttle spoolup time'
name: COM_SPOOLUP_TIME