Skip to content

Commit

Permalink
Make a cleaner explanation
Browse files Browse the repository at this point in the history
  • Loading branch information
Perrrewi committed Dec 17, 2024
1 parent 7791522 commit 6a3d9db
Show file tree
Hide file tree
Showing 3 changed files with 27 additions and 30 deletions.
34 changes: 6 additions & 28 deletions Tools/module_config/generate_params.py
Original file line number Diff line number Diff line change
Expand Up @@ -282,34 +282,12 @@ def add_local_param(param_name, param_def):
Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set.
'''
if param_prefix == 'SIM_GZ_SV':
minimum_description = \
'''
Servo output represents angles [0, 360] deg scaled with 10 for numerical precision,
with an offset of -180 deg to allow for negative angles.
Hence, myMin = (180 + myOffset) * 10 resulting angles within [-180,180] deg.
Min max must be coherent with joint limits in the airframe/model.sdf
Ex. ailerons with a minimum angle -45 deg would be 1350.
'''
maximum_description = \
'''
Servo output represents angles [0, 360] deg scaled with 10 for numerical precision,
with an offset of -180 deg to allow for negative angles.
Hence, myMin = (180 + myOffset) * 10 resulting angles within [-180,180] deg.
Min max must be coherent with joint limits in the airframe/model.sdf
Ex. ailerons with a maximum angle 45 deg would be 2250.
'''
else:
minimum_description = \
'''Minimum output value (when not disarmed).
'''
maximum_description = \
'''Maxmimum output value (when not disarmed).
'''
minimum_description = \
'''Minimum output value (when not disarmed).
'''
maximum_description = \
'''Maxmimum output value (when not disarmed).
'''
failsafe_description = \
'''This is the output value that is set when in failsafe mode.
Expand Down
18 changes: 16 additions & 2 deletions src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,6 @@

#include "GZMixingInterfaceServo.hpp"

#define SERVO_OUTPUT_SCALING (10.)
#define SERVO_OUTPUT_OFFSET (180.)

bool GZMixingInterfaceServo::init(const std::string &model_name)
{
Expand All @@ -56,6 +54,22 @@ bool GZMixingInterfaceServo::init(const std::string &model_name)
return true;
}

/**
*@brief Servo output are mapped from ms into deg.
* The outputs are uint16 and to allow for decimals the
* range is scaled by SERVO_OUTPUT_SCALING * [0, 360] deg.
* With an offset SERVO_OUTPUT_OFFSET the outputs can be mapped into negative angles.
*
* Hence,
* SIM_GZ_SV_MIN{n} = (SERVO_OUTPUT_OFFSET + myAngularOffsetDeg) * SERVO_OUTPUT_SCALING
* results with angles within [-180,180] deg.
*
* Ex. ailerons with a myAngularOffsetDeg of +-45 deg would be
* SIM_GZ_SV_MIN{n} = 1350 and SIM_GZ_SV_MAX{n} = 2250.
*
* Note: SIM_GZ_SV_MIN{n} and SIM_GZ_SV_MAX{n} must be coherent with
* joint limits in the airframe/model.sdf
*/
bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
Expand Down
5 changes: 5 additions & 0 deletions src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,11 @@ class GZMixingInterfaceServo : public OutputModuleInterface

void Run() override;

// Scales the servo outputs into the range 360 deg
static constexpr double SERVO_OUTPUT_SCALING = 10.;
// Offsets the servo value in degrees into a range of +-180 deg
static constexpr double SERVO_OUTPUT_OFFSET = 180.;

gz::transport::Node &_node;
pthread_mutex_t &_node_mutex;

Expand Down

0 comments on commit 6a3d9db

Please sign in to comment.