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

Fixed axis assignment and raw value outputs. Scaling and offsets to be done

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-26 13:51:18 +02:00
parent 60311a3778
commit 6026595d83
3 changed files with 44 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/sensors/sensors.cpp
+18 -16
View File
@@ -2,8 +2,6 @@
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -604,14 +602,16 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
read(_fd_bma180, buf, sizeof(buf));
accel_report.timestamp = hrt_absolute_time();
accel_report.x_raw = buf[0];
accel_report.y_raw = buf[1];
accel_report.z_raw = buf[2];
/* XXX scale raw values to readings */
accel_report.x = 0;
accel_report.y = 0;
accel_report.z = 0;
accel_report.x_raw = (buf[1] == -32768) ? 32767 : -buf[1];
accel_report.y_raw = (buf[0] == -32768) ? -32767 : buf[0];
accel_report.z_raw = (buf[2] == -32768) ? -32767 : buf[2];
const float range_g = 4.0f;
/* scale from 14 bit to m/s2 */
accel_report.x = (((accel_report.x_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
accel_report.y = (((accel_report.y_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
accel_report.z = (((accel_report.z_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
} else {
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
@@ -640,14 +640,16 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
read(_fd_gyro_l3gd20, buf, sizeof(buf));
gyro_report.timestamp = hrt_absolute_time();
gyro_report.x_raw = buf[0];
gyro_report.y_raw = buf[1];
gyro_report.z_raw = buf[2];
/* XXX scale raw values to readings */
gyro_report.x = 0;
gyro_report.y = 0;
gyro_report.z = 0;
gyro_report.x_raw = ((buf[1] == -32768) ? -32768 : buf[1]);
gyro_report.y_raw = ((buf[0] == -32768) ? 32767 : -buf[0]);
gyro_report.z_raw = ((buf[2] == -32768) ? -32768 : buf[2]);
/* scaling calculated as: raw * (1/(32768*(500/180*PI))) */
gyro_report.x = (gyro_report.x_raw - _parameters.gyro_offset[0]) * 0.000266316109f;
gyro_report.y = (gyro_report.y_raw - _parameters.gyro_offset[1]) * 0.000266316109f;
gyro_report.z = (gyro_report.z_raw - _parameters.gyro_offset[2]) * 0.000266316109f;
} else {
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);