diff options
Diffstat (limited to 'src/drivers')
22 files changed, 691 insertions, 34 deletions
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 9d2c1441d..7f1b21a95 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -121,7 +121,7 @@ int ardrone_interface_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 15, 1100, ardrone_interface_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp new file mode 100644 index 000000000..dd83dacaf --- /dev/null +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -0,0 +1,540 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Randy Mackay <rmackay9@yahoo.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file batt_smbus.cpp + * + * Driver for a battery monitor connected via SMBus (I2C). + * + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <sched.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> +#include <ctype.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <board_config.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +#include <uORB/uORB.h> +#include <uORB/topics/subsystem_info.h> +#include <uORB/topics/battery_status.h> + +#include <float.h> + +#include <drivers/device/i2c.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_batt_smbus.h> +#include <drivers/device/ringbuffer.h> + +#define BATT_SMBUS_ADDR_MIN 0x08 /* lowest possible address */ +#define BATT_SMBUS_ADDR_MAX 0x7F /* highest possible address */ + +#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION +#define BATT_SMBUS_ADDR 0x0B /* I2C address */ +#define BATT_SMBUS_TEMP 0x08 /* temperature register */ +#define BATT_SMBUS_VOLTAGE 0x09 /* voltage register */ +#define BATT_SMBUS_DESIGN_CAPACITY 0x18 /* design capacity register */ +#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 /* design voltage register */ +#define BATT_SMBUS_SERIALNUM 0x1c /* serial number register */ +#define BATT_SMBUS_MANUFACTURE_NAME 0x20 /* manufacturer name */ +#define BATT_SMBUS_MANUFACTURE_INFO 0x25 /* cell voltage register */ +#define BATT_SMBUS_CURRENT 0x2a /* current register */ +#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) /* time in microseconds, measure at 10hz */ + +#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 /* Polynomial for calculating PEC */ + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class BATT_SMBUS : public device::I2C +{ +public: + BATT_SMBUS(int bus = PX4_I2C_BUS_EXPANSION, uint16_t batt_smbus_addr = BATT_SMBUS_ADDR); + virtual ~BATT_SMBUS(); + + virtual int init(); + virtual int test(); + int search(); /* search all possible slave addresses for a smart battery */ + +protected: + virtual int probe(); // check if the device can be contacted + +private: + + // start periodic reads from the battery + void start(); + + // stop periodic reads from the battery + void stop(); + + // static function that is called by worker queue + static void cycle_trampoline(void *arg); + + // perform a read from the battery + void cycle(); + + // read_reg - read a word from specified register + int read_reg(uint8_t reg, uint16_t &val); + + // read_block - returns number of characters read if successful, zero if unsuccessful + uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero); + + // get_PEC - calculate PEC for a read or write from the battery + // buff is the data that was read or will be written + uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const; + + // internal variables + work_s _work; // work queue for scheduling reads + RingBuffer *_reports; // buffer of recorded voltages, currents + struct battery_status_s _last_report; // last published report, used for test() + orb_advert_t _batt_topic; + orb_id_t _batt_orb_id; +}; + +/* for now, we only support one BATT_SMBUS */ +namespace +{ +BATT_SMBUS *g_batt_smbus; +} + +void batt_smbus_usage(); + +extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); + +// constructor +BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : + I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000), + _work{}, + _reports(nullptr), + _batt_topic(-1), + _batt_orb_id(nullptr) +{ + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +// destructor +BATT_SMBUS::~BATT_SMBUS() +{ + /* make sure we are truly inactive */ + stop(); + + if (_reports != nullptr) { + delete _reports; + } +} + +int +BATT_SMBUS::init() +{ + int ret = ENOTTY; + + // attempt to initialise I2C bus + ret = I2C::init(); + + if (ret != OK) { + errx(1,"failed to init I2C"); + return ret; + } else { + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(struct battery_status_s)); + if (_reports == nullptr) { + ret = ENOTTY; + } else { + // start work queue + start(); + } + } + + // init orb id + _batt_orb_id = ORB_ID(battery_status); + + return ret; +} + +int +BATT_SMBUS::test() +{ + int sub = orb_subscribe(ORB_ID(battery_status)); + bool updated = false; + struct battery_status_s status; + uint64_t start_time = hrt_absolute_time(); + + // loop for 5 seconds + while ((hrt_absolute_time() - start_time) < 5000000) { + + // display new info that has arrived from the orb + orb_check(sub, &updated); + if (updated) { + if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { + warnx("V=%4.2f C=%4.2f", status.voltage_v, status.current_a); + } + } + + // sleep for 0.05 seconds + usleep(50000); + } + + return OK; +} + +/* search all possible slave addresses for a smart battery */ +int +BATT_SMBUS::search() +{ + bool found_slave = false; + uint16_t tmp; + + // search through all valid SMBus addresses + for (uint8_t i=BATT_SMBUS_ADDR_MIN; i<=BATT_SMBUS_ADDR_MAX; i++) { + set_address(i); + if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { + warnx("battery found at 0x%x",(int)i); + found_slave = true; + } + // short sleep + usleep(1); + } + + // display completion message + if (found_slave) { + warnx("Done."); + } else { + warnx("No smart batteries found."); + } + + return OK; +} + +int +BATT_SMBUS::probe() +{ + // always return OK to ensure device starts + return OK; +} + +// start periodic reads from the battery +void +BATT_SMBUS::start() +{ + /* reset the report ring and state machine */ + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, 1); +} + +// stop periodic reads from the battery +void +BATT_SMBUS::stop() +{ + work_cancel(HPWORK, &_work); +} + +// static function that is called by worker queue +void +BATT_SMBUS::cycle_trampoline(void *arg) +{ + BATT_SMBUS *dev = (BATT_SMBUS *)arg; + + dev->cycle(); +} + +// perform a read from the battery +void +BATT_SMBUS::cycle() +{ + // read data from sensor + struct battery_status_s new_report; + + // set time of reading + new_report.timestamp = hrt_absolute_time(); + + // read voltage + uint16_t tmp; + if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { + // initialise new_report + memset(&new_report, 0, sizeof(new_report)); + + // convert millivolts to volts + new_report.voltage_v = ((float)tmp) / 1000.0f; + + // read current + usleep(1); + uint8_t buff[4]; + if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { + new_report.current_a = (float)((int32_t)((uint32_t)buff[3]<<24 | (uint32_t)buff[2]<<16 | (uint32_t)buff[1]<<8 | (uint32_t)buff[0])) / 1000.0f; + } + + // publish to orb + if (_batt_topic != -1) { + orb_publish(_batt_orb_id, _batt_topic, &new_report); + } else { + _batt_topic = orb_advertise(_batt_orb_id, &new_report); + if (_batt_topic < 0) { + errx(1, "ADVERT FAIL"); + } + } + + // copy report for test() + _last_report = new_report; + + /* post a report to the ring */ + _reports->force(&new_report); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + } + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS)); +} + +int +BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) +{ + uint8_t buff[3]; // 2 bytes of data + PEC + + // read from register + int ret = transfer(®, 1, buff, 3); + if (ret == OK) { + // check PEC + uint8_t pec = get_PEC(reg, true, buff, 2); + if (pec == buff[2]) { + val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; + } else { + ret = ENOTTY; + } + } + + // return success or failure + return ret; +} + +// read_block - returns number of characters read if successful, zero if unsuccessful +uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) +{ + uint8_t buff[max_len+2]; // buffer to hold results + + usleep(1); + + // read bytes including PEC + int ret = transfer(®, 1, buff, max_len+2); + + // return zero on failure + if (ret != OK) { + return 0; + } + + // get length + uint8_t bufflen = buff[0]; + + // sanity check length returned by smbus + if (bufflen == 0 || bufflen > max_len) { + return 0; + } + + // check PEC + uint8_t pec = get_PEC(reg, true, buff, bufflen+1); + if (pec != buff[bufflen+1]) { + // debug + warnx("CurrPEC:%x vs MyPec:%x",(int)buff[bufflen+1],(int)pec); + return 0; + } else { + warnx("CurPEC ok: %x",(int)pec); + } + + // copy data + memcpy(data, &buff[1], bufflen); + + // optionally add zero to end + if (append_zero) { + data[bufflen] = '\0'; + } + + // return success + return bufflen; +} + +// get_PEC - calculate PEC for a read or write from the battery +// buff is the data that was read or will be written +uint8_t BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const +{ + // exit immediately if no data + if (len <= 0) { + return 0; + } + + // prepare temp buffer for calcing crc + uint8_t tmp_buff[len+3]; + tmp_buff[0] = (uint8_t)get_address() << 1; + tmp_buff[1] = cmd; + tmp_buff[2] = tmp_buff[0] | (uint8_t)reading; + memcpy(&tmp_buff[3],buff,len); + + // initialise crc to zero + uint8_t crc = 0; + uint8_t shift_reg = 0; + bool do_invert; + + // for each byte in the stream + for (uint8_t i=0; i<sizeof(tmp_buff); i++) { + // load next data byte into the shift register + shift_reg = tmp_buff[i]; + // for each bit in the current byte + for (uint8_t j=0; j<8; j++) { + do_invert = (crc ^ shift_reg) & 0x80; + crc <<= 1; + shift_reg <<= 1; + if (do_invert) { + crc ^= BATT_SMBUS_PEC_POLYNOMIAL; + } + } + } + + // return result + return crc; +} + +///////////////////////// shell functions /////////////////////// + +void +batt_smbus_usage() +{ + warnx("missing command: try 'start', 'test', 'stop', 'search'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", BATT_SMBUS_I2C_BUS); + warnx(" -a addr (0x%x)", BATT_SMBUS_ADDR); +} + +int +batt_smbus_main(int argc, char *argv[]) +{ + int i2cdevice = BATT_SMBUS_I2C_BUS; + int batt_smbusadr = BATT_SMBUS_ADDR; /* 7bit */ + + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + batt_smbusadr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + batt_smbus_usage(); + exit(0); + } + } + + if (optind >= argc) { + batt_smbus_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + if (!strcmp(verb, "start")) { + if (g_batt_smbus != nullptr) { + errx(1, "already started"); + } else { + // create new global object + g_batt_smbus = new BATT_SMBUS(i2cdevice, batt_smbusadr); + + if (g_batt_smbus == nullptr) { + errx(1, "new failed"); + } + + if (OK != g_batt_smbus->init()) { + delete g_batt_smbus; + g_batt_smbus = nullptr; + errx(1, "init failed"); + } + } + + exit(0); + } + + /* need the driver past this point */ + if (g_batt_smbus == nullptr) { + warnx("not started"); + batt_smbus_usage(); + exit(1); + } + + if (!strcmp(verb, "test")) { + g_batt_smbus->test(); + exit(0); + } + + if (!strcmp(verb, "stop")) { + delete g_batt_smbus; + g_batt_smbus = nullptr; + exit(0); + } + + if (!strcmp(verb, "search")) { + g_batt_smbus->search(); + exit(0); + } + + batt_smbus_usage(); + exit(0); +} diff --git a/src/drivers/batt_smbus/module.mk b/src/drivers/batt_smbus/module.mk new file mode 100644 index 000000000..b32ea6e55 --- /dev/null +++ b/src/drivers/batt_smbus/module.mk @@ -0,0 +1,8 @@ +# +# driver for SMBus smart batteries +# + +MODULE_COMMAND = batt_smbus +SRCS = batt_smbus.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c index e40d1730c..94a716029 100644 --- a/src/drivers/boards/aerocore/aerocore_led.c +++ b/src/drivers/boards/aerocore/aerocore_led.c @@ -45,6 +45,7 @@ #include "board_config.h" #include <arch/board/board.h> +#include <systemlib/err.h> /* * Ideally we'd be able to get these from up_internal.h, @@ -54,7 +55,7 @@ * CONFIG_ARCH_LEDS configuration switch. */ __BEGIN_DECLS -extern void led_init(); +extern void led_init(void); extern void led_on(int led); extern void led_off(int led); extern void led_toggle(int led); diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index ee365e47c..2fd8bc724 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -105,6 +105,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_SPI_CS_HMC (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN1) #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) #define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) #define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) @@ -119,6 +120,7 @@ __BEGIN_DECLS #define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_BARO 3 #define PX4_SPIDEV_MPU 4 +#define PX4_SPIDEV_HMC 5 /* External bus */ #define PX4_SPIDEV_EXT0 1 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 8c37d31a7..27f193513 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -73,6 +73,7 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_configgpio(GPIO_SPI_CS_GYRO); stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); stm32_configgpio(GPIO_SPI_CS_BARO); + stm32_configgpio(GPIO_SPI_CS_HMC); stm32_configgpio(GPIO_SPI_CS_MPU); /* De-activate all peripherals, @@ -82,6 +83,7 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); stm32_configgpio(GPIO_EXTI_GYRO_DRDY); @@ -117,6 +119,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; @@ -125,6 +128,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; @@ -133,6 +137,16 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + break; + + case PX4_SPIDEV_HMC: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC, !selected); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; @@ -141,6 +155,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); break; diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 705b398b0..206e71ddc 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -58,7 +58,7 @@ public: /** * Get the address */ - int16_t get_address() { return _address; } + int16_t get_address() const { return _address; } protected: /** diff --git a/src/drivers/drv_batt_smbus.h b/src/drivers/drv_batt_smbus.h new file mode 100644 index 000000000..ca130c84e --- /dev/null +++ b/src/drivers/drv_batt_smbus.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_batt_smbus.h + * + * SMBus battery monitor device API + */ + +#pragma once + +#include <stdint.h> +#include <sys/ioctl.h> +#include "drv_orb_dev.h" + +/* device path */ +#define BATT_SMBUS_DEVICE_PATH "/dev/batt_smbus" diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index bccdf1190..5035600ef 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -221,7 +221,7 @@ int frsky_telemetry_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2000, frsky_telemetry_thread_main, - (const char **)argv); + (char * const *)argv); while (!thread_running) { usleep(200); diff --git a/src/drivers/hott/comms.h b/src/drivers/hott/comms.h index f5608122f..0a586a8fd 100644 --- a/src/drivers/hott/comms.h +++ b/src/drivers/hott/comms.h @@ -41,6 +41,6 @@ #ifndef COMMS_H_ #define COMMS_H -int open_uart(const char *device); +__EXPORT int open_uart(const char *device); #endif /* COMMS_H_ */ diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 8ab9d8d55..4b8e0c0b0 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -214,7 +214,7 @@ hott_sensors_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 1024, hott_sensors_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk index 47aea6caf..25a6f0ac0 100644 --- a/src/drivers/hott/hott_sensors/module.mk +++ b/src/drivers/hott/hott_sensors/module.mk @@ -37,8 +37,6 @@ MODULE_COMMAND = hott_sensors -SRCS = hott_sensors.cpp \ - ../messages.cpp \ - ../comms.cpp +SRCS = hott_sensors.cpp MAXOPTIMIZATION = -Os diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index edbb14172..17a24d104 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -240,7 +240,7 @@ hott_telemetry_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2048, hott_telemetry_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/drivers/hott/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk index cd7bdbc85..9de47b356 100644 --- a/src/drivers/hott/hott_telemetry/module.mk +++ b/src/drivers/hott/hott_telemetry/module.mk @@ -37,8 +37,6 @@ MODULE_COMMAND = hott_telemetry -SRCS = hott_telemetry.cpp \ - ../messages.cpp \ - ../comms.cpp +SRCS = hott_telemetry.cpp MAXOPTIMIZATION = -Os diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h index 224f8fc56..a116a50dd 100644 --- a/src/drivers/hott/messages.h +++ b/src/drivers/hott/messages.h @@ -235,15 +235,15 @@ struct gps_module_msg { // The maximum size of a message. #define MAX_MESSAGE_BUFFER_SIZE 45 -void init_sub_messages(void); -void init_pub_messages(void); -void build_gam_request(uint8_t *buffer, size_t *size); -void publish_gam_message(const uint8_t *buffer); -void build_eam_response(uint8_t *buffer, size_t *size); -void build_gam_response(uint8_t *buffer, size_t *size); -void build_gps_response(uint8_t *buffer, size_t *size); -float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec); +__EXPORT void init_sub_messages(void); +__EXPORT void init_pub_messages(void); +__EXPORT void build_gam_request(uint8_t *buffer, size_t *size); +__EXPORT void publish_gam_message(const uint8_t *buffer); +__EXPORT void build_eam_response(uint8_t *buffer, size_t *size); +__EXPORT void build_gam_response(uint8_t *buffer, size_t *size); +__EXPORT void build_gps_response(uint8_t *buffer, size_t *size); +__EXPORT float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); +__EXPORT void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec); #endif /* MESSAGES_H_ */ diff --git a/src/drivers/hott/module.mk b/src/drivers/hott/module.mk new file mode 100644 index 000000000..31a66d491 --- /dev/null +++ b/src/drivers/hott/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Graupner HoTT Sensors messages. +# + +SRCS = messages.cpp \ + comms.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/px4flow/module.mk b/src/drivers/px4flow/module.mk index 460bec7b9..ecd3e804a 100644 --- a/src/drivers/px4flow/module.mk +++ b/src/drivers/px4flow/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = px4flow SRCS = px4flow.cpp MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-attributes diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 09ec4bf96..7e3461ba1 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -73,13 +73,14 @@ #include <board_config.h> /* Configuration Constants */ -#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 -//range 0x42 - 0x49 +#define I2C_FLOW_ADDRESS 0x42 ///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49 /* PX4FLOW Registers addresses */ -#define PX4FLOW_REG 0x16 /* Measure Register 22*/ +#define PX4FLOW_REG 0x16 ///< Measure Register 22 + +#define PX4FLOW_CONVERSION_INTERVAL 20000 ///< in microseconds! 20000 = 50 Hz 100000 = 10Hz +#define PX4FLOW_I2C_MAX_BUS_SPEED 400000 ///< 400 KHz maximum speed -#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -106,7 +107,7 @@ struct i2c_frame { }; struct i2c_frame f; -typedef struct i2c_integral_frame { +struct i2c_integral_frame { uint16_t frame_count_since_last_readout; int16_t pixel_flow_x_integral; int16_t pixel_flow_y_integral; @@ -200,7 +201,7 @@ private: extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); PX4FLOW::PX4FLOW(int bus, int address) : - I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz + I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, PX4FLOW_I2C_MAX_BUS_SPEED), /* 100-400 KHz */ _reports(nullptr), _sensor_ok(false), _measure_ticks(0), diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index 44ed03254..83086fd7c 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -109,7 +109,7 @@ int roboclaw_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 10, 2048, roboclaw_thread_main, - (const char **)argv); + (char * const *)argv); exit(0); } else if (!strcmp(argv[1], "test")) { diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 4301750f0..8e62e0d4b 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -90,7 +90,9 @@ static const int ERROR = -1; #define SF0X_TAKE_RANGE_REG 'd' #define SF02F_MIN_DISTANCE 0.0f #define SF02F_MAX_DISTANCE 40.0f -#define SF0X_DEFAULT_PORT "/dev/ttyS2" + +// designated SERIAL4/5 on Pixhawk +#define SF0X_DEFAULT_PORT "/dev/ttyS6" class SF0X : public device::CDev { diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 281f918d7..603c2eb9d 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -253,9 +253,11 @@ static uint16_t latency_baseline; static uint16_t latency_actual; /* latency histogram */ -#define LATENCY_BUCKET_COUNT 8 -static const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; -static uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; +#define LATENCY_BUCKET_COUNT 8 +__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +__EXPORT const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + /* timer-specific functions */ static void hrt_tim_init(void); diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 83b5c987e..cf3546669 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -206,7 +206,7 @@ static const uint8_t crc_table[] = { 0xfa, 0xfd, 0xf4, 0xf3 }; -uint8_t crc8(uint8_t *p, uint8_t len){ +static uint8_t crc8(uint8_t *p, uint8_t len) { uint16_t i; uint16_t crc = 0x0; |