forked from Archive/PX4-Autopilot
enable reading sensor data from simulator module for SITL
This commit is contained in:
tumbili
Mark Charlebois
parent
f0a3210e94
commit
aef3f37ae0
|
|
@ -54,6 +54,8 @@
|
|||
#include <unistd.h>
|
||||
#include <px4_getopt.h>
|
||||
|
||||
#include <simulator/simulator.h>
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
|
|
@ -86,6 +88,8 @@ static const int ERROR = -1;
|
|||
|
||||
#define DIR_READ (1<<7)
|
||||
#define DIR_WRITE (0<<7)
|
||||
#define ACC_READ (0<<6)
|
||||
#define MAG_READ (1<<6)
|
||||
|
||||
extern "C" { __EXPORT int accelsim_main(int argc, char *argv[]); }
|
||||
|
||||
|
|
@ -512,6 +516,24 @@ ACCELSIM::reset()
|
|||
int
|
||||
ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len)
|
||||
{
|
||||
uint8_t cmd = send[0];
|
||||
|
||||
if (cmd & DIR_READ) {
|
||||
// Get data from the simulator
|
||||
Simulator *sim = Simulator::getInstance();
|
||||
if (sim == NULL)
|
||||
return ENODEV;
|
||||
|
||||
// FIXME - not sure what interrupt status should be
|
||||
recv[1] = 0;
|
||||
// skip cmd and status bytes
|
||||
if (cmd & ACC_READ) {
|
||||
sim->getRawAccelReport(&recv[2], len-2);
|
||||
} else if (cmd & MAG_READ) {
|
||||
sim->getMagReport(&recv[2], len-2);
|
||||
}
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
|
|
@ -986,9 +1008,10 @@ ACCELSIM::measure()
|
|||
struct {
|
||||
uint8_t cmd;
|
||||
uint8_t status;
|
||||
int16_t x;
|
||||
int16_t y;
|
||||
int16_t z;
|
||||
float temperature;
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} raw_accel_report;
|
||||
#pragma pack(pop)
|
||||
|
||||
|
|
@ -999,8 +1022,11 @@ ACCELSIM::measure()
|
|||
|
||||
/* fetch data from the sensor */
|
||||
memset(&raw_accel_report, 0, sizeof(raw_accel_report));
|
||||
raw_accel_report.cmd = DIR_READ;
|
||||
transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
|
||||
raw_accel_report.cmd = DIR_READ | ACC_READ;
|
||||
|
||||
if(OK != transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report))) {
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* 1) Scale raw value to SI units using scaling from datasheet.
|
||||
|
|
@ -1029,54 +1055,58 @@ ACCELSIM::measure()
|
|||
// whether it has had failures
|
||||
accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
|
||||
|
||||
accel_report.x_raw = raw_accel_report.x;
|
||||
accel_report.y_raw = raw_accel_report.y;
|
||||
accel_report.z_raw = raw_accel_report.z;
|
||||
accel_report.x_raw = (int16_t)(raw_accel_report.x/_accel_range_scale);
|
||||
accel_report.y_raw = (int16_t)(raw_accel_report.y / _accel_range_scale);
|
||||
accel_report.z_raw = (int16_t)(raw_accel_report.z / _accel_range_scale);
|
||||
|
||||
float xraw_f = raw_accel_report.x;
|
||||
float yraw_f = raw_accel_report.y;
|
||||
float zraw_f = raw_accel_report.z;
|
||||
// float xraw_f = (int16_t)(raw_accel_report.x/_accel_range_scale);
|
||||
// float yraw_f = (int16_t)(raw_accel_report.y / _accel_range_scale);
|
||||
// float zraw_f = (int16_t)(raw_accel_report.z / _accel_range_scale);
|
||||
|
||||
// apply user specified rotation
|
||||
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
|
||||
// // apply user specified rotation
|
||||
// rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
|
||||
|
||||
float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
|
||||
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
|
||||
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
|
||||
// float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
|
||||
// float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
|
||||
// float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
|
||||
|
||||
/*
|
||||
we have logs where the accelerometers get stuck at a fixed
|
||||
large value. We want to detect this and mark the sensor as
|
||||
being faulty
|
||||
*/
|
||||
if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
|
||||
fabsf(_last_accel[1] - y_in_new) < 0.001f &&
|
||||
fabsf(_last_accel[2] - z_in_new) < 0.001f &&
|
||||
fabsf(x_in_new) > 20 &&
|
||||
fabsf(y_in_new) > 20 &&
|
||||
fabsf(z_in_new) > 20) {
|
||||
_constant_accel_count += 1;
|
||||
} else {
|
||||
_constant_accel_count = 0;
|
||||
}
|
||||
if (_constant_accel_count > 100) {
|
||||
// we've had 100 constant accel readings with large
|
||||
// values. The sensor is almost certainly dead. We
|
||||
// will raise the error_count so that the top level
|
||||
// flight code will know to avoid this sensor, but
|
||||
// we'll still give the data so that it can be logged
|
||||
// and viewed
|
||||
perf_count(_bad_values);
|
||||
_constant_accel_count = 0;
|
||||
}
|
||||
// if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
|
||||
// fabsf(_last_accel[1] - y_in_new) < 0.001f &&
|
||||
// fabsf(_last_accel[2] - z_in_new) < 0.001f &&
|
||||
// fabsf(x_in_new) > 20 &&
|
||||
// fabsf(y_in_new) > 20 &&
|
||||
// fabsf(z_in_new) > 20) {
|
||||
// _constant_accel_count += 1;
|
||||
// } else {
|
||||
// _constant_accel_count = 0;
|
||||
// }
|
||||
// if (_constant_accel_count > 100) {
|
||||
// // we've had 100 constant accel readings with large
|
||||
// // values. The sensor is almost certainly dead. We
|
||||
// // will raise the error_count so that the top level
|
||||
// // flight code will know to avoid this sensor, but
|
||||
// // we'll still give the data so that it can be logged
|
||||
// // and viewed
|
||||
// perf_count(_bad_values);
|
||||
// _constant_accel_count = 0;
|
||||
// }
|
||||
|
||||
_last_accel[0] = x_in_new;
|
||||
_last_accel[1] = y_in_new;
|
||||
_last_accel[2] = z_in_new;
|
||||
// _last_accel[0] = x_in_new;
|
||||
// _last_accel[1] = y_in_new;
|
||||
// _last_accel[2] = z_in_new;
|
||||
|
||||
accel_report.x = _accel_filter_x.apply(x_in_new);
|
||||
accel_report.y = _accel_filter_y.apply(y_in_new);
|
||||
accel_report.z = _accel_filter_z.apply(z_in_new);
|
||||
// accel_report.x = _accel_filter_x.apply(x_in_new);
|
||||
// accel_report.y = _accel_filter_y.apply(y_in_new);
|
||||
// accel_report.z = _accel_filter_z.apply(z_in_new);
|
||||
|
||||
accel_report.x = raw_accel_report.x;
|
||||
accel_report.y = raw_accel_report.y;
|
||||
accel_report.z = raw_accel_report.z;
|
||||
|
||||
accel_report.scaling = _accel_range_scale;
|
||||
accel_report.range_m_s2 = _accel_range_m_s2;
|
||||
|
|
@ -1109,11 +1139,11 @@ ACCELSIM::mag_measure()
|
|||
#pragma pack(push, 1)
|
||||
struct {
|
||||
uint8_t cmd;
|
||||
int16_t temperature;
|
||||
uint8_t status;
|
||||
int16_t x;
|
||||
int16_t y;
|
||||
int16_t z;
|
||||
float temperature;
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} raw_mag_report;
|
||||
#pragma pack(pop)
|
||||
|
||||
|
|
@ -1125,8 +1155,11 @@ ACCELSIM::mag_measure()
|
|||
|
||||
/* fetch data from the sensor */
|
||||
memset(&raw_mag_report, 0, sizeof(raw_mag_report));
|
||||
raw_mag_report.cmd = DIR_READ;
|
||||
transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
|
||||
raw_mag_report.cmd = DIR_READ | MAG_READ;
|
||||
|
||||
if(OK != transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report))) {
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* 1) Scale raw value to SI units using scaling from datasheet.
|
||||
|
|
@ -1146,29 +1179,33 @@ ACCELSIM::mag_measure()
|
|||
|
||||
mag_report.timestamp = hrt_absolute_time();
|
||||
|
||||
mag_report.x_raw = raw_mag_report.x;
|
||||
mag_report.y_raw = raw_mag_report.y;
|
||||
mag_report.z_raw = raw_mag_report.z;
|
||||
mag_report.x_raw = (int16_t)(raw_mag_report.x / _mag_range_scale);
|
||||
mag_report.y_raw = (int16_t)(raw_mag_report.y / _mag_range_scale);
|
||||
mag_report.z_raw = (int16_t)(raw_mag_report.z / _mag_range_scale);
|
||||
|
||||
float xraw_f = (int16_t)(raw_mag_report.x / _mag_range_scale);
|
||||
float yraw_f = (int16_t)(raw_mag_report.y / _mag_range_scale);
|
||||
float zraw_f = (int16_t)(raw_mag_report.z / _mag_range_scale);
|
||||
|
||||
float xraw_f = mag_report.x_raw;
|
||||
float yraw_f = mag_report.y_raw;
|
||||
float zraw_f = mag_report.z_raw;
|
||||
|
||||
/* apply user specified rotation */
|
||||
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
|
||||
|
||||
mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
|
||||
mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
|
||||
mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
|
||||
mag_report.scaling = _mag_range_scale;
|
||||
mag_report.range_ga = (float)_mag_range_ga;
|
||||
mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
|
||||
// mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
|
||||
// mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
|
||||
// mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
|
||||
// mag_report.scaling = _mag_range_scale;
|
||||
// mag_report.range_ga = (float)_mag_range_ga;
|
||||
// mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
|
||||
|
||||
/* remember the temperature. The datasheet isn't clear, but it
|
||||
* seems to be a signed offset from 25 degrees C in units of 0.125C
|
||||
*/
|
||||
_last_temperature = 25 + (raw_mag_report.temperature * 0.125f);
|
||||
_last_temperature = raw_mag_report.temperature;
|
||||
mag_report.temperature = _last_temperature;
|
||||
mag_report.x = raw_mag_report.x;
|
||||
mag_report.y = raw_mag_report.y;
|
||||
mag_report.z = raw_mag_report.z;
|
||||
|
||||
_mag_reports->force(&mag_report);
|
||||
|
||||
|
|
|
|||
|
|
@ -275,6 +275,13 @@ BAROSIM::init()
|
|||
_measure_phase = 0;
|
||||
_reports->flush();
|
||||
|
||||
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
|
||||
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
|
||||
|
||||
if (_baro_topic == (orb_advert_t)(-1)) {
|
||||
PX4_ERR("failed to create sensor_baro publication");
|
||||
}
|
||||
|
||||
/* this do..while is goto without goto */
|
||||
do {
|
||||
/* do temperature first */
|
||||
|
|
@ -312,12 +319,6 @@ BAROSIM::init()
|
|||
|
||||
ret = OK;
|
||||
|
||||
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
|
||||
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
|
||||
|
||||
if (_baro_topic == nullptr) {
|
||||
PX4_ERR("failed to create sensor_baro publication");
|
||||
}
|
||||
//PX4_WARN("sensor_baro publication %ld", _baro_topic);
|
||||
|
||||
} while (0);
|
||||
|
|
@ -622,7 +623,14 @@ int
|
|||
BAROSIM::collect()
|
||||
{
|
||||
int ret;
|
||||
uint32_t raw;
|
||||
|
||||
#pragma pack(push, 1)
|
||||
struct {
|
||||
float pressure;
|
||||
float altitude;
|
||||
float temperature;
|
||||
} baro_report;
|
||||
#pragma pack(pop)
|
||||
|
||||
perf_begin(_sample_perf);
|
||||
|
||||
|
|
@ -632,7 +640,7 @@ BAROSIM::collect()
|
|||
report.error_count = perf_event_count(_comms_errors);
|
||||
|
||||
/* read the most recent measurement - read offset/size are hardcoded in the interface */
|
||||
ret = _interface->dev_read(0, (void *)&raw, 0);
|
||||
ret = _interface->dev_read(0, (void *)&baro_report, 0);
|
||||
if (ret < 0) {
|
||||
perf_count(_comms_errors);
|
||||
perf_end(_sample_perf);
|
||||
|
|
@ -641,84 +649,15 @@ BAROSIM::collect()
|
|||
|
||||
/* handle a measurement */
|
||||
if (_measure_phase == 0) {
|
||||
|
||||
/* temperature offset (in ADC units) */
|
||||
int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8);
|
||||
|
||||
/* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
|
||||
_TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23);
|
||||
|
||||
/* base sensor scale/offset values */
|
||||
_SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8);
|
||||
_OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7);
|
||||
|
||||
/* temperature compensation */
|
||||
if (_TEMP < 2000) {
|
||||
|
||||
int32_t T2 = POW2(dT) >> 31;
|
||||
|
||||
int64_t f = POW2((int64_t)_TEMP - 2000);
|
||||
int64_t OFF2 = 5 * f >> 1;
|
||||
int64_t SENS2 = 5 * f >> 2;
|
||||
|
||||
if (_TEMP < -1500) {
|
||||
int64_t f2 = POW2(_TEMP + 1500);
|
||||
OFF2 += 7 * f2;
|
||||
SENS2 += 11 * f2 >> 1;
|
||||
}
|
||||
|
||||
_TEMP -= T2;
|
||||
_OFF -= OFF2;
|
||||
_SENS -= SENS2;
|
||||
}
|
||||
|
||||
report.pressure = baro_report.pressure;
|
||||
report.altitude = baro_report.altitude;
|
||||
report.temperature = baro_report.temperature;
|
||||
report.timestamp = hrt_absolute_time();
|
||||
} else {
|
||||
|
||||
/* pressure calculation, result in Pa */
|
||||
int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
|
||||
_P = P * 0.01f;
|
||||
_T = _TEMP * 0.01f;
|
||||
|
||||
/* generate a new report */
|
||||
report.temperature = _TEMP / 100.0f;
|
||||
report.pressure = P / 100.0f; /* convert to millibar */
|
||||
|
||||
/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
|
||||
|
||||
/*
|
||||
* PERFORMANCE HINT:
|
||||
*
|
||||
* The single precision calculation is 50 microseconds faster than the double
|
||||
* precision variant. It is however not obvious if double precision is required.
|
||||
* Pending more inspection and tests, we'll leave the double precision variant active.
|
||||
*
|
||||
* Measurements:
|
||||
* double precision: barosim_read: 992 events, 258641us elapsed, min 202us max 305us
|
||||
* single precision: barosim_read: 963 events, 208066us elapsed, min 202us max 241us
|
||||
*/
|
||||
|
||||
/* tropospheric properties (0-11km) for standard atmosphere */
|
||||
const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
|
||||
const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */
|
||||
const double g = 9.80665; /* gravity constant in m/s/s */
|
||||
const double R = 287.05; /* ideal gas constant in J/kg/K */
|
||||
|
||||
/* current pressure at MSL in kPa */
|
||||
double p1 = _msl_pressure / 1000.0;
|
||||
|
||||
/* measured pressure in kPa */
|
||||
double p = P / 1000.0;
|
||||
|
||||
/*
|
||||
* Solve:
|
||||
*
|
||||
* / -(aR / g) \
|
||||
* | (p / p1) . T1 | - T1
|
||||
* \ /
|
||||
* h = ------------------------------- + h1
|
||||
* a
|
||||
*/
|
||||
report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
|
||||
report.pressure = baro_report.pressure;
|
||||
report.altitude = baro_report.altitude;
|
||||
report.temperature = baro_report.temperature;
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
/* publish it */
|
||||
if (!(_pub_blocked)) {
|
||||
|
|
|
|||
|
|
@ -118,22 +118,26 @@ BARO_SIM::init()
|
|||
int
|
||||
BARO_SIM::dev_read(unsigned offset, void *data, unsigned count)
|
||||
{
|
||||
/*
|
||||
union _cvt {
|
||||
uint8_t b[4];
|
||||
uint32_t w;
|
||||
} *cvt = (_cvt *)data;
|
||||
uint8_t buf[3];
|
||||
*/
|
||||
|
||||
/* read the most recent measurement */
|
||||
uint8_t cmd = 0;
|
||||
int ret = transfer(&cmd, 1, &buf[0], 3);
|
||||
int ret = transfer(&cmd, 1, static_cast<uint8_t *>(data), 3);
|
||||
|
||||
/*
|
||||
if (ret == PX4_OK) {
|
||||
/* fetch the raw value */
|
||||
// fetch the raw value
|
||||
cvt->b[0] = buf[2];
|
||||
cvt->b[1] = buf[1];
|
||||
cvt->b[2] = buf[0];
|
||||
cvt->b[3] = 0;
|
||||
}
|
||||
*/
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -379,13 +379,13 @@ private:
|
|||
struct MPUReport {
|
||||
uint8_t cmd;
|
||||
uint8_t status;
|
||||
uint8_t accel_x[2];
|
||||
uint8_t accel_y[2];
|
||||
uint8_t accel_z[2];
|
||||
uint8_t temp[2];
|
||||
uint8_t gyro_x[2];
|
||||
uint8_t gyro_y[2];
|
||||
uint8_t gyro_z[2];
|
||||
float accel_x;
|
||||
float accel_y;
|
||||
float accel_z;
|
||||
float temp;
|
||||
float gyro_x;
|
||||
float gyro_y;
|
||||
float gyro_z;
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
|
|
@ -1194,15 +1194,6 @@ void
|
|||
GYROSIM::measure()
|
||||
{
|
||||
struct MPUReport mpu_report;
|
||||
struct Report {
|
||||
int16_t accel_x;
|
||||
int16_t accel_y;
|
||||
int16_t accel_z;
|
||||
int16_t temp;
|
||||
int16_t gyro_x;
|
||||
int16_t gyro_y;
|
||||
int16_t gyro_z;
|
||||
} report;
|
||||
|
||||
/* start measuring */
|
||||
perf_begin(_sample_perf);
|
||||
|
|
@ -1214,69 +1205,10 @@ GYROSIM::measure()
|
|||
|
||||
// sensor transfer at high clock speed
|
||||
//set_frequency(GYROSIM_HIGH_BUS_SPEED);
|
||||
|
||||
if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
|
||||
return;
|
||||
|
||||
/*
|
||||
* Convert from big to little endian
|
||||
*/
|
||||
|
||||
report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
|
||||
report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
|
||||
report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
|
||||
|
||||
report.temp = int16_t_from_bytes(mpu_report.temp);
|
||||
|
||||
report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
|
||||
report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
|
||||
report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
|
||||
|
||||
if (report.accel_x == 0 &&
|
||||
report.accel_y == 0 &&
|
||||
report.accel_z == 0 &&
|
||||
report.temp == 0 &&
|
||||
report.gyro_x == 0 &&
|
||||
report.gyro_y == 0 &&
|
||||
report.gyro_z == 0) {
|
||||
// all zero data - probably a VDev bus error
|
||||
perf_count(_bad_transfers);
|
||||
perf_end(_sample_perf);
|
||||
// note that we don't call reset() here as a reset()
|
||||
// costs 20ms with interrupts disabled. That means if
|
||||
// the mpu6k does go bad it would cause a FMU failure,
|
||||
// regardless of whether another sensor is available,
|
||||
if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) {
|
||||
return;
|
||||
}
|
||||
|
||||
perf_count(_good_transfers);
|
||||
|
||||
if (_register_wait != 0) {
|
||||
// we are waiting for some good transfers before using
|
||||
// the sensor again. We still increment
|
||||
// _good_transfers, but don't return any data yet
|
||||
_register_wait--;
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Swap axes and negate y
|
||||
*/
|
||||
int16_t accel_xt = report.accel_y;
|
||||
int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
|
||||
|
||||
int16_t gyro_xt = report.gyro_y;
|
||||
int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
|
||||
|
||||
/*
|
||||
* Apply the swap
|
||||
*/
|
||||
report.accel_x = accel_xt;
|
||||
report.accel_y = accel_yt;
|
||||
report.gyro_x = gyro_xt;
|
||||
report.gyro_y = gyro_yt;
|
||||
|
||||
/*
|
||||
* Report buffers.
|
||||
*/
|
||||
|
|
@ -1286,7 +1218,11 @@ GYROSIM::measure()
|
|||
/*
|
||||
* Adjust and scale results to m/s^2.
|
||||
*/
|
||||
grb.timestamp = arb.timestamp = hrt_absolute_time();
|
||||
grb.timestamp = hrt_absolute_time();
|
||||
arb.timestamp = grb.timestamp;
|
||||
|
||||
// this sleep is needed because the timing of the drivers is not yet working
|
||||
usleep(1000);
|
||||
|
||||
// report the error count as the sum of the number of bad
|
||||
// transfers and bad register reads. This allows the higher
|
||||
|
|
@ -1312,13 +1248,13 @@ GYROSIM::measure()
|
|||
|
||||
/* NOTE: Axes have been swapped to match the board a few lines above. */
|
||||
|
||||
arb.x_raw = report.accel_x;
|
||||
arb.y_raw = report.accel_y;
|
||||
arb.z_raw = report.accel_z;
|
||||
|
||||
float xraw_f = report.accel_x;
|
||||
float yraw_f = report.accel_y;
|
||||
float zraw_f = report.accel_z;
|
||||
arb.x_raw = (int16_t)(mpu_report.accel_x / _accel_range_scale);
|
||||
arb.y_raw = (int16_t)(mpu_report.accel_y / _accel_range_scale);
|
||||
arb.z_raw = (int16_t)(mpu_report.accel_z / _accel_range_scale);
|
||||
/*
|
||||
float xraw_f = (int16_t)(mpu_report.accel_x / _accel_range_scale);
|
||||
float yraw_f = (int16_t)(mpu_report.accel_y / _accel_range_scale);
|
||||
float zraw_f = (int16_t)(mpu_report.accel_z / _accel_range_scale);
|
||||
|
||||
// apply user specified rotation
|
||||
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
|
||||
|
|
@ -1330,22 +1266,26 @@ GYROSIM::measure()
|
|||
arb.x = _accel_filter_x.apply(x_in_new);
|
||||
arb.y = _accel_filter_y.apply(y_in_new);
|
||||
arb.z = _accel_filter_z.apply(z_in_new);
|
||||
|
||||
*/
|
||||
arb.scaling = _accel_range_scale;
|
||||
arb.range_m_s2 = _accel_range_m_s2;
|
||||
|
||||
_last_temperature = (report.temp) / 361.0f + 35.0f;
|
||||
_last_temperature = mpu_report.temp;
|
||||
|
||||
arb.temperature_raw = report.temp;
|
||||
arb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f);
|
||||
arb.temperature = _last_temperature;
|
||||
|
||||
grb.x_raw = report.gyro_x;
|
||||
grb.y_raw = report.gyro_y;
|
||||
grb.z_raw = report.gyro_z;
|
||||
arb.x = mpu_report.accel_x;
|
||||
arb.y = mpu_report.accel_y;
|
||||
arb.z = mpu_report.accel_z;
|
||||
|
||||
xraw_f = report.gyro_x;
|
||||
yraw_f = report.gyro_y;
|
||||
zraw_f = report.gyro_z;
|
||||
grb.x_raw = (int16_t)(mpu_report.gyro_x/_gyro_range_scale);
|
||||
grb.y_raw = (int16_t)(mpu_report.gyro_y/_gyro_range_scale);
|
||||
grb.z_raw = (int16_t)(mpu_report.gyro_z/_gyro_range_scale);
|
||||
/*
|
||||
xraw_f = (int16_t)(mpu_report.gyro_x/_gyro_range_scale);
|
||||
yraw_f = (int16_t)(mpu_report.gyro_y/_gyro_range_scale);
|
||||
zraw_f = (int16_t)(mpu_report.gyro_z/_gyro_range_scale);
|
||||
|
||||
// apply user specified rotation
|
||||
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
|
||||
|
|
@ -1357,20 +1297,24 @@ GYROSIM::measure()
|
|||
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
|
||||
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
|
||||
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
|
||||
|
||||
*/
|
||||
grb.scaling = _gyro_range_scale;
|
||||
grb.range_rad_s = _gyro_range_rad_s;
|
||||
|
||||
grb.temperature_raw = report.temp;
|
||||
grb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f);
|
||||
grb.temperature = _last_temperature;
|
||||
|
||||
grb.x = mpu_report.gyro_x;
|
||||
grb.y = mpu_report.gyro_y;
|
||||
grb.z = mpu_report.gyro_z;
|
||||
|
||||
_accel_reports->force(&arb);
|
||||
_gyro_reports->force(&grb);
|
||||
|
||||
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
_gyro->parent_poll_notify();
|
||||
|
||||
if (!(_pub_blocked)) {
|
||||
/* log the time of this report */
|
||||
perf_begin(_controller_latency_perf);
|
||||
|
|
|
|||
Loading…
Reference in New Issue