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

removed comments and fixed some euler bugs

Browse Source
This commit is contained in:
Roman 2016-04-20 22:06:11 +02:00 committed by Lorenz Meier
parent 5e0e522903
commit b8a219d351
6 changed files with 5 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -836,12 +836,6 @@ void AttitudePositionEstimatorEKF::publishAttitude()
_att.q[2] = _ekf->states[2];
_att.q[3] = _ekf->states[3];
_att.q_valid = true;
//_att.R_valid = true;
//_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;

7
src/examples/px4_simple_app/px4_simple_app.c
View File

@ -106,10 +106,9 @@ int px4_simple_app_main(int argc, char *argv[])
(double)raw.accelerometer_m_s2[1],
(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];
/* set att and publish this information for other apps
the following does not have any meaning, it's just an example
*/
att.q[0] = raw.accelerometer_m_s2[0];
att.q[1] = raw.accelerometer_m_s2[1];
att.q[2] = raw.accelerometer_m_s2[2];

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

@ -468,10 +468,6 @@ void AttitudeEstimatorQ::task_main()
struct vehicle_attitude_s att = {};
att.timestamp = sensors.timestamp;
//att.roll = euler(0);
//att.pitch = euler(1);
//att.yaw = euler(2);
att.rollspeed = _rates(0);
att.pitchspeed = _rates(1);
att.yawspeed = _rates(2);
@ -483,11 +479,6 @@ void AttitudeEstimatorQ::task_main()
///* copy offsets */
//memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets));
//Matrix<3, 3> R = _q.to_dcm();
///* copy rotation matrix */
//memcpy(&att.R[0], R.data, sizeof(att.R));
//att.R_valid = true;
memcpy(&att.q[0], _q.data, sizeof(att.q));
att.q_valid = true;

4
src/modules/ekf2/ekf2_main.cpp
View File

@ -721,10 +721,6 @@ void Ekf2::task_main()
// generate remaining vehicle attitude data
att.timestamp = hrt_absolute_time();
//matrix::Euler<float> euler(q);
//att.roll = euler(0);
//att.pitch = euler(1);
//att.yaw = euler(2);
att.q[0] = q(0);
att.q[1] = q(1);

7
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -1920,8 +1920,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
math::Vector<3> euler = C_nb.to_euler();
hil_attitude.timestamp = timestamp;
//memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R));
//hil_attitude.R_valid = true;
hil_attitude.q[0] = q(0);
hil_attitude.q[1] = q(1);
@ -1929,9 +1927,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_attitude.q[3] = q(3);
hil_attitude.q_valid = true;
//hil_attitude.roll = euler(0);
//hil_attitude.pitch = euler(1);
//hil_attitude.yaw = euler(2);
hil_attitude.rollspeed = hil_state.rollspeed;
hil_attitude.pitchspeed = hil_state.pitchspeed;
hil_attitude.yawspeed = hil_state.yawspeed;
@ -1957,7 +1952,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_global_pos.vel_n = hil_state.vx / 100.0f;
hil_global_pos.vel_e = hil_state.vy / 100.0f;
hil_global_pos.vel_d = hil_state.vz / 100.0f;
hil_global_pos.yaw = euler(1);
hil_global_pos.yaw = euler(2);
hil_global_pos.eph = 2.0f;
hil_global_pos.epv = 4.0f;

2
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
View File

@ -1337,7 +1337,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.z = z_est[0];
local_pos.vz = z_est[1];
matrix::Euler<float> euler(R);
local_pos.yaw = euler(0);
local_pos.yaw = euler(2);
local_pos.dist_bottom_valid = dist_bottom_valid;
local_pos.eph = eph;
local_pos.epv = epv;