aboutsummaryrefslogtreecommitdiff
path: root/src/modules/sensors
diff options
context:
space:
mode:
authorAnton Babushkin <anton.babushkin@me.com>2013-12-19 14:10:25 +0400
committerAnton Babushkin <anton.babushkin@me.com>2013-12-19 14:10:25 +0400
commitba612c3ee8b88b9352e7cfa723997887dd736b76 (patch)
tree8b101b50259ffebd4de12891f4f684d6d84c3f53 /src/modules/sensors
parente3a5a384d7b3678d1cbef63dc28fbe9a8f1de940 (diff)
downloadpx4-firmware-ba612c3ee8b88b9352e7cfa723997887dd736b76.tar.gz
px4-firmware-ba612c3ee8b88b9352e7cfa723997887dd736b76.tar.bz2
px4-firmware-ba612c3ee8b88b9352e7cfa723997887dd736b76.zip
mathlib fixes
Diffstat (limited to 'src/modules/sensors')
-rw-r--r--src/modules/sensors/sensors.cpp14
1 files changed, 7 insertions, 7 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 6d38b98ec..5baab4a5d 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -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;