diff options
Diffstat (limited to 'src/drivers')
42 files changed, 400 insertions, 145 deletions
diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 0b8e949c9..d6a88714b 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -107,6 +107,10 @@ private: RingBuffer *_reports; perf_counter_t _buffer_overflows; + /* this class has pointer data members and should not be copied */ + Airspeed(const Airspeed&); + Airspeed& operator=(const Airspeed&); + protected: virtual int probe(); diff --git a/src/drivers/airspeed/module.mk b/src/drivers/airspeed/module.mk index 4eef06161..5fbc75309 100644 --- a/src/drivers/airspeed/module.mk +++ b/src/drivers/airspeed/module.mk @@ -36,3 +36,5 @@ # SRCS = airspeed.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/bma180/module.mk b/src/drivers/bma180/module.mk index 4c60ee082..33433307a 100644 --- a/src/drivers/bma180/module.mk +++ b/src/drivers/bma180/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = bma180 SRCS = bma180.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 7df234cab..9d684e394 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -240,6 +240,7 @@ private: * @param context Pointer to the interrupted context. */ static void dev_interrupt(int irq, void *context); + }; /** @@ -469,6 +470,10 @@ private: * @return OK, or -errno on error. */ int remove_poll_waiter(struct pollfd *fds); + + /* do not allow copying this class */ + CDev(const CDev&); + CDev operator=(const CDev&); }; /** @@ -538,6 +543,10 @@ private: } // namespace device // class instance for primary driver of each class -#define CLASS_DEVICE_PRIMARY 0 +enum CLASS_DEVICE { + CLASS_DEVICE_PRIMARY=0, + CLASS_DEVICE_SECONDARY=1, + CLASS_DEVICE_TERTIARY=2 +}; #endif /* _DEVICE_DEVICE_H */ diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 549879352..705b398b0 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -138,6 +138,9 @@ private: uint16_t _address; uint32_t _frequency; struct i2c_dev_s *_dev; + + I2C(const device::I2C&); + I2C operator=(const device::I2C&); }; } // namespace device diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index a9e22eaa6..b26e2e7c8 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -162,6 +162,10 @@ private: volatile unsigned _tail; /**< removal point in _item_size units */ unsigned _next(unsigned index); + + /* we don't want this class to be copied */ + RingBuffer(const RingBuffer&); + RingBuffer operator=(const RingBuffer&); }; RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 54849c8c3..1d9837689 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -129,10 +129,15 @@ private: uint32_t _frequency; struct spi_dev_s *_dev; + /* this class does not allow copying */ + SPI(const SPI&); + SPI operator=(const SPI&); + protected: int _bus; int _transfer(uint8_t *send, uint8_t *recv, unsigned len); + }; } // namespace device diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 7d93ed938..1f98d966b 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -81,7 +81,9 @@ struct accel_scale { /* * ObjDev tag for raw accelerometer data. */ -ORB_DECLARE(sensor_accel); +ORB_DECLARE(sensor_accel0); +ORB_DECLARE(sensor_accel1); +ORB_DECLARE(sensor_accel2); /* * ioctl() definitions diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index e2f0137ae..248b9a73d 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -63,7 +63,8 @@ struct baro_report { /* * ObjDev tag for raw barometer data. */ -ORB_DECLARE(sensor_baro); +ORB_DECLARE(sensor_baro0); +ORB_DECLARE(sensor_baro1); /* * ioctl() definitions diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 2ae8c7058..41b013a44 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -81,7 +81,9 @@ struct gyro_scale { /* * ObjDev tag for raw gyro data. */ -ORB_DECLARE(sensor_gyro); +ORB_DECLARE(sensor_gyro0); +ORB_DECLARE(sensor_gyro1); +ORB_DECLARE(sensor_gyro2); /* * ioctl() definitions diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index a259ac9c0..5ddf5d08e 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -79,8 +79,9 @@ struct mag_scale { /* * ObjDev tag for raw magnetometer data. */ -ORB_DECLARE(sensor_mag); - +ORB_DECLARE(sensor_mag0); +ORB_DECLARE(sensor_mag1); +ORB_DECLARE(sensor_mag2); /* * mag device types, for _device_id diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 972573f9f..84815fdfb 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -202,6 +202,9 @@ ORB_DECLARE(output_pwm); /** force safety switch off (to disable use of safety switch) */ #define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) +/** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */ +#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24) + /* * * diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index b7981e9c4..19f792d19 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -149,6 +149,7 @@ enum { TONE_GPS_WARNING_TUNE, TONE_ARMING_FAILURE_TUNE, TONE_PARACHUTE_RELEASE_TUNE, + TONE_EKF_WARNING_TUNE, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index c15a0cee4..f98d615a2 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -286,6 +286,9 @@ void info(); /** * Start the driver. + * + * This function only returns if the sensor is up and running + * or could not be detected successfully. */ void start(int i2c_bus) diff --git a/src/drivers/ets_airspeed/module.mk b/src/drivers/ets_airspeed/module.mk index 15346c5c5..8aaaf0ebb 100644 --- a/src/drivers/ets_airspeed/module.mk +++ b/src/drivers/ets_airspeed/module.mk @@ -36,6 +36,9 @@ # MODULE_COMMAND = ets_airspeed -MODULE_STACKSIZE = 2048 SRCS = ets_airspeed.cpp + +MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk index f8895f5d5..f1fc49fb3 100644 --- a/src/drivers/hil/module.mk +++ b/src/drivers/hil/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = hil SRCS = hil.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 4aef43102..0e9a961ac 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -162,6 +162,7 @@ private: orb_advert_t _mag_topic; orb_advert_t _subsystem_pub; + orb_id_t _mag_orb_id; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -337,6 +338,9 @@ private: */ int check_offset(); + /* this class has pointer data members, do not allow copying it */ + HMC5883(const HMC5883&); + HMC5883 operator=(const HMC5883&); }; /* @@ -347,14 +351,17 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000), + _work{}, _measure_ticks(0), _reports(nullptr), + _scale{}, _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), _collect_phase(false), _class_instance(-1), _mag_topic(-1), _subsystem_pub(-1), + _mag_orb_id(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), @@ -423,6 +430,20 @@ HMC5883::init() _class_instance = register_class_devname(MAG_DEVICE_PATH); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: + _mag_orb_id = ORB_ID(sensor_mag0); + break; + + case CLASS_DEVICE_SECONDARY: + _mag_orb_id = ORB_ID(sensor_mag1); + break; + + case CLASS_DEVICE_TERTIARY: + _mag_orb_id = ORB_ID(sensor_mag2); + break; + } + ret = OK; /* sensor is ok, but not calibrated */ _sensor_ok = true; @@ -867,7 +888,8 @@ HMC5883::collect() struct { int16_t x, y, z; } report; - int ret = -EIO; + + int ret; uint8_t cmd; uint8_t check_counter; @@ -945,16 +967,16 @@ HMC5883::collect() // apply user specified rotation rotate_3f(_rotation, new_report.x, new_report.y, new_report.z); - if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + if (!(_pub_blocked)) { if (_mag_topic != -1) { /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + orb_publish(_mag_orb_id, _mag_topic, &new_report); } else { - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report); + _mag_topic = orb_advertise(_mag_orb_id, &new_report); if (_mag_topic < 0) - debug("failed to create sensor_mag publication"); + debug("ADVERT FAIL"); } } @@ -1157,7 +1179,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) out: if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { - warn("WARNING: failed to set new scale / offsets for mag"); + warn("failed to set new scale / offsets for mag"); } /* set back to normal mode */ @@ -1341,8 +1363,8 @@ namespace hmc5883 #endif const int ERROR = -1; -HMC5883 *g_dev_int; -HMC5883 *g_dev_ext; +HMC5883 *g_dev_int = nullptr; +HMC5883 *g_dev_ext = nullptr; void start(int bus, enum Rotation rotation); void test(int bus); @@ -1353,6 +1375,9 @@ void usage(); /** * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. */ void start(int bus, enum Rotation rotation) @@ -1378,6 +1403,11 @@ start(int bus, enum Rotation rotation) errx(0, "already started internal"); g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation); if (g_dev_int != nullptr && OK != g_dev_int->init()) { + + /* tear down the failing onboard instance */ + delete g_dev_int; + g_dev_int = nullptr; + if (bus == PX4_I2C_BUS_ONBOARD) { goto fail; } @@ -1443,7 +1473,7 @@ test(int bus) int fd = open(path, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path); + err(1, "%s open failed (try 'hmc5883 start')", path); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk index 07377556d..5daa01dc5 100644 --- a/src/drivers/hmc5883/module.mk +++ b/src/drivers/hmc5883/module.mk @@ -37,7 +37,8 @@ MODULE_COMMAND = hmc5883 -# XXX seems excessive, check if 2048 is sufficient -MODULE_STACKSIZE = 4096 - SRCS = hmc5883.cpp + +MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 64d1a7e55..cfae8761c 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -213,6 +213,7 @@ private: float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; + orb_id_t _orb_id; int _class_instance; unsigned _current_rate; @@ -330,15 +331,22 @@ private: * @return 0 on success, 1 on failure */ int self_test(); + + /* this class does not allow copying */ + L3GD20(const L3GD20&); + L3GD20 operator=(const L3GD20&); }; L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), + _call{}, _call_interval(0), _reports(nullptr), + _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), + _orb_id(nullptr), _class_instance(-1), _current_rate(0), _orientation(SENSOR_BOARD_ROTATION_DEFAULT), @@ -399,21 +407,32 @@ L3GD20::init() _class_instance = register_class_devname(GYRO_DEVICE_PATH); - reset(); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: + _orb_id = ORB_ID(sensor_gyro0); + break; - measure(); + case CLASS_DEVICE_SECONDARY: + _orb_id = ORB_ID(sensor_gyro1); + break; - if (_class_instance == CLASS_DEVICE_PRIMARY) { + case CLASS_DEVICE_TERTIARY: + _orb_id = ORB_ID(sensor_gyro2); + break; + } - /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; - _reports->get(&grp); + reset(); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp); + measure(); - if (_gyro_topic < 0) - debug("failed to create sensor_gyro publication"); + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _reports->get(&grp); + _gyro_topic = orb_advertise(_orb_id, &grp); + + if (_gyro_topic < 0) { + debug("failed to create sensor_gyro publication"); } ret = OK; @@ -931,9 +950,9 @@ L3GD20::measure() poll_notify(POLLIN); /* publish for subscribers */ - if (_gyro_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); + orb_publish(_orb_id, _gyro_topic, &report); } _read++; @@ -990,6 +1009,9 @@ void info(); /** * Start the driver. + * + * This function call only returns once the driver + * started or failed to detect the sensor. */ void start(bool external_bus, enum Rotation rotation) diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk index 23e77e871..5630e7aec 100644 --- a/src/drivers/l3gd20/module.mk +++ b/src/drivers/l3gd20/module.mk @@ -4,3 +4,7 @@ MODULE_COMMAND = l3gd20 SRCS = l3gd20.cpp + +MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ diff --git a/src/drivers/led/module.mk b/src/drivers/led/module.mk index 777f3e442..5b7b4491b 100644 --- a/src/drivers/led/module.mk +++ b/src/drivers/led/module.mk @@ -36,3 +36,5 @@ # SRCS = led.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 45e775055..6880cf0f8 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -284,6 +284,7 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; + orb_id_t _accel_orb_id; int _accel_class_instance; unsigned _accel_read; @@ -461,6 +462,10 @@ private: * @return OK if the value can be supported. */ int mag_set_samplerate(unsigned frequency); + + /* this class cannot be copied */ + LSM303D(const LSM303D&); + LSM303D operator=(const LSM303D&); }; /** @@ -485,29 +490,39 @@ private: LSM303D *_parent; orb_advert_t _mag_topic; + orb_id_t _mag_orb_id; int _mag_class_instance; void measure(); void measure_trampoline(void *arg); + + /* this class does not allow copying due to ptr data members */ + LSM303D_mag(const LSM303D_mag&); + LSM303D_mag operator=(const LSM303D_mag&); }; LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */), _mag(new LSM303D_mag(this)), + _accel_call{}, + _mag_call{}, _call_accel_interval(0), _call_mag_interval(0), _accel_reports(nullptr), _mag_reports(nullptr), + _accel_scale{}, _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), _accel_samplerate(0), _accel_onchip_filter_bandwith(0), + _mag_scale{}, _mag_range_ga(0.0f), _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), + _accel_orb_id(nullptr), _accel_class_instance(-1), _accel_read(0), _mag_read(0), @@ -524,6 +539,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _reg7_expected(0), _accel_log_fd(-1), _accel_logging_enabled(false), + _last_extreme_us(0), _last_log_us(0), _last_log_sync_us(0), _last_log_reg_us(0), @@ -611,34 +627,56 @@ LSM303D::init() /* fill report structures */ measure(); - if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise sensor topic, measure manually to initialize valid report */ + struct mag_report mrp; + _mag_reports->get(&mrp); - /* advertise sensor topic, measure manually to initialize valid report */ - struct mag_report mrp; - _mag_reports->get(&mrp); + /* measurement will have generated a report, publish */ + switch (_mag->_mag_class_instance) { + case CLASS_DEVICE_PRIMARY: + _mag->_mag_orb_id = ORB_ID(sensor_mag0); + break; - /* measurement will have generated a report, publish */ - _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp); + case CLASS_DEVICE_SECONDARY: + _mag->_mag_orb_id = ORB_ID(sensor_mag1); + break; + + case CLASS_DEVICE_TERTIARY: + _mag->_mag_orb_id = ORB_ID(sensor_mag2); + break; + } - if (_mag->_mag_topic < 0) - debug("failed to create sensor_mag publication"); + _mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp); + if (_mag->_mag_topic < 0) { + warnx("ADVERT ERR"); } _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); - if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); - /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; - _accel_reports->get(&arp); + /* measurement will have generated a report, publish */ + switch (_accel_class_instance) { + case CLASS_DEVICE_PRIMARY: + _accel_orb_id = ORB_ID(sensor_accel0); + break; + + case CLASS_DEVICE_SECONDARY: + _accel_orb_id = ORB_ID(sensor_accel1); + break; - /* measurement will have generated a report, publish */ - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); + case CLASS_DEVICE_TERTIARY: + _accel_orb_id = ORB_ID(sensor_accel2); + break; + } - if (_accel_topic < 0) - debug("failed to create sensor_accel publication"); + _accel_topic = orb_advertise(_accel_orb_id, &arp); + if (_accel_topic < 0) { + warnx("ADVERT ERR"); } out: @@ -1557,9 +1595,9 @@ LSM303D::measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - if (_accel_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + orb_publish(_accel_orb_id, _accel_topic, &accel_report); } _accel_read++; @@ -1634,9 +1672,9 @@ LSM303D::mag_measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - if (_mag->_mag_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); + orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report); } _mag_read++; @@ -1730,6 +1768,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) : CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), _parent(parent), _mag_topic(-1), + _mag_orb_id(nullptr), _mag_class_instance(-1) { } @@ -1803,6 +1842,9 @@ void usage(); /** * Start the driver. + * + * This function call only returns once the driver is + * up and running or failed to detect the sensor. */ void start(bool external_bus, enum Rotation rotation) diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk index e40f718c5..b4f3974f4 100644 --- a/src/drivers/lsm303d/module.mk +++ b/src/drivers/lsm303d/module.mk @@ -5,4 +5,6 @@ MODULE_COMMAND = lsm303d SRCS = lsm303d.cpp +MODULE_STACKSIZE = 1200 +EXTRACXXFLAGS = -Weffc++ diff --git a/src/drivers/md25/module.mk b/src/drivers/md25/module.mk index 13821a6b5..3f9cf2d89 100644 --- a/src/drivers/md25/module.mk +++ b/src/drivers/md25/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = md25 SRCS = md25.cpp \ md25_main.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 07611f903..9cfa7f383 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -92,7 +92,7 @@ #include <drivers/airspeed/airspeed.h> /* I2C bus address is 1010001x */ -#define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */ +#define I2C_ADDRESS_MS4525DO 0x28 /**< 7-bit address. Depends on the order code (this is for code "I") */ #define PATH_MS4525 "/dev/ms4525" /* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ #define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */ @@ -102,9 +102,9 @@ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ /* Measurement rate is 100Hz */ -#define MEAS_RATE 100.0f -#define MEAS_DRIVER_FILTER_FREQ 3.0f -#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ +#define MEAS_RATE 100 +#define MEAS_DRIVER_FILTER_FREQ 1.5f +#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ class MEASAirspeed : public Airspeed { @@ -140,9 +140,9 @@ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, CONVERSION_INTERVAL, path), _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), - _t_system_power(-1) + _t_system_power(-1), + system_power{} { - memset(&system_power, 0, sizeof(system_power)); } int @@ -420,6 +420,9 @@ void info(); /** * Start the driver. + * + * This function call only returns once the driver is up and running + * or failed to detect the sensor. */ void start(int i2c_bus) diff --git a/src/drivers/meas_airspeed/module.mk b/src/drivers/meas_airspeed/module.mk index fed4078b6..6f5909978 100644 --- a/src/drivers/meas_airspeed/module.mk +++ b/src/drivers/meas_airspeed/module.mk @@ -36,6 +36,11 @@ # MODULE_COMMAND = meas_airspeed -MODULE_STACKSIZE = 2048 SRCS = meas_airspeed.cpp + +MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk index c7d9cd3ef..5b4893b12 100644 --- a/src/drivers/mpu6000/module.mk +++ b/src/drivers/mpu6000/module.mk @@ -37,7 +37,8 @@ MODULE_COMMAND = mpu6000 -# XXX seems excessive, check if 2048 is not sufficient -MODULE_STACKSIZE = 4096 - SRCS = mpu6000.cpp + +MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 1b3a96a0d..6f5dae7ad 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -215,6 +215,7 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; + orb_id_t _accel_orb_id; int _accel_class_instance; RingBuffer *_gyro_reports; @@ -343,6 +344,9 @@ private: */ void _set_sample_rate(uint16_t desired_sample_rate_hz); + /* do not allow to copy this class due to pointer data members */ + MPU6000(const MPU6000&); + MPU6000 operator=(const MPU6000&); }; /** @@ -367,8 +371,12 @@ protected: private: MPU6000 *_parent; orb_advert_t _gyro_topic; + orb_id_t _gyro_orb_id; int _gyro_class_instance; + /* do not allow to copy this class due to pointer data members */ + MPU6000_gyro(const MPU6000_gyro&); + MPU6000_gyro operator=(const MPU6000_gyro&); }; /** driver 'main' command */ @@ -378,13 +386,17 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), + _call{}, _call_interval(0), _accel_reports(nullptr), + _accel_scale{}, _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _accel_orb_id(nullptr), _accel_class_instance(-1), _gyro_reports(nullptr), + _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _sample_rate(1000), @@ -498,33 +510,58 @@ MPU6000::init() measure(); - if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); - /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; - _accel_reports->get(&arp); + /* measurement will have generated a report, publish */ + switch (_accel_class_instance) { + case CLASS_DEVICE_PRIMARY: + _accel_orb_id = ORB_ID(sensor_accel0); + break; + + case CLASS_DEVICE_SECONDARY: + _accel_orb_id = ORB_ID(sensor_accel1); + break; - /* measurement will have generated a report, publish */ - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); + case CLASS_DEVICE_TERTIARY: + _accel_orb_id = ORB_ID(sensor_accel2); + break; - if (_accel_topic < 0) - debug("failed to create sensor_accel publication"); + } + + _accel_topic = orb_advertise(_accel_orb_id, &arp); + if (_accel_topic < 0) { + warnx("ADVERT FAIL"); } - if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) { - /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; - _gyro_reports->get(&grp); + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _gyro_reports->get(&grp); + + switch (_gyro->_gyro_class_instance) { + case CLASS_DEVICE_PRIMARY: + _gyro->_gyro_orb_id = ORB_ID(sensor_gyro0); + break; - _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp); + case CLASS_DEVICE_SECONDARY: + _gyro->_gyro_orb_id = ORB_ID(sensor_gyro1); + break; - if (_gyro->_gyro_topic < 0) - debug("failed to create sensor_gyro publication"); + case CLASS_DEVICE_TERTIARY: + _gyro->_gyro_orb_id = ORB_ID(sensor_gyro2); + break; } + _gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp); + + if (_gyro->_gyro_topic < 0) { + warnx("ADVERT FAIL"); + } + out: return ret; } @@ -1345,14 +1382,14 @@ MPU6000::measure() poll_notify(POLLIN); _gyro->parent_poll_notify(); - if (_accel_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + orb_publish(_accel_orb_id, _accel_topic, &arb); } - if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb); } /* stop measuring */ @@ -1373,6 +1410,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) : CDev("MPU6000_gyro", path), _parent(parent), _gyro_topic(-1), + _gyro_orb_id(nullptr), _gyro_class_instance(-1) { } @@ -1437,6 +1475,9 @@ void usage(); /** * Start the driver. + * + * This function only returns if the driver is up and running + * or failed to detect the sensor. */ void start(bool external_bus, enum Rotation rotation) @@ -1507,7 +1548,7 @@ test(bool external_bus) int fd = open(path_accel, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", + err(1, "%s open failed (try 'mpu6000 start')", path_accel); /* get the driver */ diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk index 20f8aa173..ee74058fc 100644 --- a/src/drivers/ms5611/module.mk +++ b/src/drivers/ms5611/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = ms5611 SRCS = ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index fe669b5f5..873fa62c4 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -300,12 +300,17 @@ MS5611::init() ret = OK; - if (_class_instance == CLASS_DEVICE_PRIMARY) { - - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: + _baro_topic = orb_advertise(ORB_ID(sensor_baro0), &brp); + break; + case CLASS_DEVICE_SECONDARY: + _baro_topic = orb_advertise(ORB_ID(sensor_baro1), &brp); + break; + } - if (_baro_topic < 0) - debug("failed to create sensor_baro publication"); + if (_baro_topic < 0) { + warnx("failed to create sensor_baro publication"); } } while (0); @@ -722,9 +727,17 @@ MS5611::collect() report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; /* publish it */ - if (_baro_topic > 0 && !(_pub_blocked)) { + if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: + orb_publish(ORB_ID(sensor_baro0), _baro_topic, &report); + break; + + case CLASS_DEVICE_SECONDARY: + orb_publish(ORB_ID(sensor_baro1), _baro_topic, &report); + break; + } } if (_reports->force(&report)) { diff --git a/src/drivers/pca8574/module.mk b/src/drivers/pca8574/module.mk index 825ee9bb7..c53ed9ab2 100644 --- a/src/drivers/pca8574/module.mk +++ b/src/drivers/pca8574/module.mk @@ -4,3 +4,5 @@ MODULE_COMMAND = pca8574 SRCS = pca8574.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/px4flow/module.mk b/src/drivers/px4flow/module.mk index d3062e457..460bec7b9 100644 --- a/src/drivers/px4flow/module.mk +++ b/src/drivers/px4flow/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = px4flow SRCS = px4flow.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8cc1141aa..82977a032 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -179,6 +179,9 @@ private: uint32_t gpio_read(void); int gpio_ioctl(file *filp, int cmd, unsigned long arg); + /* do not allow to copy due to ptr data members */ + PX4FMU(const PX4FMU&); + PX4FMU operator=(const PX4FMU&); }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { @@ -242,6 +245,7 @@ PX4FMU::PX4FMU() : _task(-1), _armed_sub(-1), _outputs_pub(-1), + _armed{}, _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), @@ -252,6 +256,7 @@ PX4FMU::PX4FMU() : _groups_subscribed(0), _control_subs{-1}, _poll_fds_num(0), + _pwm_limit{}, _failsafe_pwm{0}, _disarmed_pwm{0}, _num_failsafe_set(0), diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index eeb59e1a1..a60f1a434 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -6,3 +6,5 @@ MODULE_COMMAND = fmu SRCS = fmu.cpp MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index c14f1f783..5b838fb75 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -46,3 +46,5 @@ SRCS = px4io.cpp \ INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 7d78b0d27..d93009c47 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -453,6 +453,9 @@ private: */ void io_handle_vservo(uint16_t vservo, uint16_t vrssi); + /* do not allow to copy this class due to ptr data members */ + PX4IO(const PX4IO&); + PX4IO operator=(const PX4IO&); }; namespace @@ -496,6 +499,8 @@ PX4IO::PX4IO(device::Device *interface) : _to_battery(0), _to_servorail(0), _to_safety(0), + _outputs{}, + _servorail_status{}, _primary_pwm_device(false), _lockdown_override(false), _battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor @@ -1144,6 +1149,12 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; } + if (armed.force_failsafe) { + set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } else { + clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -1997,7 +2008,7 @@ PX4IO::print_status(bool extended_status) ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") ); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s%s%s%s%s\n", + printf("arming 0x%04x%s%s%s%s%s%s%s%s\n", arming, ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), @@ -2005,7 +2016,9 @@ PX4IO::print_status(bool extended_status) ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""), - ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : "")); + ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""), + ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : "") + ); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), @@ -2217,6 +2230,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); break; + case PWM_SERVO_SET_FORCE_FAILSAFE: + /* force failsafe mode instantly */ + if (arg == 0) { + /* clear force failsafe flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, 0); + } else { + /* set force failsafe flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); + } + break; + case DSM_BIND_START: /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ @@ -2419,7 +2443,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) break; case PX4IO_CHECK_CRC: { - /* check IO firmware CRC against passed value */ + /* check IO firmware CRC against passed value */ uint32_t io_crc = 0; ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); if (ret != OK) @@ -2679,7 +2703,7 @@ checkcrc(int argc, char *argv[]) int fd = open(argv[1], O_RDONLY); if (fd == -1) { printf("open of %s failed - %d\n", argv[1], errno); - exit(1); + exit(1); } const uint32_t app_size_max = 0xf000; uint32_t fw_crc = 0; @@ -2694,7 +2718,7 @@ checkcrc(int argc, char *argv[]) close(fd); while (nbytes < app_size_max) { uint8_t b = 0xff; - fw_crc = crc32part(&b, 1, fw_crc); + fw_crc = crc32part(&b, 1, fw_crc); nbytes++; } @@ -2707,7 +2731,7 @@ checkcrc(int argc, char *argv[]) if (ret != OK) { printf("check CRC failed - %d\n", ret); - exit(1); + exit(1); } printf("CRCs match\n"); exit(0); @@ -2737,12 +2761,12 @@ bind(int argc, char *argv[]) pulses = DSMX_BIND_PULSES; else if (!strcmp(argv[2], "dsmx8")) pulses = DSMX8_BIND_PULSES; - else + else errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]); // Test for custom pulse parameter if (argc > 3) pulses = atoi(argv[3]); - if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) errx(1, "system must not be armed"); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 @@ -2944,7 +2968,7 @@ lockdown(int argc, char *argv[]) (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0); warnx("ACTUATORS ARE NOW SAFE IN HIL."); } - + } else { errx(1, "driver not loaded, exiting"); } diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index c39494fb0..d227e15d5 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -157,6 +157,10 @@ private: perf_counter_t _pc_idle; perf_counter_t _pc_badidle; + /* do not allow top copying this class */ + PX4IO_serial(PX4IO_serial &); + PX4IO_serial& operator = (const PX4IO_serial &); + }; IOPacket PX4IO_serial::_dma_buffer; @@ -173,7 +177,9 @@ PX4IO_serial::PX4IO_serial() : _tx_dma(nullptr), _rx_dma(nullptr), _rx_dma_status(_dma_status_inactive), - _pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")), + _bus_semaphore(SEM_INITIALIZER(0)), + _completion_semaphore(SEM_INITIALIZER(0)), + _pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")), _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), _pc_retries(perf_alloc(PC_COUNT, "io_retries ")), _pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")), diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index d134c0246..fb16f891f 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -65,7 +65,8 @@ PX4IO_Uploader::PX4IO_Uploader() : _io_fd(-1), - _fw_fd(-1) + _fw_fd(-1), + bl_rev(0) { } @@ -204,12 +205,8 @@ PX4IO_Uploader::upload(const char *filenames[]) if (bl_rev <= 2) { ret = verify_rev2(fw_size); - } else if(bl_rev == 3) { - ret = verify_rev3(fw_size); } else { - /* verify rev 4 and higher still uses the same approach and - * every version *needs* to be verified. - */ + /* verify rev 3 and higher. Every version *needs* to be verified. */ ret = verify_rev3(fw_size); } @@ -249,7 +246,7 @@ PX4IO_Uploader::upload(const char *filenames[]) } int -PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) +PX4IO_Uploader::recv_byte_with_timeout(uint8_t *c, unsigned timeout) { struct pollfd fds[1]; @@ -266,24 +263,24 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) return -ETIMEDOUT; } - read(_io_fd, &c, 1); + read(_io_fd, c, 1); #ifdef UDEBUG - log("recv 0x%02x", c); + log("recv_bytes 0x%02x", c); #endif return OK; } int -PX4IO_Uploader::recv(uint8_t *p, unsigned count) +PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count) { + int ret = OK; while (count--) { - int ret = recv(*p++, 5000); + ret = recv_byte_with_timeout(p++, 5000); if (ret != OK) - return ret; + break; } - - return OK; + return ret; } void @@ -293,10 +290,10 @@ PX4IO_Uploader::drain() int ret; do { - // the small recv timeout here is to allow for fast + // the small recv_bytes timeout here is to allow for fast // drain when rebooting the io board for a forced // update of the fw without using the safety switch - ret = recv(c, 40); + ret = recv_byte_with_timeout(&c, 40); #ifdef UDEBUG if (ret == OK) { @@ -314,21 +311,19 @@ PX4IO_Uploader::send(uint8_t c) #endif if (write(_io_fd, &c, 1) != 1) return -errno; - return OK; } int PX4IO_Uploader::send(uint8_t *p, unsigned count) { + int ret; while (count--) { - int ret = send(*p++); - + ret = send(*p++); if (ret != OK) - return ret; + break; } - - return OK; + return ret; } int @@ -337,12 +332,12 @@ PX4IO_Uploader::get_sync(unsigned timeout) uint8_t c[2]; int ret; - ret = recv(c[0], timeout); + ret = recv_byte_with_timeout(c, timeout); if (ret != OK) return ret; - ret = recv(c[1], timeout); + ret = recv_byte_with_timeout(c + 1, timeout); if (ret != OK) return ret; @@ -378,7 +373,7 @@ PX4IO_Uploader::get_info(int param, uint32_t &val) send(param); send(PROTO_EOC); - ret = recv((uint8_t *)&val, sizeof(val)); + ret = recv_bytes((uint8_t *)&val, sizeof(val)); if (ret != OK) return ret; @@ -419,12 +414,15 @@ PX4IO_Uploader::program(size_t fw_size) int ret; size_t sent = 0; - file_buf = (uint8_t *)malloc(PROG_MULTI_MAX); + file_buf = new uint8_t[PROG_MULTI_MAX]; if (!file_buf) { log("Can't allocate program buffer"); return -ENOMEM; } + ASSERT((fw_size & 3) == 0); + ASSERT((PROG_MULTI_MAX & 3) == 0); + log("programming %u bytes...", (unsigned)fw_size); ret = lseek(_fw_fd, 0, SEEK_SET); @@ -443,34 +441,26 @@ PX4IO_Uploader::program(size_t fw_size) (unsigned)sent, (int)count, (int)errno); - } - - if (count == 0) { - free(file_buf); - return OK; + ret = -errno; + break; } sent += count; - if (count < 0) - return -errno; - - ASSERT((count % 4) == 0); - send(PROTO_PROG_MULTI); send(count); - send(&file_buf[0], count); + send(file_buf, count); send(PROTO_EOC); ret = get_sync(1000); if (ret != OK) { - free(file_buf); - return ret; + break; } } - free(file_buf); - return OK; + + delete [] file_buf; + return ret; } int @@ -524,7 +514,7 @@ PX4IO_Uploader::verify_rev2(size_t fw_size) for (ssize_t i = 0; i < count; i++) { uint8_t c; - ret = recv(c, 5000); + ret = recv_byte_with_timeout(&c, 5000); if (ret != OK) { log("%d: got %d waiting for bytes", sent + i, ret); @@ -611,7 +601,7 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local) send(PROTO_GET_CRC); send(PROTO_EOC); - ret = recv((uint8_t*)(&crc), sizeof(crc)); + ret = recv_bytes((uint8_t*)(&crc), sizeof(crc)); if (ret != OK) { log("did not receive CRC checksum"); diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 55f63eef9..e17523413 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -74,20 +74,19 @@ private: INFO_BOARD_REV = 3, /**< board revision */ INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ - PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */ - READ_MULTI_MAX = 60, /**< protocol max is 255, something overflows with >= 64 */ + PROG_MULTI_MAX = 248, /**< protocol max is 255, must be multiple of 4 */ }; int _io_fd; int _fw_fd; - uint32_t bl_rev; /**< bootloader revision */ + uint32_t bl_rev; /**< bootloader revision */ void log(const char *fmt, ...); - int recv(uint8_t &c, unsigned timeout); - int recv(uint8_t *p, unsigned count); + int recv_byte_with_timeout(uint8_t *c, unsigned timeout); + int recv_bytes(uint8_t *p, unsigned count); void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk index 39b53ec9e..c287e35f3 100644 --- a/src/drivers/rgbled/module.mk +++ b/src/drivers/rgbled/module.mk @@ -4,3 +4,5 @@ MODULE_COMMAND = rgbled SRCS = rgbled.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk index 1abecf198..c5e55bdc3 100644 --- a/src/drivers/roboclaw/module.mk +++ b/src/drivers/roboclaw/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = roboclaw SRCS = roboclaw_main.cpp \ RoboClaw.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 810f4aacc..03c7bd399 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -336,6 +336,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<<BAP"; _default_tunes[TONE_PARACHUTE_RELEASE_TUNE] = "MFT255L16agagagag"; // parachute release + _default_tunes[TONE_EKF_WARNING_TUNE] = "MFT255L8ddd#d#eeff"; // ekf warning _tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune _tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone @@ -348,6 +349,7 @@ ToneAlarm::ToneAlarm() : _tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning"; // gps warning _tune_names[TONE_ARMING_FAILURE_TUNE] = "arming_failure"; //fail to arm _tune_names[TONE_PARACHUTE_RELEASE_TUNE] = "parachute_release"; // parachute release + _tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning"; // ekf warning } ToneAlarm::~ToneAlarm() |