From aef3f37ae053584ce5c21b2bd9ab791a2d49fb0b Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 31 May 2015 18:29:28 +0200 Subject: [PATCH] enable reading sensor data from simulator module for SITL --- .../posix/drivers/accelsim/accelsim.cpp | 159 +++++++++++------- src/platforms/posix/drivers/barosim/baro.cpp | 109 +++--------- .../posix/drivers/barosim/baro_sim.cpp | 10 +- .../posix/drivers/gyrosim/gyrosim.cpp | 136 +++++---------- 4 files changed, 169 insertions(+), 245 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index bb9749577a..994b1bfb9b 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -54,6 +54,8 @@ #include #include +#include + #include #include @@ -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); diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 91bd149135..f7ea4f6949 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -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)) { diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 734c2bc286..8b64962207 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -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(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; } diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 1ccaecf9a6..54e448cabb 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -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);