fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-08-21 21:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Omni Pos-Ctrl: omni_attitude_status topic is filled and published

Browse Source
This commit is contained in:
Azarakhsh Keipour
2020-08-05 22:01:41 -04:00
parent 61ef6b2255
commit 8a09314ef9
5 changed files with 41 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/ControlMath.cpp
+20 -11
View File
@@ -49,7 +49,7 @@ namespace ControlMath
void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, const int omni_att_mode,
const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir,
float &omni_att_roll, float &omni_att_pitch, const float omni_att_rate, int omni_proj_axes,
vehicle_attitude_setpoint_s &att_sp)
vehicle_attitude_setpoint_s &att_sp, omni_attitude_status_s &omni_status)
{
// Print an error if the omni_att_mode parameter is out of range
@@ -88,19 +88,28 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::
}
// Estimate the optimal tilt angle and direction to counteract the wind
// Calculate the current z axis
Vector3f curr_z;
matrix::Dcmf R_body = att;
for (int i = 0; i < 3; i++) {
curr_z(i) = R_body(i, 2);
}
// Calculate the tilt angle and direction
float current_tilt_angle = asinf(Vector2f(curr_z(0), curr_z(1)).norm() / curr_z.norm());
float current_tilt_dir = wrap_2pi(atan2f(-curr_z(1), -curr_z(0)));
omni_status.tilt_angle_meas = current_tilt_angle;
omni_status.tilt_direction_meas = current_tilt_dir;
// Save the optimal tilt angle and direction to counteract the wind
if (omni_att_mode == 5 || omni_att_mode == 6) {
// Calculate the current z axis
Vector3f curr_z;
matrix::Dcmf R_body = att;
for (int i = 0; i < 3; i++) {
curr_z(i) = R_body(i, 2);
}
// Calculate the tilt angle and direction
omni_att_tilt_angle = asinf(Vector2f(curr_z(0), curr_z(1)).norm() / curr_z.norm());
omni_att_tilt_dir = wrap_2pi(atan2f(-curr_z(1), -curr_z(0)));
omni_att_tilt_angle = current_tilt_angle;
omni_att_tilt_dir = current_tilt_dir;
// Calculate the roll and pitch
Eulerf euler = R_body;
src/modules/mc_pos_control/PositionControl/ControlMath.hpp
+2 -1
View File
@@ -42,6 +42,7 @@
#include <matrix/matrix/math.hpp>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/omni_attitude_status.h>
namespace ControlMath
{
@@ -63,7 +64,7 @@ namespace ControlMath
void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
const int omni_att_mode, const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir,
float &omni_att_roll, float &omni_att_pitch, const float omni_att_rate, const int omni_proj_axes,
vehicle_attitude_setpoint_s &att_sp);
vehicle_attitude_setpoint_s &att_sp, omni_attitude_status_s &omni_status);
/**
* Converts a body z vector and yaw set-point to a desired attitude.
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+2 -2
View File
@@ -367,9 +367,9 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
void PositionControl::getAttitudeSetpoint(const matrix::Quatf &att, const int omni_att_mode,
const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir, float &omni_att_roll,
float &omni_att_pitch, const float omni_att_rate, const int omni_proj_axes,
vehicle_attitude_setpoint_s &attitude_setpoint) const
vehicle_attitude_setpoint_s &attitude_setpoint, omni_attitude_status_s &omni_status) const
{
ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, att, omni_att_mode, omni_dfc_max_thrust, omni_att_tilt_angle,
omni_att_tilt_dir, omni_att_roll, omni_att_pitch, omni_att_rate, omni_proj_axes, attitude_setpoint);
omni_att_tilt_dir, omni_att_roll, omni_att_pitch, omni_att_rate, omni_proj_axes, attitude_setpoint, omni_status);
attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
}
src/modules/mc_pos_control/PositionControl/PositionControl.hpp
+3 -1
View File
@@ -43,6 +43,7 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_constraints.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/omni_attitude_status.h>
struct PositionControlStates {
matrix::Vector3f position;
@@ -183,7 +184,8 @@ public:
*/
void getAttitudeSetpoint(const matrix::Quatf &att, const int omni_att_mode, const float omni_dfc_max_thrust,
float &omni_att_tilt_angle, float &omni_att_tilt_dir, float &omni_att_roll, float &omni_att_pitch,
const float omni_att_rate, const int omni_proj_axes, vehicle_attitude_setpoint_s &attitude_setpoint) const;
const float omni_att_rate, const int omni_proj_axes, vehicle_attitude_setpoint_s &attitude_setpoint,
omni_attitude_status_s &omni_status) const;
private:
/**
src/modules/mc_pos_control/mc_pos_control_main.cpp
+14 -1
View File
@@ -66,6 +66,7 @@
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/omni_attitude_status.h>
#include "PositionControl/PositionControl.hpp"
#include "Takeoff/Takeoff.hpp"
@@ -114,6 +115,8 @@ private:
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */
uORB::Publication<vehicle_local_position_setpoint_s> _traj_sp_pub{ORB_ID(trajectory_setpoint)}; /**< trajectory setpoints publication */
uORB::Publication<omni_attitude_status_s> _omni_attitude_status_pub{ORB_ID(omni_attitude_status)};
uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
@@ -688,9 +691,19 @@ MulticopterPositionControl::Run()
vehicle_attitude_setpoint_s attitude_setpoint{};
attitude_setpoint.timestamp = time_stamp_now;
omni_attitude_status_s omni_status{};
omni_status.timestamp = time_stamp_now;
_control.getAttitudeSetpoint(matrix::Quatf(att.q), _param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(),
_tilt_angle, _tilt_dir, _tilt_roll, _tilt_pitch, _param_omni_att_rate.get(), _param_omni_proj_axes.get(),
attitude_setpoint);
attitude_setpoint, omni_status);
omni_status.att_mode = _param_omni_att_mode.get();
omni_status.tilt_angle_est = _tilt_angle;
omni_status.tilt_direction_est = _tilt_dir;
omni_status.tilt_roll_est = _tilt_roll;
omni_status.tilt_pitch_est = _tilt_pitch;
_omni_attitude_status_pub.publish(omni_status);
// Part of landing logic: if ground-contact/maybe landed was detected, turn off
// controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic.