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

uuv_att_control: removed redundant code, switched to new uORB API

This commit is contained in:
Thies Lennart Alff 2020-08-12 19:16:28 +02:00 committed by GitHub
parent 6937dbc5fd
commit d7d8aa9b64
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 103 additions and 255 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

283
src/modules/uuv_att_control/uuv_att_control.cpp
View File

@ -59,8 +59,9 @@ extern "C" __EXPORT int uuv_att_control_main(int argc, char *argv[]);
UUVAttitudeControl::UUVAttitudeControl():
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::attitude_ctrl),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "gnda_dt"))
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
}
@ -69,6 +70,16 @@ UUVAttitudeControl::~UUVAttitudeControl()
perf_free(_loop_perf);
}
bool UUVAttitudeControl::init()
{
if (!_vehicle_attitude_sub.registerCallback()) {
PX4_ERR("vehicle_attitude callback registration failed!");
return false;
}
return true;
}
void UUVAttitudeControl::parameters_update(bool force)
{
// check for parameter updates
@ -82,42 +93,6 @@ void UUVAttitudeControl::parameters_update(bool force)
}
}
void UUVAttitudeControl::vehicle_control_mode_poll()
{
bool updated = false;
orb_check(_vcontrol_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
}
}
void UUVAttitudeControl::manual_control_setpoint_poll()
{
bool updated = false;
orb_check(_manual_control_setpoint_sub, &updated);
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint);
}
}
void UUVAttitudeControl::vehicle_attitude_setpoint_poll()
{
bool updated = false;
orb_check(_vehicle_attitude_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _vehicle_attitude_sp_sub, &_vehicle_attitude_sp);
}
}
void UUVAttitudeControl::vehicle_rates_setpoint_poll()
{
_vehicle_rates_setpoint_sub.update(&_vehicle_rates_sp);
}
void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u, float thrust_u)
{
if (PX4_ISFINITE(roll_u)) {
@ -126,10 +101,6 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
} else {
_actuators.control[actuator_controls_s::INDEX_ROLL] = 0.0f;
if (loop_counter % 10 == 0) {
PX4_INFO("roll_u %.4f", (double)roll_u);
}
}
if (PX4_ISFINITE(pitch_u)) {
@ -138,10 +109,6 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
} else {
_actuators.control[actuator_controls_s::INDEX_PITCH] = 0.0f;
if (loop_counter % 10 == 0) {
PX4_INFO("pitch_u %.4f", (double)pitch_u);
}
}
if (PX4_ISFINITE(yaw_u)) {
@ -150,10 +117,6 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
} else {
_actuators.control[actuator_controls_s::INDEX_YAW] = 0.0f;
if (loop_counter % 10 == 0) {
PX4_INFO("yaw_u %.4f", (double)yaw_u);
}
}
if (PX4_ISFINITE(thrust_u)) {
@ -162,15 +125,12 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
} else {
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
if (loop_counter % 10 == 0) {
PX4_INFO("thrust_u %.4f", (double)thrust_u);
}
}
}
void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp)
void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude,
const vehicle_attitude_setpoint_s &attitude_setpoint, const vehicle_angular_velocity_s &angular_velocity,
const vehicle_rates_setpoint_s &rates_setpoint)
{
/** Geometric Controller
*
@ -178,45 +138,43 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &att, con
* D. Mellinger, V. Kumar, "Minimum Snap Trajectory Generation and Control for Quadrotors", IEEE ICRA 2011, pp. 2520-2525.
* D. A. Duecker, A. Hackbarth, T. Johannink, E. Kreuzer, and E. Solowjow, “Micro Underwater Vehicle Hydrobatics: A SubmergedFuruta Pendulum,” IEEE ICRA 2018, pp. 7498–7503.
*/
Eulerf euler_angles(matrix::Quatf(att.q));
Eulerf euler_angles(matrix::Quatf(attitude.q));
float roll_u;
float pitch_u;
float yaw_u;
float thrust_u;
float roll_body = _vehicle_attitude_sp.roll_body;
float pitch_body = _vehicle_attitude_sp.pitch_body;
float yaw_body = _vehicle_attitude_sp.yaw_body;
float roll_body = attitude_setpoint.roll_body;
float pitch_body = attitude_setpoint.pitch_body;
float yaw_body = attitude_setpoint.yaw_body;
float roll_rate_desired = _vehicle_rates_sp.roll;
float pitch_rate_desired = _vehicle_rates_sp.pitch;
float yaw_rate_desired = _vehicle_rates_sp.yaw;
float roll_rate_desired = rates_setpoint.roll;
float pitch_rate_desired = rates_setpoint.pitch;
float yaw_rate_desired = rates_setpoint.yaw;
/* get attitude setpoint rotational matrix */
Dcmf _rot_des = Eulerf(roll_body, pitch_body, yaw_body);
Dcmf rot_des = Eulerf(roll_body, pitch_body, yaw_body);
/* get current rotation matrix from control state quaternions */
Quatf q_att(att.q[0], att.q[1], att.q[2], att.q[3]);
Matrix3f _rot_att = matrix::Dcm<float>(q_att);
Quatf q_att(attitude.q);
Matrix3f rot_att = matrix::Dcm<float>(q_att);
Vector3f e_R_vec;
Vector3f torques;
Vector3f omega;
/* Compute matrix: attitude error */
Matrix3f e_R = (_rot_des.transpose() * _rot_att - _rot_att.transpose() * _rot_des) * 0.5;
Matrix3f e_R = (rot_des.transpose() * rot_att - rot_att.transpose() * rot_des) * 0.5;
/* vee-map the error to get a vector instead of matrix e_R */
e_R_vec(0) = e_R(2, 1); /**< Roll */
e_R_vec(1) = e_R(0, 2); /**< Pitch */
e_R_vec(2) = e_R(1, 0); /**< Yaw */
omega(0) = _angular_velocity.xyz[0] - roll_rate_desired;
omega(1) = _angular_velocity.xyz[1] - pitch_rate_desired;
omega(2) = _angular_velocity.xyz[2] - yaw_rate_desired;
Vector3f omega{angular_velocity.xyz};
omega(0) -= roll_rate_desired;
omega(1) -= pitch_rate_desired;
omega(2) -= yaw_rate_desired;
/**< P-Control */
torques(0) = - e_R_vec(0) * _param_roll_p.get(); /**< Roll */
@ -232,154 +190,79 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &att, con
pitch_u = torques(1);
yaw_u = torques(2);
//Quatf q_att(_att.q[0], _att.q[1], _att.q[2], _att.q[3]);
//Matrix3f _rot_att = q_att.to_dcm();
/** Vector3f current_velocity_boat;
current_velocity_boat(0) = _local_pos.vx;
current_velocity_boat(1) = _local_pos.vy;
current_velocity_boat(2) = _local_pos.vz;
current_velocity_boat = q_att.to_dcm() * current_velocity_boat;
*/
// take thrust as
thrust_u = _vehicle_attitude_sp.thrust_body[0];
thrust_u = attitude_setpoint.thrust_body[0];
constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_u);
/* Geometric Controller END*/
}
void UUVAttitudeControl::run()
void UUVAttitudeControl::Run()
{
_vehicle_attitude_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_angular_velocity_sub = orb_subscribe(ORB_ID(vehicle_angular_velocity));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
if (should_exit()) {
_vehicle_attitude_sub.unregisterCallback();
exit_and_cleanup();
return;
}
_manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* check vehicle control mode for changes to publication state */
_vcontrol_mode_sub.update(&_vcontrol_mode);
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
/* update parameters from storage */
parameters_update();
vehicle_attitude_s attitude;
/* rate limit control mode updates to 5Hz */
orb_set_interval(_vcontrol_mode_sub, 200);
/* only run controller if attitude changed */
if (_vehicle_attitude_sub.update(&attitude)) {
vehicle_angular_velocity_s angular_velocity {};
_angular_velocity_sub.copy(&angular_velocity);
parameters_update(true);
/* Run geometric attitude controllers if NOT manual mode*/
if (!_vcontrol_mode.flag_control_manual_enabled
&& _vcontrol_mode.flag_control_attitude_enabled
&& _vcontrol_mode.flag_control_rates_enabled) {
/* wakeup source(s) */
px4_pollfd_struct_t fds[3];
int input_mode = _param_input_mode.get();
/* Setup of loop */
fds[0].fd = _vehicle_attitude_sub;
fds[0].events = POLLIN;
fds[1].fd = _manual_control_setpoint_sub;
fds[1].events = POLLIN;
fds[2].fd = _sensor_combined_sub;
fds[2].events = POLLIN;
_vehicle_attitude_setpoint_sub.update(&_attitude_setpoint);
_vehicle_rates_setpoint_sub.update(&_rates_setpoint);
while (!should_exit()) {
/* wait for up to 500ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
/* check vehicle control mode for changes to publication state */
vehicle_control_mode_poll();
vehicle_attitude_setpoint_poll();
/* update parameters from storage */
parameters_update();
/* only run controller if attitude changed */
if (fds[0].revents & POLLIN) {
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f || fabsf(deltaT) < 0.00001f || !PX4_ISFINITE(deltaT)) {
deltaT = 0.01f;
if (input_mode == 1) { // process manual data
_attitude_setpoint.roll_body = _param_direct_roll.get();
_attitude_setpoint.pitch_body = _param_direct_pitch.get();
_attitude_setpoint.yaw_body = _param_direct_yaw.get();
_attitude_setpoint.thrust_body[0] = _param_direct_thrust.get();
}
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_vehicle_attitude);
orb_copy(ORB_ID(vehicle_angular_velocity), _angular_velocity_sub, &_angular_velocity);
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
vehicle_attitude_setpoint_poll();
vehicle_rates_setpoint_poll();
vehicle_control_mode_poll();
manual_control_setpoint_poll();
/* Run geometric attitude controllers if NOT manual mode*/
if (!_vcontrol_mode.flag_control_manual_enabled
&& _vcontrol_mode.flag_control_attitude_enabled
&& _vcontrol_mode.flag_control_rates_enabled) {
int input_mode = _param_input_mode.get();
// if (input_mode == 0) // process incoming vehicles setpoint data --> nothing to do
if (input_mode == 1) { // process manual data
_vehicle_attitude_sp.roll_body = _param_direct_roll.get();
_vehicle_attitude_sp.pitch_body = _param_direct_pitch.get();
_vehicle_attitude_sp.yaw_body = _param_direct_yaw.get();
_vehicle_attitude_sp.thrust_body[0] = _param_direct_thrust.get();
}
/* Geometric Control*/
control_attitude_geo(_vehicle_attitude, _vehicle_attitude_sp);
}
}
loop_counter++;
perf_end(_loop_perf);
/* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */
if (fds[1].revents & POLLIN) {
// This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep
// returning immediately and this loop will eat up resources.
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint);
if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) {
/* manual/direct control */
constrain_actuator_commands(_manual_control_setpoint.y, -_manual_control_setpoint.x,
_manual_control_setpoint.r, _manual_control_setpoint.z);
}
}
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
_actuators.timestamp = hrt_absolute_time();
/* Only publish if any of the proper modes are enabled */
if (_vcontrol_mode.flag_control_manual_enabled ||
_vcontrol_mode.flag_control_attitude_enabled) {
/* publish the actuator controls */
_actuator_controls_pub.publish(_actuators);
}
/* Geometric Control*/
control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint);
}
}
orb_unsubscribe(_vcontrol_mode_sub);
orb_unsubscribe(_vehicle_attitude_sp_sub);
orb_unsubscribe(_angular_velocity_sub);
orb_unsubscribe(_manual_control_setpoint_sub);
orb_unsubscribe(_vehicle_attitude_sub);
orb_unsubscribe(_local_pos_sub);
orb_unsubscribe(_sensor_combined_sub);
perf_end(_loop_perf);
/* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */
if (_manual_control_setpoint_sub.update(&_manual_control_setpoint)) {
// This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep
// returning immediately and this loop will eat up resources.
if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) {
/* manual/direct control */
constrain_actuator_commands(_manual_control_setpoint.y, -_manual_control_setpoint.x,
_manual_control_setpoint.r, _manual_control_setpoint.z);
}
}
_actuators.timestamp = hrt_absolute_time();
/* Only publish if any of the proper modes are enabled */
if (_vcontrol_mode.flag_control_manual_enabled ||
_vcontrol_mode.flag_control_attitude_enabled) {
/* publish the actuator controls */
_actuator_controls_pub.publish(_actuators);
}
warnx("exiting.\n");
}

75
src/modules/uuv_att_control/uuv_att_control.hpp
View File

@ -59,26 +59,21 @@
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/uORB.h>
using matrix::Eulerf;
using matrix::Quatf;
using matrix::Matrix3f;
@ -87,7 +82,7 @@ using matrix::Dcmf;
using uORB::SubscriptionData;
class UUVAttitudeControl: public ModuleBase<UUVAttitudeControl>, public ModuleParams
class UUVAttitudeControl: public ModuleBase<UUVAttitudeControl>, public ModuleParams, public px4::WorkItem
{
public:
UUVAttitudeControl();
@ -108,48 +103,28 @@ public:
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::run() */
void run() override;
// int start();
// bool task_running() { return _task_running; }
bool init();
private:
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)};
uORB::Publication<actuator_controls_s> _actuator_controls_pub{ORB_ID(actuator_controls_0)};
uORB::Publication<actuator_controls_s> _actuator_controls_pub{ORB_ID(actuator_controls_0)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
int _vehicle_attitude_sp_sub{-1}; /**< vehicle attitude setpoint */
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */
uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle bodyrates setpoint subscriber */
int _battery_status_sub{-1}; /**< battery status subscription */
int _vehicle_attitude_sub{-1}; /**< control state subscription */
int _angular_velocity_sub{-1}; /**< vehicle angular velocity subscription */
int _local_pos_sub{-1}; /**< local position subscription */
int _manual_control_setpoint_sub{-1}; /**< notification of manual control updates */
int _vcontrol_mode_sub{-1}; /**< vehicle status subscription */
int _sensor_combined_sub{-1}; /**< sensor combined subscription */
uORB::Subscription _angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; /**< vehicle angular velocity subscription */
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */
actuator_controls_s _actuators {}; /**< actuator control inputs */
manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
vehicle_attitude_s _vehicle_attitude {}; /**< control state */
vehicle_angular_velocity_s _angular_velocity{}; /**< angular velocity */
vehicle_attitude_setpoint_s _vehicle_attitude_sp {};/**< vehicle attitude setpoint */
vehicle_rates_setpoint_s _vehicle_rates_sp {}; /**< vehicle bodyrates setpoint */
vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
sensor_combined_s _sensor_combined{};
vehicle_local_position_s _local_pos{}; /**< vehicle local position */
uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)};
SubscriptionData<vehicle_acceleration_s> _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
hrt_abstime _control_position_last_called{0}; /**<last call of control_position */
perf_counter_t _loop_perf; /**< loop performance counter */
actuator_controls_s _actuators {}; /**< actuator control inputs */
manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
vehicle_attitude_setpoint_s _attitude_setpoint {}; /**< vehicle attitude setpoint */
vehicle_rates_setpoint_s _rates_setpoint {}; /**< vehicle bodyrates setpoint */
vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
// estimator reset counters
uint8_t _pos_reset_counter{0}; // captures the number of times the estimator has reset the horizontal position
bool _debug{false}; /**< if set to true, print debug output */
int loop_counter = 0;
perf_counter_t _loop_perf; /**< loop performance counter */
DEFINE_PARAMETERS(
(ParamFloat<px4::params::UUV_ROLL_P>) _param_roll_p,
@ -167,26 +142,16 @@ private:
(ParamFloat<px4::params::UUV_DIRCT_THRUST>) _param_direct_thrust
)
void Run() override;
/**
* Update our local parameter cache.
*/
void parameters_update(bool force = false);
void manual_control_setpoint_poll();
void position_setpoint_triplet_poll();
void vehicle_control_mode_poll();
void vehicle_attitude_poll();
void vehicle_attitude_setpoint_poll();
void vehicle_rates_setpoint_poll();
void vehicle_local_position_poll();
/**
* Control Attitude
*/
void control_attitude_geo(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp);
void control_attitude_pid(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp, float deltaT);
void control_attitude_geo(const vehicle_attitude_s &attitude, const vehicle_attitude_setpoint_s &attitude_setpoint,
const vehicle_angular_velocity_s &angular_velocity, const vehicle_rates_setpoint_s &rates_setpoint);
void constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u, float thrust_u);
};