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

POSIX Sim drivers: Fix formatting

Browse Source
This commit is contained in:
Lorenz Meier
2015-10-19 13:35:32 +02:00
parent 480bc2f3c6
commit 0b7f6902a9
10 changed files with 617 additions and 372 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/posix/drivers/accelsim/accelsim.cpp
+130 -95
View File
@@ -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; i<ACCELSIM_NUM_CHECKED_REGISTERS; i++) {
for (uint8_t i = 0; i < ACCELSIM_NUM_CHECKED_REGISTERS; i++) {
if (reg == _checked_registers[i]) {
_checked_values[i] = value;
}
@@ -1053,9 +1080,9 @@ ACCELSIM::measure()
// register reads and bad values. This allows the higher level
// code to decide if it should use this sensor based on
// whether it has had failures
accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
accel_report.x_raw = (int16_t)(raw_accel_report.x/_accel_range_scale);
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);
@@ -1157,7 +1184,7 @@ ACCELSIM::mag_measure()
memset(&raw_mag_report, 0, 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))) {
if (OK != transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report))) {
return;
}
@@ -1234,8 +1261,9 @@ ACCELSIM_mag::ACCELSIM_mag(ACCELSIM *parent) :
ACCELSIM_mag::~ACCELSIM_mag()
{
if (_mag_class_instance != -1)
if (_mag_class_instance != -1) {
unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance);
}
}
int
@@ -1244,8 +1272,10 @@ ACCELSIM_mag::init()
int ret;
ret = VDev::init();
if (ret != OK)
if (ret != OK) {
goto out;
}
_mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
@@ -1269,11 +1299,12 @@ int
ACCELSIM_mag::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->mag_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;
}
src/platforms/posix/drivers/adcsim/adcsim.cpp
+14 -6
View File
@@ -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 <px4_config.h>
@@ -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;
src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp
+42 -25
View File
@@ -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
}
src/platforms/posix/drivers/airspeedsim/airspeedsim.h
+1 -1
View File
@@ -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
src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp
+3 -3
View File
@@ -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 */
src/platforms/posix/drivers/barosim/baro.cpp
+67 -35
View File
@@ -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; i<NUM_BUS_OPTIONS; i++) {
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
struct barosim_bus_option &bus = bus_options[i];
if (bus.dev != nullptr) {
PX4_INFO("%s", bus.devpath);
bus.dev->print_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;
}
src/platforms/posix/drivers/barosim/baro_sim.cpp
+14 -11
View File
@@ -31,11 +31,11 @@
*
****************************************************************************/
/**
* @file baro_sim.cpp
*
* Simulation interface for barometer
*/
/**
* @file baro_sim.cpp
*
* Simulation interface for barometer
*/
/* XXX trim includes */
#include <px4_config.h>
@@ -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;
}
src/platforms/posix/drivers/gpssim/gpssim.cpp
+26 -19
View File
@@ -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());
src/platforms/posix/drivers/gyrosim/gyrosim.cpp
+193 -125
View File
@@ -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<accel_report *>(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<gyro_report *>(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;
src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp
+127 -52
View File
@@ -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;
}