diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index fa0a256605..1f9fa0da05 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -99,7 +99,7 @@ class ACCELSIM_mag; class ACCELSIM : public device::VDev { public: - ACCELSIM(const char* path, enum Rotation rotation); + ACCELSIM(const char *path, enum Rotation rotation); virtual ~ACCELSIM(); virtual int init(); @@ -310,8 +310,8 @@ private: int mag_set_samplerate(unsigned frequency); /* this class cannot be copied */ - ACCELSIM(const ACCELSIM&); - ACCELSIM operator=(const ACCELSIM&); + ACCELSIM(const ACCELSIM &); + ACCELSIM operator=(const ACCELSIM &); }; /* @@ -350,12 +350,12 @@ private: void measure_trampoline(void *arg); /* this class does not allow copying due to ptr data members */ - ACCELSIM_mag(const ACCELSIM_mag&); - ACCELSIM_mag operator=(const ACCELSIM_mag&); + ACCELSIM_mag(const ACCELSIM_mag &); + ACCELSIM_mag operator=(const ACCELSIM_mag &); }; -ACCELSIM::ACCELSIM(const char* path, enum Rotation rotation) : +ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : VDev("ACCELSIM", path), _mag(new ACCELSIM_mag(this)), _accel_call{}, @@ -397,7 +397,7 @@ ACCELSIM::ACCELSIM(const char* path, enum Rotation rotation) : _debug_enabled = false; _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; - + /* Prime _mag with parents devid. */ _mag->_device_id.devid = _device_id.devid; _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_ACCELSIM; @@ -425,13 +425,17 @@ ACCELSIM::~ACCELSIM() stop(); /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_mag_reports != nullptr) - delete _mag_reports; + } - if (_accel_class_instance != -1) + if (_mag_reports != nullptr) { + delete _mag_reports; + } + + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } delete _mag; @@ -460,18 +464,21 @@ ACCELSIM::init() /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); - if (_accel_reports == nullptr) + if (_accel_reports == nullptr) { goto out; + } _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); - if (_mag_reports == nullptr) + if (_mag_reports == nullptr) { goto out; + } reset(); /* do VDev init for the mag device node */ ret = _mag->init(); + if (ret != OK) { PX4_WARN("MAG init failed"); goto out; @@ -485,7 +492,7 @@ ACCELSIM::init() /* measurement will have generated a report, publish */ _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, - &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); + &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic == nullptr) { PX4_WARN("ADVERT ERR"); @@ -499,7 +506,7 @@ ACCELSIM::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, ORB_PRIO_DEFAULT); + &_accel_orb_class_instance, ORB_PRIO_DEFAULT); if (_accel_topic == nullptr) { PX4_WARN("ADVERT ERR"); @@ -522,16 +529,20 @@ ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) if (cmd & DIR_READ) { // Get data from the simulator Simulator *sim = Simulator::getInstance(); - if (sim == NULL) + + 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); + sim->getRawAccelReport(&recv[2], len - 2); + } else if (cmd & MAG_READ) { - sim->getMagReport(&recv[2], len-2); + sim->getMagReport(&recv[2], len - 2); } } @@ -546,8 +557,9 @@ ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_accel_interval > 0) { @@ -569,8 +581,9 @@ ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_accel_reports->get(arb)) + if (_accel_reports->get(arb)) { ret = sizeof(*arb); + } return ret; } @@ -583,8 +596,9 @@ ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_mag_interval > 0) { @@ -608,8 +622,9 @@ ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) _mag->measure(); /* measurement will have generated a report, copy it out */ - if (_mag_reports->get(mrb)) + if (_mag_reports->get(mrb)) { ret = sizeof(*mrb); + } return ret; } @@ -620,7 +635,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -642,52 +657,56 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); - /* convert hz to hrt interval via microseconds */ - unsigned period = 1000000 / arg; + /* convert hz to hrt interval via microseconds */ + unsigned period = 1000000 / arg; - /* check against maximum sane rate */ - if (period < 500) - return -EINVAL; + /* check against maximum sane rate */ + if (period < 500) { + return -EINVAL; + } - /* adjust filters */ - accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); + /* adjust filters */ + accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _accel_call.period = _call_accel_interval = period; + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _accel_call.period = _call_accel_interval = period; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; + return OK; + } } } - } case SENSORIOCGPOLLRATE: - if (_call_accel_interval == 0) + if (_call_accel_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_accel_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_accel_reports->resize(arg)) { - return -ENOMEM; + if (!_accel_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } - case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -702,20 +721,22 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) return _accel_samplerate; case ACCELIOCSLOWPASS: { - return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); - } + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); + } case ACCELIOCSSCALE: { - /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - } else { - return -EINVAL; + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + + } else { + return -EINVAL; + } } - } case ACCELIOCSRANGE: /* arg needs to be in G */ @@ -723,7 +744,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCGRANGE: /* convert to m/s^2 and return rounded in G */ - return (unsigned long)((_accel_range_m_s2)/ACCELSIM_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / ACCELSIM_ONE_G + 0.5f); case ACCELIOCGSCALE: /* copy scale out */ @@ -745,7 +766,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -775,8 +796,9 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) unsigned period = 1000000 / arg; /* check against maximum sane rate (1ms) */ - if (period < 10000) + if (period < 10000) { return -EINVAL; + } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ @@ -785,8 +807,9 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) //PX4_INFO("SET _call_mag_interval=%u", _call_mag_interval); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -794,23 +817,25 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_mag_interval == 0) + if (_call_mag_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_mag_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_mag_reports->resize(arg)) { - return -ENOMEM; + if (!_mag_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } - case SENSORIOCGQUEUEDEPTH: return _mag_reports->size(); @@ -854,6 +879,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) case MAGIOCSELFTEST: return OK; + default: /* give it to the superclass */ return VDev::ioctl(filp, cmd, arg); @@ -888,7 +914,8 @@ void ACCELSIM::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i=0; imag_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)VDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->mag_ioctl(filp, cmd, arg); } } @@ -1311,8 +1342,9 @@ int start(enum Rotation rotation) { int fd, fd_mag; + if (g_dev != nullptr) { - PX4_WARN( "already started"); + PX4_WARN("already started"); return 0; } @@ -1339,7 +1371,7 @@ start(enum Rotation rotation) if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); - px4_close(fd); + px4_close(fd); goto fail; } @@ -1350,14 +1382,13 @@ start(enum Rotation rotation) if (px4_ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); } - } - else - { + + } else { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); } - px4_close(fd); - px4_close(fd_mag); + px4_close(fd); + px4_close(fd_mag); return 0; fail: @@ -1405,7 +1436,7 @@ accelsim_main(int argc, char *argv[]) enum Rotation rotation = ROTATION_NONE; int ret; int myoptind = 1; - const char * myoptarg = NULL; + const char *myoptarg = NULL; /* jump over start/off/etc and look at options first */ while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { @@ -1413,6 +1444,7 @@ accelsim_main(int argc, char *argv[]) case 'R': rotation = (enum Rotation)atoi(myoptarg); break; + default: accelsim::usage(); return 0; @@ -1424,18 +1456,21 @@ accelsim_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { ret = accelsim::start(rotation); + } /* * Print driver information. */ - else if (!strcmp(verb, "info")) + else if (!strcmp(verb, "info")) { ret = accelsim::info(); + } else { accelsim::usage(); return 1; } + return ret; } diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index b7d4071111..d9f0dd4f15 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -37,7 +37,7 @@ * Driver for the ADCSIM. * * This is a designed for simulating sampling things like voltages - * and so forth. + * and so forth. */ #include @@ -83,7 +83,7 @@ protected: private: static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ - + hrt_call _call; perf_counter_t _sample_perf; @@ -126,11 +126,13 @@ ADCSIM::ADCSIM(uint32_t channels) : _channel_count++; } } + _samples = new adc_msg_s[_channel_count]; /* prefill the channel numbers in the sample array */ if (_samples != nullptr) { unsigned index = 0; + for (unsigned i = 0; i < 32; i++) { if (channels & (1 << i)) { _samples[index].am_channel = i; @@ -143,8 +145,9 @@ ADCSIM::ADCSIM(uint32_t channels) : ADCSIM::~ADCSIM() { - if (_samples != nullptr) + if (_samples != nullptr) { delete _samples; + } } int @@ -167,8 +170,9 @@ ADCSIM::read(device::file_t *filp, char *buffer, size_t len) { const size_t maxsize = sizeof(adc_msg_s) * _channel_count; - if (len > maxsize) + if (len > maxsize) { len = maxsize; + } /* block interrupts while copying samples to avoid racing with an update */ memcpy(buffer, _samples, len); @@ -205,8 +209,10 @@ void ADCSIM::_tick() { /* scan the channel set and sample each */ - for (unsigned i = 0; i < _channel_count; i++) + for (unsigned i = 0; i < _channel_count; i++) { _samples[i].am_data = _sample(_samples[i].am_channel); + } + update_system_power(); } @@ -240,6 +246,7 @@ test(void) { int fd = px4_open(ADCSIM0_DEVICE_PATH, O_RDONLY); + if (fd < 0) { PX4_ERR("can't open ADCSIM device"); return 1; @@ -290,8 +297,9 @@ adcsim_main(int argc, char *argv[]) } if (argc > 1) { - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { ret = test(); + } } return ret; diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp index 9cb0ac73d2..58f6aa9929 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp @@ -71,7 +71,7 @@ #include "airspeedsim.h" -AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char* path) : +AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char *path) : VDev("AIRSPEEDSIM", path), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), @@ -101,12 +101,14 @@ AirspeedSim::~AirspeedSim() /* make sure we are truly inactive */ stop(); - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance); + } /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } // free perf counters perf_free(_sample_perf); @@ -127,6 +129,7 @@ AirspeedSim::init() /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); + if (_reports == nullptr) { goto out; } @@ -145,8 +148,9 @@ AirspeedSim::init() /* measurement will have generated a report, publish */ _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); - if (_airspeed_pub == nullptr) + if (_airspeed_pub == nullptr) { PX4_WARN("uORB started?"); + } } ret = OK; @@ -165,7 +169,7 @@ AirspeedSim::probe() _retries = 4; int ret = measure(); - // drop back to 2 retries once initialised + // drop back to 2 retries once initialised _retries = 2; return ret; } @@ -178,20 +182,20 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; - /* external signalling (DRDY) not supported */ + /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -201,13 +205,14 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(_conversion_interval); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -216,15 +221,17 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) unsigned long ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(_conversion_interval)) + if (ticks < USEC2TICK(_conversion_interval)) { return -EINVAL; + } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -232,21 +239,24 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) + if ((arg < 1) || (arg > 100)) { return -EINVAL; + } //irqstate_t flags = irqsave(); if (!_reports->resize(arg)) { //irqrestore(flags); return -ENOMEM; } + //irqrestore(flags); return OK; @@ -260,16 +270,16 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) return -EINVAL; case AIRSPEEDIOCSSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - _diff_pres_offset = s->offset_pa; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + _diff_pres_offset = s->offset_pa; + return OK; } case AIRSPEEDIOCGSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - s->offset_pa = _diff_pres_offset; - s->scale = 1.0f; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + s->offset_pa = _diff_pres_offset; + s->scale = 1.0f; + return OK; } default: @@ -287,8 +297,9 @@ AirspeedSim::read(device::file_t *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -369,6 +380,7 @@ AirspeedSim::update_status() if (_subsys_pub != nullptr) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); + } else { _subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -402,21 +414,26 @@ AirspeedSim::print_info() void AirspeedSim::new_report(const differential_pressure_s &report) { - if (!_reports->force(&report)) + if (!_reports->force(&report)) { perf_count(_buffer_overflows); + } } int -AirspeedSim::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { +AirspeedSim::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ if (recv_len > 0) { // this is equivalent to the collect phase Simulator *sim = Simulator::getInstance(); + if (sim == NULL) { PX4_ERR("Error BARO_SIM::transfer no simulator"); return -ENODEV; } + PX4_DEBUG("BARO_SIM::transfer getting sample"); sim->getAirspeedSample(recv, recv_len); + } else { // we don't need measure phase } diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.h b/src/platforms/posix/drivers/airspeedsim/airspeedsim.h index 2c668fc97f..91db618d9b 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.h +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.h @@ -127,7 +127,7 @@ protected: virtual int collect() = 0; virtual int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); + uint8_t *recv, unsigned recv_len); /** * Update the subsystem status diff --git a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp index 3474b0bcaa..89691f7483 100644 --- a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp @@ -153,12 +153,12 @@ MEASAirspeedSim::collect() int ret = -EIO; /* read from the sensor */ - #pragma pack(push, 1) +#pragma pack(push, 1) struct { float temperature; float diff_pressure; } airspeed_report; - #pragma pack(pop) +#pragma pack(pop) perf_begin(_sample_perf); @@ -458,7 +458,7 @@ test() if (fd < 0) { PX4_ERR("%s open failed (try 'meas_airspeed_sim start' if the driver is not running", PATH_MS4525); - return 1; + return 1; } /* do a simple demand read */ diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index f69363fd40..975fa3687c 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -89,7 +89,7 @@ enum BAROSIM_BUS { class BAROSIM : public device::VDev { public: - BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char* path); + BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path); ~BAROSIM(); virtual int init(); @@ -195,7 +195,7 @@ protected: */ extern "C" __EXPORT int barosim_main(int argc, char *argv[]); -BAROSIM::BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char* path) : +BAROSIM::BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path) : VDev("BAROSIM", path), _interface(interface), _prom(prom_buf.s), @@ -224,12 +224,14 @@ BAROSIM::~BAROSIM() /* make sure we are truly inactive */ stop_cycle(); - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(get_devname(), _class_instance); + } /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } // free perf counters perf_free(_sample_perf); @@ -247,6 +249,7 @@ BAROSIM::init() DEVICE_DEBUG("BAROSIM::init"); ret = VDev::init(); + if (ret != OK) { DEVICE_DEBUG("VDev init failed"); goto out; @@ -270,7 +273,7 @@ BAROSIM::init() _reports->flush(); _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); if (_baro_topic == nullptr) { PX4_ERR("failed to create sensor_baro publication"); @@ -329,8 +332,9 @@ BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -383,8 +387,9 @@ BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - if (_reports->get(brp)) + if (_reports->get(brp)) { ret = sizeof(*brp); + } } while (0); @@ -396,23 +401,23 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - case SENSORIOCSPOLLRATE: + case SENSORIOCSPOLLRATE: switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop_cycle(); _measure_ticks = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -422,13 +427,14 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(BAROSIM_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start_cycle(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -437,36 +443,41 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) unsigned long ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) + if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) { return -EINVAL; + } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start_cycle(); + } return OK; } } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_reports->resize(arg)) { - return -ENOMEM; + if (!_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -481,8 +492,9 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case BAROIOCSMSLPRESSURE: /* range-check for sanity */ - if ((arg < 80000) || (arg > 120000)) + if ((arg < 80000) || (arg > 120000)) { return -EINVAL; + } _msl_pressure = arg; return OK; @@ -537,6 +549,7 @@ BAROSIM::cycle() /* perform collection */ ret = collect(); + if (ret != OK) { /* issue a reset command to the sensor */ _interface->dev_ioctl(IOCTL_RESET, dummy); @@ -569,6 +582,7 @@ BAROSIM::cycle() /* measurement phase */ ret = measure(); + if (ret != OK) { //DEVICE_LOG("measure error %d", ret); /* issue a reset command to the sensor */ @@ -605,8 +619,10 @@ BAROSIM::measure() * Send the command to begin measuring. */ ret = _interface->dev_ioctl(IOCTL_MEASURE, addr); - if (OK != ret) + + if (OK != ret) { perf_count(_comms_errors); + } perf_end(_measure_perf); @@ -631,10 +647,11 @@ BAROSIM::collect() struct baro_report report; /* this should be fairly close to the end of the conversion, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + 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 *)&baro_report, sizeof(baro_report)); + if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); @@ -647,6 +664,7 @@ BAROSIM::collect() report.altitude = baro_report.altitude; report.temperature = baro_report.temperature; report.timestamp = hrt_absolute_time(); + } else { report.pressure = baro_report.pressure; report.altitude = baro_report.altitude; @@ -658,6 +676,7 @@ BAROSIM::collect() if (_baro_topic != nullptr) { /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + } else { PX4_WARN("BAROSIM::collect _baro_topic not initialized"); } @@ -792,6 +811,7 @@ start_bus(struct barosim_bus_option &bus) prom_u prom_buf; device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); + if (interface->init() != OK) { delete interface; PX4_ERR("no device on bus %u", (unsigned)bus.busid); @@ -799,13 +819,14 @@ start_bus(struct barosim_bus_option &bus) } bus.dev = new BAROSIM(interface, prom_buf, bus.devpath); + if (bus.dev != nullptr && OK != bus.dev->init()) { delete bus.dev; bus.dev = NULL; PX4_ERR("bus init failed %p", bus.dev); return false; } - + int fd = px4_open(bus.devpath, O_RDONLY); /* set the poll rate to default, starts automatic data collection */ @@ -813,6 +834,7 @@ start_bus(struct barosim_bus_option &bus) PX4_ERR("can't open baro device"); return false; } + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { px4_close(fd); PX4_ERR("failed setting default poll rate"); @@ -863,6 +885,7 @@ test() int fd; fd = px4_open(bus.devpath, O_RDONLY); + if (fd < 0) { PX4_ERR("open failed (try 'barosim start' if the driver is not running)"); return 1; @@ -940,6 +963,7 @@ reset() int fd; fd = px4_open(bus.devpath, O_RDONLY); + if (fd < 0) { PX4_ERR("failed "); return 1; @@ -954,6 +978,7 @@ reset() PX4_ERR("driver poll restart failed"); return 1; } + return 0; } @@ -963,13 +988,15 @@ reset() int info() { - for (uint8_t i=0; iprint_info(); } } + return 0; } @@ -1078,26 +1105,30 @@ barosim_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { ret = barosim::start(); + } /* * Test the driver/device. */ - else if (!strcmp(verb, "test")) + else if (!strcmp(verb, "test")) { ret = barosim::test(); + } /* * Reset the driver. */ - else if (!strcmp(verb, "reset")) + else if (!strcmp(verb, "reset")) { ret = barosim::reset(); + } /* * Print driver information. */ - else if (!strcmp(verb, "info")) + else if (!strcmp(verb, "info")) { ret = barosim::info(); + } /* * Perform MSL pressure calibration given an altitude in metres @@ -1112,10 +1143,11 @@ barosim_main(int argc, char *argv[]) long altitude = strtol(argv[2], nullptr, 10); ret = barosim::calibrate(altitude); - } - else { + + } else { barosim::usage(); return 1; } + return ret; } diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 8e66dc3ac6..252d1428c9 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file baro_sim.cpp - * - * Simulation interface for barometer - */ +/** + * @file baro_sim.cpp + * + * Simulation interface for barometer + */ /* XXX trim includes */ #include @@ -182,7 +182,7 @@ int BARO_SIM::_measure(unsigned addr) { /* - * Disable retries on this command; we can't know whether failure + * Disable retries on this command; we can't know whether failure * means the device did or did not see the command. */ _retries = 0; @@ -201,25 +201,28 @@ BARO_SIM::_read_prom() int BARO_SIM::transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len) + uint8_t *recv, unsigned recv_len) { // TODO add Simulation data connection so calls retrieve // data from the simulator if (recv_len == 0) { PX4_DEBUG("BARO_SIM measurement requested"); - } - else if (send_len != 1 || send[0] != 0 ) { + + } else if (send_len != 1 || send[0] != 0) { PX4_WARN("BARO_SIM::transfer invalid param %u %u %u", send_len, send[0], recv_len); return 1; - } - else { + + } else { Simulator *sim = Simulator::getInstance(); + if (sim == NULL) { PX4_ERR("Error BARO_SIM::transfer no simulator"); return -ENODEV; } + PX4_DEBUG("BARO_SIM::transfer getting sample"); sim->getBaroSample(recv, recv_len); } + return 0; } diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp index d94fc35f8a..889d793486 100644 --- a/src/platforms/posix/drivers/gpssim/gpssim.cpp +++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp @@ -204,8 +204,9 @@ GPSSIM::~GPSSIM() } /* well, kill it anyway, though this will probably crash */ - if (_task != -1) + if (_task != -1) { px4_task_delete(_task); + } g_dev = nullptr; } @@ -216,12 +217,13 @@ GPSSIM::init() int ret = ERROR; /* do regular cdev init */ - if (VDev::init() != OK) + if (VDev::init() != OK) { goto out; + } /* start the GPS driver worker task */ _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, 1500, (px4_main_t)&GPSSIM::task_main_trampoline, nullptr); + SCHED_PRIORITY_DEFAULT, 1500, (px4_main_t)&GPSSIM::task_main_trampoline, nullptr); if (_task < 0) { PX4_ERR("task start failed: %d", errno); @@ -263,7 +265,8 @@ GPSSIM::task_main_trampoline(void *arg) } int -GPSSIM::receive(int timeout) { +GPSSIM::receive(int timeout) +{ Simulator *sim = Simulator::getInstance(); simulator::RawGPSData gps; sim->getGPSSample((uint8_t *)&gps, sizeof(gps)); @@ -275,11 +278,11 @@ GPSSIM::receive(int timeout) { _report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.eph = (float)gps.eph * 1e-2f; _report_gps_pos.epv = (float)gps.epv * 1e-2f; - _report_gps_pos.vel_m_s = (float)(gps.vel)/100.0f; - _report_gps_pos.vel_n_m_s = (float)(gps.vn)/100.0f; - _report_gps_pos.vel_e_m_s = (float)(gps.ve)/100.0f; - _report_gps_pos.vel_d_m_s = (float)(gps.vd)/100.0f; - _report_gps_pos.cog_rad = (float)(gps.cog)*3.1415f/(100.0f * 180.0f); + _report_gps_pos.vel_m_s = (float)(gps.vel) / 100.0f; + _report_gps_pos.vel_n_m_s = (float)(gps.vn) / 100.0f; + _report_gps_pos.vel_e_m_s = (float)(gps.ve) / 100.0f; + _report_gps_pos.vel_d_m_s = (float)(gps.vd) / 100.0f; + _report_gps_pos.cog_rad = (float)(gps.cog) * 3.1415f / (100.0f * 180.0f); _report_gps_pos.fix_type = gps.fix_type; _report_gps_pos.satellites_used = gps.satellites_visible; @@ -309,9 +312,10 @@ GPSSIM::task_main() _report_gps_pos.vel_n_m_s = 0.0f; _report_gps_pos.vel_e_m_s = 0.0f; _report_gps_pos.vel_d_m_s = 0.0f; - _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * + _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); _report_gps_pos.cog_rad = 0.0f; - _report_gps_pos.vel_ned_valid = true; + _report_gps_pos.vel_ned_valid = true; //no time and satellite information simulated @@ -325,6 +329,7 @@ GPSSIM::task_main() } usleep(2e5); + } else { //Publish initial report that we have access to a GPS //Make sure to clear any stale data in case driver is reset @@ -350,7 +355,8 @@ GPSSIM::task_main() /* opportunistic publishing - else invalid data would end up on the bus */ if (!(_pub_blocked)) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + if (_p_report_sat_info) { if (_report_sat_info_pub != nullptr) { orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); @@ -361,6 +367,7 @@ GPSSIM::task_main() } } } + lock(); } } @@ -383,7 +390,7 @@ void GPSSIM::print_info() { //GPS Mode - if(_fake_gps) { + if (_fake_gps) { PX4_INFO("protocol: faked"); } @@ -392,17 +399,17 @@ GPSSIM::print_info() } PX4_INFO("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - PX4_INFO("sat info: %s, noise: %d, jamming detected: %s", - (_p_report_sat_info != nullptr) ? "enabled" : "disabled", - _report_gps_pos.noise_per_ms, - _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); + PX4_INFO("sat info: %s, noise: %d, jamming detected: %s", + (_p_report_sat_info != nullptr) ? "enabled" : "disabled", + _report_gps_pos.noise_per_ms, + _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); if (_report_gps_pos.timestamp_position != 0) { PX4_INFO("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, - _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); PX4_INFO("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); PX4_INFO("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, - (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); + (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); PX4_INFO("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); //PX4_INFO("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); //PX4_INFO("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index cbe1dbef41..0ac023285c 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -34,7 +34,7 @@ /** * @file gyrosim.cpp * - * Driver for the simulated gyro + * Driver for the simulated gyro * * @author Andrew Tridgell * @author Pat Hickey @@ -242,7 +242,7 @@ private: * * @return 0 on success, 1 on failure */ - int self_test(); + int self_test(); /** * Accel self test @@ -256,7 +256,7 @@ private: * * @return 0 on success, 1 on failure */ - int gyro_self_test(); + int gyro_self_test(); /* set sample rate (approximate) - 1kHz to 5Hz @@ -264,8 +264,8 @@ private: void _set_sample_rate(unsigned desired_sample_rate_hz); /* do not allow to copy this class due to pointer data members */ - GYROSIM(const GYROSIM&); - GYROSIM operator=(const GYROSIM&); + GYROSIM(const GYROSIM &); + GYROSIM operator=(const GYROSIM &); #pragma pack(push, 1) /** @@ -314,8 +314,8 @@ private: int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ - GYROSIM_gyro(const GYROSIM_gyro&); - GYROSIM_gyro operator=(const GYROSIM_gyro&); + GYROSIM_gyro(const GYROSIM_gyro &); + GYROSIM_gyro operator=(const GYROSIM_gyro &); }; /** driver 'main' command */ @@ -389,13 +389,17 @@ GYROSIM::~GYROSIM() delete _gyro; /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_gyro_reports != nullptr) - delete _gyro_reports; + } - if (_accel_class_instance != -1) + if (_gyro_reports != nullptr) { + delete _gyro_reports; + } + + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } /* delete the perf counter */ perf_free(_sample_perf); @@ -419,16 +423,19 @@ GYROSIM::init() } struct accel_report arp = {}; + struct gyro_report grp = {}; /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + if (_accel_reports == nullptr) { PX4_WARN("_accel_reports creation failed"); goto out; } _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + if (_gyro_reports == nullptr) { PX4_WARN("_gyro_reports creation failed"); goto out; @@ -457,6 +464,7 @@ GYROSIM::init() /* do VDev init for the gyro device node, keep it optional */ ret = _gyro->init(); + /* if probe/setup failed, bail now */ if (ret != OK) { DEVICE_DEBUG("gyro init failed"); @@ -472,12 +480,12 @@ GYROSIM::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, ORB_PRIO_HIGH); + &_accel_orb_class_instance, ORB_PRIO_HIGH); if (_accel_topic == nullptr) { PX4_WARN("ADVERT FAIL"); - } - else { + + } else { _pub_blocked = false; } @@ -486,7 +494,7 @@ GYROSIM::init() _gyro_reports->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH); + &_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH); if (_gyro->_gyro_topic == nullptr) { PX4_WARN("ADVERT FAIL"); @@ -511,6 +519,7 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) if (cmd == MPUREAD) { // Get data from the simulator Simulator *sim = Simulator::getInstance(); + if (sim == NULL) { PX4_WARN("failed accessing simulator"); return ENODEV; @@ -519,17 +528,18 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) // 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) - { - PX4_DEBUG("Reading %u bytes from register %u", len-1, reg); - memcpy(&_regdata[reg-MPUREG_PRODUCT_ID], &send[1], len-1); - } - else { - PX4_DEBUG("Writing %u bytes to register %u", len-1, reg); - if (recv) - memcpy(&recv[1], &_regdata[reg-MPUREG_PRODUCT_ID], len-1); + sim->getMPUReport(&recv[2], len - 2); + + } else if (cmd & DIR_READ) { + PX4_DEBUG("Reading %u bytes from register %u", len - 1, reg); + memcpy(&_regdata[reg - MPUREG_PRODUCT_ID], &send[1], len - 1); + + } else { + PX4_DEBUG("Writing %u bytes to register %u", len - 1, reg); + + if (recv) { + memcpy(&recv[1], &_regdata[reg - MPUREG_PRODUCT_ID], len - 1); + } } return PX4_OK; @@ -542,23 +552,26 @@ void GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) { PX4_INFO("GYROSIM::_set_sample_rate %uHz", desired_sample_rate_hz); + if (desired_sample_rate_hz == 0 || - desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || - desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { + desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || + desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { desired_sample_rate_hz = GYROSIM_GYRO_DEFAULT_RATE; } uint8_t div = 1000 / desired_sample_rate_hz; - if(div>200) div=200; - if(div<1) div=1; + + if (div > 200) { div = 200; } + + if (div < 1) { div = 1; } // This does nothing in the simulator but writes the value in the "register" so // register dumps look correct - write_reg(MPUREG_SMPLRT_DIV, div-1); + write_reg(MPUREG_SMPLRT_DIV, div - 1); _sample_rate = 1000 / div; PX4_INFO("GYROSIM: Changed sample rate to %uHz", _sample_rate); - _call_interval = 1000000/_sample_rate; + _call_interval = 1000000 / _sample_rate; hrt_cancel(&_call); hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); } @@ -569,8 +582,9 @@ GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(accel_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -579,17 +593,21 @@ GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) + if (_accel_reports->empty()) { return -EAGAIN; + } perf_count(_accel_reads); /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_accel_reports->get(arp)) + if (!_accel_reports->get(arp)) { break; + } + transferred++; arp++; } @@ -614,24 +632,34 @@ GYROSIM::accel_self_test() { return OK; - if (self_test()) + if (self_test()) { return 1; + } /* inspect accel offsets */ - if (fabsf(_accel_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.z_offset) < 0.000001f) + if (fabsf(_accel_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) { return 1; + } + + if (fabsf(_accel_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) { + return 1; + } return 0; } @@ -641,8 +669,9 @@ GYROSIM::gyro_self_test() { return OK; - if (self_test()) + if (self_test()) { return 1; + } /* * Maximum deviation of 20 degrees, according to @@ -658,27 +687,35 @@ GYROSIM::gyro_self_test() const float max_scale = 0.3f; /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ - if (fabsf(_gyro_scale.x_offset) > max_offset) + if (fabsf(_gyro_scale.x_offset) > max_offset) { return 1; + } /* evaluate gyro scale, complain if off by more than 30% */ - if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) { return 1; + } - if (fabsf(_gyro_scale.y_offset) > max_offset) - return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.y_offset) > max_offset) { return 1; + } - if (fabsf(_gyro_scale.z_offset) > max_offset) + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) { return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) + } + + if (fabsf(_gyro_scale.z_offset) > max_offset) { return 1; + } + + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) { + return 1; + } /* check if all scales are zero */ if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && - (fabsf(_gyro_scale.y_offset) < 0.000001f) && - (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { /* if all are zero, this device is not calibrated */ return 1; } @@ -692,8 +729,9 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(gyro_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -702,17 +740,21 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) + if (_gyro_reports->empty()) { return -EAGAIN; + } perf_count(_gyro_reads); /* copy reports out of our buffer to the caller */ gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_gyro_reports->get(grp)) + if (!_gyro_reports->get(grp)) { break; + } + transferred++; grp++; } @@ -732,34 +774,35 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* convert hz to hrt interval via microseconds */ unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } /* update interval for next measurement */ _call_interval = ticks; @@ -778,23 +821,25 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_accel_reports->resize(arg)) { - return -ENOMEM; + if (!_accel_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } - case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -808,14 +853,15 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCSLOWPASS: return OK; - case ACCELIOCSSCALE: - { + case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ struct accel_scale *s = (struct accel_scale *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { memcpy(&_accel_scale, s, sizeof(_accel_scale)); return OK; + } else { return -EINVAL; } @@ -830,7 +876,7 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) return set_accel_range(arg); case ACCELIOCGRANGE: - return (unsigned long)((_accel_range_m_s2)/GYROSIM_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / GYROSIM_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); @@ -846,24 +892,25 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - /* these are shared with the accel side */ + /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_gyro_reports->resize(arg)) { - return -ENOMEM; + if (!_gyro_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } - case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); @@ -893,6 +940,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) // _gyro_range_scale = xx // _gyro_range_rad_s = xx return -EINVAL; + case GYROIOCGRANGE: return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); @@ -924,7 +972,7 @@ GYROSIM::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; - // general register transfer at low clock speed + // general register transfer at low clock speed transfer(cmd, nullptr, sizeof(cmd)); } @@ -933,11 +981,11 @@ GYROSIM::set_accel_range(unsigned max_g_in) { // workaround for bugged versions of MPU6k (rev C) switch (_product) { - case GYROSIMES_REV_C4: - write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); - _accel_range_scale = (GYROSIM_ONE_G / 4096.0f); - _accel_range_m_s2 = 8.0f * GYROSIM_ONE_G; - return OK; + case GYROSIMES_REV_C4: + write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); + _accel_range_scale = (GYROSIM_ONE_G / 4096.0f); + _accel_range_m_s2 = 8.0f * GYROSIM_ONE_G; + return OK; } uint8_t afs_sel; @@ -948,14 +996,17 @@ GYROSIM::set_accel_range(unsigned max_g_in) afs_sel = 3; lsb_per_g = 2048; max_accel_g = 16; + } else if (max_g_in > 4) { // 8g - AFS_SEL = 2 afs_sel = 2; lsb_per_g = 4096; max_accel_g = 8; + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 afs_sel = 1; lsb_per_g = 8192; max_accel_g = 4; + } else { // 2g - AFS_SEL = 0 afs_sel = 0; lsb_per_g = 16384; @@ -1011,10 +1062,11 @@ GYROSIM::measure() if (x == 99) { x = 0; PX4_INFO("GYROSIM::measure %" PRIu64, hrt_absolute_time()); - } - else { + + } else { x++; } + #endif struct MPUReport mpu_report = {}; @@ -1026,8 +1078,8 @@ GYROSIM::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - // sensor transfer at high clock speed - //set_frequency(GYROSIM_HIGH_BUS_SPEED); + // 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; } @@ -1045,7 +1097,7 @@ GYROSIM::measure() // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures - grb.error_count = arb.error_count = 0; // FIXME + grb.error_count = arb.error_count = 0; // FIXME /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1074,21 +1126,21 @@ GYROSIM::measure() _last_temperature = mpu_report.temp; - arb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f); + arb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f) * 361.0f); arb.temperature = _last_temperature; arb.x = mpu_report.accel_x; arb.y = mpu_report.accel_y; arb.z = mpu_report.accel_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); + 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); grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; - grb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f); + grb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f) * 361.0f); grb.temperature = _last_temperature; grb.x = mpu_report.gyro_x; @@ -1102,6 +1154,7 @@ GYROSIM::measure() /* 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); @@ -1135,22 +1188,25 @@ GYROSIM::print_info() void GYROSIM::print_registers() { - char buf[6*13+1]; - int i=0; + char buf[6 * 13 + 1]; + int i = 0; buf[0] = '\0'; PX4_INFO("GYROSIM registers"); - for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) { + + for (uint8_t reg = MPUREG_PRODUCT_ID; reg <= 108; reg++) { uint8_t v = read_reg(reg); - sprintf(&buf[i*6], "%02x:%02x ",(unsigned)reg, (unsigned)v); + sprintf(&buf[i * 6], "%02x:%02x ", (unsigned)reg, (unsigned)v); i++; - if ((i+1) % 13 == 0) { + + if ((i + 1) % 13 == 0) { PX4_INFO("%s", buf); - i=0; + i = 0; buf[i] = '\0'; } } - PX4_INFO("%s",buf); + + PX4_INFO("%s", buf); } @@ -1165,8 +1221,9 @@ GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : GYROSIM_gyro::~GYROSIM_gyro() { - if (_gyro_class_instance != -1) + if (_gyro_class_instance != -1) { unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance); + } } int @@ -1205,11 +1262,12 @@ GYROSIM_gyro::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - case DEVIOCGDEVICEID: - return (int)VDev::ioctl(filp, cmd, arg); - break; - default: - return _parent->gyro_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)VDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->gyro_ioctl(filp, cmd, arg); } } @@ -1239,7 +1297,7 @@ int start(enum Rotation rotation) { int fd; - GYROSIM **g_dev_ptr = &g_dev_sim; + GYROSIM **g_dev_ptr = &g_dev_sim; const char *path_accel = MPU_DEVICE_PATH_ACCEL; const char *path_gyro = MPU_DEVICE_PATH_GYRO; @@ -1252,17 +1310,20 @@ start(enum Rotation rotation) /* create the driver */ *g_dev_ptr = new GYROSIM(path_accel, path_gyro, rotation); - if (*g_dev_ptr == nullptr) + if (*g_dev_ptr == nullptr) { goto fail; + } - if (OK != (*g_dev_ptr)->init()) + if (OK != (*g_dev_ptr)->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = px4_open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { px4_close(fd); @@ -1274,8 +1335,8 @@ start(enum Rotation rotation) fail: if (*g_dev_ptr != nullptr) { - delete (*g_dev_ptr); - *g_dev_ptr = nullptr; + delete(*g_dev_ptr); + *g_dev_ptr = nullptr; } PX4_WARN("driver start failed"); @@ -1286,13 +1347,16 @@ int stop() { GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr != nullptr) { delete *g_dev_ptr; *g_dev_ptr = nullptr; + } else { /* warn, but not an error */ PX4_WARN("already stopped."); } + return 0; } @@ -1350,7 +1414,7 @@ test() PX4_INFO("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); PX4_INFO("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); PX4_INFO("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / GYROSIM_ONE_G)); + (double)(a_report.range_m_s2 / GYROSIM_ONE_G)); /* do a simple demand read */ sz = px4_read(fd_gyro, &g_report, sizeof(g_report)); @@ -1368,7 +1432,7 @@ test() PX4_INFO("gyro y: \t%d\traw", (int)g_report.y_raw); PX4_INFO("gyro z: \t%d\traw", (int)g_report.z_raw); PX4_INFO("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, - (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); PX4_INFO("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); PX4_INFO("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); @@ -1380,7 +1444,7 @@ test() reset(); PX4_INFO("PASS"); - + return 0; } @@ -1409,11 +1473,11 @@ reset() goto reset_fail; } - px4_close(fd); + px4_close(fd); return 0; reset_fail: - px4_close(fd); + px4_close(fd); return 1; } @@ -1423,7 +1487,8 @@ reset_fail: int info() { - GYROSIM **g_dev_ptr = &g_dev_sim; + GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr == nullptr) { PX4_ERR("driver not running"); return 1; @@ -1442,6 +1507,7 @@ int regdump() { GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr == nullptr) { PX4_ERR("driver not running"); return 1; @@ -1473,11 +1539,13 @@ gyrosim_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ int myoptind = 1; const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'R': rotation = (enum Rotation)atoi(myoptarg); break; + default: gyrosim::usage(); return 0; diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index d070017a36..b58cccb5bd 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -47,16 +47,16 @@ * From Wikibooks: * * PLAY "[string expression]" - * + * * Used to play notes and a score ... The tones are indicated by letters A through G. - * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) + * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) * immediately after the note letter. See this example: - * + * * PLAY "C C# C C#" * * Whitespaces are ignored inside the string expression. There are also codes that - * set the duration, octave and tempo. They are all case-insensitive. PLAY executes - * the commands or notes the order in which they appear in the string. Any indicators + * set the duration, octave and tempo. They are all case-insensitive. PLAY executes + * the commands or notes the order in which they appear in the string. Any indicators * that change the properties are effective for the notes following that indicator. * * Ln Sets the duration (length) of the notes. The variable n does not indicate an actual duration @@ -66,15 +66,15 @@ * The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB" * can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes. * On Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B. - * Remember that C- is equivalent to B. + * Remember that C- is equivalent to B. * < > Changes the current octave respectively down or up one level. * Nn Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.) * Cannot use with sharp and flat. Cannot use with the shorthand notation neither. * MN Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode. * ML Stand for Music Legato. Note duration is full length of that indicated by Ln. * MS Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln. - * Pn Causes a silence (pause) for the length of note indicated (same as Ln). - * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. + * Pn Causes a silence (pause) for the length of note indicated (same as Ln). + * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. * . When placed after a note, it causes the duration of the note to be 3/2 of the set duration. * This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note. * It can be used for a pause as well. @@ -120,14 +120,15 @@ public: virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); virtual ssize_t write(device::file_t *filp, const char *buffer, size_t len); - inline const char *name(int tune) { + inline const char *name(int tune) + { return _tune_names[tune]; } private: static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes - const char * _default_tunes[TONE_NUMBER_OF_TUNES]; - const char * _tune_names[TONE_NUMBER_OF_TUNES]; + const char *_default_tunes[TONE_NUMBER_OF_TUNES]; + const char *_tune_names[TONE_NUMBER_OF_TUNES]; static const uint8_t _note_tab[]; unsigned _default_tune_number; // number of currently playing default tune (0 for none) @@ -151,8 +152,8 @@ private: // unsigned note_to_divisor(unsigned note); - // Calculate the duration in microseconds of play and silence for a - // note given the current tempo, length and mode and the number of + // Calculate the duration in microseconds of play and silence for a + // note given the current tempo, length and mode and the number of // dots following in the play string. // unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); @@ -257,8 +258,9 @@ ToneAlarm::init() ret = VDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } DEVICE_DEBUG("ready"); return OK; @@ -267,7 +269,7 @@ ToneAlarm::init() unsigned ToneAlarm::note_to_divisor(unsigned note) { - const int TONE_ALARM_CLOCK = 120000000ul/4; + const int TONE_ALARM_CLOCK = 120000000ul / 4; // compute the frequency first (Hz) float freq = 880.0f * expf(logf(2.0f) * ((int)note - 46) / 12.0f); @@ -285,25 +287,31 @@ ToneAlarm::note_duration(unsigned &silence, unsigned note_length, unsigned dots) { unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if (note_length == 0) + if (note_length == 0) { note_length = 1; + } + unsigned note_period = whole_note_period / note_length; switch (_note_mode) { case MODE_NORMAL: silence = note_period / 8; break; + case MODE_STACCATO: silence = note_period / 4; break; + default: case MODE_LEGATO: silence = 0; break; } + note_period -= silence; unsigned dot_extension = note_period / 2; + while (dots--) { note_period += dot_extension; dot_extension /= 2; @@ -317,12 +325,14 @@ ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) { unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if (rest_length == 0) + if (rest_length == 0) { rest_length = 1; + } unsigned rest_period = whole_note_period / rest_length; unsigned dot_extension = rest_period / 2; + while (dots--) { rest_period += dot_extension; dot_extension /= 2; @@ -406,115 +416,155 @@ ToneAlarm::next_note() while (note == 0) { // we always need at least one character from the string int c = next_char(); - if (c == 0) + + if (c == 0) { goto tune_end; + } + _next++; switch (c) { case 'L': // select note length _note_length = next_number(); - if (_note_length < 1) + + if (_note_length < 1) { goto tune_error; + } + break; case 'O': // select octave _octave = next_number(); - if (_octave > 6) + + if (_octave > 6) { _octave = 6; + } + break; case '<': // decrease octave - if (_octave > 0) + if (_octave > 0) { _octave--; + } + break; case '>': // increase octave - if (_octave < 6) + if (_octave < 6) { _octave++; + } + break; case 'M': // select inter-note gap c = next_char(); - if (c == 0) + + if (c == 0) { goto tune_error; + } + _next++; + switch (c) { case 'N': _note_mode = MODE_NORMAL; break; + case 'L': _note_mode = MODE_LEGATO; break; + case 'S': _note_mode = MODE_STACCATO; break; + case 'F': _repeat = false; break; + case 'B': _repeat = true; break; + default: goto tune_error; } + break; case 'P': // pause for a note length stop_note(); - hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(next_number(), next_dots()), - (hrt_callout)next_trampoline, - this); + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(next_number(), next_dots()), + (hrt_callout)next_trampoline, + this); return; case 'T': { // change tempo - unsigned nt = next_number(); + unsigned nt = next_number(); - if ((nt >= 32) && (nt <= 255)) { - _tempo = nt; - } else { - goto tune_error; + if ((nt >= 32) && (nt <= 255)) { + _tempo = nt; + + } else { + goto tune_error; + } + + break; } - break; - } case 'N': // play an arbitrary note note = next_number(); - if (note > 84) + + if (note > 84) { goto tune_error; + } + if (note == 0) { // this is a rest - pause for the current note length hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(_note_length, next_dots()), - (hrt_callout)next_trampoline, - this); - return; + (hrt_abstime)rest_duration(_note_length, next_dots()), + (hrt_callout)next_trampoline, + this); + return; } + break; case 'A'...'G': // play a note in the current octave note = _note_tab[c - 'A'] + (_octave * 12) + 1; c = next_char(); + switch (c) { case '#': // up a semitone case '+': - if (note < 84) + if (note < 84) { note++; + } + _next++; break; + case '-': // down a semitone - if (note > 1) + if (note > 1) { note--; + } + _next++; break; + default: // 0 / no next char here is OK break; } + // shorthand length notation note_length = next_number(); - if (note_length == 0) + + if (note_length == 0) { note_length = _note_length; + } + break; default: @@ -540,12 +590,15 @@ tune_error: // stop (and potentially restart) the tune tune_end: stop_note(); + if (_repeat) { start_tune(_tune); + } else { _tune = nullptr; _default_tune_number = 0; } + return; } @@ -555,6 +608,7 @@ ToneAlarm::next_char() while (isspace(*_next)) { _next++; } + return toupper(*_next); } @@ -566,8 +620,11 @@ ToneAlarm::next_number() for (;;) { c = next_char(); - if (!isdigit(c)) + + if (!isdigit(c)) { return number; + } + _next++; number = (number * 10) + (c - '0'); } @@ -582,6 +639,7 @@ ToneAlarm::next_dots() _next++; dots++; } + return dots; } @@ -615,6 +673,7 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) _next = nullptr; _repeat = false; _default_tune_number = 0; + } else { /* always interrupt alarms, unless they are repeating and already playing */ if (!(_repeat && _default_tune_number == arg)) { @@ -623,6 +682,7 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) start_tune(_default_tunes[arg]); } } + } else { result = -EINVAL; } @@ -637,8 +697,9 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) // irqrestore(flags); /* give it to the superclass if we didn't like it */ - if (result == -ENOTTY) + if (result == -ENOTTY) { result = VDev::ioctl(filp, cmd, arg); + } return result; } @@ -647,8 +708,9 @@ ssize_t ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len) { // sanity-check the buffer for length and nul-termination - if (len > _tune_max) + if (len > _tune_max) { return -EFBIG; + } // if we have an existing user tune, free it if (_user_tune != nullptr) { @@ -665,13 +727,16 @@ ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len) } // if the new tune is empty, we're done - if (buffer[0] == '\0') + if (buffer[0] == '\0') { return OK; + } // allocate a copy of the new tune _user_tune = strndup(buffer, len); - if (_user_tune == nullptr) + + if (_user_tune == nullptr) { return -ENOMEM; + } // and play it start_tune(_user_tune); @@ -725,13 +790,15 @@ play_string(const char *str, bool free_buffer) ret = px4_write(fd, str, strlen(str) + 1); px4_close(fd); - if (free_buffer) + if (free_buffer) { free((void *)str); + } if (ret < 0) { PX4_WARN("play tune"); return 1; } + return ret; } @@ -766,31 +833,38 @@ tone_alarm_main(int argc, char *argv[]) ret = play_tune(TONE_STOP_TUNE); } - else if (!strcmp(argv1, "stop")) + else if (!strcmp(argv1, "stop")) { ret = play_tune(TONE_STOP_TUNE); + } - else if ((tune = strtol(argv1, nullptr, 10)) != 0) + else if ((tune = strtol(argv1, nullptr, 10)) != 0) { ret = play_tune(tune); + } /* If it is a file name then load and play it as a string */ else if (*argv1 == '/') { FILE *fd = fopen(argv1, "r"); int sz; char *buffer; + if (fd == nullptr) { PX4_WARN("couldn't open '%s'", argv1); return 1; } + fseek(fd, 0, SEEK_END); sz = ftell(fd); fseek(fd, 0, SEEK_SET); buffer = (char *)malloc(sz + 1); + if (buffer == nullptr) { PX4_WARN("not enough memory memory"); return 1; } + // FIXME - Make GCC happy if (fread(buffer, sz, 1, fd)) { } + /* terminate the string */ buffer[sz] = 0; ret = play_string(buffer, true); @@ -801,14 +875,15 @@ tone_alarm_main(int argc, char *argv[]) if (*argv1 == 'M') { ret = play_string(argv1, false); } - } - else { + + } else { /* It might be a tune name */ for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) if (!strcmp(g_dev->name(tune), argv1)) { ret = play_tune(tune); return ret; } + PX4_WARN("unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); ret = 1; }