From aa004b9c6c50ade10c60d7e9148d2cbfe2704b60 Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Tue, 13 Dec 2022 12:01:10 +0100 Subject: [PATCH] update angular accelerations Rebase fixes --- .../RoverPositionControl.cpp | 21 +++++-------------- .../RoverPositionControl.hpp | 7 +------ 2 files changed, 6 insertions(+), 22 deletions(-) diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index adc9c86658..d597534c35 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -156,8 +156,8 @@ RoverPositionControl::manual_control_setpoint_poll() // STABILIZED mode generate the attitude setpoint from manual user inputs _rates_sp.roll = 0.0; _rates_sp.pitch = 0.0; - _rates_sp.yaw = _manual_control_setpoint.y; - _rates_sp.thrust_body[0] = _manual_control_setpoint.z; + _rates_sp.yaw = _manual_control_setpoint.roll; + _rates_sp.thrust_body[0] = _manual_control_setpoint.throttle; _rates_sp.timestamp = hrt_absolute_time(); @@ -212,14 +212,6 @@ RoverPositionControl::vehicle_attitude_poll() } } -void -RoverPositionControl::vehicle_angular_acceleration_poll() -{ - if (_vehicle_angular_acceleration_sub.updated()) { - _vehicle_angular_acceleration_sub.copy(&_vehicle_angular_acceleration); - } -} - bool RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet) @@ -415,8 +407,7 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi } void -RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, const vehicle_angular_acceleration_s &acc, - const vehicle_local_position_s &local_pos, +RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, const vehicle_local_position_s &local_pos, const vehicle_rates_setpoint_s &rates_sp) { float dt = (_control_rates_last_called > 0) ? hrt_elapsed_time(&_control_rates_last_called) * 1e-6f : 0.01f; @@ -428,7 +419,7 @@ RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, con const matrix::Vector3f current_velocity(local_pos.vx, local_pos.vy, local_pos.vz); bool lock_integrator = bool(current_velocity.norm() < _param_rate_i_minspeed.get()); - const matrix::Vector3f angular_acceleration{acc.xyz}; + const matrix::Vector3f angular_acceleration{rates.xyz_derivative}; const matrix::Vector3f torque = _rate_control.update(vehicle_rates, rates_setpoint, angular_acceleration, dt, lock_integrator); ///TODO: Handle mimimum speed constraints @@ -452,8 +443,6 @@ RoverPositionControl::Run() /* run controller on gyro changes */ if (_vehicle_angular_velocity_sub.update(&_vehicle_rates)) { - // grab corresponding vehicle_angular_acceleration immediately after vehicle_angular_velocity copy - vehicle_angular_acceleration_poll(); /* check vehicle control mode for changes to publication state */ vehicle_control_mode_poll(); attitude_setpoint_poll(); @@ -548,7 +537,7 @@ RoverPositionControl::Run() //Body Rate control if (_control_mode.flag_control_rates_enabled) { - control_rates(_vehicle_rates, _vehicle_angular_acceleration, _local_pos, _rates_sp); + control_rates(_vehicle_rates, _local_pos, _rates_sp); } /* Only publish if any of the proper modes are enabled */ diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index b0767ce77a..bd6758b718 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -69,7 +69,6 @@ #include #include #include -#include #include #include #include @@ -122,7 +121,6 @@ private: uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; - uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */ position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */ vehicle_attitude_setpoint_s _att_sp{}; /**< attitude setpoint > */ @@ -131,7 +129,6 @@ private: vehicle_global_position_s _global_pos{}; /**< global vehicle position */ vehicle_local_position_s _local_pos{}; /**< global vehicle position */ vehicle_attitude_s _vehicle_att{}; - vehicle_angular_acceleration_s _vehicle_angular_acceleration{}; vehicle_angular_velocity_s _vehicle_rates{}; trajectory_setpoint_s _trajectory_setpoint{}; @@ -227,7 +224,6 @@ private: void rates_setpoint_poll(); void vehicle_control_mode_poll(); void vehicle_attitude_poll(); - void vehicle_angular_acceleration_poll(); void manual_control_setpoint_poll(); /** @@ -237,8 +233,7 @@ private: 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); - void control_rates(const vehicle_angular_velocity_s &rates, const vehicle_angular_acceleration_s &acc, - const vehicle_local_position_s &local_pos, + void control_rates(const vehicle_angular_velocity_s &rates, const vehicle_local_position_s &local_pos, const vehicle_rates_setpoint_s &rates_sp); };