From d37510a43d468de8932a9e55dbaf5fc2e796ec1d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 9 Mar 2021 10:36:34 -0500 Subject: [PATCH] update UUV and rover controllers to use trajectory_setpoint and cleanup unused position_setpoint fields --- msg/position_setpoint.msg | 5 ---- src/modules/navigator/follow_target.cpp | 1 - src/modules/navigator/mission.cpp | 4 --- .../RoverPositionControl.cpp | 26 +++++++------------ .../RoverPositionControl.hpp | 10 ++++++- .../uuv_pos_control/uuv_pos_control.cpp | 22 +++++----------- .../uuv_pos_control/uuv_pos_control.hpp | 7 +++-- 7 files changed, 29 insertions(+), 46 deletions(-) diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index e9540dca08..a3a1668a46 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -16,11 +16,6 @@ uint8 VELOCITY_FRAME_BODY_NED = 8 # MAV_FRAME_BODY_NED bool valid # true if setpoint is valid uint8 type # setpoint type to adjust behavior of position controller -float32 x # local position setpoint in m in NED -float32 y # local position setpoint in m in NED -float32 z # local position setpoint in m in NED -bool position_valid # true if local position setpoint valid - float32 vx # local velocity setpoint in m/s in NED float32 vy # local velocity setpoint in m/s in NED float32 vz # local velocity setpoint in m/s in NED diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index bbedf7be20..338d6ab20d 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -327,7 +327,6 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position, floa mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET; - pos_sp_triplet->current.position_valid = use_position; pos_sp_triplet->current.velocity_valid = use_velocity; pos_sp_triplet->current.vx = _current_vel(0); pos_sp_triplet->current.vy = _current_vel(1); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 6989f4d96a..4e9f99e388 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1838,10 +1838,6 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit { return ((p1->valid == p2->valid) && (p1->type == p2->type) && - (fabsf(p1->x - p2->x) < FLT_EPSILON) && - (fabsf(p1->y - p2->y) < FLT_EPSILON) && - (fabsf(p1->z - p2->z) < FLT_EPSILON) && - (p1->position_valid == p2->position_valid) && (fabsf(p1->vx - p2->vx) < FLT_EPSILON) && (fabsf(p1->vy - p2->vy) < FLT_EPSILON) && (fabsf(p1->vz - p2->vz) < FLT_EPSILON) && diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index d037aec79f..7d5fc91160 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -319,18 +319,15 @@ RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, } void -RoverPositionControl::control_velocity(const matrix::Vector3f ¤t_velocity, - const position_setpoint_triplet_s &pos_sp_triplet) +RoverPositionControl::control_velocity(const matrix::Vector3f ¤t_velocity) { - + const Vector3f desired_velocity{_trajectory_setpoint.vx, _trajectory_setpoint.vy, _trajectory_setpoint.vz}; float dt = 0.01; // Using non zero value to a avoid division by zero const float mission_throttle = _param_throttle_cruise.get(); - const matrix::Vector3f desired_velocity{pos_sp_triplet.current.vx, pos_sp_triplet.current.vy, pos_sp_triplet.current.vz}; const float desired_speed = desired_velocity.norm(); if (desired_speed > 0.01f) { - const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed()); const Vector3f vel = R_to_body * Vector3f(current_velocity(0), current_velocity(1), current_velocity(2)); @@ -344,13 +341,12 @@ RoverPositionControl::control_velocity(const matrix::Vector3f ¤t_velocity, Vector3f desired_body_velocity; - if (pos_sp_triplet.current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) { + if (_velocity_frame == VelocityFrame::NED) { desired_body_velocity = desired_velocity; } else { // If the frame of the velocity setpoint is unknown, assume it is in local frame desired_body_velocity = R_to_body * desired_velocity; - } const float desired_theta = atan2f(desired_body_velocity(1), desired_body_velocity(0)); @@ -363,7 +359,6 @@ RoverPositionControl::control_velocity(const matrix::Vector3f ¤t_velocity, _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f; _act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f; - } } @@ -421,12 +416,14 @@ RoverPositionControl::Run() _global_local_alt0 = _local_pos.ref_alt; } + _trajectory_setpoint_sub.update(&_trajectory_setpoint); + // local -> global map_projection_reproject(&_global_local_proj_ref, - _pos_sp_triplet.current.x, _pos_sp_triplet.current.y, + _trajectory_setpoint.x, _trajectory_setpoint.y, &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon); - _pos_sp_triplet.current.alt = _global_local_alt0 - _pos_sp_triplet.current.z; + _pos_sp_triplet.current.alt = _global_local_alt0 - _trajectory_setpoint.z; _pos_sp_triplet.current.valid = true; } @@ -445,7 +442,7 @@ RoverPositionControl::Run() float turn_distance = _param_l1_distance.get(); //_gnd_control.switch_distance(100.0f); // publish status - position_controller_status_s pos_ctrl_status = {}; + position_controller_status_s pos_ctrl_status{}; pos_ctrl_status.nav_roll = 0.0f; pos_ctrl_status.nav_pitch = 0.0f; @@ -463,14 +460,11 @@ RoverPositionControl::Run() pos_ctrl_status.timestamp = hrt_absolute_time(); _pos_ctrl_status_pub.publish(pos_ctrl_status); - - } } else if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_velocity_enabled) { - - control_velocity(current_velocity, _pos_sp_triplet); - + _trajectory_setpoint_sub.update(&_trajectory_setpoint); + control_velocity(current_velocity); } } diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index bce7cf4d15..1104939987 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -70,6 +70,7 @@ #include #include #include +#include #include using matrix::Dcmf; @@ -112,6 +113,7 @@ private: uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */ position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */ @@ -121,6 +123,7 @@ private: vehicle_local_position_s _local_pos{}; /**< global vehicle position */ actuator_controls_s _act_controls{}; /**< direct control of actuators */ vehicle_attitude_s _vehicle_att{}; + vehicle_local_position_setpoint_s _trajectory_setpoint{}; uORB::SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; @@ -155,6 +158,11 @@ private: /* previous waypoint */ matrix::Vector2d _prev_wp{0, 0}; + enum class VelocityFrame { + NED, + BODY, + } _velocity_frame{VelocityFrame::NED}; + float _manual_yaw_sp{0.0}; bool _reset_yaw_sp{true}; @@ -199,7 +207,7 @@ private: */ bool control_position(const matrix::Vector2d &global_pos, const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &_pos_sp_triplet); - void control_velocity(const matrix::Vector3f ¤t_velocity, const position_setpoint_triplet_s &pos_sp_triplet); + void control_velocity(const matrix::Vector3f ¤t_velocity); void control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp); }; diff --git a/src/modules/uuv_pos_control/uuv_pos_control.cpp b/src/modules/uuv_pos_control/uuv_pos_control.cpp index afa18d8e9f..37f277d7a3 100644 --- a/src/modules/uuv_pos_control/uuv_pos_control.cpp +++ b/src/modules/uuv_pos_control/uuv_pos_control.cpp @@ -178,21 +178,16 @@ void UUVPOSControl::Run() && _vcontrol_mode.flag_control_attitude_enabled && _vcontrol_mode.flag_control_rates_enabled) { - _vehicle_attitude_sub.update(&_vehicle_attitude);//get current vehicle attitude - _pos_sp_triplet_sub.update(&_pos_setpoint); + _trajectory_setpoint_sub.update(&_trajectory_setpoint); - float roll_des, pitch_des, yaw_des, x_pos_des, y_pos_des, z_pos_des; - - - roll_des = 0; - pitch_des = 0; - yaw_des = _pos_setpoint.current.yaw; - - x_pos_des = _pos_setpoint.current.x; - y_pos_des = _pos_setpoint.current.y; - z_pos_des = _pos_setpoint.current.z; + float roll_des = 0; + float pitch_des = 0; + float yaw_des = _trajectory_setpoint.yaw; + float x_pos_des = _trajectory_setpoint.x; + float y_pos_des = _trajectory_setpoint.y; + float z_pos_des = _trajectory_setpoint.z; //stabilization controller(keep pos and hold depth + angle) vs position controller(global + yaw) if (_param_stabilization.get() == 0) { @@ -203,9 +198,6 @@ void UUVPOSControl::Run() stabilization_controller_6dof(x_pos_des, y_pos_des, z_pos_des, roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos); } - - - } } diff --git a/src/modules/uuv_pos_control/uuv_pos_control.hpp b/src/modules/uuv_pos_control/uuv_pos_control.hpp index b3e89c8129..e78acc03d2 100644 --- a/src/modules/uuv_pos_control/uuv_pos_control.hpp +++ b/src/modules/uuv_pos_control/uuv_pos_control.hpp @@ -62,9 +62,9 @@ #include #include #include -#include #include #include +#include #include #include #include @@ -105,16 +105,15 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; /**< current vehicle attitude */ uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */ - uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; /**< position setpoint */ uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; - //actuator_controls_s _actuators {}; /**< actuator control inputs */ manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */ vehicle_attitude_s _vehicle_attitude {}; /**< vehicle attitude */ - position_setpoint_triplet_s _pos_setpoint {}; /**< vehicle position setpoint */ + vehicle_local_position_setpoint_s _trajectory_setpoint{}; /**< vehicle position setpoint */ vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */ perf_counter_t _loop_perf; /**< loop performance counter */