yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

mathlib fixes

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-19 14:10:25 +04:00
parent e3a5a384d7
commit ba612c3ee8
22 changed files with 513 additions and 1191 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/sensors/sensors.cpp
View File

@@ -211,8 +211,8 @@ private:
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
struct {
@@ -457,8 +457,8 @@ Sensors::Sensors() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
_board_rotation(3, 3),
_external_mag_rotation(3, 3),
_board_rotation(),
_external_mag_rotation(),
_mag_is_external(false)
{
@@ -904,7 +904,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
math::Vector3 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);
@@ -930,7 +930,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
math::Vector3 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);
@@ -956,7 +956,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
if (_mag_is_external)
vect = _external_mag_rotation * vect;