Skip to content

Commit

Permalink
introduce sanity checks
Browse files Browse the repository at this point in the history
  • Loading branch information
@chfriedrich98
chfriedrich98 committed Feb 7, 2025
1 parent e226138 commit b4b1d50
Show file tree
Hide file tree
Showing 6 changed files with 198 additions and 44 deletions.
73 changes: 55 additions & 18 deletions src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,13 +75,13 @@ void AckermannAttControl::updateAttControl()
_vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi();
}

if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed) {
if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
// Estimate forward speed based on throttle
if (_actuator_motors_sub.updated()) {
actuator_motors_s actuator_motors;
_actuator_motors_sub.copy(&actuator_motors);
_estimated_speed_body_x = _param_ro_max_thr_speed.get() > FLT_EPSILON ? math::interpolate<float>
(actuator_motors.control[0], -1.f, 1.f, -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()) : 0.f;
_estimated_speed_body_x = math::interpolate<float> (actuator_motors.control[0], -1.f, 1.f,
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
}

if (_vehicle_control_mode.flag_control_manual_enabled) {
Expand Down Expand Up @@ -111,10 +111,8 @@ void AckermannAttControl::generateAttitudeSetpoint()

if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode
manual_control_setpoint_s manual_control_setpoint{};
bool necessary_parameters_set = _max_yaw_rate > FLT_EPSILON
&& _param_ro_max_thr_speed.get() > FLT_EPSILON;

if (_manual_control_setpoint_sub.update(&manual_control_setpoint) && necessary_parameters_set) {
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {

rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp;
Expand Down Expand Up @@ -169,18 +167,9 @@ void AckermannAttControl::generateRateSetpoint()
}

// Calculate yaw rate limit for slew rate
float yaw_slew_rate{0.f};

if (_param_ra_wheel_base.get() > FLT_EPSILON && _param_ra_max_str_ang.get() > FLT_EPSILON
&& PX4_ISFINITE(_estimated_speed_body_x)) {
float max_possible_yaw_rate = _param_ra_wheel_base.get() > FLT_EPSILON ? fabsf(_estimated_speed_body_x) * tanf(
_param_ra_max_str_ang.get()) / _param_ra_wheel_base.get() :
_max_yaw_rate; // Maximum possible yaw rate at current velocity
yaw_slew_rate = math::min(max_possible_yaw_rate, _max_yaw_rate);

} else {
yaw_slew_rate = _max_yaw_rate;
}
float max_possible_yaw_rate = fabsf(_estimated_speed_body_x) * tanf(_param_ra_max_str_ang.get()) /
_param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity
float yaw_slew_rate = math::min(max_possible_yaw_rate, _max_yaw_rate);

float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, yaw_slew_rate,
_vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt);
Expand All @@ -191,3 +180,51 @@ void AckermannAttControl::generateRateSetpoint()
rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate);
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
}

bool AckermannAttControl::runSanityChecks()
{
bool ret = true;

if (_param_ro_max_thr_speed.get() < FLT_EPSILON) {
ret = false;

if (_sanity_check) {
events::send<float>(events::ID("ackermann_att_control_conf_invalid_max_thr_speed"), events::Log::Error,
"Invalid configuration of necessary parameter RO_MAX_THR_SPEED", _param_ro_max_thr_speed.get());
}

}

if (_param_ra_wheel_base.get() < FLT_EPSILON) {
ret = false;

if (_sanity_check) {
events::send<float>(events::ID("ackermann_att_control_conf_invalid_wheel_base"), events::Log::Error,
"Invalid configuration of necessary parameter RA_WHEEL_BASE", _param_ra_wheel_base.get());
}

}

if (_param_ra_max_str_ang.get() < FLT_EPSILON) {
ret = false;

if (_sanity_check) {
events::send<float>(events::ID("ackermann_att_control_conf_invalid_max_str_ang"), events::Log::Error,
"Invalid configuration of necessary parameter RA_MAX_STR_ANG", _param_ra_max_str_ang.get());
}

}

if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) {
ret = false;

if (_sanity_check) {
events::send<float>(events::ID("ackermann_att_control_conf_invalid_yaw_rate_lim"), events::Log::Error,
"Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get());
}

}

_sanity_check = ret;
return ret;
}
24 changes: 17 additions & 7 deletions src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,9 @@

// PX4 includes
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/events.h>

// Library includes
#include <lib/rover_control/RoverControl.hpp>
#include <lib/pid/PID.hpp>
#include <lib/slew_rate/SlewRateYaw.hpp>
Expand Down Expand Up @@ -88,6 +91,12 @@ class AckermannAttControl : public ModuleParams
*/
void generateRateSetpoint();

/**
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
bool runSanityChecks();

// uORB subscriptions
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
Expand All @@ -113,20 +122,21 @@ class AckermannAttControl : public ModuleParams
float _max_yaw_rate{0.f};
float _estimated_speed_body_x{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint, _estimated_speed_body_x]
between [0, 0] and [1, _param_ro_max_thr_speed].*/
bool _stab_yaw_ctl{false}; // Indicates if rover is doing yaw control in stab mode
float _stab_yaw_setpoint{0.f}; // Yaw setpoint if rover is doing yaw control in stab mode
bool _stab_yaw_ctl{false}; // Indicates if rover is doing yaw control in stab mode
bool _sanity_check{true};

// Controllers
PID _pid_yaw;
SlewRateYaw<float> _adjusted_yaw_setpoint;

// Parameters
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_P>) _param_ro_yaw_p,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_P>) _param_ro_yaw_p,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz
)
};
56 changes: 51 additions & 5 deletions src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ void AckermannPosVelControl::updatePosVelControl()
updateSubscriptions();

if ((_vehicle_control_mode.flag_control_position_enabled || _vehicle_control_mode.flag_control_velocity_enabled)
&& _vehicle_control_mode.flag_armed) {
&& _vehicle_control_mode.flag_armed && runSanityChecks()) {
generateAttitudeSetpoint();
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp;
Expand Down Expand Up @@ -164,9 +164,8 @@ void AckermannPosVelControl::generateAttitudeSetpoint()
void AckermannPosVelControl::manualPositionMode()
{
manual_control_setpoint_s manual_control_setpoint{};
const bool necessary_parameters_set = _max_yaw_rate > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON;

if (_manual_control_setpoint_sub.update(&manual_control_setpoint) && necessary_parameters_set) {
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
_speed_body_x_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
-1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
const float yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
Expand Down Expand Up @@ -298,8 +297,7 @@ void AckermannPosVelControl::updateWaypointsAndAcceptanceRadius()
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);

RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
_curr_pos_ned,
_home_position, _global_ned_proj_ref);
_curr_pos_ned, _home_position, _global_ned_proj_ref);

_prev_waypoint_transition_angle = _waypoint_transition_angle;
_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
Expand Down Expand Up @@ -388,3 +386,51 @@ float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, cons
}

}

bool AckermannPosVelControl::runSanityChecks()
{
bool ret = true;

if (_param_ro_max_thr_speed.get() < FLT_EPSILON) {
ret = false;

if (_sanity_check) {
events::send<float>(events::ID("ackermann_posVel_control_conf_invalid_max_thr_speed"), events::Log::Error,
"Invalid configuration of necessary parameter RO_MAX_THR_SPEED", _param_ro_max_thr_speed.get());
}

}

if (_param_ra_wheel_base.get() < FLT_EPSILON) {
ret = false;

if (_sanity_check) {
events::send<float>(events::ID("ackermann_posVel_control_conf_invalid_wheel_base"), events::Log::Error,
"Invalid configuration of necessary parameter RA_WHEEL_BASE", _param_ra_wheel_base.get());
}

}

if (_param_ra_max_str_ang.get() < FLT_EPSILON) {
ret = false;

if (_sanity_check) {
events::send<float>(events::ID("ackermann_posVel_control_conf_invalid_max_str_ang"), events::Log::Error,
"Invalid configuration of necessary parameter RA_MAX_STR_ANG", _param_ra_max_str_ang.get());
}

}

if (_param_ro_speed_limit.get() < FLT_EPSILON) {
ret = false;

if (_sanity_check) {
events::send<float>(events::ID("ackermann_posVel_control_conf_invalid_speed_lim"), events::Log::Error,
"Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_yaw_rate_limit.get());
}

}

_sanity_check = ret;
return ret;
}
10 changes: 10 additions & 0 deletions src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,9 @@

// PX4 includes
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/events.h>

// Library includes
#include <lib/rover_control/RoverControl.hpp>
#include <lib/pid/PID.hpp>
#include <matrix/matrix/math.hpp>
Expand Down Expand Up @@ -167,6 +170,12 @@ class AckermannPosVelControl : public ModuleParams
float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_decel, float max_jerk, int nav_state,
float waypoint_transition_angle, float prev_waypoint_transition_angle, float max_speed);

/**
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
bool runSanityChecks();

// uORB subscriptions
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
Expand Down Expand Up @@ -205,6 +214,7 @@ class AckermannPosVelControl : public ModuleParams
int _nav_state{0};
bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode.
bool _mission_finished{false};
bool _sanity_check{true};

// Waypoint variables
Vector2d _home_position{};
Expand Down
71 changes: 57 additions & 14 deletions src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,13 +71,13 @@ void AckermannRateControl::updateRateControl()
vehicle_angular_velocity.xyz[2] : 0.f;
}

if (_vehicle_control_mode.flag_control_rates_enabled && _vehicle_control_mode.flag_armed) {
if (_vehicle_control_mode.flag_control_rates_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
// Estimate forward speed based on throttle
if (_actuator_motors_sub.updated()) {
actuator_motors_s actuator_motors;
_actuator_motors_sub.copy(&actuator_motors);
_estimated_speed_body_x = _param_ro_max_thr_speed.get() > FLT_EPSILON ? math::interpolate<float>
(actuator_motors.control[0], -1.f, 1.f, -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()) : 0.f;
_estimated_speed_body_x = math::interpolate<float>(actuator_motors.control[0], -1.f, 1.f,
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
_estimated_speed_body_x = fabsf(_estimated_speed_body_x) > _param_ro_speed_th.get() ? _estimated_speed_body_x : 0.f;
}

Expand Down Expand Up @@ -111,18 +111,15 @@ void AckermannRateControl::generateRateSetpoint()
manual_control_setpoint_s manual_control_setpoint{};

if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
bool necessary_parameters_set = _max_yaw_rate > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON;
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp;
rover_throttle_setpoint.throttle_body_x = necessary_parameters_set ? manual_control_setpoint.throttle : 0.f;
rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
rover_throttle_setpoint.throttle_body_y = 0.f;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = _timestamp;
rover_rate_setpoint.yaw_rate_setpoint = necessary_parameters_set ? matrix::sign(_estimated_speed_body_x) *
math::interpolate<float>
(manual_control_setpoint.roll,
-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate) : 0.f;
rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_estimated_speed_body_x) * math::interpolate<float>
(manual_control_setpoint.roll, -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
}

Expand All @@ -140,9 +137,8 @@ void AckermannRateControl::generateSteeringSetpoint()

if (fabsf(_estimated_speed_body_x) > FLT_EPSILON) {
// Set up feasible yaw rate setpoint
float max_possible_yaw_rate = _param_ra_wheel_base.get() > FLT_EPSILON ? fabsf(_estimated_speed_body_x) * tanf(
_param_ra_max_str_ang.get()) / _param_ra_wheel_base.get() :
_max_yaw_rate; // Maximum possible yaw rate at current velocity
float max_possible_yaw_rate = fabsf(_estimated_speed_body_x) * tanf(_param_ra_max_str_ang.get()) /
_param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity
float yaw_rate_limit = math::min(max_possible_yaw_rate, _max_yaw_rate);
float constrained_yaw_rate = math::constrain(_rover_rate_setpoint.yaw_rate_setpoint, -yaw_rate_limit, yaw_rate_limit);

Expand All @@ -159,8 +155,7 @@ void AckermannRateControl::generateSteeringSetpoint()
}

// Feed forward
steering_setpoint = fabsf(_estimated_speed_body_x) > FLT_EPSILON ? atanf(_yaw_rate_setpoint.getState() *
_param_ra_wheel_base.get() / _estimated_speed_body_x) : 0.f;
steering_setpoint = atanf(_yaw_rate_setpoint.getState() * _param_ra_wheel_base.get() / _estimated_speed_body_x);

// Feedback (Only when driving forwards because backwards driving is NMP and can introduce instability)
if (_estimated_speed_body_x > FLT_EPSILON) {
Expand All @@ -178,3 +173,51 @@ void AckermannRateControl::generateSteeringSetpoint()
-_param_ra_max_str_ang.get(), _param_ra_max_str_ang.get(), -1.f, 1.f); // Normalize steering setpoint
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
}

bool AckermannRateControl::runSanityChecks()
{
bool ret = true;

if (_param_ro_max_thr_speed.get() < FLT_EPSILON) {
ret = false;

if (_sanity_check) {
events::send<float>(events::ID("ackermann_rate_control_conf_invalid_max_thr_speed"), events::Log::Error,
"Invalid configuration of necessary parameter RO_MAX_THR_SPEED", _param_ro_max_thr_speed.get());
}

}

if (_param_ra_wheel_base.get() < FLT_EPSILON) {
ret = false;

if (_sanity_check) {
events::send<float>(events::ID("ackermann_rate_control_conf_invalid_wheel_base"), events::Log::Error,
"Invalid configuration of necessary parameter RA_WHEEL_BASE", _param_ra_wheel_base.get());
}

}

if (_param_ra_max_str_ang.get() < FLT_EPSILON) {
ret = false;

if (_sanity_check) {
events::send<float>(events::ID("ackermann_rate_control_conf_invalid_max_str_ang"), events::Log::Error,
"Invalid configuration of necessary parameter RA_MAX_STR_ANG", _param_ra_max_str_ang.get());
}

}

if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) {
ret = false;

if (_sanity_check) {
events::send<float>(events::ID("ackermann_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error,
"Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get());
}

}

_sanity_check = ret;
return ret;
}
Loading

0 comments on commit b4b1d50

Please sign in to comment.