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

update UUV and rover controllers to use trajectory_setpoint and cleanup unused position_setpoint fields

This commit is contained in:
Daniel Agar 2021-03-09 10:36:34 -05:00 committed by GitHub
parent a0b9b44ff6
commit d37510a43d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 29 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
msg/position_setpoint.msg
View File

@ -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

1
src/modules/navigator/follow_target.cpp
View File

@ -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);

4
src/modules/navigator/mission.cpp
View File

@ -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) &&

26
src/modules/rover_pos_control/RoverPositionControl.cpp
View File

@ -319,18 +319,15 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
}
void
RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity,
const position_setpoint_triplet_s &pos_sp_triplet)
RoverPositionControl::control_velocity(const matrix::Vector3f &current_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 &current_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 &current_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);
}
}

10
src/modules/rover_pos_control/RoverPositionControl.hpp
View File

@ -70,6 +70,7 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/actuator_controls.h>
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_s> _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 &current_velocity, const position_setpoint_triplet_s &pos_sp_triplet);
void control_velocity(const matrix::Vector3f &current_velocity);
void control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp);
};

22
src/modules/uuv_pos_control/uuv_pos_control.cpp
View File

@ -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);
}
}
}

7
src/modules/uuv_pos_control/uuv_pos_control.hpp
View File

@ -62,9 +62,9 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@ -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 */