From 89bc1b86b21eedf23ae156e8bd2ede711c7e6cfa Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 15 Apr 2015 12:38:50 -0700 Subject: [PATCH] Linux: connected gyrosim to Simulator Simulator gets incoming MPU data and gives raw MPU data to the gyrosim sensor when read. Signed-off-by: Mark Charlebois --- src/modules/simulator/simulator.cpp | 93 ++++---- src/modules/simulator/simulator.h | 19 +- .../linux/drivers/gyrosim/gyrosim.cpp | 220 +++--------------- 3 files changed, 99 insertions(+), 233 deletions(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index c4c6ffbbd6..3372f25f4b 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -37,6 +37,8 @@ */ #include +#include +#include #include #include #include @@ -44,6 +46,8 @@ #include #include "simulator.h" +static px4_task_t g_sim_task = -1; + Simulator *Simulator::_instance = NULL; Simulator::Simulator() : _max_readers(3) @@ -56,33 +60,15 @@ Simulator *Simulator::getInstance() return _instance; } -int Simulator::getSample(sim_dev_t dev, sample &val) +bool Simulator::getMPUReport(uint8_t *buf, int len) { - int ret; - - switch (dev) { - case SIM_GYRO: - read_lock(); - val = _gyro[_readidx]; - read_unlock(); - ret = 0; - break; - case SIM_ACCEL: - read_lock(); - val = _accel[_readidx]; - read_unlock(); - ret = 0; - break; - case SIM_MAG: - read_lock(); - val = _mag[_readidx]; - read_unlock(); - ret = 0; - break; - default: - ret = 1; - } - return ret; + if (len != sizeof(MPUReport)) { + return false; + } + read_lock(); + memcpy(buf, &_mpureport[_readidx], sizeof(MPUReport)); + read_unlock(); + return true; } int Simulator::start(int argc, char *argv[]) @@ -109,7 +95,7 @@ void Simulator::updateSamples() const int buflen = 200; const int port = 9876; unsigned char buf[buflen]; - int writeidx, num; + int writeidx; if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { printf("create socket failed\n"); @@ -130,22 +116,18 @@ void Simulator::updateSamples() len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen); if (len > 0) { writeidx = !_readidx; - buf[len] = 0; - printf("received: %s\n", buf); - // FIXME - temp hack to read data, not safe - bad bad bad - num = sscanf((const char *)buf, "%f,%f,%f,%f,%f,%f,%f,%f,%f", - &_gyro[writeidx].x, &_gyro[writeidx].y, &_gyro[writeidx].z, - &_accel[writeidx].x, &_accel[writeidx].y, &_accel[writeidx].z, - &_mag[writeidx].x, &_mag[writeidx].y, &_mag[writeidx].z); - if (num != 9) { - printf("Only read %d items\n", num); - } - else { + if (len == sizeof(MPUReport)) { + printf("received: MPU data\n"); + memcpy((void *)&_mpureport[writeidx], (void *)buf, len); + // Swap read and write buffers write_lock(); _readidx = !_readidx; write_unlock(); } + else { + printf("bad packet: len = %d\n", len); + } } } } @@ -171,18 +153,47 @@ void Simulator::write_unlock() } } +static void usage() +{ + warnx("Usage: simulator {start|stop}"); +} + extern "C" { int simulator_main(int argc, char *argv[]) { - return (int)(px4_task_spawn_cmd("Simulator", + int ret = 0; + if (argc != 2) { + usage(); + return 1; + } + if (strcmp(argv[1], "start") == 0) { + if (g_sim_task >= 0) { + warnx("Simulator already started"); + return 0; + } + g_sim_task = px4_task_spawn_cmd("Simulator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1500, Simulator::start, - nullptr) < 0); + nullptr); + } + else if (strcmp(argv[1], "stop") == 0) { + if (g_sim_task < 0) { + warnx("Simulator not running"); + } + else { + px4_task_delete(g_sim_task); + g_sim_task = -1; + } + } + else { + usage(); + ret = -EINVAL; + } - return 0; + return ret; } } diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 4c20768caf..333a729281 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -60,7 +60,7 @@ public: static int start(int argc, char *argv[]); - int getSample(sim_dev_t dev, sample &val); + bool getMPUReport(uint8_t *buf, int len); private: Simulator(); ~Simulator() { _instance=NULL; } @@ -82,5 +82,22 @@ private: sem_t _lock; const int _max_readers; + +#pragma pack(push, 1) + /** + * Report conversation within the GYROSIM, including command byte and + * interrupt status. + */ + struct MPUReport { + 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]; + }; +#pragma pack(pop) + MPUReport _mpureport[2]; }; diff --git a/src/platforms/linux/drivers/gyrosim/gyrosim.cpp b/src/platforms/linux/drivers/gyrosim/gyrosim.cpp index 378c026f2a..d117cab9a1 100644 --- a/src/platforms/linux/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/linux/drivers/gyrosim/gyrosim.cpp @@ -59,6 +59,8 @@ #include #include +#include + #include #include #include @@ -207,13 +209,6 @@ public: void print_registers(); - /** - * Test behaviour against factory offsets - * - * @return 0 on success, 1 on failure - */ - int factory_self_test(); - protected: friend class GYROSIM_gyro; @@ -273,10 +268,6 @@ private: uint8_t _checked_values[GYROSIM_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; - // use this to avoid processing measurements when in factory - // self test - volatile bool _in_factory_test; - // last temperature reading for print_info() float _last_temperature; @@ -419,6 +410,8 @@ private: uint8_t gyro_z[2]; }; #pragma pack(pop) + + uint8_t _regdata[108]; }; /* @@ -507,7 +500,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _gyro_filter_z(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _checked_next(0), - _in_factory_test(false), _last_temperature(0) { // disable debug() calls @@ -661,6 +653,30 @@ int GYROSIM::reset() int GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) { + uint8_t cmd = send[0]; + uint8_t reg = cmd & 0x7F; + const uint8_t MPUREAD = MPUREG_INT_STATUS | DIR_READ; + + if (cmd == MPUREAD) { + // 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 + sim->getMPUReport(&recv[2], len-2); + } + else if (cmd & DIR_READ) + { + printf("Reading %u bytes from register %u\n", len-1, reg); + memcpy(&_regdata[reg-MPUREG_PRODUCT_ID], &send[1], len-1); + } + else { + printf("Writing %u bytes to register %u\n", len-1, reg); + memcpy(&recv[1], &_regdata[reg-MPUREG_PRODUCT_ID], len-1); + } return PX4_OK; } @@ -835,157 +851,6 @@ GYROSIM::gyro_self_test() return 0; } - - -/* - perform a self-test comparison to factory trim values. This takes - about 200ms and will return OK if the current values are within 14% - of the expected values (as per datasheet) - */ -int -GYROSIM::factory_self_test() -{ - _in_factory_test = true; - uint8_t saved_gyro_config = read_reg(MPUREG_GYRO_CONFIG); - uint8_t saved_accel_config = read_reg(MPUREG_ACCEL_CONFIG); - const uint16_t repeats = 100; - int ret = OK; - - // gyro self test has to be done at 250DPS - write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); - - struct MPUReport mpu_report; - float accel_baseline[3]; - float gyro_baseline[3]; - float accel[3]; - float gyro[3]; - float accel_ftrim[3]; - float gyro_ftrim[3]; - - // get baseline values without self-test enabled - //set_frequency(GYROSIM_HIGH_BUS_SPEED); - - memset(accel_baseline, 0, sizeof(accel_baseline)); - memset(gyro_baseline, 0, sizeof(gyro_baseline)); - memset(accel, 0, sizeof(accel)); - memset(gyro, 0, sizeof(gyro)); - - for (uint8_t i=0; i>3)&0x1C) | ((trims[3]>>4)&0x03); - atrim[1] = ((trims[1]>>3)&0x1C) | ((trims[3]>>2)&0x03); - atrim[2] = ((trims[2]>>3)&0x1C) | ((trims[3]>>0)&0x03); - gtrim[0] = trims[0] & 0x1F; - gtrim[1] = trims[1] & 0x1F; - gtrim[2] = trims[2] & 0x1F; - - // convert factory trims to right units - for (uint8_t i=0; i<3; i++) { - accel_ftrim[i] = 4096 * 0.34f * powf(0.92f/0.34f, (atrim[i]-1)/30.0f); - gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i]-1); - } - // Y gyro trim is negative - gyro_ftrim[1] *= -1; - - for (uint8_t i=0; i<3; i++) { - float diff = accel[i]-accel_baseline[i]; - float err = 100*(diff - accel_ftrim[i]) / accel_ftrim[i]; - ::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n", - (unsigned)i, - (int)(1000*accel_baseline[i]), - (int)(1000*accel[i]), - (int)(1000*diff), - (int)(1000*accel_ftrim[i]), - (int)err); - if (fabsf(err) > 14) { - ::printf("FAIL\n"); - ret = -EIO; - } - } - for (uint8_t i=0; i<3; i++) { - float diff = gyro[i]-gyro_baseline[i]; - float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; - ::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n", - (unsigned)i, - (int)(1000*gyro_baseline[i]), - (int)(1000*gyro[i]), - (int)(1000*(gyro[i]-gyro_baseline[i])), - (int)(1000*gyro_ftrim[i]), - (int)err); - if (fabsf(err) > 14) { - ::printf("FAIL\n"); - ret = -EIO; - } - } - - write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); - write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); - - _in_factory_test = false; - if (ret == OK) { - ::printf("PASSED\n"); - } - - return ret; -} - - ssize_t GYROSIM::gyro_read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) { @@ -1426,11 +1291,6 @@ GYROSIM::check_registers(void) void GYROSIM::measure() { - if (_in_factory_test) { - // don't publish any data while in factory test mode - return; - } - if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete return; @@ -1746,7 +1606,6 @@ int test(); int reset(); int info(); int regdump(); -int factorytest(); void usage(); /** @@ -1952,27 +1811,10 @@ regdump() return 0; } -/** - * Dump the register information - */ -int -factorytest() -{ - GYROSIM **g_dev_ptr = &g_dev_sim; - if (*g_dev_ptr == nullptr) { - warnx("driver not running"); - return 1; - } - - (*g_dev_ptr)->factory_self_test(); - - return 0; -} - void usage() { - warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest'"); + warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump'"); warnx("options:"); warnx(" -R rotation"); } @@ -2042,10 +1884,6 @@ gyrosim_main(int argc, char *argv[]) ret = gyrosim::regdump(); } - else if (!strcmp(verb, "factorytest")) { - ret = gyrosim::factorytest(); - } - else { gyrosim::usage(); ret = 1;