diff options
Diffstat (limited to 'src/drivers')
28 files changed, 3990 insertions, 171 deletions
diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index f15c74a22..ecd31a073 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -482,10 +482,10 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10; /* Failsafe logic - should never be necessary */ - motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512; - motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512; - motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512; - motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512; + motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511; + motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511; + motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511; + motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511; /* send motors via UART */ ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu/module.mk index 2cb261d30..66b776917 100644 --- a/src/drivers/boards/px4fmu/module.mk +++ b/src/drivers/boards/px4fmu/module.mk @@ -6,4 +6,5 @@ SRCS = px4fmu_can.c \ px4fmu_init.c \ px4fmu_pwm_servo.c \ px4fmu_spi.c \ - px4fmu_usb.c + px4fmu_usb.c \ + px4fmu_led.c diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c index 5dd70c3f9..69edc23ab 100644 --- a/src/drivers/boards/px4fmu/px4fmu_init.c +++ b/src/drivers/boards/px4fmu/px4fmu_init.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -91,6 +91,19 @@ # endif #endif +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + /**************************************************************************** * Protected Functions ****************************************************************************/ @@ -114,7 +127,7 @@ __EXPORT void stm32_boardinitialize(void) /* configure SPI interfaces */ stm32_spiinitialize(); - /* configure LEDs */ + /* configure LEDs (empty call to NuttX' ledinit) */ up_ledinit(); } @@ -178,11 +191,11 @@ __EXPORT int nsh_archinitialize(void) (hrt_callout)stm32_serial_dma_poll, NULL); - // initial LED state -// drv_led_start(); - up_ledoff(LED_BLUE); - up_ledoff(LED_AMBER); - up_ledon(LED_BLUE); + /* initial LED state */ + drv_led_start(); + led_off(LED_AMBER); + led_on(LED_BLUE); + /* Configure SPI-based devices */ diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu/px4fmu_led.c index fd1e159aa..34fd194c3 100644 --- a/src/drivers/boards/px4fmu/px4fmu_led.c +++ b/src/drivers/boards/px4fmu/px4fmu_led.c @@ -39,19 +39,27 @@ #include <nuttx/config.h> -#include <stdint.h> #include <stdbool.h> -#include <debug.h> -#include <arch/board/board.h> - -#include "chip.h" -#include "up_arch.h" -#include "up_internal.h" #include "stm32_internal.h" #include "px4fmu_internal.h" -__EXPORT void up_ledinit() +#include <arch/board/board.h> + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +__EXPORT void led_init() { /* Configure LED1-2 GPIOs for output */ @@ -59,7 +67,7 @@ __EXPORT void up_ledinit() stm32_configgpio(GPIO_LED2); } -__EXPORT void up_ledon(int led) +__EXPORT void led_on(int led) { if (led == 0) { @@ -73,7 +81,7 @@ __EXPORT void up_ledon(int led) } } -__EXPORT void up_ledoff(int led) +__EXPORT void led_off(int led) { if (led == 0) { diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 7daba31be..b4a9cdd53 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -53,6 +53,15 @@ namespace device __EXPORT class __EXPORT I2C : public CDev { +public: + + /** + * Get the address + */ + uint16_t get_address() { + return _address; + } + protected: /** * The number of times a read or write operation will be retried on diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h new file mode 100644 index 000000000..bffc35c62 --- /dev/null +++ b/src/drivers/drv_airspeed.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (C) 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 Airspeed driver interface. + * @author Simon Wilks + */ + +#ifndef _DRV_AIRSPEED_H +#define _DRV_AIRSPEED_H + +#include <stdint.h> +#include <sys/ioctl.h> + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +#define AIRSPEED_DEVICE_PATH "/dev/airspeed" + +/* + * ioctl() definitions + * + * Airspeed drivers also implement the generic sensor driver + * interfaces from drv_sensor.h + */ + +#define _AIRSPEEDIOCBASE (0x7700) +#define __AIRSPEEDIOC(_n) (_IOC(_AIRSPEEDIOCBASE, _n)) + + +#endif /* _DRV_AIRSPEED_H */ diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 07831f20c..56af71059 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -109,6 +109,12 @@ ORB_DECLARE(output_pwm); /** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */ #define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4) +/** set the 'ARM ok' bit, which activates the safety switch */ +#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 5) + +/** clear the 'ARM ok' bit, which deactivates the safety switch */ +#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp new file mode 100644 index 000000000..e50395e47 --- /dev/null +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -0,0 +1,832 @@ +/**************************************************************************** + * + * Copyright (C) 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 ets_airspeed.cpp + * @author Simon Wilks + * + * Driver for the Eagle Tree Airspeed V3 connected via I2C. + */ + +#include <nuttx/config.h> + +#include <drivers/device/i2c.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.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 <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <arch/board/board.h> + +#include <systemlib/airspeed.h> +#include <systemlib/err.h> +#include <systemlib/param/param.h> +#include <systemlib/perf_counter.h> + +#include <drivers/drv_airspeed.h> +#include <drivers/drv_hrt.h> + +#include <uORB/uORB.h> +#include <uORB/topics/differential_pressure.h> +#include <uORB/topics/subsystem_info.h> + +/* Default I2C bus */ +#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION + +/* I2C bus address */ +#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ + +/* Register address */ +#define READ_CMD 0x07 /* Read the data */ + +/** + * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. + */ +#define MIN_ACCURATE_DIFF_PRES_PA 12 + +/* Measurement rate is 100Hz */ +#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ + +/* Oddly, ERROR is not defined for C++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class ETSAirspeed : public device::I2C +{ +public: + ETSAirspeed(int bus, int address = I2C_ADDRESS); + virtual ~ETSAirspeed(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + work_s _work; + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + differential_pressure_s *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _diff_pres_offset; + + orb_advert_t _airspeed_pub; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); + +ETSAirspeed::ETSAirspeed(int bus, int address) : + I2C("ETSAirspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _diff_pres_offset(0), + _airspeed_pub(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "ets_airspeed_read")), + _comms_errors(perf_alloc(PC_COUNT, "ets_airspeed_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ets_airspeed_buffer_overflows")) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +ETSAirspeed::~ETSAirspeed() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +ETSAirspeed::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct differential_pressure_s[_num_reports]; + for (unsigned i = 0; i < _num_reports; i++) + _reports[i].max_differential_pressure_pa = 0; + + if (_reports == nullptr) + goto out; + + _oldest_report = _next_report = 0; + + /* get a publish handle on the airspeed topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]); + + if (_airspeed_pub < 0) + debug("failed to create airspeed sensor object. Did you start uOrb?"); + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +ETSAirspeed::probe() +{ + return measure(); +} + +int +ETSAirspeed::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + 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) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* add one to account for the sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct differential_pressure_s *buf = new struct differential_pressure_s[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +ETSAirspeed::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct differential_pressure_s); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + /* XXX really it'd be nice to lock against other readers here */ + do { + _oldest_report = _next_report = 0; + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); + + return ret; +} + +int +ETSAirspeed::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd = READ_CMD; + ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) + { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + ret = OK; + + return ret; +} + +int +ETSAirspeed::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, &val[0], 2); + + if (ret < 0) { + log("error reading from sensor: %d", ret); + return ret; + } + + uint16_t diff_pres_pa = val[1] << 8 | val[0]; + + param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset); + + if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { + diff_pres_pa = 0; + } else { + diff_pres_pa -= _diff_pres_offset; + } + + // XXX we may want to smooth out the readings to remove noise. + + _reports[_next_report].timestamp = hrt_absolute_time(); + _reports[_next_report].differential_pressure_pa = diff_pres_pa; + + // Track maximum differential pressure measured (so we can work out top speed). + if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) { + _reports[_next_report].max_differential_pressure_pa = diff_pres_pa; + } + + /* announce the airspeed if needed, just publish else */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + perf_count(_buffer_overflows); + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + + return ret; +} + +void +ETSAirspeed::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&ETSAirspeed::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_DIFFPRESSURE}; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +ETSAirspeed::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +ETSAirspeed::cycle_trampoline(void *arg) +{ + ETSAirspeed *dev = (ETSAirspeed *)arg; + + dev->cycle(); +} + +void +ETSAirspeed::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&ETSAirspeed::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) + log("measure error"); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&ETSAirspeed::cycle_trampoline, + this, + USEC2TICK(CONVERSION_INTERVAL)); +} + +void +ETSAirspeed::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); +} + +/** + * Local functions in support of the shell command. + */ +namespace ets_airspeed +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +ETSAirspeed *g_dev; + +void start(int i2c_bus); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start(int i2c_bus) +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new ETSAirspeed(i2c_bus); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); + +fail: + + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void +stop() +{ + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + else + { + errx(1, "driver not running"); + } + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct differential_pressure_s report; + ssize_t sz; + int ret; + + int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("single read"); + warnx("diff pressure: %d pa", report.differential_pressure_pa); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + errx(1, "failed to set 2Hz poll rate"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "periodic read failed"); + + warnx("periodic read %u", i); + warnx("diff pressure: %d pa", report.differential_pressure_pa); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + + +static void +ets_airspeed_usage() +{ + fprintf(stderr, "usage: ets_airspeed [options] command\n"); + fprintf(stderr, "options:\n"); + fprintf(stderr, "\t-b --bus i2cbus (%d)\n", PX4_I2C_BUS_DEFAULT); + fprintf(stderr, "command:\n"); + fprintf(stderr, "\tstart|stop|reset|test|info\n"); +} + +int +ets_airspeed_main(int argc, char *argv[]) +{ + int i2c_bus = PX4_I2C_BUS_DEFAULT; + + int i; + for (i = 1; i < argc; i++) { + if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { + if (argc > i + 1) { + i2c_bus = atoi(argv[i + 1]); + } + } + } + + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + ets_airspeed::start(i2c_bus); + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + ets_airspeed::stop(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + ets_airspeed::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + ets_airspeed::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + ets_airspeed::info(); + + ets_airspeed_usage(); + exit(0); +} diff --git a/src/drivers/ets_airspeed/module.mk b/src/drivers/ets_airspeed/module.mk new file mode 100644 index 000000000..cb5d3b1ed --- /dev/null +++ b/src/drivers/ets_airspeed/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ + +# +# Makefile to build the Eagle Tree Airspeed V3 driver. +# + +MODULE_COMMAND = ets_airspeed +MODULE_STACKSIZE = 1024 + +SRCS = ets_airspeed.cpp diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index e35bdb944..38835418b 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -285,6 +285,10 @@ GPS::task_main() unlock(); if (_Helper->configure(_baudrate) == 0) { unlock(); + + // GPS is obviously detected successfully, reset statistics + _Helper->reset_update_rates(); + while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ @@ -301,6 +305,8 @@ GPS::task_main() _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; + _Helper->store_update_rates(); + _Helper->reset_update_rates(); } if (!_healthy) { @@ -372,7 +378,10 @@ GPS::print_info() warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); - warnx("update rate: %6.2f Hz", (double)_rate); + warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); + warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); + warnx("rate publication:\t%6.2f Hz", (double)_rate); + } usleep(100000); diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 9c1fad569..ba86d370a 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Julian Oes <joes@student.ethz.ch> * @@ -36,9 +36,39 @@ #include <termios.h> #include <errno.h> #include <systemlib/err.h> +#include <drivers/drv_hrt.h> #include "gps_helper.h" -/* @file gps_helper.cpp */ +/** + * @file gps_helper.cpp + */ + +float +GPS_Helper::get_position_update_rate() +{ + return _rate_lat_lon; +} + +float +GPS_Helper::get_velocity_update_rate() +{ + return _rate_vel; +} + +float +GPS_Helper::reset_update_rates() +{ + _rate_count_vel = 0; + _rate_count_lat_lon = 0; + _interval_rate_start = hrt_absolute_time(); +} + +float +GPS_Helper::store_update_rates() +{ + _rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f); + _rate_lat_lon = _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f); +} int GPS_Helper::set_baudrate(const int &fd, unsigned baud) diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index f3d3bc40b..defc1a074 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Julian Oes <joes@student.ethz.ch> * @@ -33,7 +33,9 @@ * ****************************************************************************/ -/* @file gps_helper.h */ +/** + * @file gps_helper.h + */ #ifndef GPS_HELPER_H #define GPS_HELPER_H @@ -44,9 +46,22 @@ class GPS_Helper { public: - virtual int configure(unsigned &baud) = 0; + virtual int configure(unsigned &baud) = 0; virtual int receive(unsigned timeout) = 0; - int set_baudrate(const int &fd, unsigned baud); + int set_baudrate(const int &fd, unsigned baud); + float get_position_update_rate(); + float get_velocity_update_rate(); + float reset_update_rates(); + float store_update_rates(); + +protected: + uint8_t _rate_count_lat_lon; + uint8_t _rate_count_vel; + + float _rate_lat_lon; + float _rate_vel; + + uint64_t _interval_rate_start; }; #endif /* GPS_HELPER_H */ diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 4762bd503..62941d74b 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -263,6 +263,10 @@ MTK::handle_message(gps_mtk_packet_t &packet) _gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); + // Position and velocity update always at the same time + _rate_count_vel++; + _rate_count_lat_lon++; + return; } diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h index d4e390b01..b5cfbf0a6 100644 --- a/src/drivers/gps/mtk.h +++ b/src/drivers/gps/mtk.h @@ -87,14 +87,14 @@ class MTK : public GPS_Helper public: MTK(const int &fd, struct vehicle_gps_position_s *gps_position); ~MTK(); - int receive(unsigned timeout); - int configure(unsigned &baudrate); + int receive(unsigned timeout); + int configure(unsigned &baudrate); private: /** * Parse the binary MTK packet */ - int parse_char(uint8_t b, gps_mtk_packet_t &packet); + int parse_char(uint8_t b, gps_mtk_packet_t &packet); /** * Handle the package once it has arrived diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index c150f5271..b3093b0f6 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -60,7 +60,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _fd(fd), _gps_position(gps_position), -_waiting_for_ack(false) +_waiting_for_ack(false), +_disable_cmd_counter(0) { decode_init(); } @@ -139,12 +140,12 @@ UBX::configure(unsigned &baudrate) cfg_rate_packet.clsID = UBX_CLASS_CFG; cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; - cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE; + cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL; cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { /* try next baudrate */ continue; } @@ -164,74 +165,41 @@ UBX::configure(unsigned &baudrate) cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - type_gps_bin_cfg_msg_packet_t cfg_msg_packet; - memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); - - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_MSG; - - cfg_msg_packet.clsID = UBX_CLASS_CFG; - cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; - cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; - /* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */ - cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ; - - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; - - send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; - - send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; - /* For satelites info 1Hz is enough */ - cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ; - - send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; - - send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { /* try next baudrate */ continue; } - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; - - send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } -// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; -// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; - -// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; -// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, + UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); + /* insist of receiving the ACK for this packet */ + // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) + // continue; + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, + 1); + // /* insist of receiving the ACK for this packet */ + // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) + // continue; + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, + 1); + // /* insist of receiving the ACK for this packet */ + // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) + // continue; + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, + 1); + // /* insist of receiving the ACK for this packet */ + // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) + // continue; + // configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP, + // 0); + // /* insist of receiving the ACK for this packet */ + // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) + // continue; + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, + 0); + // /* insist of receiving the ACK for this packet */ + // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) + // continue; _waiting_for_ack = false; return 0; @@ -240,6 +208,15 @@ UBX::configure(unsigned &baudrate) } int +UBX::wait_for_ack(unsigned timeout) +{ + _waiting_for_ack = true; + int ret = receive(timeout); + _waiting_for_ack = false; + return ret; +} + +int UBX::receive(unsigned timeout) { /* poll descriptor */ @@ -498,6 +475,8 @@ UBX::handle_message() _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + _rate_count_lat_lon++; + /* Add timestamp to finish the report */ _gps_position->timestamp_position = hrt_absolute_time(); /* only return 1 when new position is available */ @@ -653,6 +632,8 @@ UBX::handle_message() _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; _gps_position->vel_ned_valid = true; _gps_position->timestamp_velocity = hrt_absolute_time(); + + _rate_count_vel++; } break; @@ -693,6 +674,12 @@ UBX::handle_message() default: //we don't know the message warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); + if (_disable_cmd_counter++ == 0) { + // Don't attempt for every message to disable, some might not be disabled */ + warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id); + configure_message_rate(_message_class, _message_id, 0); + } + return ret; ret = -1; break; } @@ -737,6 +724,25 @@ UBX::add_checksum_to_message(uint8_t* message, const unsigned length) } void +UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b) +{ + for (unsigned i = 0; i < length; i++) { + ck_a = ck_a + message[i]; + ck_b = ck_b + ck_a; + } +} + +void +UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) +{ + struct ubx_cfg_msg_rate msg; + msg.msg_class = msg_class; + msg.msg_id = msg_id; + msg.rate = rate; + send_message(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); +} + +void UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) { ssize_t ret = 0; @@ -753,3 +759,27 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning? warnx("ubx: config write fail"); } + +void +UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size) +{ + struct ubx_header header; + uint8_t ck_a=0, ck_b=0; + header.sync1 = UBX_SYNC1; + header.sync2 = UBX_SYNC2; + header.msg_class = msg_class; + header.msg_id = msg_id; + header.length = size; + + add_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b); + add_checksum((uint8_t *)msg, size, ck_a, ck_b); + + // Configure receive check + _clsID_needed = msg_class; + _msgID_needed = msg_id; + + write(_fd, (const char *)&header, sizeof(header)); + write(_fd, (const char *)msg, size); + write(_fd, (const char *)&ck_a, 1); + write(_fd, (const char *)&ck_b, 1); +} diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index e3dd1c7ea..5a433642c 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -65,26 +65,27 @@ #define UBX_MESSAGE_CFG_RATE 0x08 #define UBX_CFG_PRT_LENGTH 20 -#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ -#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ -#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ -#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ -#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ +#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ +#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ +#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ +#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ +#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ #define UBX_CFG_RATE_LENGTH 6 -#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */ +#define UBX_CFG_RATE_PAYLOAD_MEASINTERVAL 200 /**< 200ms for 5Hz */ #define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */ #define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */ #define UBX_CFG_NAV5_LENGTH 36 -#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */ +#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005 /**< XXX only update dynamic model and fix mode */ #define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */ -#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ +#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ #define UBX_CFG_MSG_LENGTH 8 #define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ #define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ +#define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10 #define UBX_MAX_PAYLOAD_LENGTH 500 @@ -92,6 +93,14 @@ /** the structures of the binary packets */ #pragma pack(push, 1) +struct ubx_header { + uint8_t sync1; + uint8_t sync2; + uint8_t msg_class; + uint8_t msg_id; + uint16_t length; +}; + typedef struct { uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ int32_t lon; /**< Longitude * 1e-7, deg */ @@ -274,11 +283,17 @@ typedef struct { uint16_t length; uint8_t msgClass_payload; uint8_t msgID_payload; - uint8_t rate[6]; + uint8_t rate; uint8_t ck_a; uint8_t ck_b; } type_gps_bin_cfg_msg_packet_t; +struct ubx_cfg_msg_rate { + uint8_t msg_class; + uint8_t msg_id; + uint8_t rate; +}; + // END the structures of the binary packets // ************ @@ -341,55 +356,64 @@ class UBX : public GPS_Helper public: UBX(const int &fd, struct vehicle_gps_position_s *gps_position); ~UBX(); - int receive(unsigned timeout); - int configure(unsigned &baudrate); + int receive(unsigned timeout); + int configure(unsigned &baudrate); private: /** * Parse the binary MTK packet */ - int parse_char(uint8_t b); + int parse_char(uint8_t b); /** * Handle the package once it has arrived */ - int handle_message(void); + int handle_message(void); /** * Reset the parse state machine for a fresh start */ - void decode_init(void); + void decode_init(void); /** * While parsing add every byte (except the sync bytes) to the checksum */ - void add_byte_to_checksum(uint8_t); + void add_byte_to_checksum(uint8_t); /** * Add the two checksum bytes to an outgoing message */ - void add_checksum_to_message(uint8_t* message, const unsigned length); + void add_checksum_to_message(uint8_t* message, const unsigned length); /** * Helper to send a config packet */ - void send_config_packet(const int &fd, uint8_t *packet, const unsigned length); + void send_config_packet(const int &fd, uint8_t *packet, const unsigned length); + + void configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate); + + void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size); + + void add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b); + + int wait_for_ack(unsigned timeout); - int _fd; + int _fd; struct vehicle_gps_position_s *_gps_position; ubx_config_state_t _config_state; - bool _waiting_for_ack; - uint8_t _clsID_needed; - uint8_t _msgID_needed; + bool _waiting_for_ack; + uint8_t _clsID_needed; + uint8_t _msgID_needed; ubx_decode_state_t _decode_state; - uint8_t _rx_buffer[RECV_BUFFER_SIZE]; - unsigned _rx_count; - uint8_t _rx_ck_a; - uint8_t _rx_ck_b; - ubx_message_class_t _message_class; + uint8_t _rx_buffer[RECV_BUFFER_SIZE]; + unsigned _rx_count; + uint8_t _rx_ck_a; + uint8_t _rx_ck_b; + ubx_message_class_t _message_class; ubx_message_id_t _message_id; - unsigned _payload_size; + unsigned _payload_size; + uint8_t _disable_cmd_counter; }; #endif /* UBX_H_ */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index c406a7588..78eda327c 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1225,19 +1225,25 @@ start() /* create the driver, attempt expansion bus first */ g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION); + if (g_dev != nullptr && OK != g_dev->init()) { + delete g_dev; + g_dev = nullptr; + } + #ifdef PX4_I2C_BUS_ONBOARD /* if this failed, attempt onboard sensor */ - if (g_dev == nullptr) + if (g_dev == nullptr) { g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD); + if (g_dev != nullptr && OK != g_dev->init()) { + goto fail; + } + } #endif if (g_dev == nullptr) goto fail; - if (OK != g_dev->init()) - goto fail; - /* set the poll rate to default, starts automatic data collection */ fd = open(MAG_DEVICE_PATH, O_RDONLY); diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c index 5fbee16ce..f0f32d244 100644 --- a/src/drivers/hott_telemetry/messages.c +++ b/src/drivers/hott_telemetry/messages.c @@ -42,9 +42,11 @@ #include <string.h> #include <systemlib/systemlib.h> #include <unistd.h> +#include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> #include <uORB/topics/sensor_combined.h> +static int airspeed_sub = -1; static int battery_sub = -1; static int sensor_sub = -1; @@ -52,6 +54,7 @@ void messages_init(void) { battery_sub = orb_subscribe(ORB_ID(battery_status)); sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + airspeed_sub = orb_subscribe(ORB_ID(airspeed)); } void build_eam_response(uint8_t *buffer, int *size) @@ -81,6 +84,15 @@ void build_eam_response(uint8_t *buffer, int *size) msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; + /* get a local copy of the current sensor values */ + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); + orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); + + uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6); + msg.speed_L = (uint8_t)speed & 0xff; + msg.speed_H = (uint8_t)(speed >> 8) & 0xff; + msg.stop = STOP_BYTE; memcpy(buffer, &msg, *size); diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index c7c45525e..04b565358 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -41,12 +41,17 @@ #include <drivers/device/device.h> #include <drivers/drv_led.h> -/* Ideally we'd be able to get these from up_internal.h */ -//#include <up_internal.h> +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ __BEGIN_DECLS -extern void up_ledinit(); -extern void up_ledon(int led); -extern void up_ledoff(int led); +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); __END_DECLS class LED : device::CDev @@ -74,7 +79,7 @@ int LED::init() { CDev::init(); - up_ledinit(); + led_init(); return 0; } @@ -86,11 +91,11 @@ LED::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case LED_ON: - up_ledon(arg); + led_on(arg); break; case LED_OFF: - up_ledoff(arg); + led_off(arg); break; default: diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp new file mode 100644 index 000000000..71932ad65 --- /dev/null +++ b/src/drivers/md25/md25.cpp @@ -0,0 +1,553 @@ +/**************************************************************************** + * + * Copyright (c) 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 md25.cpp + * + * Driver for MD25 I2C Motor Driver + * + * references: + * http://www.robot-electronics.co.uk/htm/md25tech.htm + * http://www.robot-electronics.co.uk/files/rpi_md25.c + * + */ + +#include "md25.hpp" +#include <poll.h> +#include <stdio.h> + +#include <systemlib/err.h> +#include <arch/board/board.h> + +// registers +enum { + // RW: read/write + // R: read + REG_SPEED1_RW = 0, + REG_SPEED2_RW, + REG_ENC1A_R, + REG_ENC1B_R, + REG_ENC1C_R, + REG_ENC1D_R, + REG_ENC2A_R, + REG_ENC2B_R, + REG_ENC2C_R, + REG_ENC2D_R, + REG_BATTERY_VOLTS_R, + REG_MOTOR1_CURRENT_R, + REG_MOTOR2_CURRENT_R, + REG_SW_VERSION_R, + REG_ACCEL_RATE_RW, + REG_MODE_RW, + REG_COMMAND_RW, +}; + +MD25::MD25(const char *deviceName, int bus, + uint16_t address, uint32_t speed) : + I2C("MD25", deviceName, bus, address, speed), + _controlPoll(), + _actuators(NULL, ORB_ID(actuator_controls_0), 20), + _version(0), + _motor1Speed(0), + _motor2Speed(0), + _revolutions1(0), + _revolutions2(0), + _batteryVoltage(0), + _motor1Current(0), + _motor2Current(0), + _motorAccel(0), + _mode(MODE_UNSIGNED_SPEED), + _command(CMD_RESET_ENCODERS) +{ + // setup control polling + _controlPoll.fd = _actuators.getHandle(); + _controlPoll.events = POLLIN; + + // if initialization fails raise an error, unless + // probing + int ret = I2C::init(); + + if (ret != OK) { + warnc(ret, "I2C::init failed for bus: %d address: %d\n", bus, address); + } + + // setup default settings, reset encoders + setMotor1Speed(0); + setMotor2Speed(0); + resetEncoders(); + _setMode(MD25::MODE_UNSIGNED_SPEED); + setSpeedRegulation(true); + setTimeout(true); +} + +MD25::~MD25() +{ +} + +int MD25::readData() +{ + uint8_t sendBuf[1]; + sendBuf[0] = REG_SPEED1_RW; + uint8_t recvBuf[17]; + int ret = transfer(sendBuf, sizeof(sendBuf), + recvBuf, sizeof(recvBuf)); + + if (ret == OK) { + _version = recvBuf[REG_SW_VERSION_R]; + _motor1Speed = _uint8ToNorm(recvBuf[REG_SPEED1_RW]); + _motor2Speed = _uint8ToNorm(recvBuf[REG_SPEED2_RW]); + _revolutions1 = -int32_t((recvBuf[REG_ENC1A_R] << 24) + + (recvBuf[REG_ENC1B_R] << 16) + + (recvBuf[REG_ENC1C_R] << 8) + + recvBuf[REG_ENC1D_R]) / 360.0; + _revolutions2 = -int32_t((recvBuf[REG_ENC2A_R] << 24) + + (recvBuf[REG_ENC2B_R] << 16) + + (recvBuf[REG_ENC2C_R] << 8) + + recvBuf[REG_ENC2D_R]) / 360.0; + _batteryVoltage = recvBuf[REG_BATTERY_VOLTS_R] / 10.0; + _motor1Current = recvBuf[REG_MOTOR1_CURRENT_R] / 10.0; + _motor2Current = recvBuf[REG_MOTOR2_CURRENT_R] / 10.0; + _motorAccel = recvBuf[REG_ACCEL_RATE_RW]; + _mode = e_mode(recvBuf[REG_MODE_RW]); + _command = e_cmd(recvBuf[REG_COMMAND_RW]); + } + + return ret; +} + +void MD25::status(char *string, size_t n) +{ + snprintf(string, n, + "version:\t%10d\n" \ + "motor 1 speed:\t%10.2f\n" \ + "motor 2 speed:\t%10.2f\n" \ + "revolutions 1:\t%10.2f\n" \ + "revolutions 2:\t%10.2f\n" \ + "battery volts :\t%10.2f\n" \ + "motor 1 current :\t%10.2f\n" \ + "motor 2 current :\t%10.2f\n" \ + "motor accel :\t%10d\n" \ + "mode :\t%10d\n" \ + "command :\t%10d\n", + getVersion(), + double(getMotor1Speed()), + double(getMotor2Speed()), + double(getRevolutions1()), + double(getRevolutions2()), + double(getBatteryVolts()), + double(getMotor1Current()), + double(getMotor2Current()), + getMotorAccel(), + getMode(), + getCommand()); +} + +uint8_t MD25::getVersion() +{ + return _version; +} + +float MD25::getMotor1Speed() +{ + return _motor1Speed; +} + +float MD25::getMotor2Speed() +{ + return _motor2Speed; +} + +float MD25::getRevolutions1() +{ + return _revolutions1; +} + +float MD25::getRevolutions2() +{ + return _revolutions2; +} + +float MD25::getBatteryVolts() +{ + return _batteryVoltage; +} + +float MD25::getMotor1Current() +{ + return _motor1Current; +} +float MD25::getMotor2Current() +{ + return _motor2Current; +} + +uint8_t MD25::getMotorAccel() +{ + return _motorAccel; +} + +MD25::e_mode MD25::getMode() +{ + return _mode; +} + +MD25::e_cmd MD25::getCommand() +{ + return _command; +} + +int MD25::resetEncoders() +{ + return _writeUint8(REG_COMMAND_RW, + CMD_RESET_ENCODERS); +} + +int MD25::_setMode(e_mode mode) +{ + return _writeUint8(REG_MODE_RW, + mode); +} + +int MD25::setSpeedRegulation(bool enable) +{ + if (enable) { + return _writeUint8(REG_COMMAND_RW, + CMD_ENABLE_SPEED_REGULATION); + + } else { + return _writeUint8(REG_COMMAND_RW, + CMD_DISABLE_SPEED_REGULATION); + } +} + +int MD25::setTimeout(bool enable) +{ + if (enable) { + return _writeUint8(REG_COMMAND_RW, + CMD_ENABLE_TIMEOUT); + + } else { + return _writeUint8(REG_COMMAND_RW, + CMD_DISABLE_TIMEOUT); + } +} + +int MD25::setDeviceAddress(uint8_t address) +{ + uint8_t sendBuf[1]; + sendBuf[0] = CMD_CHANGE_I2C_SEQ_0; + int ret = OK; + ret = transfer(sendBuf, sizeof(sendBuf), + nullptr, 0); + + if (ret != OK) { + warnc(ret, "MD25::setDeviceAddress"); + return ret; + } + + usleep(5000); + sendBuf[0] = CMD_CHANGE_I2C_SEQ_1; + ret = transfer(sendBuf, sizeof(sendBuf), + nullptr, 0); + + if (ret != OK) { + warnc(ret, "MD25::setDeviceAddress"); + return ret; + } + + usleep(5000); + sendBuf[0] = CMD_CHANGE_I2C_SEQ_2; + ret = transfer(sendBuf, sizeof(sendBuf), + nullptr, 0); + + if (ret != OK) { + warnc(ret, "MD25::setDeviceAddress"); + return ret; + } + + return OK; +} + +int MD25::setMotor1Speed(float value) +{ + return _writeUint8(REG_SPEED1_RW, + _normToUint8(value)); +} + +int MD25::setMotor2Speed(float value) +{ + return _writeUint8(REG_SPEED2_RW, + _normToUint8(value)); +} + +void MD25::update() +{ + // wait for an actuator publication, + // check for exit condition every second + // note "::poll" is required to distinguish global + // poll from member function for driver + if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error + + // if new data, send to motors + if (_actuators.updated()) { + _actuators.update(); + setMotor1Speed(_actuators.control[CH_SPEED_LEFT]); + setMotor2Speed(_actuators.control[CH_SPEED_RIGHT]); + } +} + +int MD25::probe() +{ + uint8_t goodAddress = 0; + bool found = false; + int ret = OK; + + // try initial address first, if good, then done + if (readData() == OK) return ret; + + // try all other addresses + uint8_t testAddress = 0; + + //printf("searching for MD25 address\n"); + while (true) { + set_address(testAddress); + ret = readData(); + + if (ret == OK && !found) { + //printf("device found at address: 0x%X\n", testAddress); + if (!found) { + found = true; + goodAddress = testAddress; + } + } + + if (testAddress > 254) { + break; + } + + testAddress++; + } + + if (found) { + set_address(goodAddress); + return OK; + + } else { + set_address(0); + return ret; + } +} + +int MD25::search() +{ + uint8_t goodAddress = 0; + bool found = false; + int ret = OK; + // try all other addresses + uint8_t testAddress = 0; + + //printf("searching for MD25 address\n"); + while (true) { + set_address(testAddress); + ret = readData(); + + if (ret == OK && !found) { + printf("device found at address: 0x%X\n", testAddress); + + if (!found) { + found = true; + goodAddress = testAddress; + } + } + + if (testAddress > 254) { + break; + } + + testAddress++; + } + + if (found) { + set_address(goodAddress); + return OK; + + } else { + set_address(0); + return ret; + } +} + +int MD25::_writeUint8(uint8_t reg, uint8_t value) +{ + uint8_t sendBuf[2]; + sendBuf[0] = reg; + sendBuf[1] = value; + return transfer(sendBuf, sizeof(sendBuf), + nullptr, 0); +} + +int MD25::_writeInt8(uint8_t reg, int8_t value) +{ + uint8_t sendBuf[2]; + sendBuf[0] = reg; + sendBuf[1] = value; + return transfer(sendBuf, sizeof(sendBuf), + nullptr, 0); +} + +float MD25::_uint8ToNorm(uint8_t value) +{ + // TODO, should go from 0 to 255 + // possibly should handle this differently + return (value - 128) / 127.0; +} + +uint8_t MD25::_normToUint8(float value) +{ + if (value > 1) value = 1; + + if (value < -1) value = -1; + + // TODO, should go from 0 to 255 + // possibly should handle this differently + return 127 * value + 128; +} + +int md25Test(const char *deviceName, uint8_t bus, uint8_t address) +{ + printf("md25 test: starting\n"); + + // setup + MD25 md25("/dev/md25", bus, address); + + // print status + char buf[200]; + md25.status(buf, sizeof(buf)); + printf("%s\n", buf); + + // setup for test + md25.setSpeedRegulation(true); + md25.setTimeout(true); + float dt = 0.1; + float speed = 0.2; + float t = 0; + + // motor 1 test + printf("md25 test: spinning motor 1 forward for 1 rev at 0.1 speed\n"); + t = 0; + + while (true) { + t += dt; + md25.setMotor1Speed(speed); + md25.readData(); + usleep(1000000 * dt); + + if (md25.getRevolutions1() > 1) { + printf("finished 1 revolution fwd\n"); + break; + } + + if (t > 2.0f) break; + } + + md25.setMotor1Speed(0); + printf("revolution of wheel 1: %8.4f\n", double(md25.getRevolutions1())); + md25.resetEncoders(); + + t = 0; + + while (true) { + t += dt; + md25.setMotor1Speed(-speed); + md25.readData(); + usleep(1000000 * dt); + + if (md25.getRevolutions1() < -1) { + printf("finished 1 revolution rev\n"); + break; + } + + if (t > 2.0f) break; + } + + md25.setMotor1Speed(0); + printf("revolution of wheel 1: %8.4f\n", double(md25.getRevolutions1())); + md25.resetEncoders(); + + // motor 2 test + printf("md25 test: spinning motor 2 forward for 1 rev at 0.1 speed\n"); + t = 0; + + while (true) { + t += dt; + md25.setMotor2Speed(speed); + md25.readData(); + usleep(1000000 * dt); + + if (md25.getRevolutions2() > 1) { + printf("finished 1 revolution fwd\n"); + break; + } + + if (t > 2.0f) break; + } + + md25.setMotor2Speed(0); + printf("revolution of wheel 2: %8.4f\n", double(md25.getRevolutions2())); + md25.resetEncoders(); + + t = 0; + + while (true) { + t += dt; + md25.setMotor2Speed(-speed); + md25.readData(); + usleep(1000000 * dt); + + if (md25.getRevolutions2() < -1) { + printf("finished 1 revolution rev\n"); + break; + } + + if (t > 2.0f) break; + } + + md25.setMotor2Speed(0); + printf("revolution of wheel 2: %8.4f\n", double(md25.getRevolutions2())); + md25.resetEncoders(); + + printf("Test complete\n"); + return 0; +} + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp new file mode 100644 index 000000000..e77511b16 --- /dev/null +++ b/src/drivers/md25/md25.hpp @@ -0,0 +1,293 @@ +/**************************************************************************** + * + * Copyright (C) 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 md25.cpp + * + * Driver for MD25 I2C Motor Driver + * + * references: + * http://www.robot-electronics.co.uk/htm/md25tech.htm + * http://www.robot-electronics.co.uk/files/rpi_md25.c + * + */ + +#pragma once + +#include <poll.h> +#include <stdio.h> +#include <controllib/block/UOrbSubscription.hpp> +#include <uORB/topics/actuator_controls.h> +#include <drivers/device/i2c.h> + +/** + * This is a driver for the MD25 motor controller utilizing the I2C interface. + */ +class MD25 : public device::I2C +{ +public: + + /** + * modes + * + * NOTE: this driver assumes we are always + * in mode 0! + * + * seprate speed mode: + * motor speed1 = speed1 + * motor speed2 = speed2 + * turn speed mode: + * motor speed1 = speed1 + speed2 + * motor speed2 = speed2 - speed2 + * unsigned: + * full rev (0), stop(128), full fwd (255) + * signed: + * full rev (-127), stop(0), full fwd (128) + * + * modes numbers: + * 0 : unsigned separate (default) + * 1 : signed separate + * 2 : unsigned turn + * 3 : signed turn + */ + enum e_mode { + MODE_UNSIGNED_SPEED = 0, + MODE_SIGNED_SPEED, + MODE_UNSIGNED_SPEED_TURN, + MODE_SIGNED_SPEED_TURN, + }; + + /** commands */ + enum e_cmd { + CMD_RESET_ENCODERS = 32, + CMD_DISABLE_SPEED_REGULATION = 48, + CMD_ENABLE_SPEED_REGULATION = 49, + CMD_DISABLE_TIMEOUT = 50, + CMD_ENABLE_TIMEOUT = 51, + CMD_CHANGE_I2C_SEQ_0 = 160, + CMD_CHANGE_I2C_SEQ_1 = 170, + CMD_CHANGE_I2C_SEQ_2 = 165, + }; + + /** control channels */ + enum e_channels { + CH_SPEED_LEFT = 0, + CH_SPEED_RIGHT + }; + + /** + * constructor + * @param deviceName the name of the device e.g. "/dev/md25" + * @param bus the I2C bus + * @param address the adddress on the I2C bus + * @param speed the speed of the I2C communication + */ + MD25(const char *deviceName, + int bus, + uint16_t address, + uint32_t speed = 100000); + + /** + * deconstructor + */ + virtual ~MD25(); + + /** + * @return software version + */ + uint8_t getVersion(); + + /** + * @return speed of motor, normalized (-1, 1) + */ + float getMotor1Speed(); + + /** + * @return speed of motor 2, normalized (-1, 1) + */ + float getMotor2Speed(); + + /** + * @return number of rotations since reset + */ + float getRevolutions1(); + + /** + * @return number of rotations since reset + */ + float getRevolutions2(); + + /** + * @return battery voltage, volts + */ + float getBatteryVolts(); + + /** + * @return motor 1 current, amps + */ + float getMotor1Current(); + + /** + * @return motor 2 current, amps + */ + float getMotor2Current(); + + /** + * @return the motor acceleration + * controls motor speed change (1-10) + * accel rate | time for full fwd. to full rev. + * 1 | 6.375 s + * 2 | 1.6 s + * 3 | 0.675 s + * 5(default) | 1.275 s + * 10 | 0.65 s + */ + uint8_t getMotorAccel(); + + /** + * @return motor output mode + * */ + e_mode getMode(); + + /** + * @return current command register value + */ + e_cmd getCommand(); + + /** + * resets the encoders + * @return non-zero -> error + * */ + int resetEncoders(); + + /** + * enable/disable speed regulation + * @return non-zero -> error + */ + int setSpeedRegulation(bool enable); + + /** + * set the timeout for the motors + * enable/disable timeout (motor stop) + * after 2 sec of no i2c messages + * @return non-zero -> error + */ + int setTimeout(bool enable); + + /** + * sets the device address + * can only be done with one MD25 + * on the bus + * @return non-zero -> error + */ + int setDeviceAddress(uint8_t address); + + /** + * set motor 1 speed + * @param normSpeed normalize speed between -1 and 1 + * @return non-zero -> error + */ + int setMotor1Speed(float normSpeed); + + /** + * set motor 2 speed + * @param normSpeed normalize speed between -1 and 1 + * @return non-zero -> error + */ + int setMotor2Speed(float normSpeed); + + /** + * main update loop that updates MD25 motor + * speeds based on actuator publication + */ + void update(); + + /** + * probe for device + */ + virtual int probe(); + + /** + * search for device + */ + int search(); + + /** + * read data from i2c + */ + int readData(); + + /** + * print status + */ + void status(char *string, size_t n); + +private: + /** poll structure for control packets */ + struct pollfd _controlPoll; + + /** actuator controls subscription */ + control::UOrbSubscription<actuator_controls_s> _actuators; + + // local copy of data from i2c device + uint8_t _version; + float _motor1Speed; + float _motor2Speed; + float _revolutions1; + float _revolutions2; + float _batteryVoltage; + float _motor1Current; + float _motor2Current; + uint8_t _motorAccel; + e_mode _mode; + e_cmd _command; + + // private methods + int _writeUint8(uint8_t reg, uint8_t value); + int _writeInt8(uint8_t reg, int8_t value); + float _uint8ToNorm(uint8_t value); + uint8_t _normToUint8(float value); + + /** + * set motor control mode, + * this driver assumed we are always in mode 0 + * so we don't let the user change the mode + * @return non-zero -> error + */ + int _setMode(e_mode); +}; + +// unit testing +int md25Test(const char *deviceName, uint8_t bus, uint8_t address); + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp new file mode 100644 index 000000000..80850e708 --- /dev/null +++ b/src/drivers/md25/md25_main.cpp @@ -0,0 +1,264 @@ +/**************************************************************************** + * + * Copyright (c) 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 md25.cpp + * + * Driver for MD25 I2C Motor Driver + * + * references: + * http://www.robot-electronics.co.uk/htm/md25tech.htm + * http://www.robot-electronics.co.uk/files/rpi_md25.c + * + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <math.h> + +#include <systemlib/systemlib.h> +#include <systemlib/param/param.h> + +#include <arch/board/board.h> +#include "md25.hpp" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int md25_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int md25_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: md25 {start|stop|status|search|test|change_address}\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int md25_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("md25 already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("md25", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + md25_thread_main, + (const char **)argv); + exit(0); + } + + if (!strcmp(argv[1], "test")) { + + if (argc < 4) { + printf("usage: md25 test bus address\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[2], nullptr, 0); + + uint8_t address = strtoul(argv[3], nullptr, 0); + + md25Test(deviceName, bus, address); + + exit(0); + } + + if (!strcmp(argv[1], "probe")) { + if (argc < 4) { + printf("usage: md25 probe bus address\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[2], nullptr, 0); + + uint8_t address = strtoul(argv[3], nullptr, 0); + + MD25 md25(deviceName, bus, address); + + int ret = md25.probe(); + + if (ret == OK) { + printf("MD25 found on bus %d at address 0x%X\n", bus, md25.get_address()); + + } else { + printf("MD25 not found on bus %d\n", bus); + } + + exit(0); + } + + if (!strcmp(argv[1], "search")) { + if (argc < 3) { + printf("usage: md25 search bus\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[2], nullptr, 0); + + uint8_t address = strtoul(argv[3], nullptr, 0); + + MD25 md25(deviceName, bus, address); + + md25.search(); + + exit(0); + } + + if (!strcmp(argv[1], "change_address")) { + if (argc < 5) { + printf("usage: md25 change_address bus old_address new_address\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[2], nullptr, 0); + + uint8_t old_address = strtoul(argv[3], nullptr, 0); + + uint8_t new_address = strtoul(argv[4], nullptr, 0); + + MD25 md25(deviceName, bus, old_address); + + int ret = md25.setDeviceAddress(new_address); + + if (ret == OK) { + printf("MD25 new address set to 0x%X\n", new_address); + + } else { + printf("MD25 failed to set address to 0x%X\n", new_address); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tmd25 app is running\n"); + + } else { + printf("\tmd25 app not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int md25_thread_main(int argc, char *argv[]) +{ + printf("[MD25] starting\n"); + + if (argc < 5) { + // extra md25 in arg list since this is a thread + printf("usage: md25 start bus address\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[3], nullptr, 0); + + uint8_t address = strtoul(argv[4], nullptr, 0); + + // start + MD25 md25("/dev/md25", bus, address); + + thread_running = true; + + // loop + while (!thread_should_exit) { + md25.update(); + } + + // exit + printf("[MD25] exiting.\n"); + thread_running = false; + return 0; +} + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/md25/module.mk b/src/drivers/md25/module.mk new file mode 100644 index 000000000..13821a6b5 --- /dev/null +++ b/src/drivers/md25/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ + +# +# MD25 I2C Based Motor Controller +# http://www.robot-electronics.co.uk/htm/md25tech.htm +# + +MODULE_COMMAND = md25 + +SRCS = md25.cpp \ + md25_main.cpp diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp new file mode 100644 index 000000000..3a735e26f --- /dev/null +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -0,0 +1,1442 @@ +/**************************************************************************** + * + * 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 mkblctrl.cpp + * + * Driver/configurator for the Mikrokopter BL-Ctrl. + * Marco Bauer + * + */ + +#include <nuttx/config.h> + +#include <drivers/device/i2c.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.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 <nuttx/arch.h> +#include <nuttx/i2c.h> + +#include <drivers/device/device.h> +#include <drivers/drv_pwm_output.h> +#include <drivers/drv_gpio.h> +#include <drivers/boards/px4fmu/px4fmu_internal.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_rc_input.h> + +#include <systemlib/systemlib.h> +#include <systemlib/err.h> +#include <systemlib/mixer/mixer.h> +#include <drivers/drv_mixer.h> + +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_effective.h> +#include <uORB/topics/actuator_outputs.h> + +#include <systemlib/err.h> +#include <systemlib/ppm_decode.h> + +#define I2C_BUS_SPEED 400000 +#define UPDATE_RATE 400 +#define MAX_MOTORS 8 +#define BLCTRL_BASE_ADDR 0x29 +#define BLCTRL_OLD 0 +#define BLCTRL_NEW 1 +#define BLCTRL_MIN_VALUE -0.920F +#define MOTOR_STATE_PRESENT_MASK 0x80 +#define MOTOR_STATE_ERROR_MASK 0x7F +#define MOTOR_SPINUP_COUNTER 2500 + + +class MK : public device::I2C +{ +public: + enum Mode { + MODE_2PWM, + MODE_4PWM, + MODE_NONE + }; + + enum MappingMode { + MAPPING_MK = 0, + MAPPING_PX4, + }; + + enum FrameType { + FRAME_PLUS = 0, + FRAME_X, + }; + + MK(int bus); + ~MK(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual int init(unsigned motors); + + int set_mode(Mode mode); + int set_pwm_rate(unsigned rate); + int set_motor_count(unsigned count); + int set_motor_test(bool motortest); + int set_px4mode(int px4mode); + int set_frametype(int frametype); + unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput); + +private: + static const unsigned _max_actuators = MAX_MOTORS; + static const bool showDebug = false; + + Mode _mode; + int _update_rate; + int _current_update_rate; + int _task; + int _t_actuators; + int _t_armed; + unsigned int _motor; + int _px4mode; + int _frametype; + orb_advert_t _t_outputs; + orb_advert_t _t_actuators_effective; + unsigned int _num_outputs; + bool _primary_pwm_device; + bool _motortest; + + volatile bool _task_should_exit; + bool _armed; + + unsigned long debugCounter; + + MixerGroup *_mixers; + + actuator_controls_s _controls; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main() __attribute__((noreturn)); + + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); + + int pwm_ioctl(file *filp, int cmd, unsigned long arg); + + struct GPIOConfig { + uint32_t input; + uint32_t output; + uint32_t alt; + }; + + static const GPIOConfig _gpio_tab[]; + static const unsigned _ngpio; + + void gpio_reset(void); + void gpio_set_function(uint32_t gpios, int function); + void gpio_write(uint32_t gpios, int function); + uint32_t gpio_read(void); + int gpio_ioctl(file *filp, int cmd, unsigned long arg); + int mk_servo_arm(bool status); + + int mk_servo_set(unsigned int chan, float val); + int mk_servo_set_test(unsigned int chan, float val); + int mk_servo_test(unsigned int chan); + + +}; + +const MK::GPIOConfig MK::_gpio_tab[] = { + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, + {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, + {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, +}; + +const unsigned MK::_ngpio = sizeof(MK::_gpio_tab) / sizeof(MK::_gpio_tab[0]); + +const int blctrlAddr_quad_plus[] = { 2, 2, -2, -2, 0, 0, 0, 0 }; // Addresstranslator for Quad + configuration +const int blctrlAddr_hexa_plus[] = { 0, 2, 2, -2, 1, -3, 0, 0 }; // Addresstranslator for Hexa + configuration +const int blctrlAddr_octo_plus[] = { 0, 3, -1, 0, 3, 0, 0, -5 }; // Addresstranslator for Octo + configuration + +const int blctrlAddr_quad_x[] = { 2, 2, -2, -2, 0, 0, 0, 0 }; // Addresstranslator for Quad X configuration +const int blctrlAddr_hexa_x[] = { 2, 4, -2, 0, -3, -1, 0, 0 }; // Addresstranslator for Hexa X configuration +const int blctrlAddr_octo_x[] = { 1, 4, 0, 1, -4, 1, 1, -4 }; // Addresstranslator for Octo X configuration + +const int blctrlAddr_px4[] = { 0, 0, 0, 0, 0, 0, 0, 0}; + +int addrTranslator[] = {0,0,0,0,0,0,0,0}; + +struct MotorData_t +{ + unsigned int Version; // the version of the BL (0 = old) + unsigned int SetPoint; // written by attitude controller + unsigned int SetPointLowerBits; // for higher Resolution of new BLs + unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present + unsigned int ReadMode; // select data to read + // the following bytes must be exactly in that order! + unsigned int Current; // in 0.1 A steps, read back from BL + unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit + unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in + unsigned int RoundCount; +}; + +MotorData_t Motor[MAX_MOTORS]; + + +namespace +{ + +MK *g_mk; + +} // namespace + +MK::MK(int bus) : + I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED), + _mode(MODE_NONE), + _update_rate(50), + _task(-1), + _t_actuators(-1), + _t_armed(-1), + _t_outputs(0), + _t_actuators_effective(0), + _num_outputs(0), + _motortest(false), + _motor(-1), + _px4mode(MAPPING_MK), + _frametype(FRAME_PLUS), + _primary_pwm_device(false), + _task_should_exit(false), + _armed(false), + _mixers(nullptr) +{ + _debug_enabled = true; +} + +MK::~MK() +{ + if (_task != -1) { + /* tell the task we want it to go away */ + _task_should_exit = true; + + unsigned i = 10; + + do { + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); + + /* if we have given up, kill it */ + if (--i == 0) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + /* clean up the alternate device node */ + if (_primary_pwm_device) + unregister_driver(PWM_OUTPUT_DEVICE_PATH); + + g_mk = nullptr; +} + +int +MK::init(unsigned motors) +{ + _num_outputs = motors; + debugCounter = 0; + int ret; + ASSERT(_task == -1); + + ret = I2C::init(); + + if (ret != OK) { + warnx("I2C init failed"); + return ret; + } + + usleep(500000); + + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default PWM output device"); + _primary_pwm_device = true; + } + + /* reset GPIOs */ + gpio_reset(); + + /* start the IO interface task */ + _task = task_spawn("mkblctrl", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX -20, + 2048, + (main_t)&MK::task_main_trampoline, + nullptr); + + + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + + return OK; +} + +void +MK::task_main_trampoline(int argc, char *argv[]) +{ + g_mk->task_main(); +} + +int +MK::set_mode(Mode mode) +{ + /* + * Configure for PWM output. + * + * Note that regardless of the configured mode, the task is always + * listening and mixing; the mode just selects which of the channels + * are presented on the output pins. + */ + switch (mode) { + case MODE_2PWM: + if(_num_outputs == 4) { + //debug("MODE_QUAD"); + } else if(_num_outputs == 6) { + //debug("MODE_HEXA"); + } else if(_num_outputs == 8) { + //debug("MODE_OCTO"); + } + //up_pwm_servo_init(0x3); + up_pwm_servo_deinit(); + _update_rate = UPDATE_RATE; /* default output rate */ + break; + + case MODE_4PWM: + if(_num_outputs == 4) { + //debug("MODE_QUADRO"); + } else if(_num_outputs == 6) { + //debug("MODE_HEXA"); + } else if(_num_outputs == 8) { + //debug("MODE_OCTO"); + } + //up_pwm_servo_init(0xf); + up_pwm_servo_deinit(); + _update_rate = UPDATE_RATE; /* default output rate */ + break; + + case MODE_NONE: + debug("MODE_NONE"); + /* disable servo outputs and set a very low update rate */ + up_pwm_servo_deinit(); + _update_rate = UPDATE_RATE; + break; + + default: + return -EINVAL; + } + + _mode = mode; + return OK; +} + +int +MK::set_pwm_rate(unsigned rate) +{ + if ((rate > 500) || (rate < 10)) + return -EINVAL; + + _update_rate = rate; + return OK; +} + +int +MK::set_px4mode(int px4mode) +{ + _px4mode = px4mode; +} + +int +MK::set_frametype(int frametype) +{ + _frametype = frametype; +} + + +int +MK::set_motor_count(unsigned count) +{ + if(count > 0) { + + _num_outputs = count; + + if(_px4mode == MAPPING_MK) { + if(_frametype == FRAME_PLUS) { + fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: +\n"); + } else if(_frametype == FRAME_X) { + fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: X\n"); + } + if(_num_outputs == 4) { + if(_frametype == FRAME_PLUS) { + memcpy(&addrTranslator, &blctrlAddr_quad_plus, sizeof(blctrlAddr_quad_plus)); + } else if(_frametype == FRAME_X) { + memcpy(&addrTranslator, &blctrlAddr_quad_x, sizeof(blctrlAddr_quad_x)); + } + } else if(_num_outputs == 6) { + if(_frametype == FRAME_PLUS) { + memcpy(&addrTranslator, &blctrlAddr_hexa_plus, sizeof(blctrlAddr_hexa_plus)); + } else if(_frametype == FRAME_X) { + memcpy(&addrTranslator, &blctrlAddr_hexa_x, sizeof(blctrlAddr_hexa_x)); + } + } else if(_num_outputs == 8) { + if(_frametype == FRAME_PLUS) { + memcpy(&addrTranslator, &blctrlAddr_octo_plus, sizeof(blctrlAddr_octo_plus)); + } else if(_frametype == FRAME_X) { + memcpy(&addrTranslator, &blctrlAddr_octo_x, sizeof(blctrlAddr_octo_x)); + } + } + } else { + fprintf(stderr, "[mkblctrl] PX4 native addressing used.\n"); + memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4)); + } + + if(_num_outputs == 4) { + fprintf(stderr, "[mkblctrl] Quadrocopter Mode (4)\n"); + } else if(_num_outputs == 6) { + fprintf(stderr, "[mkblctrl] Hexacopter Mode (6)\n"); + } else if(_num_outputs == 8) { + fprintf(stderr, "[mkblctrl] Octocopter Mode (8)\n"); + } + + return OK; + + } else { + return -1; + } + +} + +int +MK::set_motor_test(bool motortest) +{ + _motortest = motortest; + return OK; +} + + +void +MK::task_main() +{ + /* + * Subscribe to the appropriate PWM output topic based on whether we are the + * primary PWM output or not. + */ + _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1)); + /* force a reset of the update rate */ + _current_update_rate = 0; + + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + + /* advertise the mixed control outputs */ + actuator_outputs_s outputs; + memset(&outputs, 0, sizeof(outputs)); + /* advertise the mixed control outputs */ + _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), + &outputs); + + /* advertise the effective control inputs */ + actuator_controls_effective_s controls_effective; + memset(&controls_effective, 0, sizeof(controls_effective)); + /* advertise the effective control inputs */ + _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), + &controls_effective); + + pollfd fds[2]; + fds[0].fd = _t_actuators; + fds[0].events = POLLIN; + fds[1].fd = _t_armed; + fds[1].events = POLLIN; + + // rc input, published to ORB + struct rc_input_values rc_in; + orb_advert_t to_input_rc = 0; + + memset(&rc_in, 0, sizeof(rc_in)); + rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; + + log("starting"); + long update_rate_in_us = 0; + + /* loop until killed */ + while (!_task_should_exit) { + + /* handle update rate changes */ + if (_current_update_rate != _update_rate) { + int update_rate_in_ms = int(1000 / _update_rate); + update_rate_in_us = long(1000000 / _update_rate); + + /* reject faster than 500 Hz updates */ + if (update_rate_in_ms < 2) { + update_rate_in_ms = 2; + _update_rate = 500; + } + /* reject slower than 50 Hz updates */ + if (update_rate_in_ms > 20) { + update_rate_in_ms = 20; + _update_rate = 50; + } + + orb_set_interval(_t_actuators, update_rate_in_ms); + up_pwm_servo_set_rate(_update_rate); + _current_update_rate = _update_rate; + } + + /* sleep waiting for data, but no more than a second */ + int ret = ::poll(&fds[0], 2, 1000); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + usleep(1000000); + continue; + } + + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { + + /* get controls - must always do this to avoid spinning */ + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls); + + /* can we mix? */ + if (_mixers != nullptr) { + + /* do mixing */ + outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs); + outputs.timestamp = hrt_absolute_time(); + + // XXX output actual limited values + memcpy(&controls_effective, &_controls, sizeof(controls_effective)); + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); + + /* iterate actuators */ + for (unsigned int i = 0; i < _num_outputs; i++) { + + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + //outputs.output[i] = 1500 + (600 * outputs.output[i]); + //outputs.output[i] = 127 + (127 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + if(outputs.output[i] < -1.0f) { + outputs.output[i] = -1.0f; + } else if(outputs.output[i] > 1.0f) { + outputs.output[i] = 1.0f; + } else { + outputs.output[i] = -1.0f; + } + } + + /* don't go under BLCTRL_MIN_VALUE */ + if(outputs.output[i] < BLCTRL_MIN_VALUE) { + outputs.output[i] = BLCTRL_MIN_VALUE; + } + //_motortest = true; + /* output to BLCtrl's */ + if(_motortest == true) { + mk_servo_test(i); + } else { + //mk_servo_set(i, outputs.output[i]); + mk_servo_set_test(i, outputs.output[i]); // 8Bit + } + + + } + + /* and publish for anyone that cares to see */ + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + } + } + + /* how about an arming update? */ + if (fds[1].revents & POLLIN) { + actuator_armed_s aa; + + /* get new value */ + orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + + /* update PWM servo armed status if armed and not locked down */ + ////up_pwm_servo_arm(aa.armed && !aa.lockdown); + mk_servo_arm(aa.armed && !aa.lockdown); + } + + // see if we have new PPM input data + if (ppm_last_valid_decode != rc_in.timestamp) { + // we have a new PPM frame. Publish it. + rc_in.channel_count = ppm_decoded_channels; + if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { + rc_in.channel_count = RC_INPUT_MAX_CHANNELS; + } + for (uint8_t i=0; i<rc_in.channel_count; i++) { + rc_in.values[i] = ppm_buffer[i]; + } + rc_in.timestamp = ppm_last_valid_decode; + + /* lazily advertise on first publication */ + if (to_input_rc == 0) { + to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in); + } else { + orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in); + } + } + + } + + ::close(_t_actuators); + ::close(_t_actuators_effective); + ::close(_t_armed); + + + /* make sure servos are off */ + up_pwm_servo_deinit(); + + log("stopping"); + + /* note - someone else is responsible for restoring the GPIO config */ + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + + +int +MK::mk_servo_arm(bool status) +{ + _armed = status; + return 0; +} + + +unsigned int +MK::mk_check_for_blctrl(unsigned int count, bool showOutput) +{ + _retries = 50; + uint8_t foundMotorCount = 0; + + for(unsigned i=0; i<MAX_MOTORS; i++) { + Motor[i].Version = 0; + Motor[i].SetPoint = 0; + Motor[i].SetPointLowerBits = 0; + Motor[i].State = 0; + Motor[i].ReadMode = 0; + Motor[i].Current = 0; + Motor[i].MaxPWM = 0; + Motor[i].Temperature = 0; + Motor[i].RoundCount = 0; + } + + uint8_t msg = 0; + uint8_t result[3]; + + for(unsigned i=0; i< count; i++) { + result[0] = 0; + result[1] = 0; + result[2] = 0; + + set_address( BLCTRL_BASE_ADDR + i ); + + if (OK == transfer(&msg, 1, &result[0], 3)) { + Motor[i].Current = result[0]; + Motor[i].MaxPWM = result[1]; + Motor[i].Temperature = result[2]; + Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit; + foundMotorCount++; + if(Motor[i].MaxPWM == 250) { + Motor[i].Version = BLCTRL_NEW; + } else { + Motor[i].Version = BLCTRL_OLD; + } + } + } + + if(showOutput) { + fprintf(stderr, "[mkblctrl] MotorsFound: %i\n",foundMotorCount); + for(unsigned i=0; i< foundMotorCount; i++) { + fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i,Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); + } + + if(foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) { + _task_should_exit = true; + } + } + + return foundMotorCount; +} + + + + +int +MK::mk_servo_set(unsigned int chan, float val) +{ + float tmpVal = 0; + _retries = 0; + uint8_t result[3] = { 0,0,0 }; + uint8_t msg[2] = { 0,0 }; + uint8_t rod=0; + uint8_t bytesToSendBL2 = 2; + + + tmpVal = (1023 + (1023 * val)); + if(tmpVal > 2047) { + tmpVal = 2047; + } + + + Motor[chan].SetPoint = (uint8_t) tmpVal / 3; // divide 8 + Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 8; // rest of divide 8 + //rod = (uint8_t) tmpVal % 8; + //Motor[chan].SetPointLowerBits = rod<<1; // rest of divide 8 + Motor[chan].SetPointLowerBits = 0; + + if(_armed == false) { + Motor[chan].SetPoint = 0; + Motor[chan].SetPointLowerBits = 0; + } + + //if(Motor[chan].State & MOTOR_STATE_PRESENT_MASK) { + set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); + + if(Motor[chan].Version == BLCTRL_OLD) { + /* + * Old BL-Ctrl 8Bit served. Version < 2.0 + */ + msg[0] = Motor[chan].SetPoint; + if(Motor[chan].RoundCount >= 16) { + // on each 16th cyle we read out the status messages from the blctrl + if (OK == transfer(&msg[0], 1, &result[0], 2)) { + Motor[chan].Current = result[0]; + Motor[chan].MaxPWM = result[1]; + Motor[chan].Temperature = 255;; + } else { + if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + } + Motor[chan].RoundCount = 0; + } else { + if (OK != transfer(&msg[0], 1, nullptr, 0)) { + if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + } + } + + } else { + /* + * New BL-Ctrl 11Bit served. Version >= 2.0 + */ + msg[0] = Motor[chan].SetPoint; + msg[1] = Motor[chan].SetPointLowerBits; + + if(Motor[chan].SetPointLowerBits == 0) { + bytesToSendBL2 = 1; // if setpoint lower bits are zero, we send only the higher bits - this saves time + } + + if(Motor[chan].RoundCount >= 16) { + // on each 16th cyle we read out the status messages from the blctrl + if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) { + Motor[chan].Current = result[0]; + Motor[chan].MaxPWM = result[1]; + Motor[chan].Temperature = result[2]; + } else { + if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + } + Motor[chan].RoundCount = 0; + } else { + if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) { + if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + } + } + + } + + Motor[chan].RoundCount++; + //} + + if(showDebug == true) { + debugCounter++; + if(debugCounter == 2000) { + debugCounter = 0; + for(int i=0; i<_num_outputs; i++){ + if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) { + fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State); + } + } + fprintf(stderr, "\n"); + } + } + return 0; +} + +int +MK::mk_servo_set_test(unsigned int chan, float val) +{ + _retries = 0; + int ret; + + float tmpVal = 0; + + uint8_t msg[2] = { 0,0 }; + + tmpVal = (1023 + (1023 * val)); + if(tmpVal > 2048) { + tmpVal = 2048; + } + + Motor[chan].SetPoint = (uint8_t) (tmpVal / 8); + + if(_armed == false) { + Motor[chan].SetPoint = 0; + Motor[chan].SetPointLowerBits = 0; + } + + msg[0] = Motor[chan].SetPoint; + + set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); + ret = transfer(&msg[0], 1, nullptr, 0); + + ret = OK; + + return ret; +} + + +int +MK::mk_servo_test(unsigned int chan) +{ + int ret=0; + float tmpVal = 0; + float val = -1; + _retries = 0; + uint8_t msg[2] = { 0,0 }; + + if(debugCounter >= MOTOR_SPINUP_COUNTER) { + debugCounter = 0; + _motor++; + + if(_motor < _num_outputs) { + fprintf(stderr, "[mkblctrl] Motortest - #%i:\tspinup\n", _motor); + } + + if(_motor >= _num_outputs) { + _motor = -1; + _motortest = false; + } + + } + debugCounter++; + + if(_motor == chan) { + val = BLCTRL_MIN_VALUE; + } else { + val = -1; + } + + tmpVal = (1023 + (1023 * val)); + if(tmpVal > 2048) { + tmpVal = 2048; + } + + //Motor[chan].SetPoint = (uint8_t) (tmpVal / 8); + //Motor[chan].SetPointLowerBits = (uint8_t) (tmpVal % 8) & 0x07; + Motor[chan].SetPoint = (uint8_t) tmpVal>>3; + Motor[chan].SetPointLowerBits = (uint8_t) tmpVal & 0x07; + + if(_motor != chan) { + Motor[chan].SetPoint = 0; + Motor[chan].SetPointLowerBits = 0; + } + + if(Motor[chan].Version == BLCTRL_OLD) { + msg[0] = Motor[chan].SetPoint; + } else { + msg[0] = Motor[chan].SetPoint; + msg[1] = Motor[chan].SetPointLowerBits; + } + + set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); + if(Motor[chan].Version == BLCTRL_OLD) { + ret = transfer(&msg[0], 1, nullptr, 0); + } else { + ret = transfer(&msg[0], 2, nullptr, 0); + } + + return ret; +} + + +int +MK::control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = controls->control[control_index]; + return 0; +} + +int +MK::ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret; + + // XXX disabled, confusing users + //debug("ioctl 0x%04x 0x%08x", cmd, arg); + + /* try it as a GPIO ioctl first */ + ret = gpio_ioctl(filp, cmd, arg); + + if (ret != -ENOTTY) + return ret; + + /* if we are in valid PWM mode, try it as a PWM ioctl as well */ + switch (_mode) { + case MODE_2PWM: + case MODE_4PWM: + ret = pwm_ioctl(filp, cmd, arg); + break; + + default: + debug("not in a PWM mode"); + break; + } + + /* if nobody wants it, let CDev have it */ + if (ret == -ENOTTY) + ret = CDev::ioctl(filp, cmd, arg); + + return ret; +} + +int +MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + int channel; + + lock(); + + switch (cmd) { + case PWM_SERVO_ARM: + ////up_pwm_servo_arm(true); + mk_servo_arm(true); + break; + + case PWM_SERVO_DISARM: + ////up_pwm_servo_arm(false); + mk_servo_arm(false); + break; + + case PWM_SERVO_SET_UPDATE_RATE: + set_pwm_rate(arg); + break; + + + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): + + /* fake an update to the selected 'servo' channel */ + if ((arg >= 0) && (arg <= 255)) { + channel = cmd - PWM_SERVO_SET(0); + //mk_servo_set(channel, arg); + } else { + ret = -EINVAL; + } + + break; + + case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): + /* copy the current output value from the channel */ + *(servo_position_t *)arg = cmd - PWM_SERVO_GET(0); + break; + + case MIXERIOCGETOUTPUTCOUNT: + /* + if (_mode == MODE_4PWM) { + *(unsigned *)arg = 4; + } else { + *(unsigned *)arg = 2; + } + */ + + *(unsigned *)arg = _num_outputs; + + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + } + + break; + + case MIXERIOCADDSIMPLE: { + mixer_simple_s *mixinfo = (mixer_simple_s *)arg; + + SimpleMixer *mixer = new SimpleMixer(control_callback, + (uintptr_t)&_controls, mixinfo); + + if (mixer->check()) { + delete mixer; + ret = -EINVAL; + + } else { + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, + (uintptr_t)&_controls); + + _mixers->add_mixer(mixer); + } + + break; + } + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); + + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + + if (_mixers == nullptr) { + ret = -ENOMEM; + + } else { + + ret = _mixers->load_from_buf(buf, buflen); + + if (ret != 0) { + debug("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + ret = -EINVAL; + } + } + break; + } + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; +} + +void +MK::gpio_reset(void) +{ + /* + * Setup default GPIO config - all pins as GPIOs, GPIO driver chip + * to input mode. + */ + for (unsigned i = 0; i < _ngpio; i++) + stm32_configgpio(_gpio_tab[i].input); + + stm32_gpiowrite(GPIO_GPIO_DIR, 0); + stm32_configgpio(GPIO_GPIO_DIR); +} + +void +MK::gpio_set_function(uint32_t gpios, int function) +{ + /* + * GPIOs 0 and 1 must have the same direction as they are buffered + * by a shared 2-port driver. Any attempt to set either sets both. + */ + if (gpios & 3) { + gpios |= 3; + + /* flip the buffer to output mode if required */ + if (GPIO_SET_OUTPUT == function) + stm32_gpiowrite(GPIO_GPIO_DIR, 1); + } + + /* configure selected GPIOs as required */ + for (unsigned i = 0; i < _ngpio; i++) { + if (gpios & (1 << i)) { + switch (function) { + case GPIO_SET_INPUT: + stm32_configgpio(_gpio_tab[i].input); + break; + + case GPIO_SET_OUTPUT: + stm32_configgpio(_gpio_tab[i].output); + break; + + case GPIO_SET_ALT_1: + if (_gpio_tab[i].alt != 0) + stm32_configgpio(_gpio_tab[i].alt); + + break; + } + } + } + + /* flip buffer to input mode if required */ + if ((GPIO_SET_INPUT == function) && (gpios & 3)) + stm32_gpiowrite(GPIO_GPIO_DIR, 0); +} + +void +MK::gpio_write(uint32_t gpios, int function) +{ + int value = (function == GPIO_SET) ? 1 : 0; + + for (unsigned i = 0; i < _ngpio; i++) + if (gpios & (1 << i)) + stm32_gpiowrite(_gpio_tab[i].output, value); +} + +uint32_t +MK::gpio_read(void) +{ + uint32_t bits = 0; + + for (unsigned i = 0; i < _ngpio; i++) + if (stm32_gpioread(_gpio_tab[i].input)) + bits |= (1 << i); + + return bits; +} + +int +MK::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + lock(); + + switch (cmd) { + + case GPIO_RESET: + gpio_reset(); + break; + + case GPIO_SET_OUTPUT: + case GPIO_SET_INPUT: + case GPIO_SET_ALT_1: + gpio_set_function(arg, cmd); + break; + + case GPIO_SET_ALT_2: + case GPIO_SET_ALT_3: + case GPIO_SET_ALT_4: + ret = -EINVAL; + break; + + case GPIO_SET: + case GPIO_CLEAR: + gpio_write(arg, cmd); + break; + + case GPIO_GET: + *(uint32_t *)arg = gpio_read(); + break; + + default: + ret = -ENOTTY; + } + + unlock(); + + return ret; +} + +namespace +{ + +enum PortMode { + PORT_MODE_UNSET = 0, + PORT_FULL_GPIO, + PORT_FULL_SERIAL, + PORT_FULL_PWM, + PORT_GPIO_AND_SERIAL, + PORT_PWM_AND_SERIAL, + PORT_PWM_AND_GPIO, +}; + +enum MappingMode { + MAPPING_MK = 0, + MAPPING_PX4, +}; + + enum FrameType { + FRAME_PLUS = 0, + FRAME_X, + }; + +PortMode g_port_mode; + +int +mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype) +{ + uint32_t gpio_bits; + int shouldStop = 0; + MK::Mode servo_mode; + + /* reset to all-inputs */ + g_mk->ioctl(0, GPIO_RESET, 0); + + gpio_bits = 0; + servo_mode = MK::MODE_NONE; + + switch (new_mode) { + case PORT_FULL_GPIO: + case PORT_MODE_UNSET: + /* nothing more to do here */ + break; + + case PORT_FULL_SERIAL: + /* set all multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_FULL_PWM: + /* select 4-pin PWM mode */ + servo_mode = MK::MODE_4PWM; + break; + + case PORT_GPIO_AND_SERIAL: + /* set RX/TX multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_PWM_AND_SERIAL: + /* select 2-pin PWM mode */ + servo_mode = MK::MODE_2PWM; + /* set RX/TX multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_PWM_AND_GPIO: + /* select 2-pin PWM mode */ + servo_mode = MK::MODE_2PWM; + break; + } + + /* adjust GPIO config for serial mode(s) */ + if (gpio_bits != 0) + g_mk->ioctl(0, GPIO_SET_ALT_1, gpio_bits); + + /* native PX4 addressing) */ + g_mk->set_px4mode(px4mode); + + /* set frametype (geometry) */ + g_mk->set_frametype(frametype); + + /* motortest if enabled */ + g_mk->set_motor_test(motortest); + + + /* (re)set count of used motors */ + ////g_mk->set_motor_count(motorcount); + /* count used motors */ + + do { + if(g_mk->mk_check_for_blctrl(8, false) != 0) { + shouldStop = 4; + } else { + shouldStop++; + } + sleep(1); + } while ( shouldStop < 3); + + g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true)); + + /* (re)set the PWM output mode */ + g_mk->set_mode(servo_mode); + + + if ((servo_mode != MK::MODE_NONE) && (update_rate != 0)) + g_mk->set_pwm_rate(update_rate); + + return OK; +} + +int +mk_start(unsigned bus, unsigned motors) +{ + int ret = OK; + + if (g_mk == nullptr) { + + g_mk = new MK(bus); + + if (g_mk == nullptr) { + ret = -ENOMEM; + + } else { + ret = g_mk->init(motors); + + if (ret != OK) { + delete g_mk; + g_mk = nullptr; + } + } + } + + return ret; +} + + +} // namespace + +extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]); + +int +mkblctrl_main(int argc, char *argv[]) +{ + PortMode port_mode = PORT_FULL_PWM; + int pwm_update_rate_in_hz = UPDATE_RATE; + int motorcount = 8; + int bus = 1; + int px4mode = MAPPING_PX4; + int frametype = FRAME_PLUS; // + plus is default + bool motortest = false; + bool showHelp = false; + bool newMode = false; + + /* + * optional parameters + */ + for (int i = 1; i < argc; i++) { + + /* look for the optional i2c bus parameter */ + if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { + if (argc > i + 1) { + bus = atoi(argv[i + 1]); + newMode = true; + } else { + errx(1, "missing argument for i2c bus (-b)"); + return 1; + } + } + + /* look for the optional frame parameter */ + if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) { + if (argc > i + 1) { + if(strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) { + px4mode = MAPPING_MK; + newMode = true; + if(strcmp(argv[i + 1], "+") == 0) { + frametype = FRAME_PLUS; + } else { + frametype = FRAME_X; + } + } else { + errx(1, "only + or x for frametype supported !"); + } + } else { + errx(1, "missing argument for mkmode (-mkmode)"); + return 1; + } + } + + /* look for the optional test parameter */ + if (strcmp(argv[i], "-t") == 0) { + motortest = true; + newMode = true; + } + + /* look for the optional -h --help parameter */ + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + showHelp == true; + } + + } + + if(showHelp) { + fprintf(stderr, "mkblctrl: help:\n"); + fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n"); + exit(1); + } + + + if (g_mk == nullptr) { + if (mk_start(bus, motorcount) != OK) { + errx(1, "failed to start the MK-BLCtrl driver"); + } else { + newMode = true; + } + } + + + /* parameter set ? */ + if (newMode) { + /* switch parameter */ + return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype); + } + + /* test, etc. here g*/ + + exit(1); +} diff --git a/src/drivers/mkblctrl/module.mk b/src/drivers/mkblctrl/module.mk new file mode 100644 index 000000000..3ac263b00 --- /dev/null +++ b/src/drivers/mkblctrl/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Interface driver for the Mikrokopter BLCtrl +# + +MODULE_COMMAND = mkblctrl + +SRCS = mkblctrl.cpp + +INCLUDE_DIRS += $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index d3865f053..e199a5998 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -574,7 +574,11 @@ PX4FMU::task_main() orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); /* update PWM servo armed status if armed and not locked down */ - up_pwm_servo_arm(aa.armed && !aa.lockdown); + bool set_armed = aa.armed && !aa.lockdown; + if (set_armed != _armed) { + _armed = set_armed; + up_pwm_servo_arm(set_armed); + } } #ifdef FMU_HAVE_PPM @@ -675,6 +679,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) up_pwm_servo_arm(true); break; + case PWM_SERVO_SET_ARM_OK: + case PWM_SERVO_CLEAR_ARM_OK: + // these are no-ops, as no safety switch + break; + case PWM_SERVO_DISARM: up_pwm_servo_arm(false); break; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4c2f1736f..a40142792 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -109,6 +109,14 @@ public: int set_update_rate(int rate); /** + * Set the battery current scaling and bias + * + * @param amp_per_volt + * @param amp_bias + */ + void set_battery_current_scaling(float amp_per_volt, float amp_bias); + + /** * Print the current status of IO */ void print_status(); @@ -151,6 +159,10 @@ private: bool _primary_pwm_device; ///< true if we are the default PWM output + float _battery_amp_per_volt; + float _battery_amp_bias; + float _battery_mamphour_total; + uint64_t _battery_last_timestamp; /** * Trampoline to the worker task @@ -314,6 +326,10 @@ PX4IO::PX4IO() : _to_actuators_effective(0), _to_outputs(0), _to_battery(0), + _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor + _battery_amp_bias(0), + _battery_mamphour_total(0), + _battery_last_timestamp(0), _primary_pwm_device(false) { /* we need this potentially before it could be set in task_main */ @@ -400,7 +416,7 @@ PX4IO::init() * already armed, and has been configured for in-air restart */ if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && - (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) { + (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); @@ -484,10 +500,9 @@ PX4IO::init() /* dis-arm IO before touching anything */ io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, - PX4IO_P_SETUP_ARMING_ARM_OK | + PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | - PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0); + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0); /* publish RC config to IO */ ret = io_set_rc_config(); @@ -686,16 +701,18 @@ PX4IO::io_set_arming_state() uint16_t set = 0; uint16_t clear = 0; - if (armed.armed) { - set |= PX4IO_P_SETUP_ARMING_ARM_OK; + if (armed.armed && !armed.lockdown) { + set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { - clear |= PX4IO_P_SETUP_ARMING_ARM_OK; + clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } - if (vstatus.flag_vector_flight_mode_ok) { - set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; + + if (armed.ready_to_arm) { + set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } else { - clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; + clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } + if (vstatus.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } else { @@ -884,11 +901,22 @@ PX4IO::io_get_status() /* voltage is scaled to mV */ battery_status.voltage_v = regs[2] / 1000.0f; - /* current scaling should be to cA in order to avoid limiting at 65A */ - battery_status.current_a = regs[3] / 100.f; + /* + regs[3] contains the raw ADC count, as 12 bit ADC + value, with full range being 3.3v + */ + battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt; + battery_status.current_a += _battery_amp_bias; - /* this requires integration over time - not currently implemented */ - battery_status.discharged_mah = -1.0f; + /* + integrate battery over time to get total mAh used + */ + if (_battery_last_timestamp != 0) { + _battery_mamphour_total += battery_status.current_a * + (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600; + } + battery_status.discharged_mah = _battery_mamphour_total; + _battery_last_timestamp = battery_status.timestamp; /* lazily publish the battery voltage */ if (_to_battery > 0) { @@ -1245,9 +1273,14 @@ PX4IO::print_status() ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : "")); - printf("vbatt %u ibatt %u\n", - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT)); + printf("vbatt %u ibatt %u vbatt scale %u\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); + printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n", + (double)_battery_amp_per_volt, + (double)_battery_amp_bias, + (double)_battery_mamphour_total); printf("actuators"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); @@ -1279,19 +1312,15 @@ PX4IO::print_status() uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s\n", arming, - ((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? " ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""), + ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); printf("rates 0x%04x default %u alt %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); - printf("vbatt scale %u ibatt scale %u ibatt bias %u\n", - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS)); printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); printf("controls"); for (unsigned i = 0; i < _max_controls; i++) @@ -1326,21 +1355,27 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) switch (cmd) { case PWM_SERVO_ARM: /* set the 'armed' bit */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_ARM_OK); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED); + break; + + case PWM_SERVO_SET_ARM_OK: + /* set the 'OK to arm' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_IO_ARM_OK); + break; + + case PWM_SERVO_CLEAR_ARM_OK: + /* clear the 'OK to arm' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_IO_ARM_OK, 0); break; case PWM_SERVO_DISARM: /* clear the 'armed' bit */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0); break; case PWM_SERVO_SET_UPDATE_RATE: /* set the requested alternate rate */ - if ((arg >= 50) && (arg <= 400)) { /* TODO: we could go higher for e.g. TurboPWM */ - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); - } else { - ret = -EINVAL; - } + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); break; case PWM_SERVO_SELECT_UPDATE_RATE: { @@ -1525,6 +1560,12 @@ PX4IO::set_update_rate(int rate) return 0; } +void +PX4IO::set_battery_current_scaling(float amp_per_volt, float amp_bias) +{ + _battery_amp_per_volt = amp_per_volt; + _battery_amp_bias = amp_bias; +} extern "C" __EXPORT int px4io_main(int argc, char *argv[]); @@ -1662,6 +1703,18 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "current")) { + if (g_dev != nullptr) { + if ((argc > 3)) { + g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3])); + } else { + errx(1, "missing argument (apm_per_volt, amp_offset)"); + return 1; + } + } + exit(0); + } + if (!strcmp(argv[1], "recovery")) { if (g_dev != nullptr) { @@ -1789,5 +1842,5 @@ px4io_main(int argc, char *argv[]) monitor(); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current' or 'update'"); } diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp index 9a67875e8..15524c3ae 100644 --- a/src/drivers/px4io/uploader.cpp +++ b/src/drivers/px4io/uploader.cpp @@ -127,6 +127,8 @@ PX4IO_Uploader::upload(const char *filenames[]) if (ret != OK) { /* this is immediately fatal */ log("bootloader not responding"); + close(_io_fd); + _io_fd = -1; return -EIO; } @@ -145,18 +147,25 @@ PX4IO_Uploader::upload(const char *filenames[]) if (filename == NULL) { log("no firmware found"); + close(_io_fd); + _io_fd = -1; return -ENOENT; } struct stat st; if (stat(filename, &st) != 0) { log("Failed to stat %s - %d\n", filename, (int)errno); + close(_io_fd); + _io_fd = -1; return -errno; } fw_size = st.st_size; - if (_fw_fd == -1) + if (_fw_fd == -1) { + close(_io_fd); + _io_fd = -1; return -ENOENT; + } /* do the usual program thing - allow for failure */ for (unsigned retries = 0; retries < 1; retries++) { @@ -167,6 +176,8 @@ PX4IO_Uploader::upload(const char *filenames[]) if (ret != OK) { /* this is immediately fatal */ log("bootloader not responding"); + close(_io_fd); + _io_fd = -1; return -EIO; } } @@ -178,6 +189,8 @@ PX4IO_Uploader::upload(const char *filenames[]) log("found bootloader revision: %d", bl_rev); } else { log("found unsupported bootloader revision %d, exiting", bl_rev); + close(_io_fd); + _io_fd = -1; return OK; } } @@ -221,6 +234,8 @@ PX4IO_Uploader::upload(const char *filenames[]) } close(_fw_fd); + close(_io_fd); + _io_fd = -1; return ret; } |