Skip to content

Commit

Permalink
Remove vehicle specific information from gzBridge
Browse files Browse the repository at this point in the history
  • Loading branch information
@Perrrewi
Perrrewi committed Dec 18, 2024
1 parent a58ad4f commit 8e80c12
Show file tree
Hide file tree
Showing 6 changed files with 54 additions and 158 deletions.
8 changes: 4 additions & 4 deletions ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,8 +65,8 @@ param set-default SIM_GZ_SV_MIN1 0
param set-default SIM_GZ_SV_MAX1 1000
param set-default SIM_GZ_SV_DIS1 500
param set-default SIM_GZ_SV_FAIL1 500
param set-default SIM_GZ_SV_MAXA0 90
param set-default SIM_GZ_SV_MINA0 -90
param set-default SIM_GZ_SV_MAXA1 90
param set-default SIM_GZ_SV_MINA1 -90

# Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204):
# - on minimum when disarmed or failed:
Expand All @@ -75,8 +75,8 @@ param set-default SIM_GZ_SV_MIN2 0
param set-default SIM_GZ_SV_MAX2 1000
param set-default SIM_GZ_SV_DIS2 500
param set-default SIM_GZ_SV_FAIL2 500
param set-default SIM_GZ_SV_MAXA1 90
param set-default SIM_GZ_SV_MINA1 -90
param set-default SIM_GZ_SV_MAXA2 90
param set-default SIM_GZ_SV_MINA2 -90

param set-default CA_SV_CS_COUNT 2

Expand Down
4 changes: 2 additions & 2 deletions ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,8 +47,8 @@ param set-default SIM_GZ_WH_MAX1 200
param set-default SIM_GZ_WH_DIS1 100

# Steering
param set-default SIM_GZ_SV_MAXA0 30
param set-default SIM_GZ_SV_MINA0 -30
param set-default SIM_GZ_SV_MAXA1 30
param set-default SIM_GZ_SV_MINA1 -30
param set-default CA_SV_CS_COUNT 1
param set-default SIM_GZ_SV_FUNC1 201
param set-default SIM_GZ_SV_REV 1
5 changes: 5 additions & 0 deletions ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,11 @@ param set-default SIM_GZ_SV_FUNC3 203
param set-default SIM_GZ_SV_FUNC4 204
param set-default SIM_GZ_SV_FUNC5 205

param set-default SIM_GZ_SV_MAXA4 90
param set-default SIM_GZ_SV_MINA4 0
param set-default SIM_GZ_SV_MAXA5 90
param set-default SIM_GZ_SV_MINA5 0

param set-default NPFG_PERIOD 12
param set-default FW_PR_FF 0.2
param set-default FW_PR_P 0.9
Expand Down
148 changes: 33 additions & 115 deletions src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,151 +33,69 @@

#include "GZMixingInterfaceServo.hpp"

float
GZMixingInterfaceServo::get_tilt_angle_max(const size_t index)
{
switch (index) {
case 0: return _tilt_max_angle_0.get();

case 1: return _tilt_max_angle_1.get();

case 2: return _tilt_max_angle_2.get();

case 3: return _tilt_max_angle_3.get();

default: return NAN;
}
}

float
GZMixingInterfaceServo::get_tilt_angle_min(const size_t index)
{
switch (index) {
case 0: return _tilt_min_angle_0.get();

case 1: return _tilt_min_angle_1.get();

case 2: return _tilt_min_angle_2.get();

case 3: return _tilt_min_angle_3.get();

default: return NAN;
}
}

float
GZMixingInterfaceServo::get_servo_angle_max(const size_t index)
{
switch (index) {
case 0: return _servo_max_angle_0.get();

case 1: return _servo_max_angle_1.get();

case 2: return _servo_max_angle_2.get();

case 3: return _servo_max_angle_3.get();

case 4: return _servo_max_angle_4.get();

case 5: return _servo_max_angle_5.get();

case 6: return _servo_max_angle_6.get();

case 7: return _servo_max_angle_7.get();

default: return NAN;
}
}
float angle;

float
GZMixingInterfaceServo::get_servo_angle_min(const size_t index)
{
switch (index) {
case 0: return _servo_min_angle_0.get();
case 0: {angle = _servo_max_angle_1.get(); break;}

case 1: return _servo_min_angle_1.get();
case 1: {angle = _servo_max_angle_2.get(); break;}

case 2: return _servo_min_angle_2.get();
case 2: {angle = _servo_max_angle_3.get(); break;}

case 3: return _servo_min_angle_3.get();
case 3: {angle = _servo_max_angle_4.get(); break;}

case 4: return _servo_min_angle_4.get();
case 4: {angle = _servo_max_angle_5.get(); break;}

case 5: return _servo_min_angle_5.get();
case 5: {angle = _servo_max_angle_6.get(); break;}

case 6: return _servo_min_angle_6.get();
case 6: {angle = _servo_max_angle_7.get(); break;}

case 7: return _servo_min_angle_7.get();
case 7: {angle = _servo_max_angle_8.get(); break;}

default: return NAN;
default: {angle = NAN; break;}
}
}

int GZMixingInterfaceServo::get_active_output_count()
{
return _control_surface_count.get() + _tilt_count.get();
}

bool GZMixingInterfaceServo::is_tiltrotor(const int index)
{
if (_tilt_count.get() < 1) {
return false;
if (!PX4_ISFINITE(angle)) {
PX4_ERR("max_angle: index out of range, i= %ld, expected i < 8", index);
return NAN;
}

return (index > _control_surface_count.get() - 1) && (index < get_active_output_count());
return math::radians(angle);
}

double GZMixingInterfaceServo::get_angle_max_wrapper(const int index)
float
GZMixingInterfaceServo::get_servo_angle_min(const size_t index)
{
if (index >= get_active_output_count()) {
return NAN;
}
float angle;

double angle;
switch (index) {
case 0: {angle = _servo_min_angle_1.get(); break;}

if (is_tiltrotor(index)) {
angle = (double)get_tilt_angle_max(index - _control_surface_count.get());
case 1: {angle = _servo_min_angle_2.get(); break;}

if (!PX4_ISFINITE(angle)) {
PX4_ERR("max_angle: index out of range, i= %i, expected i < 4", index - _control_surface_count.get());
return NAN;
}
case 2: {angle = _servo_min_angle_3.get(); break;}

} else {
angle = (double)get_servo_angle_max(index);
case 3: {angle = _servo_min_angle_4.get(); break;}

if (!PX4_ISFINITE(angle)) {
PX4_ERR("max_angle: index out of range, i= %i, expected i < 8", index);
return NAN;
}
}

return math::radians(angle);
}
case 4: {angle = _servo_min_angle_5.get(); break;}

double GZMixingInterfaceServo::get_angle_min_wrapper(const int index)
{
if (index >= get_active_output_count()) {
return NAN;
}
case 5: {angle = _servo_min_angle_6.get(); break;}

double angle;
case 6: {angle = _servo_min_angle_7.get(); break;}

if (is_tiltrotor(index)) {
angle = (double)get_tilt_angle_min(index - _control_surface_count.get());
case 7: {angle = _servo_min_angle_8.get(); break;}

if (!PX4_ISFINITE(angle)) {
PX4_ERR("min_angle: index out of range, i= %i, expected i < 4", index - _control_surface_count.get());
return NAN;
}
default: {angle = NAN; break;}

} else {
angle = (double)get_servo_angle_min(index);
}

if (!PX4_ISFINITE(angle)) {
PX4_ERR("min_angle: index out of range, i= %i, expected i < 8", index);
return NAN;
}
if (!PX4_ISFINITE(angle)) {
PX4_ERR("min_angle: index out of range, i= %ld, expected i < 8", index);
return NAN;
}

return math::radians(angle);
Expand All @@ -197,8 +115,8 @@ bool GZMixingInterfaceServo::init(const std::string &model_name)
return false;
}

double min_val = get_angle_min_wrapper(i);
double max_val = get_angle_max_wrapper(i);
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);
}
Expand Down
35 changes: 4 additions & 31 deletions src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,37 +67,20 @@ class GZMixingInterfaceServo : public OutputModuleInterface
friend class GZBridge;

void Run() override;
int get_active_output_count();
float get_tilt_angle_max(const size_t index);
float get_tilt_angle_min(const size_t index);
float get_servo_angle_max(const size_t index);
float get_servo_angle_min(const size_t index);

/**
* @brief Checks if the servo index is configured to tiltrotor.
* Tiltrotors should be ordered after the control surfaces.
* @param index: int
* @return True if the index is as expected, otherwise False.
*/
bool is_tiltrotor(const int index);

/**
* @brief Get maximum configured angle of servo.
* Takes the maximum value from CA_SV_TL{n}_MAXA or SIM_GZ_SV_MAXA{n} param
* dependent on input index.
* @param index: servo index
* @return angle_max [rad]
*/
double get_angle_max_wrapper(const int index);
float get_servo_angle_max(const size_t index);

/**
* @brief Get minimum configured angle of servo.
* Takes the minimum value from CA_SV_TL{n}_MINA or SIM_GZ_SV_MINA{n} param
* dependent on input index.
* @param index: servo index
* @return angle_min [rad]
*/
double get_angle_min_wrapper(const int index);
float get_servo_angle_min(const size_t index);

gz::transport::Node &_node;
pthread_mutex_t &_node_mutex;
Expand All @@ -109,16 +92,6 @@ class GZMixingInterfaceServo : public OutputModuleInterface
std::vector<double> _angular_range_rad;

DEFINE_PARAMETERS(
(ParamFloat<px4::params::CA_SV_TL0_MAXA>) _tilt_max_angle_0,
(ParamFloat<px4::params::CA_SV_TL0_MINA>) _tilt_min_angle_0,
(ParamFloat<px4::params::CA_SV_TL1_MAXA>) _tilt_max_angle_1,
(ParamFloat<px4::params::CA_SV_TL1_MINA>) _tilt_min_angle_1,
(ParamFloat<px4::params::CA_SV_TL2_MAXA>) _tilt_max_angle_2,
(ParamFloat<px4::params::CA_SV_TL2_MINA>) _tilt_min_angle_2,
(ParamFloat<px4::params::CA_SV_TL3_MAXA>) _tilt_max_angle_3,
(ParamFloat<px4::params::CA_SV_TL3_MINA>) _tilt_min_angle_3,
(ParamFloat<px4::params::SIM_GZ_SV_MAXA0>) _servo_max_angle_0,
(ParamFloat<px4::params::SIM_GZ_SV_MINA0>) _servo_min_angle_0,
(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,
Expand All @@ -133,7 +106,7 @@ class GZMixingInterfaceServo : public OutputModuleInterface
(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,
(ParamInt<px4::params::CA_SV_CS_COUNT>) _control_surface_count,
(ParamInt<px4::params::CA_SV_TL_COUNT>) _tilt_count
(ParamFloat<px4::params::SIM_GZ_SV_MAXA8>) _servo_max_angle_8,
(ParamFloat<px4::params::SIM_GZ_SV_MINA8>) _servo_min_angle_8
)
};
12 changes: 6 additions & 6 deletions src/modules/simulation/gz_bridge/module.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,29 +43,29 @@ parameters:
definitions:
SIM_GZ_SV_MAXA${i}:
description:
short: Control Surface ${i} Angle at Maximum
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.
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: 0
instance_start: 1
min: -180.0
max: 180.0
default: 45.0
SIM_GZ_SV_MINA${i}:
description:
short: Control Surface ${i} Angle at Minimum
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.
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: 0
instance_start: 1
min: -180.0
max: 180.0
default: -45.0

0 comments on commit 8e80c12

Please sign in to comment.