mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-01 15:04:06 +08:00
Merge remote-tracking branch 'origin' into pr-metric-allocation
This commit is contained in:
Pedro-Roque
commit
ef41cd8aee
9
.github/workflows/dev_container.yml
vendored
9
.github/workflows/dev_container.yml
vendored
@ -17,7 +17,7 @@ on:
|
||||
jobs:
|
||||
build:
|
||||
name: Build and Push Container
|
||||
runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false]
|
||||
runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
|
||||
steps:
|
||||
- uses: actions/checkout@v4
|
||||
with:
|
||||
@ -52,12 +52,7 @@ jobs:
|
||||
ghcr.io/PX4/px4-dev
|
||||
${{ (github.event_name != 'pull_request') && 'px4io/px4-dev' || '' }}
|
||||
tags: |
|
||||
type=semver,pattern={{version}}
|
||||
type=semver,pattern={{major}}.{{minor}}
|
||||
type=semver,pattern={{major}}
|
||||
type=ref,event=branch,value=${{ steps.px4-tag.outputs.tag }},priority=700
|
||||
type=ref,event=branch,suffix=-{{date 'YYYY-MM-DD'}},priority=600
|
||||
type=ref,event=branch,suffix=,priority=500
|
||||
type=raw,enable=true,value=${{ steps.px4-tag.outputs.tag }},priority=1000
|
||||
|
||||
- name: Set up Docker Buildx
|
||||
uses: docker/setup-buildx-action@v3
|
||||
|
||||
@ -12,6 +12,7 @@ param set-default MAV_TYPE 4
|
||||
|
||||
param set-default COM_PREARM_MODE 2
|
||||
param set-default COM_SPOOLUP_TIME 10
|
||||
param set-default COM_DISARM_PRFLT 60
|
||||
|
||||
# No need for minimum collective pitch (or airmode) to keep torque authority
|
||||
param set-default MPC_MANTHR_MIN 0
|
||||
|
||||
@ -3,6 +3,8 @@
|
||||
# board specific defaults
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
param set-default EKF2_MULTI_IMU 0
|
||||
|
||||
# Mavlink ethernet (CFG 1000)
|
||||
param set-default MAV_2_CONFIG 1000
|
||||
param set-default MAV_2_BROADCAST 1
|
||||
@ -17,6 +19,7 @@ param set-default SENS_EN_INA228 0
|
||||
param set-default SENS_EN_INA226 1
|
||||
|
||||
param set-default SENS_EN_THERMAL 1
|
||||
param set-default SENS_IMU_MODE 1
|
||||
param set-default SENS_IMU_TEMP 10.0
|
||||
#param set-default SENS_IMU_TEMP_FF 0.0
|
||||
#param set-default SENS_IMU_TEMP_I 0.025
|
||||
|
||||
@ -32,10 +32,11 @@ then
|
||||
param set-default SENS_TEMP_ID 2490378
|
||||
fi
|
||||
|
||||
param set-default EKF2_MULTI_IMU 2
|
||||
param set-default EKF2_MULTI_IMU 0
|
||||
param set-default EKF2_OF_CTRL 1
|
||||
param set-default EKF2_OF_N_MIN 0.05
|
||||
param set-default EKF2_RNG_A_HMAX 25
|
||||
param set-default EKF2_RNG_QLTY_T 0.1
|
||||
|
||||
param set-default SENS_FLOW_RATE 150
|
||||
param set-default SENS_IMU_MODE 1
|
||||
|
||||
@ -23,6 +23,7 @@ CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
|
||||
# CONFIG_EKF2_AUXVEL is not set
|
||||
# CONFIG_EKF2_BARO_COMPENSATION is not set
|
||||
# CONFIG_EKF2_DRAG_FUSION is not set
|
||||
|
||||
@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
|
||||
# CONFIG_EKF2_AUXVEL is not set
|
||||
# CONFIG_EKF2_BARO_COMPENSATION is not set
|
||||
# CONFIG_EKF2_DRAG_FUSION is not set
|
||||
|
||||
@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
|
||||
# CONFIG_EKF2_AUXVEL is not set
|
||||
# CONFIG_EKF2_BARO_COMPENSATION is not set
|
||||
# CONFIG_EKF2_DRAG_FUSION is not set
|
||||
|
||||
@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
|
||||
# CONFIG_EKF2_AUXVEL is not set
|
||||
# CONFIG_EKF2_BARO_COMPENSATION is not set
|
||||
# CONFIG_EKF2_DRAG_FUSION is not set
|
||||
|
||||
@ -10,11 +10,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
|
||||
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
||||
CONFIG_DRIVERS_BAROMETER_DPS310=y
|
||||
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
|
||||
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y
|
||||
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y
|
||||
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y
|
||||
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y
|
||||
CONFIG_DRIVERS_DSHOT=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI270=y
|
||||
CONFIG_COMMON_INS=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_DRIVERS_PPS_CAPTURE=y
|
||||
CONFIG_DRIVERS_PWM_OUT=y
|
||||
|
||||
@ -11,11 +11,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
|
||||
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
||||
CONFIG_DRIVERS_BAROMETER_GOERTEK_SPL06=y
|
||||
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
|
||||
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y
|
||||
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y
|
||||
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y
|
||||
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y
|
||||
CONFIG_DRIVERS_DSHOT=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI270=y
|
||||
CONFIG_COMMON_INS=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_DRIVERS_PPS_CAPTURE=y
|
||||
CONFIG_DRIVERS_PWM_OUT=y
|
||||
|
||||
@ -10,11 +10,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
|
||||
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
||||
CONFIG_DRIVERS_BAROMETER_DPS310=y
|
||||
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
|
||||
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y
|
||||
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y
|
||||
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y
|
||||
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y
|
||||
CONFIG_DRIVERS_DSHOT=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI270=y
|
||||
CONFIG_COMMON_INS=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_DRIVERS_PPS_CAPTURE=y
|
||||
CONFIG_DRIVERS_PWM_OUT=y
|
||||
|
||||
@ -104,3 +104,4 @@ CONFIG_SYSTEMCMDS_USB_CONNECTED=y
|
||||
CONFIG_SYSTEMCMDS_VER=y
|
||||
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
|
||||
CONFIG_EXAMPLES_FAKE_GPS=y
|
||||
# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
|
||||
|
||||
@ -25,6 +25,7 @@ CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
|
||||
# CONFIG_EKF2_AUXVEL is not set
|
||||
# CONFIG_EKF2_BARO_COMPENSATION is not set
|
||||
# CONFIG_EKF2_DRAG_FUSION is not set
|
||||
|
||||
@ -4,5 +4,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
|
||||
CONFIG_BOARD_CRYPTO=y
|
||||
CONFIG_DRIVERS_STUB_KEYSTORE=y
|
||||
CONFIG_DRIVERS_SW_CRYPTO=y
|
||||
# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
|
||||
CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub"
|
||||
CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub"
|
||||
|
||||
@ -16,7 +16,6 @@ CONFIG_MODULES_UUV_ATT_CONTROL=n
|
||||
CONFIG_MODULES_UUV_POS_CONTROL=n
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_DRIVERS_ROBOCLAW=y
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_ROVER_ACKERMANN=y
|
||||
CONFIG_MODULES_ROVER_DIFFERENTIAL=y
|
||||
|
||||
@ -45,4 +45,3 @@ CONFIG_SYSTEMCMDS_REFLECT=n
|
||||
CONFIG_BOARD_CONSTRAINED_FLASH=y
|
||||
CONFIG_BOARD_TESTING=y
|
||||
CONFIG_DRIVERS_BAROMETER_MS5611=y
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
|
||||
@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=n
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_DRIVERS_ROBOCLAW=y
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_ROVER_ACKERMANN=y
|
||||
CONFIG_MODULES_ROVER_DIFFERENTIAL=y
|
||||
|
||||
@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=n
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_DRIVERS_ROBOCLAW=y
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_ROVER_ACKERMANN=y
|
||||
CONFIG_MODULES_ROVER_DIFFERENTIAL=y
|
||||
|
||||
@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=n
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_DRIVERS_ROBOCLAW=y
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_ROVER_ACKERMANN=y
|
||||
CONFIG_MODULES_ROVER_DIFFERENTIAL=y
|
||||
|
||||
@ -7,7 +7,6 @@ CONFIG_MODULES_FW_POS_CONTROL=n
|
||||
CONFIG_MODULES_FW_RATE_CONTROL=n
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_COMMON_RC=y
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_SIDESLIP is not set
|
||||
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
|
||||
|
||||
@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=n
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_DRIVERS_ROBOCLAW=y
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_ROVER_ACKERMANN=y
|
||||
CONFIG_MODULES_ROVER_DIFFERENTIAL=y
|
||||
|
||||
@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=n
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_DRIVERS_ROBOCLAW=y
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_ROVER_ACKERMANN=y
|
||||
CONFIG_MODULES_ROVER_DIFFERENTIAL=y
|
||||
|
||||
@ -15,7 +15,6 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
CONFIG_EKF2_VERBOSE_STATUS=y
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
CONFIG_MODULES_EVENTS=y
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||
CONFIG_MODULES_FW_ATT_CONTROL=y
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
uint64 timestamp # Time since system start (microseconds)
|
||||
uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5)
|
||||
uint32 pulse_width # Pulse width, timer counts
|
||||
uint32 period # Period, timer counts
|
||||
uint64 timestamp # Time since system start (microseconds)
|
||||
uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5)
|
||||
uint32 pulse_width # Pulse width, timer counts (microseconds)
|
||||
uint32 period # Period, timer counts (microseconds)
|
||||
|
||||
@ -1,4 +1,5 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
# rpm values of 0.0 mean within a timeout there is no movement measured
|
||||
float32 rpm_estimate # filtered revolutions per minute
|
||||
float32 rpm_raw
|
||||
|
||||
@ -1 +1 @@
|
||||
Subproject commit 4c875d0fdffb4f9ba0e0e341ae567d4d0a544e46
|
||||
Subproject commit 9a213327fbb2dab252185d468fb29348e1833d8c
|
||||
@ -41,7 +41,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
@ -34,10 +34,13 @@
|
||||
px4_add_library(ActuatorEffectiveness
|
||||
ActuatorEffectiveness.cpp
|
||||
ActuatorEffectiveness.hpp
|
||||
<<<<<<<< HEAD:src/lib/actuator_effectiveness/CMakeLists.txt
|
||||
ActuatorEffectivenessRotors.cpp
|
||||
ActuatorEffectivenessRotors.hpp
|
||||
ActuatorEffectivenessThrusters.cpp
|
||||
ActuatorEffectivenessThrusters.hpp
|
||||
========
|
||||
>>>>>>>> origin:src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt
|
||||
)
|
||||
|
||||
target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
@ -45,7 +48,11 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D
|
||||
target_link_libraries(ActuatorEffectiveness
|
||||
PRIVATE
|
||||
mathlib
|
||||
PID
|
||||
)
|
||||
<<<<<<<< HEAD:src/lib/actuator_effectiveness/CMakeLists.txt
|
||||
|
||||
px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness)
|
||||
px4_add_functional_gtest(SRC ActuatorEffectivenessThrustersTest.cpp LINKLIBS ActuatorEffectiveness)
|
||||
========
|
||||
>>>>>>>> origin:src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt
|
||||
|
||||
@ -1,6 +1,10 @@
|
||||
############################################################################
|
||||
#
|
||||
<<<<<<< HEAD
|
||||
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
=======
|
||||
# Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
>>>>>>> origin
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
@ -48,3 +52,5 @@ target_link_libraries(ControlAllocation PRIVATE mathlib)
|
||||
px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation)
|
||||
px4_add_unit_gtest(SRC ControlAllocationMetric.cpp LINKLIBS ControlAllocation)
|
||||
# px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness)
|
||||
add_subdirectory(control_allocation)
|
||||
add_subdirectory(actuator_effectiveness)
|
||||
|
||||
@ -0,0 +1,103 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 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 "ActuatorEffectiveness.hpp"
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
int ActuatorEffectiveness::Configuration::addActuator(ActuatorType type, const matrix::Vector3f &torque,
|
||||
const matrix::Vector3f &thrust)
|
||||
{
|
||||
int actuator_idx = num_actuators_matrix[selected_matrix];
|
||||
|
||||
if (actuator_idx >= NUM_ACTUATORS) {
|
||||
PX4_ERR("Too many actuators");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if ((int)type < (int)ActuatorType::COUNT - 1 && num_actuators[(int)type + 1] > 0) {
|
||||
PX4_ERR("Trying to add actuators in the wrong order (add motors first, then servos)");
|
||||
return -1;
|
||||
}
|
||||
|
||||
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::ROLL, actuator_idx) = torque(0);
|
||||
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::PITCH, actuator_idx) = torque(1);
|
||||
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::YAW, actuator_idx) = torque(2);
|
||||
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_X, actuator_idx) = thrust(0);
|
||||
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Y, actuator_idx) = thrust(1);
|
||||
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Z, actuator_idx) = thrust(2);
|
||||
matrix_selection_indexes[totalNumActuators()] = selected_matrix;
|
||||
++num_actuators[(int)type];
|
||||
return num_actuators_matrix[selected_matrix]++;
|
||||
}
|
||||
|
||||
void ActuatorEffectiveness::Configuration::actuatorsAdded(ActuatorType type, int count)
|
||||
{
|
||||
int total_count = totalNumActuators();
|
||||
|
||||
for (int i = 0; i < count; ++i) {
|
||||
matrix_selection_indexes[i + total_count] = selected_matrix;
|
||||
}
|
||||
|
||||
num_actuators[(int)type] += count;
|
||||
num_actuators_matrix[selected_matrix] += count;
|
||||
}
|
||||
|
||||
int ActuatorEffectiveness::Configuration::totalNumActuators() const
|
||||
{
|
||||
int total_count = 0;
|
||||
|
||||
for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
|
||||
total_count += num_actuators_matrix[i];
|
||||
}
|
||||
|
||||
return total_count;
|
||||
}
|
||||
|
||||
void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp)
|
||||
{
|
||||
for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) {
|
||||
const uint32_t motor_mask = (1u << actuator_idx);
|
||||
|
||||
if (stoppable_motors_mask & motor_mask) {
|
||||
|
||||
// Stop motor if its setpoint is below 2%. This value was determined empirically (RC stick inaccuracy)
|
||||
if (fabsf(actuator_sp(actuator_idx)) < .02f) {
|
||||
_stopped_motors_mask |= motor_mask;
|
||||
|
||||
} else {
|
||||
_stopped_motors_mask &= ~motor_mask;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,226 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ActuatorEffectiveness.hpp
|
||||
*
|
||||
* Interface for Actuator Effectiveness
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <uORB/topics/control_allocator_status.h>
|
||||
|
||||
enum class AllocationMethod {
|
||||
NONE = -1,
|
||||
PSEUDO_INVERSE = 0,
|
||||
SEQUENTIAL_DESATURATION = 1,
|
||||
AUTO = 2,
|
||||
METRIC = 3,
|
||||
};
|
||||
|
||||
enum class ActuatorType {
|
||||
MOTORS = 0,
|
||||
SERVOS,
|
||||
THRUSTERS,
|
||||
COUNT
|
||||
};
|
||||
|
||||
enum class EffectivenessUpdateReason {
|
||||
NO_EXTERNAL_UPDATE = 0,
|
||||
CONFIGURATION_UPDATE = 1,
|
||||
MOTOR_ACTIVATION_UPDATE = 2,
|
||||
};
|
||||
|
||||
|
||||
class ActuatorEffectiveness
|
||||
{
|
||||
public:
|
||||
ActuatorEffectiveness() = default;
|
||||
virtual ~ActuatorEffectiveness() = default;
|
||||
|
||||
static constexpr int NUM_ACTUATORS = 16;
|
||||
static constexpr int NUM_AXES = 6;
|
||||
|
||||
enum ControlAxis {
|
||||
ROLL = 0,
|
||||
PITCH,
|
||||
YAW,
|
||||
THRUST_X,
|
||||
THRUST_Y,
|
||||
THRUST_Z
|
||||
};
|
||||
|
||||
static constexpr int MAX_NUM_MATRICES = 2;
|
||||
|
||||
using EffectivenessMatrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>;
|
||||
using ActuatorVector = matrix::Vector<float, NUM_ACTUATORS>;
|
||||
|
||||
enum class FlightPhase {
|
||||
HOVER_FLIGHT = 0,
|
||||
FORWARD_FLIGHT = 1,
|
||||
TRANSITION_HF_TO_FF = 2,
|
||||
TRANSITION_FF_TO_HF = 3
|
||||
};
|
||||
|
||||
struct Configuration {
|
||||
/**
|
||||
* Add an actuator to the selected matrix, returning the index, or -1 on error
|
||||
*/
|
||||
int addActuator(ActuatorType type, const matrix::Vector3f &torque, const matrix::Vector3f &thrust);
|
||||
|
||||
/**
|
||||
* Call this after manually adding N actuators to the selected matrix
|
||||
*/
|
||||
void actuatorsAdded(ActuatorType type, int count);
|
||||
|
||||
int totalNumActuators() const;
|
||||
|
||||
/// Configured effectiveness matrix. Actuators are expected to be filled in order, motors first, then servos
|
||||
EffectivenessMatrix effectiveness_matrices[MAX_NUM_MATRICES];
|
||||
|
||||
int num_actuators_matrix[MAX_NUM_MATRICES]; ///< current amount, and next actuator index to fill in to effectiveness_matrices
|
||||
ActuatorVector trim[MAX_NUM_MATRICES];
|
||||
|
||||
ActuatorVector linearization_point[MAX_NUM_MATRICES];
|
||||
|
||||
int selected_matrix;
|
||||
|
||||
uint8_t matrix_selection_indexes[NUM_ACTUATORS * MAX_NUM_MATRICES];
|
||||
int num_actuators[(int)ActuatorType::COUNT];
|
||||
};
|
||||
|
||||
/**
|
||||
* Set the current flight phase
|
||||
*
|
||||
* @param Flight phase
|
||||
*/
|
||||
virtual void setFlightPhase(const FlightPhase &flight_phase)
|
||||
{
|
||||
_flight_phase = flight_phase;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the number of effectiveness matrices. Must be <= MAX_NUM_MATRICES.
|
||||
* This is expected to stay constant.
|
||||
*/
|
||||
virtual int numMatrices() const { return 1; }
|
||||
|
||||
/**
|
||||
* Get the desired allocation method(s) for each matrix, if configured as AUTO
|
||||
*/
|
||||
virtual void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const
|
||||
{
|
||||
for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
|
||||
allocation_method_out[i] = AllocationMethod::PSEUDO_INVERSE;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Query if the roll, pitch and yaw columns of the mixing matrix should be normalized
|
||||
*/
|
||||
virtual void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const
|
||||
{
|
||||
for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
|
||||
normalize[i] = false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the control effectiveness matrix if updated
|
||||
*
|
||||
* @return true if updated and matrix is set
|
||||
*/
|
||||
virtual bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { return false;}
|
||||
|
||||
/**
|
||||
* Get the current flight phase
|
||||
*
|
||||
* @return Flight phase
|
||||
*/
|
||||
const FlightPhase &getFlightPhase() const
|
||||
{
|
||||
return _flight_phase;
|
||||
}
|
||||
|
||||
/**
|
||||
* Display name
|
||||
*/
|
||||
virtual const char *name() const = 0;
|
||||
|
||||
/**
|
||||
* Callback from the control allocation, allowing to manipulate the setpoint.
|
||||
* Used to allocate auxiliary controls to actuators (e.g. flaps and spoilers).
|
||||
*
|
||||
* @param actuator_sp input & output setpoint
|
||||
*/
|
||||
virtual void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) {}
|
||||
|
||||
/**
|
||||
* Callback from the control allocation, allowing to manipulate the setpoint.
|
||||
* This can be used to e.g. add non-linear or external terms.
|
||||
* It is called after the matrix multiplication and before final clipping.
|
||||
* @param actuator_sp input & output setpoint
|
||||
*/
|
||||
virtual 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) {}
|
||||
|
||||
/**
|
||||
* Get a bitmask of motors to be stopped
|
||||
*/
|
||||
virtual uint32_t getStoppedMotors() const { return _stopped_motors_mask; }
|
||||
|
||||
/**
|
||||
* Fill in the unallocated torque and thrust, customized by effectiveness type.
|
||||
* Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead.
|
||||
*/
|
||||
virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {}
|
||||
|
||||
/**
|
||||
* Stops motors which are masked by stoppable_motors_mask and whose demanded thrust is zero
|
||||
*
|
||||
* @param stoppable_motors_mask mask of motors that should be stopped if there's no thrust demand
|
||||
* @param actuator_sp outcome of the allocation to determine if the motor should be stopped
|
||||
*/
|
||||
virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp);
|
||||
|
||||
protected:
|
||||
FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
|
||||
uint32_t _stopped_motors_mask{0};
|
||||
};
|
||||
@ -0,0 +1,58 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(ActuatorEffectiveness
|
||||
ActuatorEffectiveness.cpp
|
||||
ActuatorEffectiveness.hpp
|
||||
<<<<<<<< HEAD:src/lib/actuator_effectiveness/CMakeLists.txt
|
||||
ActuatorEffectivenessRotors.cpp
|
||||
ActuatorEffectivenessRotors.hpp
|
||||
ActuatorEffectivenessThrusters.cpp
|
||||
ActuatorEffectivenessThrusters.hpp
|
||||
========
|
||||
>>>>>>>> origin:src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt
|
||||
)
|
||||
|
||||
target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(ActuatorEffectiveness
|
||||
PRIVATE
|
||||
mathlib
|
||||
PID
|
||||
)
|
||||
<<<<<<<< HEAD:src/lib/actuator_effectiveness/CMakeLists.txt
|
||||
|
||||
px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness)
|
||||
px4_add_functional_gtest(SRC ActuatorEffectivenessThrustersTest.cpp LINKLIBS ActuatorEffectiveness)
|
||||
========
|
||||
>>>>>>>> origin:src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt
|
||||
53
src/lib/control_allocation/control_allocation/CMakeLists.txt
Normal file
53
src/lib/control_allocation/control_allocation/CMakeLists.txt
Normal file
@ -0,0 +1,53 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(ControlAllocation
|
||||
ControlAllocation.cpp
|
||||
ControlAllocation.hpp
|
||||
ControlAllocationMetric.cpp
|
||||
ControlAllocationMetric.hpp
|
||||
ControlAllocationPseudoInverse.cpp
|
||||
ControlAllocationPseudoInverse.hpp
|
||||
ControlAllocationSequentialDesaturation.cpp
|
||||
ControlAllocationSequentialDesaturation.hpp
|
||||
)
|
||||
target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(ControlAllocation PRIVATE mathlib)
|
||||
|
||||
px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation)
|
||||
<<<<<<<< HEAD:src/lib/control_allocation/CMakeLists.txt
|
||||
px4_add_unit_gtest(SRC ControlAllocationMetric.cpp LINKLIBS ControlAllocation)
|
||||
========
|
||||
>>>>>>>> origin:src/lib/control_allocation/control_allocation/CMakeLists.txt
|
||||
# px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness)
|
||||
@ -0,0 +1,126 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ControlAllocation.cpp
|
||||
*
|
||||
* Interface for Control Allocation Algorithms
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#include "ControlAllocation.hpp"
|
||||
|
||||
ControlAllocation::ControlAllocation()
|
||||
{
|
||||
_control_allocation_scale.setAll(1.f);
|
||||
_actuator_min.setAll(0.f);
|
||||
_actuator_max.setAll(1.f);
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocation::setEffectivenessMatrix(
|
||||
const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,
|
||||
const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
|
||||
bool update_normalization_scale)
|
||||
{
|
||||
_effectiveness = effectiveness;
|
||||
ActuatorVector linearization_point_clipped = linearization_point;
|
||||
clipActuatorSetpoint(linearization_point_clipped);
|
||||
_actuator_trim = actuator_trim + linearization_point_clipped;
|
||||
clipActuatorSetpoint(_actuator_trim);
|
||||
_num_actuators = num_actuators;
|
||||
_control_trim = _effectiveness * linearization_point_clipped;
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocation::setActuatorSetpoint(
|
||||
const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator_sp)
|
||||
{
|
||||
// Set actuator setpoint
|
||||
_actuator_sp = actuator_sp;
|
||||
|
||||
// Clip
|
||||
clipActuatorSetpoint(_actuator_sp);
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocation::clipActuatorSetpoint(matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator) const
|
||||
{
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
if (_actuator_max(i) < _actuator_min(i)) {
|
||||
actuator(i) = _actuator_trim(i);
|
||||
|
||||
} else if (actuator(i) < _actuator_min(i)) {
|
||||
actuator(i) = _actuator_min(i);
|
||||
|
||||
} else if (actuator(i) > _actuator_max(i)) {
|
||||
actuator(i) = _actuator_max(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
matrix::Vector<float, ControlAllocation::NUM_ACTUATORS>
|
||||
ControlAllocation::normalizeActuatorSetpoint(const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator)
|
||||
const
|
||||
{
|
||||
matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> actuator_normalized;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
if (_actuator_min(i) < _actuator_max(i)) {
|
||||
actuator_normalized(i) = (actuator(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i));
|
||||
|
||||
} else {
|
||||
actuator_normalized(i) = (_actuator_trim(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i));
|
||||
}
|
||||
}
|
||||
|
||||
return actuator_normalized;
|
||||
}
|
||||
|
||||
void ControlAllocation::applySlewRateLimit(float dt)
|
||||
{
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
if (_actuator_slew_rate_limit(i) > FLT_EPSILON) {
|
||||
float delta_sp_max = dt * (_actuator_max(i) - _actuator_min(i)) / _actuator_slew_rate_limit(i);
|
||||
float delta_sp = _actuator_sp(i) - _prev_actuator_sp(i);
|
||||
|
||||
if (delta_sp > delta_sp_max) {
|
||||
_actuator_sp(i) = _prev_actuator_sp(i) + delta_sp_max;
|
||||
|
||||
} else if (delta_sp < -delta_sp_max) {
|
||||
_actuator_sp(i) = _prev_actuator_sp(i) - delta_sp_max;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,251 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ControlAllocation.hpp
|
||||
*
|
||||
* Interface for Control Allocation Algorithms
|
||||
*
|
||||
* Implementers of this interface are expected to update the members
|
||||
* of this base class in the `allocate` method.
|
||||
*
|
||||
* Example usage:
|
||||
* ```
|
||||
* [...]
|
||||
* // Initialization
|
||||
* ControlAllocationMethodImpl alloc();
|
||||
* alloc.setEffectivenessMatrix(effectiveness, actuator_trim);
|
||||
* alloc.setActuatorMin(actuator_min);
|
||||
* alloc.setActuatorMin(actuator_max);
|
||||
*
|
||||
* while (1) {
|
||||
* [...]
|
||||
*
|
||||
* // Set control setpoint, allocate actuator setpoint, retrieve actuator setpoint
|
||||
* alloc.setControlSetpoint(control_sp);
|
||||
* alloc.allocate();
|
||||
* actuator_sp = alloc.getActuatorSetpoint();
|
||||
*
|
||||
* // Check if the control setpoint was fully allocated
|
||||
* unallocated_control = control_sp - alloc.getAllocatedControl()
|
||||
*
|
||||
* [...]
|
||||
* }
|
||||
* ```
|
||||
*
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
<<<<<<<< HEAD:src/lib/control_allocation/ControlAllocation.hpp
|
||||
#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
========
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
>>>>>>>> origin:src/lib/control_allocation/control_allocation/ControlAllocation.hpp
|
||||
|
||||
class ControlAllocation
|
||||
{
|
||||
public:
|
||||
ControlAllocation();
|
||||
virtual ~ControlAllocation() = default;
|
||||
|
||||
static constexpr uint8_t NUM_ACTUATORS = ActuatorEffectiveness::NUM_ACTUATORS;
|
||||
static constexpr uint8_t NUM_AXES = ActuatorEffectiveness::NUM_AXES;
|
||||
|
||||
typedef matrix::Vector<float, NUM_ACTUATORS> ActuatorVector;
|
||||
|
||||
enum ControlAxis {
|
||||
ROLL = 0,
|
||||
PITCH,
|
||||
YAW,
|
||||
THRUST_X,
|
||||
THRUST_Y,
|
||||
THRUST_Z
|
||||
};
|
||||
|
||||
/**
|
||||
* Allocate control setpoint to actuators
|
||||
*/
|
||||
virtual void allocate() = 0;
|
||||
|
||||
/**
|
||||
* Set actuator failure flag
|
||||
* This prevents a change of the scaling in the matrix normalization step
|
||||
* in case of a motor failure.
|
||||
*
|
||||
* @param failure Motor failure flag
|
||||
*/
|
||||
void setHadActuatorFailure(bool failure) { _had_actuator_failure = failure; }
|
||||
|
||||
/**
|
||||
* Set the control effectiveness matrix
|
||||
*
|
||||
* @param B Effectiveness matrix
|
||||
*/
|
||||
virtual void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
|
||||
const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
|
||||
bool update_normalization_scale);
|
||||
|
||||
/**
|
||||
* Get the allocated actuator vector
|
||||
*
|
||||
* @return Actuator vector
|
||||
*/
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorSetpoint() const { return _actuator_sp; }
|
||||
|
||||
/**
|
||||
* Set the desired control vector
|
||||
*
|
||||
* @param Control vector
|
||||
*/
|
||||
void setControlSetpoint(const matrix::Vector<float, NUM_AXES> &control) { _control_sp = control; }
|
||||
|
||||
/**
|
||||
* Get the desired control vector
|
||||
*
|
||||
* @return Control vector
|
||||
*/
|
||||
const matrix::Vector<float, NUM_AXES> &getControlSetpoint() const { return _control_sp; }
|
||||
|
||||
/**
|
||||
* Get the allocated control vector
|
||||
*
|
||||
* @return Control vector
|
||||
*/
|
||||
matrix::Vector<float, NUM_AXES> getAllocatedControl() const
|
||||
{ return (_effectiveness * (_actuator_sp - _actuator_trim)).emult(_control_allocation_scale); }
|
||||
|
||||
/**
|
||||
* Get the control effectiveness matrix
|
||||
*
|
||||
* @return Effectiveness matrix
|
||||
*/
|
||||
const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &getEffectivenessMatrix() const { return _effectiveness; }
|
||||
|
||||
/**
|
||||
* Set the minimum actuator values
|
||||
*
|
||||
* @param actuator_min Minimum actuator values
|
||||
*/
|
||||
void setActuatorMin(const matrix::Vector<float, NUM_ACTUATORS> &actuator_min) { _actuator_min = actuator_min; }
|
||||
|
||||
/**
|
||||
* Get the minimum actuator values
|
||||
*
|
||||
* @return Minimum actuator values
|
||||
*/
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorMin() const { return _actuator_min; }
|
||||
|
||||
/**
|
||||
* Set the maximum actuator values
|
||||
*
|
||||
* @param actuator_max Maximum actuator values
|
||||
*/
|
||||
void setActuatorMax(const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) { _actuator_max = actuator_max; }
|
||||
|
||||
/**
|
||||
* Get the maximum actuator values
|
||||
*
|
||||
* @return Maximum actuator values
|
||||
*/
|
||||
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorMax() const { return _actuator_max; }
|
||||
|
||||
/**
|
||||
* Set the current actuator setpoint.
|
||||
*
|
||||
* Use this when a new allocation method is started to initialize it properly.
|
||||
* In most cases, it is not needed to call this method before `allocate()`.
|
||||
* Indeed the previous actuator setpoint is expected to be stored during calls to `allocate()`.
|
||||
*
|
||||
* @param actuator_sp Actuator setpoint
|
||||
*/
|
||||
void setActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp);
|
||||
|
||||
void setSlewRateLimit(const matrix::Vector<float, NUM_ACTUATORS> &slew_rate_limit)
|
||||
{ _actuator_slew_rate_limit = slew_rate_limit; }
|
||||
|
||||
/**
|
||||
* Apply slew rate to current actuator setpoint
|
||||
*/
|
||||
void applySlewRateLimit(float dt);
|
||||
|
||||
/**
|
||||
* Clip the actuator setpoint between minimum and maximum values.
|
||||
*
|
||||
* The output is in the range [min; max]
|
||||
*
|
||||
* @param actuator Actuator vector to clip
|
||||
*/
|
||||
void clipActuatorSetpoint(matrix::Vector<float, NUM_ACTUATORS> &actuator) const;
|
||||
|
||||
void clipActuatorSetpoint() { clipActuatorSetpoint(_actuator_sp); }
|
||||
|
||||
/**
|
||||
* Normalize the actuator setpoint between minimum and maximum values.
|
||||
*
|
||||
* The output is in the range [-1; +1]
|
||||
*
|
||||
* @param actuator Actuator vector to normalize
|
||||
*
|
||||
* @return Clipped actuator setpoint
|
||||
*/
|
||||
matrix::Vector<float, NUM_ACTUATORS> normalizeActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator)
|
||||
const;
|
||||
|
||||
virtual void updateParameters() {}
|
||||
|
||||
int numConfiguredActuators() const { return _num_actuators; }
|
||||
|
||||
void setNormalizeRPY(bool normalize_rpy) { _normalize_rpy = normalize_rpy; }
|
||||
|
||||
protected:
|
||||
friend class ControlAllocator; // for _actuator_sp
|
||||
|
||||
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness; ///< Effectiveness matrix
|
||||
matrix::Vector<float, NUM_AXES> _control_allocation_scale; ///< Scaling applied during allocation
|
||||
matrix::Vector<float, NUM_ACTUATORS> _actuator_trim; ///< Neutral actuator values
|
||||
matrix::Vector<float, NUM_ACTUATORS> _actuator_min; ///< Minimum actuator values
|
||||
matrix::Vector<float, NUM_ACTUATORS> _actuator_max; ///< Maximum actuator values
|
||||
matrix::Vector<float, NUM_ACTUATORS> _actuator_slew_rate_limit; ///< Slew rate limit
|
||||
matrix::Vector<float, NUM_ACTUATORS> _prev_actuator_sp; ///< Previous actuator setpoint
|
||||
matrix::Vector<float, NUM_ACTUATORS> _actuator_sp; ///< Actuator setpoint
|
||||
matrix::Vector<float, NUM_AXES> _control_sp; ///< Control setpoint
|
||||
matrix::Vector<float, NUM_AXES> _control_trim; ///< Control at trim actuator values
|
||||
int _num_actuators{0};
|
||||
bool _normalize_rpy{false}; ///< if true, normalize roll, pitch and yaw columns
|
||||
bool _had_actuator_failure{false};
|
||||
};
|
||||
@ -0,0 +1,181 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ControlAllocationPseudoInverse.cpp
|
||||
*
|
||||
* Simple Control Allocation Algorithm
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#include "ControlAllocationPseudoInverse.hpp"
|
||||
|
||||
void
|
||||
ControlAllocationPseudoInverse::setEffectivenessMatrix(
|
||||
const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,
|
||||
const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
|
||||
bool update_normalization_scale)
|
||||
{
|
||||
ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, num_actuators,
|
||||
update_normalization_scale);
|
||||
_mix_update_needed = true;
|
||||
_normalization_needs_update = update_normalization_scale;
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationPseudoInverse::updatePseudoInverse()
|
||||
{
|
||||
if (_mix_update_needed) {
|
||||
matrix::geninv(_effectiveness, _mix);
|
||||
|
||||
if (_normalization_needs_update && !_had_actuator_failure) {
|
||||
updateControlAllocationMatrixScale();
|
||||
_normalization_needs_update = false;
|
||||
}
|
||||
|
||||
normalizeControlAllocationMatrix();
|
||||
_mix_update_needed = false;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationPseudoInverse::updateControlAllocationMatrixScale()
|
||||
{
|
||||
// Same scale on roll and pitch
|
||||
if (_normalize_rpy) {
|
||||
|
||||
int num_non_zero_roll_torque = 0;
|
||||
int num_non_zero_pitch_torque = 0;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
|
||||
if (fabsf(_mix(i, 0)) > 1e-3f) {
|
||||
++num_non_zero_roll_torque;
|
||||
}
|
||||
|
||||
if (fabsf(_mix(i, 1)) > 1e-3f) {
|
||||
++num_non_zero_pitch_torque;
|
||||
}
|
||||
}
|
||||
|
||||
float roll_norm_scale = 1.f;
|
||||
|
||||
if (num_non_zero_roll_torque > 0) {
|
||||
roll_norm_scale = sqrtf(_mix.col(0).norm_squared() / (num_non_zero_roll_torque / 2.f));
|
||||
}
|
||||
|
||||
float pitch_norm_scale = 1.f;
|
||||
|
||||
if (num_non_zero_pitch_torque > 0) {
|
||||
pitch_norm_scale = sqrtf(_mix.col(1).norm_squared() / (num_non_zero_pitch_torque / 2.f));
|
||||
}
|
||||
|
||||
_control_allocation_scale(0) = fmaxf(roll_norm_scale, pitch_norm_scale);
|
||||
_control_allocation_scale(1) = _control_allocation_scale(0);
|
||||
|
||||
// Scale yaw separately
|
||||
_control_allocation_scale(2) = _mix.col(2).max();
|
||||
|
||||
} else {
|
||||
_control_allocation_scale(0) = 1.f;
|
||||
_control_allocation_scale(1) = 1.f;
|
||||
_control_allocation_scale(2) = 1.f;
|
||||
}
|
||||
|
||||
// Scale thrust by the sum of the individual thrust axes, and use the scaling for the Z axis if there's no actuators
|
||||
// (for tilted actuators)
|
||||
_control_allocation_scale(THRUST_Z) = 1.f;
|
||||
|
||||
for (int axis_idx = 2; axis_idx >= 0; --axis_idx) {
|
||||
int num_non_zero_thrust = 0;
|
||||
float norm_sum = 0.f;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
float norm = fabsf(_mix(i, 3 + axis_idx));
|
||||
norm_sum += norm;
|
||||
|
||||
if (norm > FLT_EPSILON) {
|
||||
++num_non_zero_thrust;
|
||||
}
|
||||
}
|
||||
|
||||
if (num_non_zero_thrust > 0) {
|
||||
_control_allocation_scale(3 + axis_idx) = norm_sum / num_non_zero_thrust;
|
||||
|
||||
} else {
|
||||
_control_allocation_scale(3 + axis_idx) = _control_allocation_scale(THRUST_Z);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationPseudoInverse::normalizeControlAllocationMatrix()
|
||||
{
|
||||
if (_control_allocation_scale(0) > FLT_EPSILON) {
|
||||
_mix.col(0) /= _control_allocation_scale(0);
|
||||
_mix.col(1) /= _control_allocation_scale(1);
|
||||
}
|
||||
|
||||
if (_control_allocation_scale(2) > FLT_EPSILON) {
|
||||
_mix.col(2) /= _control_allocation_scale(2);
|
||||
}
|
||||
|
||||
if (_control_allocation_scale(3) > FLT_EPSILON) {
|
||||
_mix.col(3) /= _control_allocation_scale(3);
|
||||
_mix.col(4) /= _control_allocation_scale(4);
|
||||
_mix.col(5) /= _control_allocation_scale(5);
|
||||
}
|
||||
|
||||
// Set all the small elements to 0 to avoid issues
|
||||
// in the control allocation algorithms
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
for (int j = 0; j < NUM_AXES; j++) {
|
||||
if (fabsf(_mix(i, j)) < 1e-3f) {
|
||||
_mix(i, j) = 0.f;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationPseudoInverse::allocate()
|
||||
{
|
||||
//Compute new gains if needed
|
||||
updatePseudoInverse();
|
||||
|
||||
_prev_actuator_sp = _actuator_sp;
|
||||
|
||||
// Allocate
|
||||
_actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim);
|
||||
}
|
||||
@ -0,0 +1,76 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ControlAllocationPseudoInverse.hpp
|
||||
*
|
||||
* Simple Control Allocation Algorithm
|
||||
*
|
||||
* It computes the pseudo-inverse of the effectiveness matrix
|
||||
* Actuator saturation is handled by simple clipping, do not
|
||||
* expect good performance in case of actuator saturation.
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ControlAllocation.hpp"
|
||||
|
||||
class ControlAllocationPseudoInverse: public ControlAllocation
|
||||
{
|
||||
public:
|
||||
ControlAllocationPseudoInverse() = default;
|
||||
virtual ~ControlAllocationPseudoInverse() = default;
|
||||
|
||||
void allocate() override;
|
||||
void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
|
||||
const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
|
||||
bool update_normalization_scale) override;
|
||||
|
||||
protected:
|
||||
matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
|
||||
|
||||
bool _mix_update_needed{false};
|
||||
|
||||
/**
|
||||
* Recalculate pseudo inverse if required.
|
||||
*
|
||||
*/
|
||||
void updatePseudoInverse();
|
||||
|
||||
private:
|
||||
void normalizeControlAllocationMatrix();
|
||||
void updateControlAllocationMatrixScale();
|
||||
bool _normalization_needs_update{false};
|
||||
};
|
||||
@ -0,0 +1,69 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2019 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ControlAllocationPseudoInverseTest.cpp
|
||||
*
|
||||
* Tests for Control Allocation Algorithms
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <ControlAllocationPseudoInverse.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
TEST(ControlAllocationTest, AllZeroCase)
|
||||
{
|
||||
ControlAllocationPseudoInverse method;
|
||||
|
||||
matrix::Vector<float, 6> control_sp;
|
||||
matrix::Vector<float, 6> control_allocated;
|
||||
matrix::Vector<float, 6> control_allocated_expected;
|
||||
matrix::Matrix<float, 6, 16> effectiveness;
|
||||
matrix::Vector<float, 16> actuator_sp;
|
||||
matrix::Vector<float, 16> actuator_trim;
|
||||
matrix::Vector<float, 16> linearization_point;
|
||||
matrix::Vector<float, 16> actuator_sp_expected;
|
||||
|
||||
method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16, false);
|
||||
method.setControlSetpoint(control_sp);
|
||||
method.allocate();
|
||||
method.clipActuatorSetpoint();
|
||||
actuator_sp = method.getActuatorSetpoint();
|
||||
control_allocated_expected = method.getAllocatedControl();
|
||||
|
||||
EXPECT_EQ(actuator_sp, actuator_sp_expected);
|
||||
EXPECT_EQ(control_allocated, control_allocated_expected);
|
||||
}
|
||||
@ -0,0 +1,234 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ControlAllocationSequentialDesaturation.cpp
|
||||
*
|
||||
* @author Roman Bapst <bapstroman@gmail.com>
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*/
|
||||
|
||||
#include "ControlAllocationSequentialDesaturation.hpp"
|
||||
|
||||
|
||||
void
|
||||
ControlAllocationSequentialDesaturation::allocate()
|
||||
{
|
||||
//Compute new gains if needed
|
||||
updatePseudoInverse();
|
||||
|
||||
_prev_actuator_sp = _actuator_sp;
|
||||
|
||||
switch (_param_mc_airmode.get()) {
|
||||
case 1:
|
||||
mixAirmodeRP();
|
||||
break;
|
||||
|
||||
case 2:
|
||||
mixAirmodeRPY();
|
||||
break;
|
||||
|
||||
default:
|
||||
mixAirmodeDisabled();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void ControlAllocationSequentialDesaturation::desaturateActuators(
|
||||
ActuatorVector &actuator_sp,
|
||||
const ActuatorVector &desaturation_vector, bool increase_only)
|
||||
{
|
||||
float gain = computeDesaturationGain(desaturation_vector, actuator_sp);
|
||||
|
||||
if (increase_only && gain < 0.f) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
actuator_sp(i) += gain * desaturation_vector(i);
|
||||
}
|
||||
|
||||
gain = 0.5f * computeDesaturationGain(desaturation_vector, actuator_sp);
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
actuator_sp(i) += gain * desaturation_vector(i);
|
||||
}
|
||||
}
|
||||
|
||||
float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector,
|
||||
const ActuatorVector &actuator_sp)
|
||||
{
|
||||
float k_min = 0.f;
|
||||
float k_max = 0.f;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
// Do not use try to desaturate using an actuator with weak effectiveness to avoid large desaturation gains
|
||||
if (fabsf(desaturation_vector(i)) < 0.2f) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (actuator_sp(i) < _actuator_min(i)) {
|
||||
float k = (_actuator_min(i) - actuator_sp(i)) / desaturation_vector(i);
|
||||
|
||||
if (k < k_min) { k_min = k; }
|
||||
|
||||
if (k > k_max) { k_max = k; }
|
||||
}
|
||||
|
||||
if (actuator_sp(i) > _actuator_max(i)) {
|
||||
float k = (_actuator_max(i) - actuator_sp(i)) / desaturation_vector(i);
|
||||
|
||||
if (k < k_min) { k_min = k; }
|
||||
|
||||
if (k > k_max) { k_max = k; }
|
||||
}
|
||||
}
|
||||
|
||||
// Reduce the saturation as much as possible
|
||||
return k_min + k_max;
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationSequentialDesaturation::mixAirmodeRP()
|
||||
{
|
||||
// Airmode for roll and pitch, but not yaw
|
||||
|
||||
// Mix without yaw
|
||||
ActuatorVector thrust_z;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
_actuator_sp(i) = _actuator_trim(i) +
|
||||
_mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
|
||||
_mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
|
||||
_mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
|
||||
_mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
|
||||
_mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
|
||||
thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
|
||||
}
|
||||
|
||||
desaturateActuators(_actuator_sp, thrust_z);
|
||||
|
||||
// Mix yaw independently
|
||||
mixYaw();
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationSequentialDesaturation::mixAirmodeRPY()
|
||||
{
|
||||
// Airmode for roll, pitch and yaw
|
||||
|
||||
// Do full mixing
|
||||
ActuatorVector thrust_z;
|
||||
ActuatorVector yaw;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
_actuator_sp(i) = _actuator_trim(i) +
|
||||
_mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
|
||||
_mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
|
||||
_mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) +
|
||||
_mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
|
||||
_mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
|
||||
_mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
|
||||
thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
|
||||
yaw(i) = _mix(i, ControlAxis::YAW);
|
||||
}
|
||||
|
||||
desaturateActuators(_actuator_sp, thrust_z);
|
||||
|
||||
// Unsaturate yaw (in case upper and lower bounds are exceeded)
|
||||
// to prioritize roll/pitch over yaw.
|
||||
desaturateActuators(_actuator_sp, yaw);
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationSequentialDesaturation::mixAirmodeDisabled()
|
||||
{
|
||||
// Airmode disabled: never allow to increase the thrust to unsaturate a motor
|
||||
|
||||
// Mix without yaw
|
||||
ActuatorVector thrust_z;
|
||||
ActuatorVector roll;
|
||||
ActuatorVector pitch;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
_actuator_sp(i) = _actuator_trim(i) +
|
||||
_mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) +
|
||||
_mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) +
|
||||
_mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) +
|
||||
_mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) +
|
||||
_mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z));
|
||||
thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
|
||||
roll(i) = _mix(i, ControlAxis::ROLL);
|
||||
pitch(i) = _mix(i, ControlAxis::PITCH);
|
||||
}
|
||||
|
||||
// only reduce thrust
|
||||
desaturateActuators(_actuator_sp, thrust_z, true);
|
||||
|
||||
// Reduce roll/pitch acceleration if needed to unsaturate
|
||||
desaturateActuators(_actuator_sp, roll);
|
||||
desaturateActuators(_actuator_sp, pitch);
|
||||
|
||||
// Mix yaw independently
|
||||
mixYaw();
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationSequentialDesaturation::mixYaw()
|
||||
{
|
||||
// Add yaw to outputs
|
||||
ActuatorVector yaw;
|
||||
ActuatorVector thrust_z;
|
||||
|
||||
for (int i = 0; i < _num_actuators; i++) {
|
||||
_actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW));
|
||||
yaw(i) = _mix(i, ControlAxis::YAW);
|
||||
thrust_z(i) = _mix(i, ControlAxis::THRUST_Z);
|
||||
}
|
||||
|
||||
// Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
|
||||
// and allow some yaw response at maximum thrust
|
||||
ActuatorVector max_prev = _actuator_max;
|
||||
_actuator_max += (_actuator_max - _actuator_min) * MINIMUM_YAW_MARGIN;
|
||||
desaturateActuators(_actuator_sp, yaw);
|
||||
_actuator_max = max_prev;
|
||||
|
||||
// reduce thrust only
|
||||
desaturateActuators(_actuator_sp, thrust_z, true);
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocationSequentialDesaturation::updateParameters()
|
||||
{
|
||||
updateParams();
|
||||
}
|
||||
@ -0,0 +1,132 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ControlAllocationSequentialDesaturation.hpp
|
||||
*
|
||||
* Control Allocation Algorithm which sequentially modifies control demands in order to
|
||||
* eliminate the saturation of the actuator setpoint vector.
|
||||
*
|
||||
*
|
||||
* @author Roman Bapst <bapstroman@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ControlAllocationPseudoInverse.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse, public ModuleParams
|
||||
{
|
||||
public:
|
||||
|
||||
ControlAllocationSequentialDesaturation() : ModuleParams(nullptr) {}
|
||||
virtual ~ControlAllocationSequentialDesaturation() = default;
|
||||
|
||||
void allocate() override;
|
||||
|
||||
void updateParameters() override;
|
||||
|
||||
// This is the minimum actuator yaw granted when the controller is saturated.
|
||||
// In the yaw-only case where outputs are saturated, thrust is reduced by up to this amount.
|
||||
static constexpr float MINIMUM_YAW_MARGIN{0.15f};
|
||||
private:
|
||||
|
||||
/**
|
||||
* Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector.
|
||||
* desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular
|
||||
* acceleration on a specific axis.
|
||||
* For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the
|
||||
* saturation will be minimized by shifting the vertical thrust setpoint, without changing the
|
||||
* roll/pitch/yaw accelerations.
|
||||
*
|
||||
* Note that as we only slide along the given axis, in extreme cases outputs can still contain values
|
||||
* outside of [min_output, max_output].
|
||||
*
|
||||
* @param actuator_sp Actuator setpoint, vector that is modified
|
||||
* @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale
|
||||
* @param increase_only if true, only allow to increase (add) a fraction of desaturation_vector
|
||||
*/
|
||||
void desaturateActuators(ActuatorVector &actuator_sp, const ActuatorVector &desaturation_vector,
|
||||
bool increase_only = false);
|
||||
|
||||
/**
|
||||
* Computes the gain k by which desaturation_vector has to be multiplied
|
||||
* in order to unsaturate the output that has the greatest saturation.
|
||||
*
|
||||
* @return desaturation gain
|
||||
*/
|
||||
float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp);
|
||||
|
||||
/**
|
||||
* Mix roll, pitch, yaw, thrust and set the actuator setpoint.
|
||||
*
|
||||
* Desaturation behavior: airmode for roll/pitch:
|
||||
* thrust is increased/decreased as much as required to meet the demanded roll/pitch.
|
||||
* Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior.
|
||||
*/
|
||||
void mixAirmodeRP();
|
||||
|
||||
/**
|
||||
* Mix roll, pitch, yaw, thrust and set the actuator setpoint.
|
||||
*
|
||||
* Desaturation behavior: full airmode for roll/pitch/yaw:
|
||||
* thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw,
|
||||
* while giving priority to roll and pitch over yaw.
|
||||
*/
|
||||
void mixAirmodeRPY();
|
||||
|
||||
/**
|
||||
* Mix roll, pitch, yaw, thrust and set the actuator setpoint.
|
||||
*
|
||||
* Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded
|
||||
* roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed.
|
||||
* Thrust can be reduced to unsaturate the upper side.
|
||||
* @see mixYaw() for the exact yaw behavior.
|
||||
*/
|
||||
void mixAirmodeDisabled();
|
||||
|
||||
/**
|
||||
* Mix yaw by updating the actuator setpoint (that already contains roll/pitch/thrust).
|
||||
*
|
||||
* Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow
|
||||
* some yaw control on the upper end. On the lower end thrust will never be increased,
|
||||
* but yaw is decreased as much as required.
|
||||
*/
|
||||
void mixYaw();
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode ///< air-mode
|
||||
);
|
||||
};
|
||||
@ -0,0 +1,385 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2024 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ControlAllocationSequentialDesaturationTest.cpp
|
||||
*
|
||||
* Tests for Control Allocation Sequential Desaturation Algorithms
|
||||
*
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <ControlAllocationSequentialDesaturation.hpp>
|
||||
#include <actuator_effectiveness/ActuatorEffectivenessRotors.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
// Makes and returns a Geometry object for a "standard" quad-x quadcopter.
|
||||
ActuatorEffectivenessRotors::Geometry make_quad_x_geometry()
|
||||
{
|
||||
ActuatorEffectivenessRotors::Geometry geometry = {};
|
||||
geometry.rotors[0].position(0) = 1.0f;
|
||||
geometry.rotors[0].position(1) = 1.0f;
|
||||
geometry.rotors[0].position(2) = 0.0f;
|
||||
geometry.rotors[0].axis(0) = 0.0f;
|
||||
geometry.rotors[0].axis(1) = 0.0f;
|
||||
geometry.rotors[0].axis(2) = -1.0f;
|
||||
geometry.rotors[0].thrust_coef = 1.0f;
|
||||
geometry.rotors[0].moment_ratio = 0.05f;
|
||||
|
||||
geometry.rotors[1].position(0) = -1.0f;
|
||||
geometry.rotors[1].position(1) = -1.0f;
|
||||
geometry.rotors[1].position(2) = 0.0f;
|
||||
geometry.rotors[1].axis(0) = 0.0f;
|
||||
geometry.rotors[1].axis(1) = 0.0f;
|
||||
geometry.rotors[1].axis(2) = -1.0f;
|
||||
geometry.rotors[1].thrust_coef = 1.0f;
|
||||
geometry.rotors[1].moment_ratio = 0.05f;
|
||||
|
||||
geometry.rotors[2].position(0) = 1.0f;
|
||||
geometry.rotors[2].position(1) = -1.0f;
|
||||
geometry.rotors[2].position(2) = 0.0f;
|
||||
geometry.rotors[2].axis(0) = 0.0f;
|
||||
geometry.rotors[2].axis(1) = 0.0f;
|
||||
geometry.rotors[2].axis(2) = -1.0f;
|
||||
geometry.rotors[2].thrust_coef = 1.0f;
|
||||
geometry.rotors[2].moment_ratio = -0.05f;
|
||||
|
||||
geometry.rotors[3].position(0) = -1.0f;
|
||||
geometry.rotors[3].position(1) = 1.0f;
|
||||
geometry.rotors[3].position(2) = 0.0f;
|
||||
geometry.rotors[3].axis(0) = 0.0f;
|
||||
geometry.rotors[3].axis(1) = 0.0f;
|
||||
geometry.rotors[3].axis(2) = -1.0f;
|
||||
geometry.rotors[3].thrust_coef = 1.0f;
|
||||
geometry.rotors[3].moment_ratio = -0.05f;
|
||||
|
||||
geometry.num_rotors = 4;
|
||||
|
||||
return geometry;
|
||||
}
|
||||
|
||||
// Returns an effective matrix for a sample quad-copter configuration.
|
||||
ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness()
|
||||
{
|
||||
ActuatorEffectiveness::EffectivenessMatrix effectiveness;
|
||||
effectiveness.setZero();
|
||||
const auto geometry = make_quad_x_geometry();
|
||||
ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness);
|
||||
return effectiveness;
|
||||
}
|
||||
|
||||
// Configures a ControlAllocationSequentialDesaturation object for a sample quad-copter.
|
||||
void setup_quad_allocator(ControlAllocationSequentialDesaturation &allocator)
|
||||
{
|
||||
const auto effectiveness = make_quad_x_effectiveness();
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_ACTUATORS> actuator_trim;
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_ACTUATORS> linearization_point;
|
||||
constexpr bool UPDATE_NORMALIZATION_SCALE{false};
|
||||
allocator.setEffectivenessMatrix(
|
||||
effectiveness,
|
||||
actuator_trim,
|
||||
linearization_point,
|
||||
ActuatorEffectiveness::NUM_ACTUATORS,
|
||||
UPDATE_NORMALIZATION_SCALE
|
||||
);
|
||||
}
|
||||
|
||||
static constexpr float EXPECT_NEAR_TOL{1e-4f};
|
||||
|
||||
} // namespace
|
||||
|
||||
// This tests that yaw-only control setpoint at zero actuator setpoint results in zero actuator
|
||||
// allocation.
|
||||
TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledOnlyYaw)
|
||||
{
|
||||
ControlAllocationSequentialDesaturation allocator;
|
||||
setup_quad_allocator(allocator);
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
|
||||
control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::YAW) = 1.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Z) = 0.f;
|
||||
allocator.setControlSetpoint(control_sp);
|
||||
|
||||
// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
|
||||
allocator.allocate();
|
||||
|
||||
const auto &actuator_sp = allocator.getActuatorSetpoint();
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_ACTUATORS> zero;
|
||||
EXPECT_EQ(actuator_sp, zero);
|
||||
}
|
||||
|
||||
// This tests that a control setpoint for z-thrust returns the desired actuator setpoint.
|
||||
// Each motor should have an actuator setpoint that when summed together should be equal to
|
||||
// control setpoint.
|
||||
TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustZ)
|
||||
{
|
||||
ControlAllocationSequentialDesaturation allocator;
|
||||
setup_quad_allocator(allocator);
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
|
||||
// Negative, because +z is "downward".
|
||||
constexpr float THRUST_Z_TOTAL{-0.75f};
|
||||
control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::YAW) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
|
||||
allocator.setControlSetpoint(control_sp);
|
||||
|
||||
// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
|
||||
allocator.allocate();
|
||||
|
||||
const auto &actuator_sp = allocator.getActuatorSetpoint();
|
||||
constexpr int MOTOR_COUNT{4};
|
||||
constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT};
|
||||
|
||||
for (int i{0}; i < MOTOR_COUNT; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
}
|
||||
|
||||
for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
|
||||
}
|
||||
}
|
||||
|
||||
// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint.
|
||||
// This test does not saturate the yaw response.
|
||||
TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndYaw)
|
||||
{
|
||||
ControlAllocationSequentialDesaturation allocator;
|
||||
setup_quad_allocator(allocator);
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
|
||||
// Negative, because +z is "downward".
|
||||
constexpr float THRUST_Z_TOTAL{-0.75f};
|
||||
// This is low enough to not saturate the motors.
|
||||
constexpr float YAW_CONTROL_SP{0.02f};
|
||||
control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
|
||||
allocator.setControlSetpoint(control_sp);
|
||||
|
||||
// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
|
||||
allocator.allocate();
|
||||
|
||||
const auto &actuator_sp = allocator.getActuatorSetpoint();
|
||||
// This value is based off of the effectiveness matrix. If the effectiveness matrix is changed,
|
||||
// this will need to be changed.
|
||||
constexpr float YAW_EFFECTIVENESS_FACTOR{5.f};
|
||||
constexpr float YAW_DIFF_PER_MOTOR{YAW_CONTROL_SP * YAW_EFFECTIVENESS_FACTOR};
|
||||
// At yaw condition, there will be 2 different actuator values.
|
||||
constexpr int MOTOR_COUNT{4};
|
||||
constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + YAW_DIFF_PER_MOTOR};
|
||||
constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - YAW_DIFF_PER_MOTOR};
|
||||
|
||||
for (int i{0}; i < MOTOR_COUNT / 2; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
}
|
||||
|
||||
for (int i{MOTOR_COUNT / 2}; i < MOTOR_COUNT; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
}
|
||||
|
||||
for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
|
||||
}
|
||||
}
|
||||
|
||||
// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint.
|
||||
// This test saturates the yaw response, but does not reduce total thrust.
|
||||
TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndSaturatedYaw)
|
||||
{
|
||||
ControlAllocationSequentialDesaturation allocator;
|
||||
setup_quad_allocator(allocator);
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
|
||||
// Negative, because +z is "downward".
|
||||
constexpr float THRUST_Z_TOTAL{-0.75f};
|
||||
// This is arbitrarily high to trigger strongest possible (saturated) yaw response.
|
||||
constexpr float YAW_CONTROL_SP{0.25f};
|
||||
control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
|
||||
allocator.setControlSetpoint(control_sp);
|
||||
|
||||
// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
|
||||
allocator.allocate();
|
||||
|
||||
const auto &actuator_sp = allocator.getActuatorSetpoint();
|
||||
// At max yaw, only 2 motors will carry all of the thrust.
|
||||
constexpr int YAW_MOTORS{2};
|
||||
constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / YAW_MOTORS};
|
||||
|
||||
for (int i{0}; i < YAW_MOTORS; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
}
|
||||
|
||||
for (int i{YAW_MOTORS}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
|
||||
}
|
||||
}
|
||||
|
||||
// This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint.
|
||||
// This test does not saturate the pitch response.
|
||||
TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndPitch)
|
||||
{
|
||||
ControlAllocationSequentialDesaturation allocator;
|
||||
setup_quad_allocator(allocator);
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
|
||||
// Negative, because +z is "downward".
|
||||
constexpr float THRUST_Z_TOTAL{-0.75f};
|
||||
// This is low enough to not saturate the motors.
|
||||
constexpr float PITCH_CONTROL_SP{0.1f};
|
||||
control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP;
|
||||
control_sp(ControlAllocation::ControlAxis::YAW) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
|
||||
allocator.setControlSetpoint(control_sp);
|
||||
|
||||
// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
|
||||
allocator.allocate();
|
||||
|
||||
const auto &actuator_sp = allocator.getActuatorSetpoint();
|
||||
// This value is based off of the effectiveness matrix. If the effectiveness matrix is changed,
|
||||
// this will need to be changed.
|
||||
constexpr int MOTOR_COUNT{4};
|
||||
constexpr float PITCH_DIFF_PER_MOTOR{PITCH_CONTROL_SP / MOTOR_COUNT};
|
||||
// At control set point, there will be 2 different actuator values.
|
||||
constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_DIFF_PER_MOTOR};
|
||||
constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_DIFF_PER_MOTOR};
|
||||
EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
|
||||
for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
|
||||
}
|
||||
}
|
||||
|
||||
// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint.
|
||||
// This test saturates yaw and demonstrates reduction of thrust for yaw.
|
||||
TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndYaw)
|
||||
{
|
||||
ControlAllocationSequentialDesaturation allocator;
|
||||
setup_quad_allocator(allocator);
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
|
||||
// Negative, because +z is "downward".
|
||||
constexpr float DESIRED_THRUST_Z_PER_MOTOR{0.8f};
|
||||
constexpr int MOTOR_COUNT{4};
|
||||
constexpr float THRUST_Z_TOTAL{-DESIRED_THRUST_Z_PER_MOTOR * MOTOR_COUNT};
|
||||
// This is arbitrarily high to trigger strongest possible (saturated) yaw response.
|
||||
constexpr float YAW_CONTROL_SP{1.f};
|
||||
control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
|
||||
allocator.setControlSetpoint(control_sp);
|
||||
|
||||
// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
|
||||
allocator.allocate();
|
||||
|
||||
const auto &actuator_sp = allocator.getActuatorSetpoint();
|
||||
// In the case of yaw saturation, thrust per motor will be reduced by the hard-coded
|
||||
// magic-number yaw margin of 0.15f.
|
||||
constexpr float YAW_MARGIN{0.15f}; // get this from a centralized source when available.
|
||||
constexpr float YAW_DIFF_PER_MOTOR{1.0f + YAW_MARGIN - DESIRED_THRUST_Z_PER_MOTOR};
|
||||
// At control set point, there will be 2 different actuator values.
|
||||
constexpr float HIGH_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR + YAW_DIFF_PER_MOTOR - YAW_MARGIN};
|
||||
constexpr float LOW_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR - YAW_DIFF_PER_MOTOR - YAW_MARGIN};
|
||||
EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(1), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(2), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
|
||||
for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
|
||||
}
|
||||
}
|
||||
|
||||
// This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint.
|
||||
// This test saturates the pitch response such that thrust is reduced to (partially) compensate.
|
||||
TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndPitch)
|
||||
{
|
||||
ControlAllocationSequentialDesaturation allocator;
|
||||
setup_quad_allocator(allocator);
|
||||
matrix::Vector<float, ActuatorEffectiveness::NUM_AXES> control_sp;
|
||||
// Negative, because +z is "downward".
|
||||
constexpr float THRUST_Z_TOTAL{-0.75f * 4.f};
|
||||
// This is high enough to saturate the pitch control.
|
||||
constexpr float PITCH_CONTROL_SP{2.f};
|
||||
control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP;
|
||||
control_sp(ControlAllocation::ControlAxis::YAW) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f;
|
||||
control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL;
|
||||
allocator.setControlSetpoint(control_sp);
|
||||
|
||||
// Since MC_AIRMODE was not set explicitly, assume airmode is disabled.
|
||||
allocator.allocate();
|
||||
|
||||
const auto &actuator_sp = allocator.getActuatorSetpoint();
|
||||
constexpr int MOTOR_COUNT{4};
|
||||
// The maximum actuator value is
|
||||
// THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT.
|
||||
// The amount over 1 is the amount that each motor is reduced by.
|
||||
// At control set point, there will be 2 different actuator values.
|
||||
constexpr float OVERAGE_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - 1};
|
||||
EXPECT_TRUE(OVERAGE_PER_MOTOR > 0.f);
|
||||
constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR};
|
||||
constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR};
|
||||
EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL);
|
||||
|
||||
for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) {
|
||||
EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL);
|
||||
}
|
||||
}
|
||||
@ -2268,7 +2268,8 @@ void Commander::handleAutoDisarm()
|
||||
_auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time());
|
||||
|
||||
} else if (_param_com_disarm_prflt.get() > 0 && !_have_taken_off_since_arming) {
|
||||
_auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_prflt.get() * 1_s);
|
||||
_auto_disarm_landed.set_hysteresis_time_from(false,
|
||||
(_param_com_spoolup_time.get() + _param_com_disarm_prflt.get()) * 1_s);
|
||||
_auto_disarm_landed.set_state_and_update(true, hrt_absolute_time());
|
||||
}
|
||||
|
||||
|
||||
@ -330,7 +330,6 @@ private:
|
||||
param_t _param_rc_map_fltmode{PARAM_INVALID};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
|
||||
(ParamFloat<px4::params::COM_DISARM_LAND>) _param_com_disarm_land,
|
||||
(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_prflt,
|
||||
(ParamBool<px4::params::COM_DISARM_MAN>) _param_com_disarm_man,
|
||||
@ -347,6 +346,7 @@ private:
|
||||
(ParamFloat<px4::params::COM_OBC_LOSS_T>) _param_com_obc_loss_t,
|
||||
(ParamInt<px4::params::COM_PREARM_MODE>) _param_com_prearm_mode,
|
||||
(ParamInt<px4::params::COM_RC_OVERRIDE>) _param_com_rc_override,
|
||||
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
|
||||
(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_com_flight_uuid,
|
||||
(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_com_takeoff_act,
|
||||
(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max
|
||||
|
||||
@ -314,7 +314,7 @@ ControlAllocator::Run()
|
||||
#endif
|
||||
|
||||
// Check if parameters have changed
|
||||
if (_parameter_update_sub.updated() && !_armed) {
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s param_update;
|
||||
_parameter_update_sub.copy(¶m_update);
|
||||
|
||||
@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <lib/slew_rate/SlewRate.hpp>
|
||||
|
||||
@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
|
||||
|
||||
@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
|
||||
|
||||
@ -134,9 +134,12 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
|
||||
{
|
||||
_saturation_flags = {};
|
||||
|
||||
const float spoolup_progress = throttleSpoolupProgress();
|
||||
_rpm_control.setSpoolupProgress(spoolup_progress);
|
||||
|
||||
// throttle/collective pitch curve
|
||||
const float throttle = math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
|
||||
_geometry.throttle_curve) * throttleSpoolupProgress();
|
||||
const float throttle = (math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
|
||||
_geometry.throttle_curve) + _rpm_control.getActuatorCorrection()) * spoolup_progress;
|
||||
const float collective_pitch = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.pitch_curve);
|
||||
|
||||
// actuator mapping
|
||||
|
||||
@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
@ -41,6 +41,8 @@
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/manual_control_switches.h>
|
||||
|
||||
#include "RpmControl.hpp"
|
||||
|
||||
class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness
|
||||
{
|
||||
public:
|
||||
@ -131,4 +133,6 @@ private:
|
||||
bool _main_motor_engaged{true};
|
||||
|
||||
const ActuatorType _tail_actuator_type;
|
||||
|
||||
RpmControl _rpm_control{this};
|
||||
};
|
||||
|
||||
@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
|
||||
@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessTilts.hpp"
|
||||
|
||||
|
||||
@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
|
||||
class ActuatorEffectivenessMultirotor : public ModuleParams, public ActuatorEffectiveness
|
||||
|
||||
@ -41,7 +41,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
|
||||
class ActuatorEffectivenessRoverAckermann : public ActuatorEffectiveness
|
||||
{
|
||||
|
||||
@ -41,7 +41,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
|
||||
|
||||
@ -39,7 +39,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
|
||||
|
||||
@ -41,7 +41,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
#include "ActuatorEffectivenessControlSurfaces.hpp"
|
||||
#include "ActuatorEffectivenessTilts.hpp"
|
||||
|
||||
@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
@ -33,7 +33,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
|
||||
#include "ActuatorEffectivenessRotors.hpp"
|
||||
|
||||
class ActuatorEffectivenessUUV : public ModuleParams, public ActuatorEffectiveness
|
||||
|
||||
@ -27,6 +27,8 @@ px4_add_library(VehicleActuatorEffectiveness
|
||||
ActuatorEffectivenessTailsitterVTOL.hpp
|
||||
ActuatorEffectivenessRoverAckermann.hpp
|
||||
ActuatorEffectivenessRoverAckermann.cpp
|
||||
RpmControl.hpp
|
||||
RpmControl.cpp
|
||||
)
|
||||
|
||||
target_compile_options(VehicleActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
@ -0,0 +1,85 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 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 "RpmControl.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent)
|
||||
{
|
||||
_pid.setOutputLimit(PID_OUTPUT_LIMIT);
|
||||
_pid.setIntegralLimit(PID_OUTPUT_LIMIT);
|
||||
};
|
||||
|
||||
void RpmControl::setSpoolupProgress(float spoolup_progress)
|
||||
{
|
||||
_spoolup_progress = spoolup_progress;
|
||||
_pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get());
|
||||
|
||||
if (_spoolup_progress < SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED) {
|
||||
_pid.resetIntegral();
|
||||
}
|
||||
}
|
||||
|
||||
float RpmControl::getActuatorCorrection()
|
||||
{
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
// RPM measurement update
|
||||
if (_rpm_sub.updated()) {
|
||||
rpm_s rpm{};
|
||||
|
||||
if (_rpm_sub.copy(&rpm)) {
|
||||
const float dt = math::min((now - _timestamp_last_measurement) * 1e-6f, 1.f);
|
||||
_timestamp_last_measurement = rpm.timestamp;
|
||||
|
||||
const float gain_scale = math::interpolate(_spoolup_progress, .8f, 1.f, 0.f, 1e-3f);
|
||||
_pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f);
|
||||
_actuator_correction = _pid.update(rpm.rpm_estimate, dt, true);
|
||||
|
||||
_rpm_invalid = rpm.rpm_estimate < 1.f;
|
||||
}
|
||||
}
|
||||
|
||||
// Timeout
|
||||
const bool timeout = now > _timestamp_last_measurement + 1_s;
|
||||
|
||||
if (_rpm_invalid || timeout) {
|
||||
_pid.resetIntegral();
|
||||
_actuator_correction = 0.f;
|
||||
}
|
||||
|
||||
return _actuator_correction;
|
||||
}
|
||||
@ -0,0 +1,77 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file RpmControl.hpp
|
||||
*
|
||||
* Control rpm of a helicopter rotor.
|
||||
* Input: PWM input pulse period from an rpm sensor
|
||||
* Output: Duty cycle command for the ESC
|
||||
*
|
||||
* @author Matthias Grob <maetugr@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <lib/pid/PID.hpp>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/rpm.h>
|
||||
|
||||
class RpmControl : public ModuleParams
|
||||
{
|
||||
public:
|
||||
RpmControl(ModuleParams *parent);
|
||||
~RpmControl() = default;
|
||||
|
||||
void setSpoolupProgress(float spoolup_progress);
|
||||
float getActuatorCorrection();
|
||||
|
||||
private:
|
||||
static constexpr float SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED = .8f; // [0,1]
|
||||
static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1]
|
||||
|
||||
uORB::Subscription _rpm_sub{ORB_ID(rpm)};
|
||||
bool _rpm_invalid{true};
|
||||
PID _pid;
|
||||
float _spoolup_progress{0.f}; // [0,1]
|
||||
hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout
|
||||
float _actuator_correction{0.f};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::CA_HELI_RPM_SP>) _param_ca_heli_rpm_sp,
|
||||
(ParamFloat<px4::params::CA_HELI_RPM_P>) _param_ca_heli_rpm_p,
|
||||
(ParamFloat<px4::params::CA_HELI_RPM_I>) _param_ca_heli_rpm_i
|
||||
)
|
||||
};
|
||||
@ -528,6 +528,41 @@ parameters:
|
||||
which is mostly the case when the main rotor turns counter-clockwise.
|
||||
type: boolean
|
||||
default: 0
|
||||
CA_HELI_RPM_SP:
|
||||
description:
|
||||
short: Setpoint for main rotor rpm
|
||||
long: |
|
||||
Requires rpm feedback for the controller.
|
||||
type: float
|
||||
decimal: 0
|
||||
increment: 1
|
||||
min: 100
|
||||
max: 10000
|
||||
default: 1500
|
||||
CA_HELI_RPM_P:
|
||||
description:
|
||||
short: Proportional gain for rpm control
|
||||
long: |
|
||||
Ratio between rpm error devided by 1000 to how much normalized output gets added to correct for it.
|
||||
|
||||
motor_command = throttle_curve + CA_HELI_RPM_P * (rpm_setpoint - rpm_measurement) / 1000
|
||||
type: float
|
||||
decimal: 3
|
||||
increment: 0.1
|
||||
min: 0
|
||||
max: 10
|
||||
default: 0.0
|
||||
CA_HELI_RPM_I:
|
||||
description:
|
||||
short: Integral gain for rpm control
|
||||
long: |
|
||||
Same definition as the proportional gain but for integral.
|
||||
type: float
|
||||
decimal: 3
|
||||
increment: 0.1
|
||||
min: 0
|
||||
max: 10
|
||||
default: 0.0
|
||||
|
||||
# Others
|
||||
CA_FAILURE_MODE:
|
||||
|
||||
@ -280,7 +280,17 @@ void Ekf::constrainStateVariances()
|
||||
void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
|
||||
{
|
||||
for (unsigned i = state.idx; i < (state.idx + state.dof); i++) {
|
||||
P(i, i) = math::constrain(P(i, i), min, max);
|
||||
if (P(i, i) < min) {
|
||||
P(i, i) = min;
|
||||
|
||||
} else if (P(i, i) > max) {
|
||||
// Constrain the variance growth by fusing zero innovation as clipping the variance
|
||||
// would artifically increase the correlation between states and destabilize the filter.
|
||||
const float innov = 0.f;
|
||||
const float R = 10.f * P(i, i); // This reduces the variance by ~10% as K = P / (P + R)
|
||||
const float innov_var = P(i, i) + R;
|
||||
fuseDirectStateMeasurement(innov, innov_var, R, i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -298,9 +308,7 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max,
|
||||
float limited_max = math::constrain(state_var_max, min, max);
|
||||
float limited_min = math::constrain(limited_max / max_ratio, min, max);
|
||||
|
||||
for (unsigned i = state.idx; i < (state.idx + state.dof); i++) {
|
||||
P(i, i) = math::constrain(P(i, i), limited_min, limited_max);
|
||||
}
|
||||
constrainStateVar(state, limited_min, limited_max);
|
||||
}
|
||||
|
||||
void Ekf::resetQuatCov(const float yaw_noise)
|
||||
|
||||
@ -34,7 +34,7 @@ depends on MODULES_EKF2
|
||||
menuconfig EKF2_AUX_GLOBAL_POSITION
|
||||
depends on MODULES_EKF2
|
||||
bool "aux global position fusion support"
|
||||
default n
|
||||
default y
|
||||
---help---
|
||||
EKF2 auxiliary global position fusion support.
|
||||
|
||||
|
||||
@ -53,7 +53,6 @@ void LoggedTopics::add_default_topics()
|
||||
add_optional_topic("autotune_attitude_control_status", 100);
|
||||
add_optional_topic("camera_capture");
|
||||
add_optional_topic("camera_trigger");
|
||||
add_optional_topic("can_interface_status", 10);
|
||||
add_topic("cellular_status", 200);
|
||||
add_topic("commander_state");
|
||||
add_topic("config_overrides");
|
||||
@ -251,6 +250,10 @@ void LoggedTopics::add_default_topics()
|
||||
add_optional_topic_multi("yaw_estimator_status");
|
||||
|
||||
#endif /* CONFIG_ARCH_BOARD_PX4_SITL */
|
||||
|
||||
#ifdef CONFIG_BOARD_UAVCAN_INTERFACES
|
||||
add_topic_multi("can_interface_status", 100, CONFIG_BOARD_UAVCAN_INTERFACES);
|
||||
#endif
|
||||
}
|
||||
|
||||
void LoggedTopics::add_high_rate_topics()
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-2025 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
|
||||
@ -47,6 +47,7 @@
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/autotune_attitude_control_status.h>
|
||||
#include <uORB/topics/hover_thrust_estimate.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
@ -98,9 +99,11 @@ private:
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
|
||||
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
|
||||
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
|
||||
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
@ -118,8 +121,10 @@ private:
|
||||
|
||||
matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */
|
||||
|
||||
float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */
|
||||
float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */
|
||||
float _hover_thrust{NAN};
|
||||
|
||||
float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */
|
||||
float _man_tilt_max{0.f}; /**< maximum tilt allowed for manual flight [rad] */
|
||||
|
||||
SlewRate<float> _manual_throttle_minimum{0.f}; ///< 0 when landed and ramped to MPC_MANTHR_MIN in air
|
||||
SlewRate<float> _manual_throttle_maximum{0.f}; ///< 0 when disarmed ramped to 1 when spooled up
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-2025 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
|
||||
@ -103,15 +103,31 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
|
||||
{
|
||||
float thrust = 0.f;
|
||||
|
||||
{
|
||||
hover_thrust_estimate_s hte;
|
||||
|
||||
if (_hover_thrust_estimate_sub.update(&hte)) {
|
||||
if (hte.valid) {
|
||||
_hover_thrust = hte.hover_thrust;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_hover_thrust)) {
|
||||
_hover_thrust = _param_mpc_thr_hover.get();
|
||||
}
|
||||
|
||||
// throttle_stick_input is in range [-1, 1]
|
||||
switch (_param_mpc_thr_curve.get()) {
|
||||
case 1: // no rescaling to hover throttle
|
||||
thrust = math::interpolate(throttle_stick_input, -1.f, 1.f,
|
||||
_manual_throttle_minimum.getState(), _param_mpc_thr_max.get());
|
||||
break;
|
||||
|
||||
default: // 0 or other: rescale such that a centered throttle stick corresponds to hover throttle
|
||||
thrust = math::interpolateNXY(throttle_stick_input, {-1.f, 0.f, 1.f},
|
||||
{_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()});
|
||||
default: // 0 or other: rescale to hover throttle at 0 stick input
|
||||
thrust = math::interpolateNXY(throttle_stick_input,
|
||||
{-1.f, 0.f, 1.f},
|
||||
{_manual_throttle_minimum.getState(), _hover_thrust, _param_mpc_thr_max.get()});
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
@ -134,11 +134,6 @@ void MulticopterHoverThrustEstimator::Run()
|
||||
}
|
||||
}
|
||||
|
||||
// new local position setpoint needed every iteration
|
||||
if (!_vehicle_local_position_setpoint_sub.updated()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
@ -166,10 +161,25 @@ void MulticopterHoverThrustEstimator::Run()
|
||||
|
||||
_hover_thrust_ekf.predict(dt);
|
||||
|
||||
vehicle_local_position_setpoint_s local_pos_sp;
|
||||
vehicle_attitude_s vehicle_attitude{};
|
||||
_vehicle_attitude_sub.copy(&vehicle_attitude);
|
||||
|
||||
if (_vehicle_local_position_setpoint_sub.copy(&local_pos_sp)) {
|
||||
if (PX4_ISFINITE(local_pos_sp.thrust[2])) {
|
||||
vehicle_thrust_setpoint_s vehicle_thrust_setpoint;
|
||||
control_allocator_status_s control_allocator_status;
|
||||
|
||||
if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)
|
||||
&& _control_allocator_status_sub.update(&control_allocator_status)
|
||||
&& (hrt_elapsed_time(&vehicle_thrust_setpoint.timestamp) < 20_ms)
|
||||
&& (hrt_elapsed_time(&vehicle_attitude.timestamp) < 20_ms)
|
||||
) {
|
||||
matrix::Vector3f thrust_body_sp(vehicle_thrust_setpoint.xyz);
|
||||
matrix::Vector3f thrust_body_unallocated(control_allocator_status.unallocated_thrust);
|
||||
matrix::Vector3f thrust_body_allocated = thrust_body_sp - thrust_body_unallocated;
|
||||
|
||||
const matrix::Quatf q_att{vehicle_attitude.q};
|
||||
matrix::Vector3f thrust_allocated = q_att.rotateVector(thrust_body_allocated);
|
||||
|
||||
if (PX4_ISFINITE(thrust_allocated(2))) {
|
||||
// Inform the hover thrust estimator about the measured vertical
|
||||
// acceleration (positive acceleration is up) and the current thrust (positive thrust is up)
|
||||
// Guard against fast up and down motions biasing the estimator due to large drag and prop wash effects
|
||||
@ -179,7 +189,7 @@ void MulticopterHoverThrustEstimator::Run()
|
||||
1.f);
|
||||
|
||||
_hover_thrust_ekf.setMeasurementNoiseScale(fmaxf(meas_noise_coeff_xy, meas_noise_coeff_z));
|
||||
_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]);
|
||||
_hover_thrust_ekf.fuseAccZ(-local_pos.az, -thrust_allocated(2));
|
||||
|
||||
bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f);
|
||||
|
||||
@ -191,7 +201,7 @@ void MulticopterHoverThrustEstimator::Run()
|
||||
_valid_hysteresis.set_state_and_update(valid, local_pos.timestamp);
|
||||
_valid = _valid_hysteresis.get_state();
|
||||
|
||||
publishStatus(local_pos.timestamp);
|
||||
publishStatus(vehicle_thrust_setpoint.timestamp);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -54,10 +54,12 @@
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/hover_thrust_estimate.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_thrust_setpoint.h>
|
||||
#include <uORB/topics/control_allocator_status.h>
|
||||
|
||||
#include "zero_order_hover_thrust_ekf.hpp"
|
||||
|
||||
@ -101,9 +103,12 @@ private:
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)};
|
||||
uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
|
||||
|
||||
hrt_abstime _timestamp_last{0};
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user