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

mathlib: Vector class major cleanup

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-24 23:56:28 +04:00
parent a26e46bede
commit 9dfe366e90
7 changed files with 210 additions and 193 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+4 -4
View File
@@ -132,7 +132,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
_sensors.magnetometer_ga[2]);
// initialize dcm
C_nb.from_quaternion(q);
C_nb = q.to_dcm();
// HPos is constant
HPos(0, 3) = 1.0f;
@@ -409,7 +409,7 @@ int KalmanNav::predictState(float dt)
}
// C_nb update
C_nb.from_quaternion(q);
C_nb = q.to_dcm();
// euler update
Vector<3> euler = C_nb.to_euler();
@@ -628,7 +628,7 @@ int KalmanNav::correctAtt()
Vector<9> xCorrect = K * y;
// check correciton is sane
for (size_t i = 0; i < xCorrect.getRows(); i++) {
for (size_t i = 0; i < xCorrect.get_size(); i++) {
float val = xCorrect(i);
if (isnan(val) || isinf(val)) {
@@ -694,7 +694,7 @@ int KalmanNav::correctPos()
Vector<9> xCorrect = K * y;
// check correction is sane
for (size_t i = 0; i < xCorrect.getRows(); i++) {
for (size_t i = 0; i < xCorrect.get_size(); i++) {
float val = xCorrect(i);
if (!isfinite(val)) {
src/modules/mavlink/mavlink_receiver.cpp
+1 -2
View File
@@ -683,8 +683,7 @@ handle_message(mavlink_message_t *msg)
/* Calculate Rotation Matrix */
math::Quaternion q(hil_state.attitude_quaternion);
math::Matrix<3,3> C_nb;
C_nb.from_quaternion(q);
math::Matrix<3,3> C_nb = q.to_dcm();
math::Vector<3> euler = C_nb.to_euler();
/* set rotation matrix */
src/modules/sensors/sensors.cpp
+2 -4
View File
@@ -465,8 +465,6 @@ Sensors::Sensors() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
_board_rotation(),
_external_mag_rotation(),
_mag_is_external(false)
{
@@ -920,7 +918,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
math::Vector<3> vect = {accel_report.x, accel_report.y, accel_report.z};
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
raw.accelerometer_m_s2[0] = vect(0);
@@ -946,7 +944,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
math::Vector<3> vect = {gyro_report.x, gyro_report.y, gyro_report.z};
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
raw.gyro_rad_s[0] = vect(0);