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

update angular accelerations 

Rebase fixes
This commit is contained in:
Jaeyoung Lim 2022-12-13 12:01:10 +01:00
parent 62f0fafe29
commit aa004b9c6c
2 changed files with 6 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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 &current_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 */

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

@ -69,7 +69,6 @@
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
@ -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 &current_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);
};