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

Temporary RPM control for helicopters

This commit is contained in:
Matthias Grob 2024-03-01 20:52:00 +01:00
parent 28db3e1c8c
commit 2ff9c558f9
19 changed files with 448 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ROMFS/px4fmu_common/init.d/airframes/16001_helicopter
View File

@ -11,7 +11,6 @@
. ${R}etc/init.d/rc.heli_defaults
# Disable PID gains for initial setup. These should be enabled after setting the FF gain.
# P is expected to be lower than FF.
param set-default MC_ROLLRATE_P 0
@ -24,5 +23,3 @@ param set-default MC_PITCHRATE_D 0
param set-default MC_PITCHRATE_FF 0.1
param set-default CA_AIRFRAME 10

1
ROMFS/px4fmu_common/init.d/rcS
View File

@ -388,6 +388,7 @@ else
commander start
dshot start
pwm_input start
pwm_out start
fi

3
boards/px4/fmu-v5x/default.px4board
View File

@ -38,6 +38,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
CONFIG_DRIVERS_POWER_MONITOR_INA228=y
CONFIG_DRIVERS_POWER_MONITOR_INA238=y
CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y
CONFIG_DRIVERS_PWM_INPUT=y
CONFIG_DRIVERS_PWM_OUT=y
CONFIG_DRIVERS_PX4IO=y
CONFIG_COMMON_RC=y
@ -81,7 +82,7 @@ CONFIG_MODULES_NAVIGATOR=y
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
CONFIG_MODULES_PAYLOAD_DELIVERER=y
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_ROVER_POS_CONTROL=y
CONFIG_MODULES_ROVER_POS_CONTROL=n
CONFIG_MODULES_SENSORS=y
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y

1
msg/CMakeLists.txt
View File

@ -173,6 +173,7 @@ set(msg_files
RegisterExtComponentReply.msg
RegisterExtComponentRequest.msg
Rpm.msg
RpmControlStatus.msg
RtlStatus.msg
RtlTimeEstimate.msg
SatelliteInfo.msg

8
msg/PwmInput.msg
View File

@ -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)

7
msg/RpmControlStatus.msg Normal file
View File

@ -0,0 +1,7 @@
uint64 timestamp # time since system start (microseconds)
float32 rpm_raw # measured rpm
float32 rpm_estimate # filtered rpm
float32 rpm_setpoint # desired rpm
float32 output

2
src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp
View File

@ -109,7 +109,7 @@ LidarLitePWM::measure()
return PX4_ERROR;
}
const float current_distance = float(_pwm.pulse_width) * 1e-3f; /* 10 usec = 1 cm distance for LIDAR-Lite */
const float current_distance = float(_pwm.pulse_width) * 1e-3f; /* 1 usec = 1 mm distance for LIDAR-Lite */
/* Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) */
if (current_distance <= 0.0f) {

4
src/drivers/rpm/pcf8583/PCF8583.cpp
View File

@ -195,8 +195,8 @@ void PCF8583::RunImpl()
}
// Calculate RPM and accuracy estimation
float indicated_rpm = (((float)diffCount / _param_pcf8583_magnet.get()) / ((float)diffTime / 1000000.f)) * 60.f;
float estimated_accurancy = 1 / (float)_param_pcf8583_magnet.get() / ((float)diffTime / 1000000) * 60.f;
float indicated_rpm = (((float)diffCount / _param_pcf8583_magnet.get()) / ((float)diffTime / 1e6f)) * 60.f;
float estimated_accurancy = 1 / (float)_param_pcf8583_magnet.get() / ((float)diffTime / 1e6f) * 60.f;
// publish data to uorb
rpm_s msg{};

12
src/lib/pid/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
# Copyright (c) 2022 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
@ -31,4 +31,12 @@
#
############################################################################
px4_add_library(pid pid.cpp)
px4_add_library(PID
PID.cpp
PID.hpp
)
target_include_directories(PID PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
px4_add_unit_gtest(SRC PIDTest.cpp LINKLIBS PID)
px4_add_library(pid pid.cpp) # TODO: Remove duplication

65
src/lib/pid/PID.cpp Normal file
View File

@ -0,0 +1,65 @@
/****************************************************************************
*
* Copyright (c) 2022-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 "PID.hpp"
#include "lib/mathlib/math/Functions.hpp"
void PID::setGains(const float P, const float I, const float D)
{
_gain_proportional = P;
_gain_integral = I;
_gain_derivative = D;
}
float PID::update(const float feedback, const float dt, const bool update_integral)
{
const float error = _setpoint - feedback;
const float feedback_change = std::isfinite(_last_feedback) ? (feedback - _last_feedback) / dt : 0.f;
const float output = (_gain_proportional * error) + _integral + (_gain_derivative * feedback_change);
if (update_integral) {
updateIntegral(error, dt);
}
_last_feedback = feedback;
return math::constrain(output, -_limit_output, _limit_output);
}
void PID::updateIntegral(float error, const float dt)
{
const float integral_new = _integral + _gain_integral * error * dt;
if (std::isfinite(integral_new)) {
_integral = math::constrain(integral_new, -_limit_integral, _limit_integral);
}
}

61
src/lib/pid/PID.hpp Normal file
View File

@ -0,0 +1,61 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <cmath>
class PID
{
public:
void setOutputLimit(const float limit) { _limit_output = limit; }
void setIntegralLimit(const float limit) { _limit_integral = limit; }
void setGains(const float P, const float I, const float D);
void setSetpoint(const float setpoint) { _setpoint = setpoint; }
float update(const float feedback, const float dt, const bool update_integral);
void resetIntegral() { _integral = 0.f; };
void resetDerivative() { _last_feedback = NAN; };
private:
void updateIntegral(float error, const float dt);
float _setpoint{0.f}; ///< current setpoint to track
float _integral{0.f}; ///< integral state
float _last_feedback{NAN};
// Gains, Limits
float _gain_proportional{0.f};
float _gain_integral{0.f};
float _gain_derivative{0.f};
float _limit_integral{0.f};
float _limit_output{0.f};
};

129
src/lib/pid/PIDTest.cpp Normal file
View File

@ -0,0 +1,129 @@
/****************************************************************************
*
* Copyright (C) 2022 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 <gtest/gtest.h>
#include <PID.hpp>
TEST(PIDTest, AllZeroCase)
{
PID pid;
EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
}
TEST(PIDTest, OutputLimit)
{
PID pid;
pid.setOutputLimit(.01f);
pid.setGains(.1f, 0.f, 0.f);
pid.setSetpoint(1.f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .01f);
EXPECT_FLOAT_EQ(pid.update(.9f, 0.f, false), .01f);
EXPECT_NEAR(pid.update(.95f, 0.f, false), .005f, 1e-6f);
EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
EXPECT_NEAR(pid.update(1.05f, 0.f, false), -.005f, 1e-6f);
EXPECT_FLOAT_EQ(pid.update(1.1f, 0.f, false), -.01f);
EXPECT_FLOAT_EQ(pid.update(1.15f, 0.f, false), -.01f);
EXPECT_FLOAT_EQ(pid.update(2.f, 0.f, false), -.01f);
}
TEST(PIDTest, ProportinalOnly)
{
PID pid;
pid.setOutputLimit(1.f);
pid.setGains(.1f, 0.f, 0.f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
pid.setSetpoint(1.f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .1f);
EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
float plant = 0.f;
float output = 10000.f;
int i; // need function scope to check how many steps
for (i = 1000; i > 0; i--) {
const float output_new = pid.update(plant, 0.f, false);
plant += output_new;
// expect the output to get smaller with each iteration
if (output_new >= output) {
break;
}
output = output_new;
}
EXPECT_FLOAT_EQ(plant, 1.f);
EXPECT_GT(i, 0); // it shouldn't have taken longer than an iteration timeout to converge
}
TEST(PIDTest, InteralOpenLoop)
{
PID pid;
pid.setOutputLimit(1.f);
pid.setGains(0.f, .1f, 0.f);
pid.setIntegralLimit(.05f);
pid.setSetpoint(1.f);
// Zero error
EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
// Open loop ramp up
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .01f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
// Open loop ramp down
pid.setSetpoint(-1.f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
EXPECT_NEAR(pid.update(0.f, 0.1f, true), .01f, 1e-6f);
EXPECT_NEAR(pid.update(0.f, 0.1f, true), 0.f, 1e-6f);
EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.01f, 1e-6f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.02f);
EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.03f, 1e-6f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.04f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
pid.resetIntegral();
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.01f);
}

4
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
View File

@ -135,8 +135,8 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
_saturation_flags = {};
// 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()) * throttleSpoolupProgress();
const float collective_pitch = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.pitch_curve);
// actuator mapping

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

@ -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};
};

2
src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt
View File

@ -62,6 +62,7 @@ px4_add_library(ActuatorEffectiveness
ActuatorEffectivenessTailsitterVTOL.hpp
ActuatorEffectivenessRoverAckermann.hpp
ActuatorEffectivenessRoverAckermann.cpp
RpmControl.hpp
)
target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@ -69,6 +70,7 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D
target_link_libraries(ActuatorEffectiveness
PRIVATE
mathlib
PID
)
px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness)

118
src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp Normal file
View File

@ -0,0 +1,118 @@
/****************************************************************************
*
* 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/mathlib/math/filter/AlphaFilter.hpp>
#include <lib/pid/PID.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/pwm_input.h>
#include <uORB/topics/rpm_control_status.h>
class RpmControl : public ModuleParams
{
public:
RpmControl(ModuleParams *parent) : ModuleParams(parent) {};
~RpmControl() = default;
float getActuatorCorrection()
{
if (_pwm_input_sub.updated()) {
pwm_input_s pwm_input{};
if (_pwm_input_sub.copy(&pwm_input)) {
// 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute
if ((1 < pwm_input.period) && (pwm_input.period < 10000)) {
_rpm_raw = 60.f * 1e6f / (static_cast<float>(pwm_input.period) * 18.f);
} else {
_rpm_raw = 0;
}
const float dt = math::constrain((pwm_input.timestamp - _timestamp_last_rpm_measurement) * 1e-6f, 0.01f, 1.f);
_timestamp_last_rpm_measurement = pwm_input.timestamp;
_rpm_filter.setParameters(dt, 0.5f);
_rpm_filter.update(_rpm_raw);
}
}
hrt_abstime now = hrt_absolute_time();
const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f);
_timestamp_last_update = now;
_pid.setGains(_param_ca_heli_rpm_p.get() * 1e-3f, _param_ca_heli_rpm_i.get() * 1e-3f, 0.f);
_pid.setOutputLimit(.5f);
_pid.setIntegralLimit(.5f);
_pid.setSetpoint(_param_ca_heli_rpm_sp.get());
float output = _pid.update(_rpm_filter.getState(), dt, true);
//float output = _param_ca_heli_rpm_p.get() * (_rpm_setpoint - _rpm_filter.getState()) * 1e-3f;
rpm_control_status_s rpm_control_status{};
rpm_control_status.rpm_raw = _rpm_raw;
rpm_control_status.rpm_estimate = _rpm_filter.getState();
rpm_control_status.rpm_setpoint = _param_ca_heli_rpm_sp.get();
rpm_control_status.output = output;
rpm_control_status.timestamp = hrt_absolute_time();
_rpm_control_status_pub.publish(rpm_control_status);
return output;
}
private:
uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)};
uORB::Publication<rpm_control_status_s> _rpm_control_status_pub{ORB_ID(rpm_control_status)};
float _rpm_raw{0.f};
AlphaFilter<float> _rpm_filter;
PID _pid;
hrt_abstime _timestamp_last_update{0};
hrt_abstime _timestamp_last_rpm_measurement{0};
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
)
};

2
src/modules/control_allocator/ControlAllocator.cpp
View File

@ -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(&param_update);

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

@ -527,6 +527,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:

3
src/modules/logger/logged_topics.cpp
View File

@ -48,6 +48,7 @@ void LoggedTopics::add_default_topics()
add_topic("action_request");
add_topic("actuator_armed");
add_optional_topic("actuator_controls_status_0", 300);
add_topic("actuator_test");
add_topic("airspeed", 1000);
add_optional_topic("airspeed_validated", 200);
add_optional_topic("autotune_attitude_control_status", 100);
@ -99,8 +100,10 @@ void LoggedTopics::add_default_topics()
add_topic("position_controller_landing_status", 100);
add_topic("goto_setpoint", 200);
add_topic("position_setpoint_triplet", 200);
add_topic("pwm_input");
add_optional_topic("px4io_status");
add_topic("radio_status");
add_optional_topic_multi("rpm_control_status");
add_topic("rtl_time_estimate", 1000);
add_optional_topic("rtl_status", 5000);
add_optional_topic("sensor_airflow", 100);