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

Adding min-max angles to use for normalized servo outputs

Browse Source
This commit is contained in:
Pernilla
2024-12-19 09:57:55 +01:00
committed by Silvan Fuhrer
parent f1855de006
commit be42c7c88a
3 changed files with 149 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp
+76 -2
View File
@@ -33,6 +33,74 @@
#include "GZMixingInterfaceServo.hpp"
float
GZMixingInterfaceServo::get_servo_angle_max(const size_t index)
{
float angle;
switch (index) {
case 0: {angle = _servo_max_angle_1.get(); break;}
case 1: {angle = _servo_max_angle_2.get(); break;}
case 2: {angle = _servo_max_angle_3.get(); break;}
case 3: {angle = _servo_max_angle_4.get(); break;}
case 4: {angle = _servo_max_angle_5.get(); break;}
case 5: {angle = _servo_max_angle_6.get(); break;}
case 6: {angle = _servo_max_angle_7.get(); break;}
case 7: {angle = _servo_max_angle_8.get(); break;}
default: {angle = NAN; break;}
}
if (!PX4_ISFINITE(angle)) {
PX4_ERR("max_angle: index out of range, i= %ld, expected i < 8", index);
return NAN;
}
return math::radians(angle);
}
float
GZMixingInterfaceServo::get_servo_angle_min(const size_t index)
{
float angle;
switch (index) {
case 0: {angle = _servo_min_angle_1.get(); break;}
case 1: {angle = _servo_min_angle_2.get(); break;}
case 2: {angle = _servo_min_angle_3.get(); break;}
case 3: {angle = _servo_min_angle_4.get(); break;}
case 4: {angle = _servo_min_angle_5.get(); break;}
case 5: {angle = _servo_min_angle_6.get(); break;}
case 6: {angle = _servo_min_angle_7.get(); break;}
case 7: {angle = _servo_min_angle_8.get(); break;}
default: {angle = NAN; break;}
}
if (!PX4_ISFINITE(angle)) {
PX4_ERR("min_angle: index out of range, i= %ld, expected i < 8", index);
return NAN;
}
return math::radians(angle);
}
bool GZMixingInterfaceServo::init(const std::string &model_name)
{
// /model/rascal_110_0/servo_2
@@ -46,6 +114,11 @@ bool GZMixingInterfaceServo::init(const std::string &model_name)
PX4_ERR("failed to advertise %s", servo_topic.c_str());
return false;
}
double min_val = get_servo_angle_min(i);
double max_val = get_servo_angle_max(i);
_angle_min_rad.push_back(min_val);
_angular_range_rad.push_back(max_val - min_val);
}
ScheduleNow();
@@ -64,8 +137,9 @@ bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MA
for (auto &servo_pub : _servos_pub) {
if (_mixing_output.isFunctionSet(i)) {
gz::msgs::Double servo_output;
///TODO: Normalize output data
double output = (outputs[i] - 500) / 500.0;
double output_range = _mixing_output.maxValue(i) - _mixing_output.minValue(i);
double output = _angle_min_rad[i] + _angular_range_rad[i] * (outputs[i] - _mixing_output.minValue(i)) / output_range;
// std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl;
// std::cout << " output: " << output << std::endl;
servo_output.set_data(output);
src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp
+36
View File
@@ -36,6 +36,7 @@
#include <lib/mixer_module/mixer_module.hpp>
#include <gz/transport.hh>
#include <px4_platform_common/module_params.h>
// GZBridge mixing class for Servos.
// It is separate from GZBridge to have separate WorkItems and therefore allowing independent scheduling
@@ -67,10 +68,45 @@ private:
void Run() override;
/**
* @brief Get maximum configured angle of servo.
* @param index: servo index
* @return angle_max [rad]
*/
float get_servo_angle_max(const size_t index);
/**
* @brief Get minimum configured angle of servo.
* @param index: servo index
* @return angle_min [rad]
*/
float get_servo_angle_min(const size_t index);
gz::transport::Node &_node;
pthread_mutex_t &_node_mutex;
MixingOutput _mixing_output{"SIM_GZ_SV", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
std::vector<gz::transport::Node::Publisher> _servos_pub;
std::vector<double> _angle_min_rad;
std::vector<double> _angular_range_rad;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::SIM_GZ_SV_MAXA1>) _servo_max_angle_1,
(ParamFloat<px4::params::SIM_GZ_SV_MINA1>) _servo_min_angle_1,
(ParamFloat<px4::params::SIM_GZ_SV_MAXA2>) _servo_max_angle_2,
(ParamFloat<px4::params::SIM_GZ_SV_MINA2>) _servo_min_angle_2,
(ParamFloat<px4::params::SIM_GZ_SV_MAXA3>) _servo_max_angle_3,
(ParamFloat<px4::params::SIM_GZ_SV_MINA3>) _servo_min_angle_3,
(ParamFloat<px4::params::SIM_GZ_SV_MAXA4>) _servo_max_angle_4,
(ParamFloat<px4::params::SIM_GZ_SV_MINA4>) _servo_min_angle_4,
(ParamFloat<px4::params::SIM_GZ_SV_MAXA5>) _servo_max_angle_5,
(ParamFloat<px4::params::SIM_GZ_SV_MINA5>) _servo_min_angle_5,
(ParamFloat<px4::params::SIM_GZ_SV_MAXA6>) _servo_max_angle_6,
(ParamFloat<px4::params::SIM_GZ_SV_MINA6>) _servo_min_angle_6,
(ParamFloat<px4::params::SIM_GZ_SV_MAXA7>) _servo_max_angle_7,
(ParamFloat<px4::params::SIM_GZ_SV_MINA7>) _servo_min_angle_7,
(ParamFloat<px4::params::SIM_GZ_SV_MAXA8>) _servo_max_angle_8,
(ParamFloat<px4::params::SIM_GZ_SV_MINA8>) _servo_min_angle_8
)
};
src/modules/simulation/gz_bridge/module.yaml
+37 -2
View File
@@ -1,3 +1,6 @@
__max_num_servos: &max_num_servos 8
__max_num_tilts: &max_num_tilts 4
module_name: SIM_GZ
actuator_output:
show_subgroups_if: 'SIM_GZ_EN>=1'
@@ -15,7 +18,7 @@ actuator_output:
min: { min: 0, max: 1000, default: 0 }
max: { min: 0, max: 1000, default: 1000 }
failsafe: { min: 0, max: 1000 }
num_channels: 8
num_channels: *max_num_servos
- param_prefix: SIM_GZ_SV
group_label: 'Servos'
channel_label: 'Servo'
@@ -24,7 +27,7 @@ actuator_output:
min: { min: 0, max: 1000, default: 0 }
max: { min: 0, max: 1000, default: 1000 }
failsafe: { min: 0, max: 1000 }
num_channels: 8
num_channels: *max_num_servos
- param_prefix: SIM_GZ_WH
group_label: 'Wheels'
channel_label: 'Wheels'
@@ -34,3 +37,35 @@ actuator_output:
max: { min: 0, max: 200, default: 200 }
failsafe: { min: 0, max: 200 }
num_channels: 4
parameters:
- group: Geometry
definitions:
SIM_GZ_SV_MAXA${i}:
description:
short: Servo ${i} Angle at Maximum
long: |
Defines the angle when the servo is at the maximum.
Currently only supported in gz simulation and must be coherent with .sdf file and CA_SV_TL{n}_MAXA.
type: float
decimal: 0
unit: 'deg'
num_instances: *max_num_servos
instance_start: 1
min: -180.0
max: 180.0
default: 45.0
SIM_GZ_SV_MINA${i}:
description:
short: Servo ${i} Angle at Minimum
long: |
Defines the angle when the servo is at the minimum.
Currently only supported in gz simulation and must be coherent with .sdf file and CA_SV_TL{n}_MINA.
type: float
decimal: 0
unit: 'deg'
num_instances: *max_num_servos
instance_start: 1
min: -180.0
max: 180.0
default: -45.0