mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-16 18:11:37 +08:00
update angular accelerations
Rebase fixes
This commit is contained in:
@@ -156,8 +156,8 @@ RoverPositionControl::manual_control_setpoint_poll()
|
|||||||
// STABILIZED mode generate the attitude setpoint from manual user inputs
|
// STABILIZED mode generate the attitude setpoint from manual user inputs
|
||||||
_rates_sp.roll = 0.0;
|
_rates_sp.roll = 0.0;
|
||||||
_rates_sp.pitch = 0.0;
|
_rates_sp.pitch = 0.0;
|
||||||
_rates_sp.yaw = _manual_control_setpoint.y;
|
_rates_sp.yaw = _manual_control_setpoint.roll;
|
||||||
_rates_sp.thrust_body[0] = _manual_control_setpoint.z;
|
_rates_sp.thrust_body[0] = _manual_control_setpoint.throttle;
|
||||||
|
|
||||||
_rates_sp.timestamp = hrt_absolute_time();
|
_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
|
bool
|
||||||
RoverPositionControl::control_position(const matrix::Vector2d ¤t_position,
|
RoverPositionControl::control_position(const matrix::Vector2d ¤t_position,
|
||||||
const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
|
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
|
void
|
||||||
RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, const vehicle_angular_acceleration_s &acc,
|
RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, const vehicle_local_position_s &local_pos,
|
||||||
const vehicle_local_position_s &local_pos,
|
|
||||||
const vehicle_rates_setpoint_s &rates_sp)
|
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;
|
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);
|
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());
|
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,
|
const matrix::Vector3f torque = _rate_control.update(vehicle_rates, rates_setpoint, angular_acceleration, dt,
|
||||||
lock_integrator);
|
lock_integrator);
|
||||||
///TODO: Handle mimimum speed constraints
|
///TODO: Handle mimimum speed constraints
|
||||||
@@ -452,8 +443,6 @@ RoverPositionControl::Run()
|
|||||||
|
|
||||||
/* run controller on gyro changes */
|
/* run controller on gyro changes */
|
||||||
if (_vehicle_angular_velocity_sub.update(&_vehicle_rates)) {
|
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 */
|
/* check vehicle control mode for changes to publication state */
|
||||||
vehicle_control_mode_poll();
|
vehicle_control_mode_poll();
|
||||||
attitude_setpoint_poll();
|
attitude_setpoint_poll();
|
||||||
@@ -548,7 +537,7 @@ RoverPositionControl::Run()
|
|||||||
|
|
||||||
//Body Rate control
|
//Body Rate control
|
||||||
if (_control_mode.flag_control_rates_enabled) {
|
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 */
|
/* Only publish if any of the proper modes are enabled */
|
||||||
|
|||||||
@@ -69,7 +69,6 @@
|
|||||||
#include <uORB/topics/vehicle_acceleration.h>
|
#include <uORB/topics/vehicle_acceleration.h>
|
||||||
#include <uORB/topics/vehicle_attitude.h>
|
#include <uORB/topics/vehicle_attitude.h>
|
||||||
#include <uORB/topics/vehicle_angular_velocity.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_rates_setpoint.h>
|
||||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||||
#include <uORB/topics/vehicle_angular_velocity.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 _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
|
||||||
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
|
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
|
||||||
uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_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 */
|
manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */
|
||||||
position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */
|
position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */
|
||||||
vehicle_attitude_setpoint_s _att_sp{}; /**< attitude setpoint > */
|
vehicle_attitude_setpoint_s _att_sp{}; /**< attitude setpoint > */
|
||||||
@@ -131,7 +129,6 @@ private:
|
|||||||
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
|
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
|
||||||
vehicle_local_position_s _local_pos{}; /**< global vehicle position */
|
vehicle_local_position_s _local_pos{}; /**< global vehicle position */
|
||||||
vehicle_attitude_s _vehicle_att{};
|
vehicle_attitude_s _vehicle_att{};
|
||||||
vehicle_angular_acceleration_s _vehicle_angular_acceleration{};
|
|
||||||
vehicle_angular_velocity_s _vehicle_rates{};
|
vehicle_angular_velocity_s _vehicle_rates{};
|
||||||
|
|
||||||
trajectory_setpoint_s _trajectory_setpoint{};
|
trajectory_setpoint_s _trajectory_setpoint{};
|
||||||
@@ -227,7 +224,6 @@ private:
|
|||||||
void rates_setpoint_poll();
|
void rates_setpoint_poll();
|
||||||
void vehicle_control_mode_poll();
|
void vehicle_control_mode_poll();
|
||||||
void vehicle_attitude_poll();
|
void vehicle_attitude_poll();
|
||||||
void vehicle_angular_acceleration_poll();
|
|
||||||
void manual_control_setpoint_poll();
|
void manual_control_setpoint_poll();
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -237,8 +233,7 @@ private:
|
|||||||
const position_setpoint_triplet_s &_pos_sp_triplet);
|
const position_setpoint_triplet_s &_pos_sp_triplet);
|
||||||
void control_velocity(const matrix::Vector3f ¤t_velocity);
|
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_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,
|
void control_rates(const vehicle_angular_velocity_s &rates, const vehicle_local_position_s &local_pos,
|
||||||
const vehicle_local_position_s &local_pos,
|
|
||||||
const vehicle_rates_setpoint_s &rates_sp);
|
const vehicle_rates_setpoint_s &rates_sp);
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|||||||
Reference in New Issue
Block a user