fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 14:38:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

adapted to new vehicle attitude message

Browse Source
This commit is contained in:
tumbili
2016-10-18 20:46:08 +02:00
committed by Lorenz Meier
parent 215bfaa377
commit 5e0e522903
16 changed files with 142 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+16 -14
View File
@@ -820,15 +820,15 @@ void AttitudePositionEstimatorEKF::initializeGPS()
void AttitudePositionEstimatorEKF::publishAttitude()
{
// Output results
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
math::Matrix<3, 3> R = q.to_dcm();
math::Vector<3> euler = R.to_euler();
matrix::Quaternion<float> q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
//math::Matrix<3, 3> R = q.to_dcm();
//math::Vector<3> euler = R.to_euler();
for (int i = 0; i < 3; i++) {
/*for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
PX4_R(_att.R, i, j) = R(i, j);
}
}
}*/
_att.timestamp = _last_sensor_timestamp;
_att.q[0] = _ekf->states[0];
@@ -836,21 +836,21 @@ void AttitudePositionEstimatorEKF::publishAttitude()
_att.q[2] = _ekf->states[2];
_att.q[3] = _ekf->states[3];
_att.q_valid = true;
_att.R_valid = true;
//_att.R_valid = true;
_att.timestamp = _last_sensor_timestamp;
_att.roll = euler(0);
_att.pitch = euler(1);
_att.yaw = euler(2);
//_att.timestamp = _last_sensor_timestamp;
//_att.roll = euler(0);
//_att.pitch = euler(1);
//_att.yaw = euler(2);
_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;
_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMUfilt;
_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMUfilt;
//_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
//_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMUfilt;
//_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMUfilt;
/* lazily publish the attitude only once available */
if (_att_pub != nullptr) {
@@ -973,7 +973,9 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
_local_pos.xy_global = _gps_initialized; //TODO: Handle optical flow mode here
_local_pos.z_global = false;
_local_pos.yaw = _att.yaw;
matrix::Quaternion<float> q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
matrix::Euler<float> euler(q);
_local_pos.yaw = euler(2);
if (!PX4_ISFINITE(_local_pos.x) ||
!PX4_ISFINITE(_local_pos.y) ||
src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp
+4 -2
View File
@@ -87,11 +87,13 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
/* construct attitude setpoint rotation matrix */
math::Matrix<3, 3> R_sp;
R_sp.set(_v_att_sp->data().R_body);
matrix::Quaternion<float> q_sp(&_v_att_sp->data().q_d[0]);
R_sp.set(&q_sp._data[0][0]);
/* rotation matrix for current state */
math::Matrix<3, 3> R;
R.set(_v_att->data().R);
matrix::Quaternion<float> q(&_v_att->data().q[0]);
R.set(&q._data[0][0]);
/* all input data is ready, run controller itself */
src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp
+16 -10
View File
@@ -250,7 +250,9 @@ MulticopterPositionControlMultiplatform::reset_alt_sp()
//XXX hack until #1741 is in/ported
/* reset yaw sp */
_att_sp_msg.data().yaw_body = _att->data().yaw;
matrix::Quaternion<float> q(&_att->data().q[0]);
matrix::Euler<float> euler(q);
_att_sp_msg.data().yaw_body = euler(2);
//XXX: port this once a mavlink like interface is available
// mavlink_log_info(&_mavlink_log_pub, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
@@ -582,6 +584,10 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
static bool was_armed = false;
static uint64_t t_prev = 0;
matrix::Quaternion<float> q(&_att->data().q[0]);
matrix::Euler<float> euler(q);
matrix::Dcm<float> R(q);
uint64_t t = get_time_micros();
float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.005f;
t_prev = t;
@@ -641,7 +647,7 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
_att_sp_msg.data().roll_body = 0.0f;
_att_sp_msg.data().pitch_body = 0.0f;
_att_sp_msg.data().yaw_body = _att->data().yaw;
_att_sp_msg.data().yaw_body = euler(2);
_att_sp_msg.data().thrust = 0.0f;
_att_sp_msg.data().timestamp = get_time_micros();
@@ -815,11 +821,11 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
/* thrust compensation for altitude only control mode */
float att_comp;
if (PX4_R(_att->data().R, 2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / PX4_R(_att->data().R, 2, 2);
if (R(2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / R(2, 2);
} else if (PX4_R(_att->data().R, 2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att->data().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 {
@@ -1005,7 +1011,7 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
/* reset yaw setpoint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp_msg.data().yaw_body = _att->data().yaw;
_att_sp_msg.data().yaw_body = euler(2);
}
/* do not move yaw while arming */
@@ -1014,13 +1020,13 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
_att_sp_msg.data().yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
_att_sp_msg.data().yaw_body = _wrap_pi(_att_sp_msg.data().yaw_body + _att_sp_msg.data().yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - _att->data().yaw);
float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - euler(2));
if (yaw_offs < - YAW_OFFSET_MAX) {
_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw - YAW_OFFSET_MAX);
_att_sp_msg.data().yaw_body = _wrap_pi(euler(2) - YAW_OFFSET_MAX);
} else if (yaw_offs > YAW_OFFSET_MAX) {
_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw + YAW_OFFSET_MAX);
_att_sp_msg.data().yaw_body = _wrap_pi(euler(2) + YAW_OFFSET_MAX);
}
}
src/examples/px4_simple_app/px4_simple_app.c
+7 -3
View File
@@ -107,9 +107,13 @@ int px4_simple_app_main(int argc, char *argv[])
(double)raw.accelerometer_m_s2[2]);
/* set att and publish this information for other apps */
att.roll = raw.accelerometer_m_s2[0];
att.pitch = raw.accelerometer_m_s2[1];
att.yaw = raw.accelerometer_m_s2[2];
//att.roll = raw.accelerometer_m_s2[0];
//att.pitch = raw.accelerometer_m_s2[1];
//att.yaw = raw.accelerometer_m_s2[2];
att.q[0] = raw.accelerometer_m_s2[0];
att.q[1] = raw.accelerometer_m_s2[1];
att.q[2] = raw.accelerometer_m_s2[2];
orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
}