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

Merge pull request #3140 from tumbili/control_state 

Control state
This commit is contained in:
Roman Bapst 2015-11-06 17:26:06 +01:00
parent 9b45daa8f7 e1924c5d66
commit b3554f2520
16 changed files with 332 additions and 197 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/jMAVSim

@ -1 +1 @@
Subproject commit fd9ed5df0496d42f20b91cb405639ceccbcf9e17
Subproject commit 96029523d13fc7fdac04c1a50b08c0eb0b39f9a9

2
Tools/sitl_gazebo

@ -1 +1 @@
Subproject commit 7d855cd3853c6c6c41237b114bbc1fd9a3cf102f
Subproject commit d362e661b46474153f43f51a6eb947d4fda1bebe

1
cmake/ros-CMakeLists.txt
View File

@ -81,6 +81,7 @@ add_message_files(
offboard_control_mode.msg
vehicle_force_setpoint.msg
distance_sensor.msg
control_state.msg
)
## Generate services in the 'srv' folder

18
msg/control_state.msg Normal file
View File

@ -0,0 +1,18 @@
# This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */
uint64 timestamp # in microseconds since system start
float32 x_acc # X acceleration in body frame
float32 y_acc # Y acceleration in body frame
float32 z_acc # Z acceleration in body frame
float32 x_vel # X velocity in body frame
float32 y_vel # Y velocity in body frame
float32 z_vel # Z velocity in body frame
float32 x_pos # X position in local frame
float32 y_pos # Y position in local frame
float32 z_pos # z position in local frame
float32 airspeed # Airspeed, estimated
float32[3] vel_variance # Variance in body velocity estimate
float32[3] pos_variance # Variance in local position estimate
float32[4] q # Attitude Quaternion
float32 roll_rate # Roll body angular rate (rad/s, x forward/y right/z down)
float32 pitch_rate # Pitch body angular rate (rad/s, x forward/y right/z down)
float32 yaw_rate # Yaw body angular rate (rad/s, x forward/y right/z down)

30
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@ -54,6 +54,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vision_position_estimate.h>
@ -123,6 +124,7 @@ private:
int _mocap_sub = -1;
int _global_pos_sub = -1;
orb_advert_t _att_pub = nullptr;
orb_advert_t _ctrl_state_pub = nullptr;
struct {
param_t w_acc;
@ -523,6 +525,34 @@ void AttitudeEstimatorQ::task_main()
} else {
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
}
struct control_state_s ctrl_state = {};
ctrl_state.timestamp = sensors.timestamp;
/* Attitude quaternions for control state */
ctrl_state.q[0] = _q(0);
ctrl_state.q[1] = _q(1);
ctrl_state.q[2] = _q(2);
ctrl_state.q[3] = _q(3);
/* Attitude rates for control state */
ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));
ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));
ctrl_state.roll_rate = _rates(2);
/* Publish to control state topic */
if (_ctrl_state_pub == nullptr) {
_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
} else {
orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state);
}
}
}

9
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -49,6 +49,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_status.h>
@ -154,12 +155,14 @@ private:
int _armedSub;
orb_advert_t _att_pub; /**< vehicle attitude */
orb_advert_t _ctrl_state_pub; /**< control state */
orb_advert_t _global_pos_pub; /**< global position */
orb_advert_t _local_pos_pub; /**< position in local frame */
orb_advert_t _estimator_status_pub; /**< status of the estimator */
orb_advert_t _wind_pub; /**< wind estimate */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct gyro_report _gyro;
struct accel_report _accel;
struct mag_report _mag;
@ -320,6 +323,12 @@ private:
**/
void publishAttitude();
/**
* @brief
* Publish the system state for control modules
**/
void publishControlState();
/**
* @brief
* Publish local position relative to reference point where filter was initialized

83
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -134,12 +134,14 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
/* publications */
_att_pub(nullptr),
_ctrl_state_pub(nullptr),
_global_pos_pub(nullptr),
_local_pos_pub(nullptr),
_estimator_status_pub(nullptr),
_wind_pub(nullptr),
_att{},
_ctrl_state{},
_gyro{},
_accel{},
_mag{},
@ -698,6 +700,9 @@ void AttitudePositionEstimatorEKF::task_main()
// Publish attitude estimations
publishAttitude();
// publish control state
publishControlState();
// Publish Local Position estimations
publishLocalPosition();
@ -822,9 +827,9 @@ void AttitudePositionEstimatorEKF::publishAttitude()
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
_att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _ekf->dAngIMU.z / _ekf->dtIMU - _ekf->states[12] / _ekf->dtIMUfilt;
// gyro offsets
_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
@ -833,7 +838,7 @@ void AttitudePositionEstimatorEKF::publishAttitude()
/* lazily publish the attitude only once available */
if (_att_pub != nullptr) {
/* publish the attitude setpoint */
/* publish the attitude */
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
} else {
@ -842,6 +847,76 @@ void AttitudePositionEstimatorEKF::publishAttitude()
}
}
void AttitudePositionEstimatorEKF::publishControlState()
{
/* Accelerations in Body Frame */
_ctrl_state.x_acc = _ekf->accel.x;
_ctrl_state.y_acc = _ekf->accel.y;
_ctrl_state.z_acc = _ekf->accel.z;
/* Velocity in Body Frame */
Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]);
Vector3f v_n_var(_ekf->P[4][4], _ekf->P[5][5], _ekf->P[6][6]);
Vector3f v_b = _ekf->Tnb * v_n;
Vector3f v_b_var = _ekf->Tnb * v_n_var;
_ctrl_state.x_vel = v_b.x;
_ctrl_state.y_vel = v_b.y;
_ctrl_state.z_vel = v_b.z;
_ctrl_state.vel_variance[0] = v_b_var.x;
_ctrl_state.vel_variance[1] = v_b_var.y;
_ctrl_state.vel_variance[2] = v_b_var.z;
/* Local Position */
_ctrl_state.x_pos = _ekf->states[7];
_ctrl_state.y_pos = _ekf->states[8];
// XXX need to announce change of Z reference somehow elegantly
_ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset;
_ctrl_state.pos_variance[0] = _ekf->P[7][7];
_ctrl_state.pos_variance[1] = _ekf->P[8][8];
_ctrl_state.pos_variance[2] = _ekf->P[9][9];
/* Attitude */
_ctrl_state.timestamp = _last_sensor_timestamp;
_ctrl_state.q[0] = _ekf->states[0];
_ctrl_state.q[1] = _ekf->states[1];
_ctrl_state.q[2] = _ekf->states[2];
_ctrl_state.q[3] = _ekf->states[3];
/* Airspeed (Groundspeed - Windspeed) */
_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2));
/* Attitude Rates */
_ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
_ctrl_state.yaw_rate = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
/* Guard from bad data */
if (!PX4_ISFINITE(_ctrl_state.x_vel) ||
!PX4_ISFINITE(_ctrl_state.y_vel) ||
!PX4_ISFINITE(_ctrl_state.z_vel) ||
!PX4_ISFINITE(_ctrl_state.x_pos) ||
!PX4_ISFINITE(_ctrl_state.y_pos) ||
!PX4_ISFINITE(_ctrl_state.z_pos))
{
// bad data, abort publication
return;
}
/* lazily publish the control state only once available */
if (_ctrl_state_pub != nullptr) {
/* publish the control state */
orb_publish(ORB_ID(control_state), _ctrl_state_pub, &_ctrl_state);
} else {
/* advertise and publish */
_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &_ctrl_state);
}
}
void AttitudePositionEstimatorEKF::publishLocalPosition()
{
_local_pos.timestamp = _last_sensor_timestamp;

6
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
View File

@ -48,6 +48,8 @@
#define M_PI_F static_cast<float>(M_PI)
#endif
#define MIN_AIRSPEED_MEAS 5.0f
constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f;
AttPosEKF::AttPosEKF() :
@ -1686,7 +1688,7 @@ void AttPosEKF::FuseAirspeed()
// Calculate the predicted airspeed
VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
// Perform fusion of True Airspeed measurement
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > MIN_AIRSPEED_MEAS))
{
// Calculate observation jacobians
SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
@ -2594,7 +2596,7 @@ void AttPosEKF::CovarianceInit()
P[13][13] = sq(0.2f*dtIMU);
//Wind velocities
P[14][14] = 0.0f;
P[14][14] = 0.01f;
P[15][15] = P[14][14];
//Earth magnetic field

138
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -58,7 +58,6 @@
#include <drivers/drv_accel.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
@ -67,7 +66,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
@ -125,11 +124,10 @@ private:
bool _task_running; /**< if true, task is running in its mainloop */
int _control_task; /**< task handle */
int _att_sub; /**< vehicle attitude subscription */
int _ctrl_state_sub; /**< control state subscription */
int _accel_sub; /**< accelerometer subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _attitude_sub; /**< raw rc channels data subscription */
int _airspeed_sub; /**< airspeed subscription */
int _vcontrol_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
@ -144,12 +142,11 @@ private:
orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure
orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct accel_report _accel; /**< body frame accelerations */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */
struct actuator_controls_s _actuators; /**< actuator control inputs */
struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */
@ -242,6 +239,11 @@ private:
} _parameter_handles; /**< handles for interesting parameters */
// Rotation matrix and euler angles to extract from control state
math::Matrix<3, 3> _R;
float _roll;
float _pitch;
float _yaw;
ECL_RollController _roll_ctrl;
ECL_PitchController _pitch_ctrl;
@ -269,12 +271,6 @@ private:
*/
void vehicle_manual_poll();
/**
* Check for airspeed updates.
*/
void vehicle_airspeed_poll();
/**
* Check for accel updates.
*/
@ -326,9 +322,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_control_task(-1),
/* subscriptions */
_att_sub(-1),
_ctrl_state_sub(-1),
_accel_sub(-1),
_airspeed_sub(-1),
_vcontrol_mode_sub(-1),
_params_sub(-1),
_manual_sub(-1),
@ -353,12 +348,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_debug(false)
{
/* safely initialize structs */
_att = {};
_ctrl_state = {};
_accel = {};
_att_sp = {};
_rates_sp = {};
_manual = {};
_airspeed = {};
_vcontrol_mode = {};
_actuators = {};
_actuators_airframe = {};
@ -539,18 +533,6 @@ FixedwingAttitudeControl::vehicle_manual_poll()
}
}
void
FixedwingAttitudeControl::vehicle_airspeed_poll()
{
/* check if there is a new position */
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
}
}
void
FixedwingAttitudeControl::vehicle_accel_poll()
{
@ -623,9 +605,8 @@ FixedwingAttitudeControl::task_main()
* do subscriptions
*/
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@ -641,7 +622,6 @@ FixedwingAttitudeControl::task_main()
parameters_update();
/* get an initial update for all sensor and status data */
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
@ -654,7 +634,7 @@ FixedwingAttitudeControl::task_main()
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _att_sub;
fds[1].fd = _ctrl_state_sub;
fds[1].events = POLLIN;
_task_running = true;
@ -699,9 +679,19 @@ FixedwingAttitudeControl::task_main()
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
/* get current rotation matrix and euler angles from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
*
@ -719,50 +709,36 @@ FixedwingAttitudeControl::task_main()
* Rxy Ryy Rzy -Rzy Ryy Rxy
* Rxz Ryz Rzz -Rzz Ryz Rxz
* */
math::Matrix<3, 3> R; //original rotation matrix
math::Matrix<3, 3> R_adapted; //modified rotation matrix
R.set(_att.R);
R_adapted.set(_att.R);
math::Matrix<3, 3> R_adapted = _R; //modified rotation matrix
/* move z to x */
R_adapted(0, 0) = R(0, 2);
R_adapted(1, 0) = R(1, 2);
R_adapted(2, 0) = R(2, 2);
R_adapted(0, 0) = _R(0, 2);
R_adapted(1, 0) = _R(1, 2);
R_adapted(2, 0) = _R(2, 2);
/* move x to z */
R_adapted(0, 2) = R(0, 0);
R_adapted(1, 2) = R(1, 0);
R_adapted(2, 2) = R(2, 0);
R_adapted(0, 2) = _R(0, 0);
R_adapted(1, 2) = _R(1, 0);
R_adapted(2, 2) = _R(2, 0);
/* change direction of pitch (convert to right handed system) */
R_adapted(0, 0) = -R_adapted(0, 0);
R_adapted(1, 0) = -R_adapted(1, 0);
R_adapted(2, 0) = -R_adapted(2, 0);
math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation
euler_angles = R_adapted.to_euler();
euler_angles = R_adapted.to_euler(); //adapted euler angles for fixed wing operation
/* fill in new attitude data */
_att.roll = euler_angles(0);
_att.pitch = euler_angles(1);
_att.yaw = euler_angles(2);
PX4_R(_att.R, 0, 0) = R_adapted(0, 0);
PX4_R(_att.R, 0, 1) = R_adapted(0, 1);
PX4_R(_att.R, 0, 2) = R_adapted(0, 2);
PX4_R(_att.R, 1, 0) = R_adapted(1, 0);
PX4_R(_att.R, 1, 1) = R_adapted(1, 1);
PX4_R(_att.R, 1, 2) = R_adapted(1, 2);
PX4_R(_att.R, 2, 0) = R_adapted(2, 0);
PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
PX4_R(_att.R, 2, 2) = R_adapted(2, 2);
_R = R_adapted;
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
/* lastly, roll- and yawspeed have to be swaped */
float helper = _att.rollspeed;
_att.rollspeed = -_att.yawspeed;
_att.yawspeed = helper;
float helper = _ctrl_state.roll_rate;
_ctrl_state.roll_rate = -_ctrl_state.yaw_rate;
_ctrl_state.yaw_rate = helper;
}
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
@ -813,15 +789,14 @@ FixedwingAttitudeControl::task_main()
float airspeed;
/* if airspeed is not updating, we assume the normal average speed */
if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed)) {
airspeed = _parameters.airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
airspeed = math::max(0.5f, _ctrl_state.airspeed);
}
/*
@ -866,7 +841,7 @@ FixedwingAttitudeControl::task_main()
/* the pilot does not want to change direction,
* take straight attitude setpoint from position controller
*/
if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) {
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
} else {
roll_sp = (_manual.y * _parameters.man_roll_max)
@ -957,27 +932,18 @@ FixedwingAttitudeControl::task_main()
}
/* Prepare speed_body_u and speed_body_w */
float speed_body_u = 0.0f;
float speed_body_v = 0.0f;
float speed_body_w = 0.0f;
if(_att.R_valid) {
speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d;
speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d;
speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d;
} else {
if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
}
}
float speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d;
float speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d;
float speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d;
/* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {};
control_input.roll = _att.roll;
control_input.pitch = _att.pitch;
control_input.yaw = _att.yaw;
control_input.roll_rate = _att.rollspeed;
control_input.pitch_rate = _att.pitchspeed;
control_input.yaw_rate = _att.yawspeed;
control_input.roll = _roll;
control_input.pitch = _pitch;
control_input.yaw = _yaw;
control_input.roll_rate = _ctrl_state.roll_rate;
control_input.pitch_rate = _ctrl_state.pitch_rate;
control_input.yaw_rate = _ctrl_state.yaw_rate;
control_input.speed_body_u = speed_body_u;
control_input.speed_body_v = speed_body_v;
control_input.speed_body_w = speed_body_w;
@ -1100,9 +1066,9 @@ FixedwingAttitudeControl::task_main()
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _att.timestamp;
_actuators.timestamp_sample = _ctrl_state.timestamp;
_actuators_airframe.timestamp = hrt_absolute_time();
_actuators_airframe.timestamp_sample = _att.timestamp;
_actuators_airframe.timestamp_sample = _ctrl_state.timestamp;
/* Only publish if any of the proper modes are enabled */
if(_vcontrol_mode.flag_control_rates_enabled ||

124
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -68,14 +68,13 @@
#include <drivers/drv_accel.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/sensor_combined.h>
@ -153,8 +152,7 @@ private:
int _global_pos_sub;
int _pos_sp_triplet_sub;
int _att_sub; /**< vehicle attitude subscription */
int _airspeed_sub; /**< airspeed subscription */
int _ctrl_state_sub; /**< control state subscription */
int _control_mode_sub; /**< control mode subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
@ -165,11 +163,10 @@ private:
orb_advert_t _tecs_status_pub; /**< TECS status publication */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _control_mode; /**< control mode */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
@ -222,6 +219,9 @@ private:
float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s
bool _global_pos_valid; ///< global position is valid
math::Matrix<3, 3> _R_nb; ///< current attitude
float _roll;
float _pitch;
float _yaw;
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
@ -356,14 +356,9 @@ private:
bool vehicle_manual_control_setpoint_poll();
/**
* Check for airspeed updates.
* Check for changes in control state.
*/
bool vehicle_airspeed_poll();
/**
* Check for position updates.
*/
void vehicle_attitude_poll();
void control_state_poll();
/**
* Check for accel updates.
@ -481,8 +476,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
/* subscriptions */
_global_pos_sub(-1),
_pos_sp_triplet_sub(-1),
_att_sub(-1),
_airspeed_sub(-1),
_ctrl_state_sub(-1),
_control_mode_sub(-1),
_vehicle_status_sub(-1),
_params_sub(-1),
@ -495,11 +489,10 @@ FixedwingPositionControl::FixedwingPositionControl() :
_nav_capabilities_pub(nullptr),
/* states */
_att(),
_ctrl_state(),
_att_sp(),
_nav_capabilities(),
_manual(),
_airspeed(),
_control_mode(),
_vehicle_status(),
_global_pos(),
@ -749,32 +742,6 @@ FixedwingPositionControl::vehicle_status_poll()
}
}
bool
FixedwingPositionControl::vehicle_airspeed_poll()
{
/* check if there is an airspeed update or if it timed out */
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
_airspeed_valid = true;
_airspeed_last_valid = hrt_absolute_time();
} else {
/* no airspeed updates for one second */
if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
_airspeed_valid = false;
}
}
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
return airspeed_updated;
}
bool
FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
{
@ -790,21 +757,38 @@ FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
return manual_updated;
}
void
FixedwingPositionControl::vehicle_attitude_poll()
FixedwingPositionControl::control_state_poll()
{
/* check if there is a new position */
bool att_updated;
orb_check(_att_sub, &att_updated);
bool ctrl_state_updated;
orb_check(_ctrl_state_sub, &ctrl_state_updated);
if (att_updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
if (ctrl_state_updated) {
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
_airspeed_valid = true;
_airspeed_last_valid = hrt_absolute_time();
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_R_nb(i, j) = PX4_R(_att.R, i, j);
} else {
/* no airspeed updates for one second */
if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
_airspeed_valid = false;
}
}
/* set rotation matrix and euler angles */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R_nb = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R_nb.to_euler();
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
/* update TECS state */
_tecs.enable_airspeed(_airspeed_valid);
}
void
@ -853,7 +837,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
float airspeed;
if (_airspeed_valid) {
airspeed = _airspeed.true_airspeed_m_s;
airspeed = _ctrl_state.airspeed;
} else {
airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
@ -1084,7 +1068,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* update TECS filters */
if (!_mTecs.getEnabled()) {
_tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb,
_tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb,
accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
}
@ -1117,7 +1101,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
@ -1127,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset hold altitude */
_hold_alt = _global_pos.alt;
/* reset hold yaw */
_hdg_hold_yaw = _att.yaw;
_hdg_hold_yaw = _yaw;
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
@ -1221,14 +1205,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (pos_sp_triplet.previous.valid) {
target_bearing = bearing_lastwp_currwp;
} else {
target_bearing = _att.yaw;
target_bearing = _yaw;
}
mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
}
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw));
_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d);
_l1_control.navigate_heading(target_bearing, _yaw, ground_speed_2d);
land_noreturn_horizontal = true;
@ -1469,7 +1453,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _global_pos.alt;
_hdg_hold_yaw = _att.yaw;
_hdg_hold_yaw = _yaw;
_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
_yaw_lock_engaged = false;
}
@ -1478,7 +1462,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
@ -1524,7 +1508,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) {
/* heading / roll is zero, lock onto current heading */
if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
// little yaw movement, lock to current heading
_yaw_lock_engaged = true;
@ -1543,7 +1527,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* just switched back from non heading-hold to heading hold */
if (!_hdg_hold_enabled) {
_hdg_hold_enabled = true;
_hdg_hold_yaw = _att.yaw;
_hdg_hold_yaw = _yaw;
get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
}
@ -1591,7 +1575,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
_mTecs.resetDerivatives(_ctrl_state.airspeed);
} else {
_tecs.reset_state();
}
@ -1704,11 +1688,10 @@ FixedwingPositionControl::task_main()
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@ -1785,10 +1768,9 @@ FixedwingPositionControl::task_main()
// XXX add timestamp check
_global_pos_valid = true;
vehicle_attitude_poll();
control_state_poll();
vehicle_setpoint_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
vehicle_manual_control_setpoint_poll();
// vehicle_baro_poll();
@ -1894,7 +1876,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
/* use pitch max set by MT param */
limitOverride.disablePitchMaxOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode,
limitOverride);
} else {
if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
@ -1907,8 +1889,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
_tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));
/* Using tecs library */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
_tecs.update_pitch_throttle(_R_nb, _pitch, altitude, alt_sp, v_sp,
_ctrl_state.airspeed, eas2tas,
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);

32
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -74,7 +74,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
@ -126,7 +126,7 @@ private:
bool _task_should_exit; /**< if true, task_main() should exit */
int _control_task; /**< task handle */
int _v_att_sub; /**< vehicle attitude subscription */
int _ctrl_state_sub; /**< control state subscription */
int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */
int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */
int _v_control_mode_sub; /**< vehicle control mode subscription */
@ -145,7 +145,7 @@ private:
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
struct vehicle_attitude_s _v_att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
@ -297,7 +297,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_control_task(-1),
/* subscriptions */
_v_att_sub(-1),
_ctrl_state_sub(-1),
_v_att_sp_sub(-1),
_v_control_mode_sub(-1),
_params_sub(-1),
@ -319,7 +319,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency"))
{
memset(&_v_att, 0, sizeof(_v_att));
memset(&_ctrl_state, 0, sizeof(_ctrl_state));
memset(&_v_att_sp, 0, sizeof(_v_att_sp));
memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
@ -599,9 +599,9 @@ MulticopterAttitudeControl::control_attitude(float dt)
math::Matrix<3, 3> R_sp;
R_sp.set(_v_att_sp.R_body);
/* rotation matrix for current state */
math::Matrix<3, 3> R;
R.set(_v_att.R);
/* get current rotation matrix from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Matrix<3, 3> R = q_att.to_dcm();
/* all input data is ready, run controller itself */
@ -693,9 +693,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
/* current body angular rates */
math::Vector<3> rates;
rates(0) = _v_att.rollspeed;
rates(1) = _v_att.pitchspeed;
rates(2) = _v_att.yawspeed;
rates(0) = _ctrl_state.roll_rate;
rates(1) = _ctrl_state.pitch_rate;
rates(2) = _ctrl_state.yaw_rate;
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
@ -733,7 +733,7 @@ MulticopterAttitudeControl::task_main()
*/
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@ -747,7 +747,7 @@ MulticopterAttitudeControl::task_main()
/* wakeup source: vehicle attitude */
px4_pollfd_struct_t fds[1];
fds[0].fd = _v_att_sub;
fds[0].fd = _ctrl_state_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
@ -783,8 +783,8 @@ MulticopterAttitudeControl::task_main()
dt = 0.02f;
}
/* copy attitude topic */
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
/* copy attitude and control state topics */
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
/* check for updates in other topics */
parameter_update_poll();
@ -862,7 +862,7 @@ MulticopterAttitudeControl::task_main()
_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _v_att.timestamp;
_actuators.timestamp_sample = _ctrl_state.timestamp;
_controller_status.roll_rate_integ = _rates_int(0);
_controller_status.pitch_rate_integ = _rates_int(1);

46
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -68,7 +68,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
@ -126,7 +126,7 @@ private:
int _mavlink_fd; /**< mavlink fd */
int _vehicle_status_sub; /**< vehicle status subscription */
int _att_sub; /**< vehicle attitude subscription */
int _ctrl_state_sub; /**< control state subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _control_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< notification of parameter updates */
@ -142,7 +142,7 @@ private:
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
@ -230,6 +230,9 @@ private:
math::Vector<3> _vel_prev; /**< velocity on previous step */
math::Vector<3> _vel_ff;
math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */
float _yaw; /**< yaw angle (euler) */
/**
* Update our local parameter cache.
*/
@ -319,7 +322,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_mavlink_fd(-1),
/* subscriptions */
_att_sub(-1),
_ctrl_state_sub(-1),
_att_sp_sub(-1),
_control_mode_sub(-1),
_params_sub(-1),
@ -347,10 +350,11 @@ MulticopterPositionControl::MulticopterPositionControl() :
_pos_hold_engaged(false),
_alt_hold_engaged(false),
_run_pos_control(true),
_run_alt_control(true)
_run_alt_control(true),
_yaw(0.0f)
{
memset(&_vehicle_status, 0, sizeof(_vehicle_status));
memset(&_att, 0, sizeof(_att));
memset(&_ctrl_state, 0, sizeof(_ctrl_state));
memset(&_att_sp, 0, sizeof(_att_sp));
memset(&_manual, 0, sizeof(_manual));
memset(&_control_mode, 0, sizeof(_control_mode));
@ -377,6 +381,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_vel_prev.zero();
_vel_ff.zero();
_R.identity();
_params_handles.thr_min = param_find("MPC_THR_MIN");
_params_handles.thr_max = param_find("MPC_THR_MAX");
_params_handles.z_p = param_find("MPC_Z_P");
@ -529,10 +535,10 @@ MulticopterPositionControl::poll_subscriptions()
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}
orb_check(_att_sub, &updated);
orb_check(_ctrl_state_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
}
orb_check(_att_sp_sub, &updated);
@ -1009,7 +1015,7 @@ MulticopterPositionControl::task_main()
* do subscriptions
*/
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
@ -1064,6 +1070,14 @@ MulticopterPositionControl::task_main()
}
poll_subscriptions();
/* get current rotation matrix and euler angles from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_yaw = euler_angles(2);
parameters_update(false);
hrt_abstime t = hrt_absolute_time();
@ -1139,7 +1153,7 @@ MulticopterPositionControl::task_main()
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _att.yaw;
_att_sp.yaw_body = _yaw;
_att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time();
@ -1324,11 +1338,11 @@ MulticopterPositionControl::task_main()
/* thrust compensation for altitude only control mode */
float att_comp;
if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / PX4_R(_att.R, 2, 2);
if (_R(2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / _R(2, 2);
} else if (PX4_R(_att.R, 2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f;
} else if (_R(2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f;
saturation_z = true;
} else {
@ -1515,7 +1529,7 @@ MulticopterPositionControl::task_main()
/* reset yaw setpoint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _att.yaw;
_att_sp.yaw_body = _yaw;
}
/* do not move yaw while arming */
@ -1525,7 +1539,7 @@ MulticopterPositionControl::task_main()
_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(yaw_target - _att.yaw);
float yaw_offs = _wrap_pi(yaw_target - _yaw);
// If the yaw offset became too big for the system to track stop
// shifting it

18
src/modules/sdlog2/sdlog2.c
View File

@ -73,6 +73,7 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
@ -1089,6 +1090,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vtol_vehicle_status_s vtol_status;
struct time_offset_s time_offset;
struct mc_att_ctrl_status_s mc_att_ctrl_status;
struct control_state_s ctrl_state;
} buf;
memset(&buf, 0, sizeof(buf));
@ -1137,6 +1139,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_ENCD_s log_ENCD;
struct log_TSYN_s log_TSYN;
struct log_MACS_s log_MACS;
struct log_CTS_s log_CTS;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@ -1179,6 +1182,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int encoders_sub;
int tsync_sub;
int mc_att_ctrl_status_sub;
int ctrl_state_sub;
} subs;
subs.cmd_sub = -1;
@ -1212,6 +1216,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.wind_sub = -1;
subs.tsync_sub = -1;
subs.mc_att_ctrl_status_sub = -1;
subs.ctrl_state_sub = -1;
subs.encoders_sub = -1;
/* add new topics HERE */
@ -1856,6 +1861,19 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(MACS);
}
/* --- CONTROL STATE --- */
if (copy_if_updated(ORB_ID(control_state), &subs.ctrl_state_sub, &buf.ctrl_state)) {
log_msg.msg_type = LOG_CTS_MSG;
log_msg.body.log_CTS.vx_body = buf.ctrl_state.x_vel;
log_msg.body.log_CTS.vy_body = buf.ctrl_state.y_vel;
log_msg.body.log_CTS.vz_body = buf.ctrl_state.z_vel;
log_msg.body.log_CTS.airspeed = buf.ctrl_state.airspeed;
log_msg.body.log_CTS.roll_rate = buf.ctrl_state.roll_rate;
log_msg.body.log_CTS.pitch_rate = buf.ctrl_state.pitch_rate;
log_msg.body.log_CTS.yaw_rate = buf.ctrl_state.yaw_rate;
LOGBUFFER_WRITE_AND_COUNT(CTS);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */

13
src/modules/sdlog2/sdlog2_messages.h
View File

@ -477,6 +477,18 @@ struct log_MACS_s {
float yaw_rate_integ;
};
/* --- CONTROL STATE --- */
#define LOG_CTS_MSG 47
struct log_CTS_s {
float vx_body;
float vy_body;
float vz_body;
float airspeed;
float roll_rate;
float pitch_rate;
float yaw_rate;
};
/* WARNING: ID 46 is already in use for ATTC1 */
/********** SYSTEM MESSAGES, ID > 0x80 **********/
@ -519,6 +531,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"),
LOG_FORMAT(VTOL, "f", "Arsp"),
LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),

3
src/modules/uORB/objects_common.cpp
View File

@ -69,6 +69,9 @@ ORB_DEFINE(pwm_input, struct pwm_input_s);
#include "topics/vehicle_attitude.h"
ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s);
#include "topics/control_state.h"
ORB_DEFINE(control_state, struct control_state_s);
#include "topics/sensor_combined.h"
ORB_DEFINE(sensor_combined, struct sensor_combined_s);

4
src/platforms/px4_includes.h
View File

@ -58,6 +58,7 @@
#include <px4_vehicle_rates_setpoint.h>
#include <px4_mc_virtual_rates_setpoint.h>
#include <px4_vehicle_attitude.h>
#include <px4_control_state.h>
#include <px4_vehicle_control_mode.h>
#include <px4_actuator_armed.h>
#include <px4_parameter_update.h>
@ -87,6 +88,7 @@
#include <platforms/nuttx/px4_messages/px4_actuator_controls_3.h>
#include <platforms/nuttx/px4_messages/px4_vehicle_rates_setpoint.h>
#include <platforms/nuttx/px4_messages/px4_vehicle_attitude.h>
#include <platforms/nuttx/px4_messages/px4_control_state.h>
#include <platforms/nuttx/px4_messages/px4_vehicle_control_mode.h>
#include <platforms/nuttx/px4_messages/px4_actuator_armed.h>
#include <platforms/nuttx/px4_messages/px4_parameter_update.h>
@ -120,6 +122,7 @@
#include <platforms/posix/px4_messages/px4_actuator_controls_3.h>
#include <platforms/posix/px4_messages/px4_vehicle_rates_setpoint.h>
#include <platforms/posix/px4_messages/px4_vehicle_attitude.h>
#include <platforms/posix/px4_messages/px4_control_state.h>
#include <platforms/posix/px4_messages/px4_vehicle_control_mode.h>
#include <platforms/posix/px4_messages/px4_actuator_armed.h>
#include <platforms/posix/px4_messages/px4_parameter_update.h>
@ -150,6 +153,7 @@
#include <platforms/qurt/px4_messages/px4_actuator_controls_3.h>
#include <platforms/qurt/px4_messages/px4_vehicle_rates_setpoint.h>
#include <platforms/qurt/px4_messages/px4_vehicle_attitude.h>
#include <platforms/qurt/px4_messages/px4_control_state.h>
#include <platforms/qurt/px4_messages/px4_vehicle_control_mode.h>
#include <platforms/qurt/px4_messages/px4_actuator_armed.h>
#include <platforms/qurt/px4_messages/px4_parameter_update.h>