aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/boards/px4fmu-v2/px4fmu2_init.c68
-rw-r--r--src/drivers/boards/px4io-v2/board_config.h2
-rw-r--r--src/drivers/boards/px4io-v2/px4iov2_init.c4
-rw-r--r--src/drivers/device/spi.cpp34
-rw-r--r--src/drivers/device/spi.h11
-rw-r--r--src/drivers/drv_gps.h3
-rw-r--r--src/drivers/drv_pwm_output.h3
-rw-r--r--src/drivers/drv_tone_alarm.h3
-rw-r--r--src/drivers/gps/gps.cpp106
-rw-r--r--src/drivers/gps/gps_helper.cpp7
-rw-r--r--src/drivers/gps/gps_helper.h2
-rw-r--r--src/drivers/gps/mtk.cpp26
-rw-r--r--src/drivers/gps/ubx.cpp57
-rw-r--r--src/drivers/gps/ubx.h6
-rw-r--r--src/drivers/hmc5883/hmc5883.cpp4
-rw-r--r--src/drivers/l3gd20/l3gd20.cpp3
-rw-r--r--src/drivers/lsm303d/lsm303d.cpp6
-rw-r--r--src/drivers/mkblctrl/mkblctrl.cpp7
-rw-r--r--src/drivers/ms5611/ms5611_spi.cpp16
-rw-r--r--src/drivers/px4fmu/fmu.cpp17
-rw-r--r--src/drivers/px4io/px4io.cpp254
-rw-r--r--src/drivers/rgbled/rgbled.cpp462
-rw-r--r--src/drivers/stm32/tone_alarm/tone_alarm.cpp12
23 files changed, 720 insertions, 393 deletions
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
index 135767b26..ae2a645f7 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -58,6 +58,7 @@
#include <nuttx/sdio.h>
#include <nuttx/mmcsd.h>
#include <nuttx/analog/adc.h>
+#include <nuttx/gran.h>
#include <stm32.h>
#include "board_config.h"
@@ -69,6 +70,7 @@
#include <drivers/drv_led.h>
#include <systemlib/cpuload.h>
+#include <systemlib/perf_counter.h>
/****************************************************************************
* Pre-Processor Definitions
@@ -96,10 +98,70 @@
* Protected Functions
****************************************************************************/
+#if defined(CONFIG_FAT_DMAMEMORY)
+# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
+# error microSD DMA support requires CONFIG_GRAN
+# endif
+
+static GRAN_HANDLE dma_allocator;
+
+/*
+ * The DMA heap size constrains the total number of things that can be
+ * ready to do DMA at a time.
+ *
+ * For example, FAT DMA depends on one sector-sized buffer per filesystem plus
+ * one sector-sized buffer per file.
+ *
+ * We use a fundamental alignment / granule size of 64B; this is sufficient
+ * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
+ */
+static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
+static perf_counter_t g_dma_perf;
+
+static void
+dma_alloc_init(void)
+{
+ dma_allocator = gran_initialize(g_dma_heap,
+ sizeof(g_dma_heap),
+ 7, /* 128B granule - must be > alignment (XXX bug?) */
+ 6); /* 64B alignment */
+ if (dma_allocator == NULL) {
+ message("[boot] DMA allocator setup FAILED");
+ } else {
+ g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
+ }
+}
+
/****************************************************************************
* Public Functions
****************************************************************************/
+/*
+ * DMA-aware allocator stubs for the FAT filesystem.
+ */
+
+__EXPORT void *fat_dma_alloc(size_t size);
+__EXPORT void fat_dma_free(FAR void *memory, size_t size);
+
+void *
+fat_dma_alloc(size_t size)
+{
+ perf_count(g_dma_perf);
+ return gran_alloc(dma_allocator, size);
+}
+
+void
+fat_dma_free(FAR void *memory, size_t size)
+{
+ gran_free(dma_allocator, memory, size);
+}
+
+#else
+
+# define dma_alloc_init()
+
+#endif
+
/************************************************************************************
* Name: stm32_boardinitialize
*
@@ -110,7 +172,8 @@
*
************************************************************************************/
-__EXPORT void stm32_boardinitialize(void)
+__EXPORT void
+stm32_boardinitialize(void)
{
/* configure SPI interfaces */
stm32_spiinitialize();
@@ -170,6 +233,9 @@ __EXPORT int nsh_archinitialize(void)
/* configure the high-resolution time/callout interface */
hrt_init();
+ /* configure the DMA allocator */
+ dma_alloc_init();
+
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h
index 818b64436..4d41d0d07 100644
--- a/src/drivers/boards/px4io-v2/board_config.h
+++ b/src/drivers/boards/px4io-v2/board_config.h
@@ -84,7 +84,7 @@
/* Power switch controls ******************************************************/
-#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)
diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c
index 0ea95bded..ccd01edf5 100644
--- a/src/drivers/boards/px4io-v2/px4iov2_init.c
+++ b/src/drivers/boards/px4io-v2/px4iov2_init.c
@@ -111,9 +111,7 @@ __EXPORT void stm32_boardinitialize(void)
stm32_configgpio(GPIO_BTN_SAFETY);
- /* spektrum power enable is active high - disable it by default */
- /* XXX might not want to do this on warm restart? */
- stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false);
+ /* spektrum power enable is active high - enable it by default */
stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
stm32_configgpio(GPIO_SERVO_FAULT_DETECT);
diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp
index 8fffd60cb..fa6b78d64 100644
--- a/src/drivers/device/spi.cpp
+++ b/src/drivers/device/spi.cpp
@@ -67,6 +67,7 @@ SPI::SPI(const char *name,
CDev(name, devname, irq),
// public
// protected
+ locking_mode(LOCK_PREEMPTION),
// private
_bus(bus),
_device(device),
@@ -132,13 +133,25 @@ SPI::probe()
int
SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
{
+ irqstate_t state;
if ((send == nullptr) && (recv == nullptr))
return -EINVAL;
- /* do common setup */
- if (!up_interrupt_context())
- SPI_LOCK(_dev, true);
+ /* lock the bus as required */
+ if (!up_interrupt_context()) {
+ switch (locking_mode) {
+ default:
+ case LOCK_PREEMPTION:
+ state = irqsave();
+ break;
+ case LOCK_THREADS:
+ SPI_LOCK(_dev, true);
+ break;
+ case LOCK_NONE:
+ break;
+ }
+ }
SPI_SETFREQUENCY(_dev, _frequency);
SPI_SETMODE(_dev, _mode);
@@ -151,8 +164,19 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
/* and clean up */
SPI_SELECT(_dev, _device, false);
- if (!up_interrupt_context())
- SPI_LOCK(_dev, false);
+ if (!up_interrupt_context()) {
+ switch (locking_mode) {
+ default:
+ case LOCK_PREEMPTION:
+ irqrestore(state);
+ break;
+ case LOCK_THREADS:
+ SPI_LOCK(_dev, false);
+ break;
+ case LOCK_NONE:
+ break;
+ }
+ }
return OK;
}
diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h
index e0122372a..9103dca2e 100644
--- a/src/drivers/device/spi.h
+++ b/src/drivers/device/spi.h
@@ -101,6 +101,17 @@ protected:
*/
int transfer(uint8_t *send, uint8_t *recv, unsigned len);
+ /**
+ * Locking modes supported by the driver.
+ */
+ enum LockMode {
+ LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */
+ LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */
+ LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */
+ };
+
+ LockMode locking_mode; /**< selected locking mode */
+
private:
int _bus;
enum spi_dev_e _device;
diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h
index 1dda8ef0b..398cf4870 100644
--- a/src/drivers/drv_gps.h
+++ b/src/drivers/drv_gps.h
@@ -51,8 +51,7 @@
typedef enum {
GPS_DRIVER_MODE_NONE = 0,
GPS_DRIVER_MODE_UBX,
- GPS_DRIVER_MODE_MTK,
- GPS_DRIVER_MODE_NMEA,
+ GPS_DRIVER_MODE_MTK
} gps_driver_mode_t;
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index ec9d4ca09..94e923d71 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -118,6 +118,9 @@ ORB_DECLARE(output_pwm);
/** start DSM bind */
#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7)
+#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
+#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
+
/** Power up DSM receiver */
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8)
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index f0b860620..2fab37dd2 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -60,6 +60,8 @@
#include <sys/ioctl.h>
+#define TONEALARM_DEVICE_PATH "/dev/tone_alarm"
+
#define _TONE_ALARM_BASE 0x7400
#define TONE_SET_ALARM _IOC(_TONE_ALARM_BASE, 1)
@@ -142,6 +144,7 @@ enum {
TONE_ARMING_WARNING_TUNE,
TONE_BATTERY_WARNING_SLOW_TUNE,
TONE_BATTERY_WARNING_FAST_TUNE,
+ TONE_GPS_WARNING_TUNE,
TONE_NUMBER_OF_TUNES
};
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 38835418b..fc500a9ec 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -85,7 +85,7 @@ static const int ERROR = -1;
class GPS : public device::CDev
{
public:
- GPS(const char* uart_path);
+ GPS(const char *uart_path);
virtual ~GPS();
virtual int init();
@@ -156,7 +156,7 @@ GPS *g_dev;
}
-GPS::GPS(const char* uart_path) :
+GPS::GPS(const char *uart_path) :
CDev("gps", GPS_DEVICE_PATH),
_task_should_exit(false),
_healthy(false),
@@ -192,6 +192,7 @@ GPS::~GPS()
/* well, kill it anyway, though this will probably crash */
if (_task != -1)
task_delete(_task);
+
g_dev = nullptr;
}
@@ -270,19 +271,20 @@ GPS::task_main()
}
switch (_mode) {
- case GPS_DRIVER_MODE_UBX:
- _Helper = new UBX(_serial_fd, &_report);
- break;
- case GPS_DRIVER_MODE_MTK:
- _Helper = new MTK(_serial_fd, &_report);
- break;
- case GPS_DRIVER_MODE_NMEA:
- //_Helper = new NMEA(); //TODO: add NMEA
- break;
- default:
- break;
+ case GPS_DRIVER_MODE_UBX:
+ _Helper = new UBX(_serial_fd, &_report);
+ break;
+
+ case GPS_DRIVER_MODE_MTK:
+ _Helper = new MTK(_serial_fd, &_report);
+ break;
+
+ default:
+ break;
}
+
unlock();
+
if (_Helper->configure(_baudrate) == 0) {
unlock();
@@ -294,6 +296,7 @@ GPS::task_main()
/* opportunistic publishing - else invalid data would end up on the bus */
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}
@@ -310,10 +313,26 @@ GPS::task_main()
}
if (!_healthy) {
- warnx("module found");
+ char *mode_str = "unknown";
+
+ switch (_mode) {
+ case GPS_DRIVER_MODE_UBX:
+ mode_str = "UBX";
+ break;
+
+ case GPS_DRIVER_MODE_MTK:
+ mode_str = "MTK";
+ break;
+
+ default:
+ break;
+ }
+
+ warnx("module found: %s", mode_str);
_healthy = true;
}
}
+
if (_healthy) {
warnx("module lost");
_healthy = false;
@@ -322,25 +341,26 @@ GPS::task_main()
lock();
}
+
lock();
/* select next mode */
switch (_mode) {
- case GPS_DRIVER_MODE_UBX:
- _mode = GPS_DRIVER_MODE_MTK;
- break;
- case GPS_DRIVER_MODE_MTK:
- _mode = GPS_DRIVER_MODE_UBX;
- break;
- // case GPS_DRIVER_MODE_NMEA:
- // _mode = GPS_DRIVER_MODE_UBX;
- // break;
- default:
- break;
+ case GPS_DRIVER_MODE_UBX:
+ _mode = GPS_DRIVER_MODE_MTK;
+ break;
+
+ case GPS_DRIVER_MODE_MTK:
+ _mode = GPS_DRIVER_MODE_UBX;
+ break;
+
+ default:
+ break;
}
}
- debug("exiting");
+
+ warnx("exiting");
::close(_serial_fd);
@@ -361,23 +381,25 @@ void
GPS::print_info()
{
switch (_mode) {
- case GPS_DRIVER_MODE_UBX:
- warnx("protocol: UBX");
- break;
- case GPS_DRIVER_MODE_MTK:
- warnx("protocol: MTK");
- break;
- case GPS_DRIVER_MODE_NMEA:
- warnx("protocol: NMEA");
- break;
- default:
- break;
+ case GPS_DRIVER_MODE_UBX:
+ warnx("protocol: UBX");
+ break;
+
+ case GPS_DRIVER_MODE_MTK:
+ warnx("protocol: MTK");
+ break;
+
+ default:
+ break;
}
+
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
+
if (_report.timestamp_position != 0) {
- 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("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type,
+ _report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
+ warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m);
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);
@@ -428,6 +450,7 @@ start(const char *uart_path)
errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
goto fail;
}
+
exit(0);
fail:
@@ -503,7 +526,7 @@ gps_main(int argc, char *argv[])
{
/* set to default */
- char* device_name = GPS_DEFAULT_UART_PORT;
+ char *device_name = GPS_DEFAULT_UART_PORT;
/*
* Start/load the driver.
@@ -513,15 +536,18 @@ gps_main(int argc, char *argv[])
if (argc > 3) {
if (!strcmp(argv[2], "-d")) {
device_name = argv[3];
+
} else {
goto out;
}
}
+
gps::start(device_name);
}
if (!strcmp(argv[1], "stop"))
gps::stop();
+
/*
* Test the driver/device.
*/
diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp
index ba86d370a..2e2cbc8dd 100644
--- a/src/drivers/gps/gps_helper.cpp
+++ b/src/drivers/gps/gps_helper.cpp
@@ -87,13 +87,15 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
case 115200: speed = B115200; break;
- warnx("try baudrate: %d\n", speed);
+ warnx("try baudrate: %d\n", speed);
default:
warnx("ERROR: Unsupported baudrate: %d\n", baud);
return -EINVAL;
}
+
struct termios uart_config;
+
int termios_state;
/* fill the struct for the new configuration */
@@ -109,14 +111,17 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
return -1;
}
+
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
return -1;
}
+
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
warnx("ERROR setting baudrate (tcsetattr)\n");
return -1;
}
+
/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
return 0;
}
diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
index defc1a074..73d4b889c 100644
--- a/src/drivers/gps/gps_helper.h
+++ b/src/drivers/gps/gps_helper.h
@@ -33,7 +33,7 @@
*
****************************************************************************/
-/**
+/**
* @file gps_helper.h
*/
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index 62941d74b..56b702ea6 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -48,9 +48,9 @@
MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
-_fd(fd),
-_gps_position(gps_position),
-_mtk_revision(0)
+ _fd(fd),
+ _gps_position(gps_position),
+ _mtk_revision(0)
{
decode_init();
}
@@ -73,24 +73,28 @@ MTK::configure(unsigned &baudrate)
warnx("mtk: config write failed");
return -1;
}
+
usleep(10000);
if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
warnx("mtk: config write failed");
return -1;
}
+
usleep(10000);
if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
warnx("mtk: config write failed");
return -1;
}
+
usleep(10000);
if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
warnx("mtk: config write failed");
return -1;
}
+
usleep(10000);
if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
@@ -128,12 +132,15 @@ MTK::receive(unsigned timeout)
handle_message(packet);
return 1;
}
+
/* in case we keep trying but only get crap from GPS */
- if (time_started + timeout*1000 < hrt_absolute_time() ) {
+ if (time_started + timeout * 1000 < hrt_absolute_time()) {
return -1;
}
+
j++;
}
+
/* everything is read */
j = count = 0;
}
@@ -181,6 +188,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
if (b == MTK_SYNC1_V16) {
_decode_state = MTK_DECODE_GOT_CK_A;
_mtk_revision = 16;
+
} else if (b == MTK_SYNC1_V19) {
_decode_state = MTK_DECODE_GOT_CK_A;
_mtk_revision = 19;
@@ -201,7 +209,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
add_byte_to_checksum(b);
// Fill packet buffer
- ((uint8_t*)(&packet))[_rx_count] = b;
+ ((uint8_t *)(&packet))[_rx_count] = b;
_rx_count++;
/* Packet size minus checksum, XXX ? */
@@ -209,14 +217,17 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
/* Compare checksum */
if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
ret = 1;
+
} else {
warnx("MTK Checksum invalid");
ret = -1;
}
+
// Reset state machine to decode next packet
decode_init();
}
}
+
return ret;
}
@@ -226,19 +237,22 @@ MTK::handle_message(gps_mtk_packet_t &packet)
if (_mtk_revision == 16) {
_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
+
} else if (_mtk_revision == 19) {
_gps_position->lat = packet.latitude; // both degrees*1e7
_gps_position->lon = packet.longitude; // both degrees*1e7
+
} else {
warnx("mtk: unknown revision");
_gps_position->lat = 0;
_gps_position->lon = 0;
}
+
_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
_gps_position->fix_type = packet.fix_type;
_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
_gps_position->epv_m = 0.0; //unknown in mtk custom mode
- _gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
+ _gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
_gps_position->satellites_visible = packet.satellites;
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index ba5d14cc4..86291901c 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -60,13 +60,14 @@
#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK
#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received
#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls
+#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
_fd(fd),
_gps_position(gps_position),
_configured(false),
_waiting_for_ack(false),
- _disable_cmd_counter(0)
+ _disable_cmd_last(0)
{
decode_init();
}
@@ -191,35 +192,35 @@ UBX::configure(unsigned &baudrate)
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: msg rate configuration failed: NAV POSLLH\n");
+ warnx("ubx: msg rate configuration failed: NAV POSLLH");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: msg rate configuration failed: NAV TIMEUTC\n");
+ warnx("ubx: msg rate configuration failed: NAV TIMEUTC");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: msg rate configuration failed: NAV SOL\n");
+ warnx("ubx: msg rate configuration failed: NAV SOL");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: msg rate configuration failed: NAV VELNED\n");
+ warnx("ubx: msg rate configuration failed: NAV VELNED");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: msg rate configuration failed: NAV SVINFO\n");
+ warnx("ubx: msg rate configuration failed: NAV SVINFO");
return 1;
}
@@ -271,11 +272,17 @@ UBX::receive(unsigned timeout)
if (ret < 0) {
/* something went wrong when polling */
+ warnx("ubx: poll error");
return -1;
} else if (ret == 0) {
/* return success after short delay after receiving a packet or timeout after long delay */
- return handled ? 1 : -1;
+ if (handled) {
+ return 1;
+
+ } else {
+ return -1;
+ }
} else if (ret > 0) {
/* if we have new data from GPS, go handle it */
@@ -292,8 +299,6 @@ UBX::receive(unsigned timeout)
/* pass received bytes to the packet decoder */
for (int i = 0; i < count; i++) {
if (parse_char(buf[i]) > 0) {
- /* return to configure during configuration or to the gps driver during normal work
- * if a packet has arrived */
if (handle_message() > 0)
handled = true;
}
@@ -303,6 +308,7 @@ UBX::receive(unsigned timeout)
/* abort after timeout if no useful packets received */
if (time_started + timeout * 1000 < hrt_absolute_time()) {
+ warnx("ubx: timeout - no useful messages");
return -1;
}
}
@@ -453,16 +459,16 @@ UBX::handle_message()
timeinfo.tm_min = packet->min;
timeinfo.tm_sec = packet->sec;
time_t epoch = mktime(&timeinfo);
-
+
#ifndef CONFIG_RTC
- //Since we lack a hardware RTC, set the system time clock based on GPS UTC
- //TODO generalize this by moving into gps.cpp?
- timespec ts;
- ts.tv_sec = epoch;
- ts.tv_nsec = packet->time_nanoseconds;
- clock_settime(CLOCK_REALTIME,&ts);
+ //Since we lack a hardware RTC, set the system time clock based on GPS UTC
+ //TODO generalize this by moving into gps.cpp?
+ timespec ts;
+ ts.tv_sec = epoch;
+ ts.tv_nsec = packet->time_nanoseconds;
+ clock_settime(CLOCK_REALTIME, &ts);
#endif
-
+
_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
_gps_position->timestamp_time = hrt_absolute_time();
@@ -564,10 +570,13 @@ UBX::handle_message()
if (ret == 0) {
/* message not handled */
- warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id);
+ warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
+
+ hrt_abstime t = hrt_absolute_time();
- if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) {
+ if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
/* don't attempt for every message to disable, some might not be disabled */
+ _disable_cmd_last = t;
warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
configure_message_rate(_message_class, _message_id, 0);
}
@@ -640,7 +649,7 @@ UBX::add_checksum_to_message(uint8_t *message, const unsigned length)
ck_b = ck_b + ck_a;
}
- /* The checksum is written to the last to bytes of a message */
+ /* the checksum is written to the last to bytes of a message */
message[length - 2] = ck_a;
message[length - 1] = ck_b;
}
@@ -669,17 +678,17 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
{
ssize_t ret = 0;
- /* Calculate the checksum now */
+ /* calculate the checksum now */
add_checksum_to_message(packet, length);
const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
- /* Start with the two sync bytes */
+ /* start with the two sync bytes */
ret += write(fd, sync_bytes, sizeof(sync_bytes));
ret += write(fd, packet, 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");
+ warnx("ubx: configuration write fail");
}
void
@@ -696,7 +705,7 @@ UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t 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
+ /* configure ACK check */
_message_class_needed = msg_class;
_message_id_needed = msg_id;
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 4fc276975..76ef873a3 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -347,7 +347,7 @@ private:
/**
* 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
@@ -358,7 +358,7 @@ private:
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);
+ void add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
int wait_for_ack(unsigned timeout);
@@ -376,7 +376,7 @@ private:
uint8_t _message_class;
uint8_t _message_id;
unsigned _payload_size;
- uint8_t _disable_cmd_counter;
+ uint8_t _disable_cmd_last;
};
#endif /* UBX_H_ */
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index de043db64..5f0ce4ff8 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1293,10 +1293,6 @@ test()
if (fd < 0)
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
- /* set the queue depth to 10 */
- if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
- errx(1, "failed to set queue depth");
-
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index ad6de0ab1..8f5674823 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -367,7 +367,6 @@ out:
int
L3GD20::probe()
{
- irqstate_t flags = irqsave();
/* read dummy value to void to clear SPI statemachine on sensor */
(void)read_reg(ADDR_WHO_AM_I);
@@ -393,8 +392,6 @@ L3GD20::probe()
success = true;
}
- irqrestore(flags);
-
if (success)
return OK;
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 7244019b1..8bed8a8df 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -525,7 +525,6 @@ out:
void
LSM303D::reset()
{
- irqstate_t flags = irqsave();
/* enable accel*/
write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
@@ -540,7 +539,6 @@ LSM303D::reset()
mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
- irqrestore(flags);
_accel_read = 0;
_mag_read = 0;
@@ -549,15 +547,12 @@ LSM303D::reset()
int
LSM303D::probe()
{
- irqstate_t flags = irqsave();
/* read dummy value to void to clear SPI statemachine on sensor */
(void)read_reg(ADDR_WHO_AM_I);
/* verify that the device is attached and functioning */
bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
- irqrestore(flags);
-
if (success)
return OK;
@@ -1013,6 +1008,7 @@ LSM303D::accel_set_range(unsigned max_g)
_accel_range_scale = new_scale_g_digit * LSM303D_ONE_G;
+
modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
return OK;
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 1bc3e97a4..d0de26a1a 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -96,9 +96,10 @@ class MK : public device::I2C
{
public:
enum Mode {
+ MODE_NONE,
MODE_2PWM,
MODE_4PWM,
- MODE_NONE
+ MODE_6PWM,
};
enum MappingMode {
@@ -1023,9 +1024,11 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
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:
+ case MODE_6PWM:
ret = pwm_ioctl(filp, cmd, arg);
break;
@@ -1033,6 +1036,8 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
debug("not in a PWM mode");
break;
}
+ */
+ ret = pwm_ioctl(filp, cmd, arg);
/* if nobody wants it, let CDev have it */
if (ret == -ENOTTY)
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
index 21caed2ff..e547c913b 100644
--- a/src/drivers/ms5611/ms5611_spi.cpp
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -142,23 +142,15 @@ MS5611_SPI::init()
goto out;
}
- /* disable interrupts, make this section atomic */
- flags = irqsave();
/* send reset command */
ret = _reset();
- /* re-enable interrupts */
- irqrestore(flags);
if (ret != OK) {
debug("reset failed");
goto out;
}
- /* disable interrupts, make this section atomic */
- flags = irqsave();
/* read PROM */
ret = _read_prom();
- /* re-enable interrupts */
- irqrestore(flags);
if (ret != OK) {
debug("prom readout failed");
goto out;
@@ -270,13 +262,7 @@ MS5611_SPI::_reg16(unsigned reg)
int
MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len)
{
- irqstate_t flags = irqsave();
-
- int ret = transfer(send, recv, len);
-
- irqrestore(flags);
-
- return ret;
+ return transfer(send, recv, len);
}
#endif /* PX4_SPIDEV_BARO */
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 6d4019f24..b1dd55dd7 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1096,10 +1096,11 @@ fmu_start(void)
void
test(void)
{
- int fd;
+ int fd;
unsigned servo_count = 0;
unsigned pwm_value = 1000;
int direction = 1;
+ int ret;
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
@@ -1114,9 +1115,9 @@ test(void)
warnx("Testing %u servos", (unsigned)servo_count);
- int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
- if (!console)
- err(1, "failed opening console");
+ struct pollfd fds;
+ fds.fd = 0; /* stdin */
+ fds.events = POLLIN;
warnx("Press CTRL-C or 'c' to abort.");
@@ -1166,15 +1167,17 @@ test(void)
/* Check if user wants to quit */
char c;
- if (read(console, &c, 1) == 1) {
- if (c == 0x03 || c == 0x63) {
+ ret = poll(&fds, 1, 0);
+ if (ret > 0) {
+
+ read(0, &c, 1);
+ if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
break;
}
}
}
- close(console);
close(fd);
exit(0);
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 27b6a12da..7f67d02b5 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -205,6 +205,7 @@ public:
*/
int disable_rc_handling();
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/**
* Set the DSM VCC is controlled by relay one flag
*
@@ -224,6 +225,9 @@ public:
{
return _dsm_vcc_ctl;
};
+#endif
+
+ inline uint16_t system_status() const {return _status;}
private:
device::Device *_interface;
@@ -242,7 +246,8 @@ private:
volatile int _task; ///<worker task id
volatile bool _task_should_exit; ///<worker terminate flag
- int _mavlink_fd; ///<mavlink file descriptor
+ int _mavlink_fd; ///<mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
+ int _thread_mavlink_fd; ///<mavlink file descriptor for thread.
perf_counter_t _perf_update; ///<local performance counter
@@ -255,6 +260,7 @@ private:
int _t_actuator_armed; ///< system armed control topic
int _t_vehicle_control_mode;///< vehicle control mode topic
int _t_param; ///< parameter update topic
+ int _t_vehicle_command; ///< vehicle command topic
/* advertised topics */
orb_advert_t _to_input_rc; ///< rc inputs from io
@@ -274,7 +280,9 @@ private:
float _battery_mamphour_total;///<amp hours consumed so far
uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power
+#endif
/**
* Trampoline to the worker task
@@ -391,7 +399,7 @@ private:
/**
* Send mixer definition text to IO
*/
- int mixer_send(const char *buf, unsigned buflen);
+ int mixer_send(const char *buf, unsigned buflen, unsigned retries=3);
/**
* Handle a status update from IO.
@@ -412,6 +420,13 @@ private:
int io_handle_alarms(uint16_t alarms);
/**
+ * Handle issuing dsm bind ioctl to px4io.
+ *
+ * @param dsmMode 0:dsm2, 1:dsmx
+ */
+ void dsm_bind_ioctl(int dsmMode);
+
+ /**
* Handle a battery update from IO.
*
* Publish IO battery information if necessary.
@@ -454,6 +469,7 @@ PX4IO::PX4IO(device::Device *interface) :
_task(-1),
_task_should_exit(false),
_mavlink_fd(-1),
+ _thread_mavlink_fd(-1),
_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
_status(0),
_alarms(0),
@@ -461,6 +477,7 @@ PX4IO::PX4IO(device::Device *interface) :
_t_actuator_armed(-1),
_t_vehicle_control_mode(-1),
_t_param(-1),
+ _t_vehicle_command(-1),
_to_input_rc(0),
_to_actuators_effective(0),
_to_outputs(0),
@@ -471,8 +488,11 @@ PX4IO::PX4IO(device::Device *interface) :
_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),
- _dsm_vcc_ctl(false)
+ _battery_last_timestamp(0)
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+ ,_dsm_vcc_ctl(false)
+#endif
+
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@@ -593,6 +613,9 @@ PX4IO::init()
if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
(reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+ /* get a status update from IO */
+ io_get_status();
+
mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
log("INAIR RESTART RECOVERY (needs commander app running)");
@@ -732,10 +755,10 @@ void
PX4IO::task_main()
{
hrt_abstime last_poll_time = 0;
- int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
log("starting");
+ _thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
@@ -754,16 +777,20 @@ PX4IO::task_main()
_t_param = orb_subscribe(ORB_ID(parameter_update));
orb_set_interval(_t_param, 500); /* 2Hz update rate max. */
+ _t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
+ orb_set_interval(_t_param, 1000); /* 1Hz update rate max. */
+
if ((_t_actuators < 0) ||
(_t_actuator_armed < 0) ||
(_t_vehicle_control_mode < 0) ||
- (_t_param < 0)) {
+ (_t_param < 0) ||
+ (_t_vehicle_command < 0)) {
log("subscription(s) failed");
goto out;
}
/* poll descriptor */
- pollfd fds[4];
+ pollfd fds[5];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_actuator_armed;
@@ -772,8 +799,10 @@ PX4IO::task_main()
fds[2].events = POLLIN;
fds[3].fd = _t_param;
fds[3].events = POLLIN;
+ fds[4].fd = _t_vehicle_command;
+ fds[4].events = POLLIN;
- debug("ready");
+ log("ready");
/* lock against the ioctl handler */
lock();
@@ -813,6 +842,16 @@ PX4IO::task_main()
if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN))
io_set_arming_state();
+ /* if we have a vehicle command, handle it */
+ if (fds[4].revents & POLLIN) {
+ struct vehicle_command_s cmd;
+ orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
+ // Check for a DSM pairing command
+ if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) {
+ dsm_bind_ioctl((int)cmd.param2);
+ }
+ }
+
/*
* If it's time for another tick of the polling status machine,
* try it now.
@@ -847,20 +886,11 @@ PX4IO::task_main()
int32_t dsm_bind_val;
param_t dsm_bind_param;
- // See if bind parameter has been set, and reset it to 0
+ // See if bind parameter has been set, and reset it to -1
param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
- if (dsm_bind_val) {
- if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
- if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) {
- mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x');
- ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7);
- } else {
- mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected");
- }
- } else {
- mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected");
- }
- dsm_bind_val = 0;
+ if (dsm_bind_val > -1) {
+ dsm_bind_ioctl(dsm_bind_val);
+ dsm_bind_val = -1;
param_set(dsm_bind_param, &dsm_bind_val);
}
@@ -1167,6 +1197,23 @@ PX4IO::io_handle_status(uint16_t status)
return ret;
}
+void
+PX4IO::dsm_bind_ioctl(int dsmMode)
+{
+ if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
+ /* 0: dsm2, 1:dsmx */
+ if ((dsmMode == 0) || (dsmMode == 1)) {
+ mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x');
+ ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES);
+ } else {
+ mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected");
+ }
+ } else {
+ mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected");
+ }
+}
+
+
int
PX4IO::io_handle_alarms(uint16_t alarms)
{
@@ -1485,61 +1532,70 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
}
int
-PX4IO::mixer_send(const char *buf, unsigned buflen)
+PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
{
- uint8_t frame[_max_transfer];
- px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
- unsigned max_len = _max_transfer - sizeof(px4io_mixdata);
- msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
- msg->action = F2I_MIXER_ACTION_RESET;
+ uint8_t frame[_max_transfer];
do {
- unsigned count = buflen;
- if (count > max_len)
- count = max_len;
+ px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
+ unsigned max_len = _max_transfer - sizeof(px4io_mixdata);
- if (count > 0) {
- memcpy(&msg->text[0], buf, count);
- buf += count;
- buflen -= count;
- }
+ msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
+ msg->action = F2I_MIXER_ACTION_RESET;
- /*
- * We have to send an even number of bytes. This
- * will only happen on the very last transfer of a
- * mixer, and we are guaranteed that there will be
- * space left to round up as _max_transfer will be
- * even.
- */
- unsigned total_len = sizeof(px4io_mixdata) + count;
- if (total_len % 1) {
- msg->text[count] = '\0';
- total_len++;
- }
+ do {
+ unsigned count = buflen;
- int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+ if (count > max_len)
+ count = max_len;
- if (ret) {
- log("mixer send error %d", ret);
- return ret;
- }
+ if (count > 0) {
+ memcpy(&msg->text[0], buf, count);
+ buf += count;
+ buflen -= count;
+ }
- msg->action = F2I_MIXER_ACTION_APPEND;
+ /*
+ * We have to send an even number of bytes. This
+ * will only happen on the very last transfer of a
+ * mixer, and we are guaranteed that there will be
+ * space left to round up as _max_transfer will be
+ * even.
+ */
+ unsigned total_len = sizeof(px4io_mixdata) + count;
+ if (total_len % 2) {
+ msg->text[count] = '\0';
+ total_len++;
+ }
+
+ int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+
+ if (ret) {
+ log("mixer send error %d", ret);
+ return ret;
+ }
- } while (buflen > 0);
+ msg->action = F2I_MIXER_ACTION_APPEND;
+
+ } while (buflen > 0);
+
+ retries--;
+
+ log("mixer sent");
+
+ } while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK)));
/* check for the mixer-OK flag */
if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) {
- debug("mixer upload OK");
mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok");
return 0;
- } else {
- debug("mixer rejected by IO");
- mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
}
+ log("mixer rejected by IO");
+ mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
+
/* load must have failed for some reason */
return -EINVAL;
}
@@ -1646,11 +1702,19 @@ PX4IO::print_status()
((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""),
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""));
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
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));
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+ printf("rates 0x%04x default %u alt %u\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));
+#endif
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++)
@@ -1791,36 +1855,58 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
}
case GPIO_RESET: {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
uint32_t bits = (1 << _max_relays) - 1;
/* don't touch relay1 if it's controlling RX vcc */
if (_dsm_vcc_ctl)
bits &= ~PX4IO_P_SETUP_RELAYS_POWER1;
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+ ret = -EINVAL;
+#endif
break;
}
case GPIO_SET:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
arg &= ((1 << _max_relays) - 1);
/* don't touch relay1 if it's controlling RX vcc */
- if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
+ if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
ret = -EINVAL;
- else
- ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
+ break;
+ }
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+ ret = -EINVAL;
+#endif
break;
case GPIO_CLEAR:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
arg &= ((1 << _max_relays) - 1);
/* don't touch relay1 if it's controlling RX vcc */
- if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
+ if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
ret = -EINVAL;
- else
+ break;
+ }
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+ ret = -EINVAL;
+#endif
break;
case GPIO_GET:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS);
if (*(uint32_t *)arg == _io_reg_get_error)
ret = -EIO;
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+ ret = -EINVAL;
+#endif
break;
case MIXERIOCGETOUTPUTCOUNT:
@@ -1998,6 +2084,7 @@ start(int argc, char *argv[])
}
}
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
int dsm_vcc_ctl;
if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
@@ -2006,6 +2093,7 @@ start(int argc, char *argv[])
g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
}
}
+#endif
exit(0);
}
@@ -2045,21 +2133,26 @@ bind(int argc, char *argv[])
if (g_dev == nullptr)
errx(1, "px4io must be started first");
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
if (!g_dev->get_dsm_vcc_ctl())
errx(1, "DSM bind feature not enabled");
+#endif
if (argc < 3)
errx(0, "needs argument, use dsm2 or dsmx");
if (!strcmp(argv[2], "dsm2"))
- pulses = 3;
+ pulses = DSM2_BIND_PULSES;
else if (!strcmp(argv[2], "dsmx"))
- pulses = 7;
+ pulses = DSMX_BIND_PULSES;
else
errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
+ if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+ errx(1, "system must not be armed");
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
-
+#endif
g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
exit(0);
@@ -2091,10 +2184,9 @@ test(void)
if (ioctl(fd, PWM_SERVO_ARM, 0))
err(1, "failed to arm servos");
- /* Open console directly to grab CTRL-C signal */
- int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
- if (!console)
- err(1, "failed opening console");
+ struct pollfd fds;
+ fds.fd = 0; /* stdin */
+ fds.events = POLLIN;
warnx("Press CTRL-C or 'c' to abort.");
@@ -2135,10 +2227,12 @@ test(void)
/* Check if user wants to quit */
char c;
- if (read(console, &c, 1) == 1) {
+ ret = poll(&fds, 1, 0);
+ if (ret > 0) {
+
+ read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
- close(console);
exit(0);
}
}
@@ -2208,7 +2302,7 @@ px4io_main(int argc, char *argv[])
}
PX4IO_Uploader *up;
- const char *fn[5];
+ const char *fn[3];
/* work out what we're uploading... */
if (argc > 2) {
@@ -2216,11 +2310,19 @@ px4io_main(int argc, char *argv[])
fn[1] = nullptr;
} else {
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+ fn[0] = "/etc/extras/px4io-v1_default.bin";
+ fn[1] = "/fs/microsd/px4io1.bin";
+ fn[2] = "/fs/microsd/px4io.bin";
+ fn[3] = nullptr;
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
fn[0] = "/etc/extras/px4io-v2_default.bin";
- fn[1] = "/etc/extras/px4io-v1_default.bin";
+ fn[1] = "/fs/microsd/px4io2.bin";
fn[2] = "/fs/microsd/px4io.bin";
- fn[3] = "/fs/microsd/px4io2.bin";
- fn[4] = nullptr;
+ fn[3] = nullptr;
+#else
+#error "unknown board"
+#endif
}
up = new PX4IO_Uploader;
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index ee1d472a2..fedff769b 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -1,6 +1,8 @@
/****************************************************************************
*
* Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
+ * Author: Julian Oes <joes@student.ethz.ch>
+ * Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,7 +38,6 @@
*
* Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
*
- *
*/
#include <nuttx/config.h>
@@ -92,16 +93,14 @@ public:
private:
work_s _work;
- rgbled_color_t _color;
rgbled_mode_t _mode;
rgbled_pattern_t _pattern;
- float _brightness;
uint8_t _r;
uint8_t _g;
uint8_t _b;
+ float _brightness;
- bool _should_run;
bool _running;
int _led_interval;
int _counter;
@@ -109,35 +108,33 @@ private:
void set_color(rgbled_color_t ledcolor);
void set_mode(rgbled_mode_t mode);
void set_pattern(rgbled_pattern_t *pattern);
- void set_brightness(float brightness);
static void led_trampoline(void *arg);
void led();
- int set(bool on, uint8_t r, uint8_t g, uint8_t b);
- int set_on(bool on);
- int set_rgb(uint8_t r, uint8_t g, uint8_t b);
- int get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+ int send_led_enable(bool enable);
+ int send_led_rgb();
+ int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
};
/* for now, we only support one RGBLED */
namespace
{
- RGBLED *g_rgbled;
+RGBLED *g_rgbled;
}
+void rgbled_usage();
extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
RGBLED::RGBLED(int bus, int rgbled) :
I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000),
- _color(RGBLED_COLOR_OFF),
_mode(RGBLED_MODE_OFF),
- _running(false),
- _brightness(1.0f),
_r(0),
_g(0),
_b(0),
+ _brightness(1.0f),
+ _running(false),
_led_interval(0),
_counter(0)
{
@@ -159,8 +156,9 @@ RGBLED::init()
return ret;
}
- /* start off */
- set(false, 0, 0, 0);
+ /* switch off LED on start */
+ send_led_enable(false);
+ send_led_rgb();
return OK;
}
@@ -169,10 +167,10 @@ int
RGBLED::probe()
{
int ret;
- bool on, not_powersave;
+ bool on, powersave;
uint8_t r, g, b;
- ret = get(on, not_powersave, r, g, b);
+ ret = get(on, powersave, r, g, b);
return ret;
}
@@ -181,15 +179,16 @@ int
RGBLED::info()
{
int ret;
- bool on, not_powersave;
+ bool on, powersave;
uint8_t r, g, b;
- ret = get(on, not_powersave, r, g, b);
+ ret = get(on, powersave, r, g, b);
if (ret == OK) {
/* we don't care about power-save mode */
log("state: %s", on ? "ON" : "OFF");
log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
+
} else {
warnx("failed to read led");
}
@@ -201,28 +200,30 @@ int
RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = ENOTTY;
+
switch (cmd) {
case RGBLED_SET_RGB:
- /* set the specified RGB values */
- rgbled_rgbset_t rgbset;
- memcpy(&rgbset, (rgbled_rgbset_t*)arg, sizeof(rgbset));
- set_rgb(rgbset.red, rgbset.green, rgbset.blue);
- set_mode(RGBLED_MODE_ON);
+ /* set the specified color */
+ _r = ((rgbled_rgbset_t *) arg)->red;
+ _g = ((rgbled_rgbset_t *) arg)->green;
+ _b = ((rgbled_rgbset_t *) arg)->blue;
+ send_led_rgb();
return OK;
case RGBLED_SET_COLOR:
/* set the specified color name */
set_color((rgbled_color_t)arg);
+ send_led_rgb();
return OK;
case RGBLED_SET_MODE:
- /* set the specified blink speed */
+ /* set the specified mode */
set_mode((rgbled_mode_t)arg);
return OK;
case RGBLED_SET_PATTERN:
/* set a special pattern */
- set_pattern((rgbled_pattern_t*)arg);
+ set_pattern((rgbled_pattern_t *)arg);
return OK;
default:
@@ -241,39 +242,54 @@ RGBLED::led_trampoline(void *arg)
rgbl->led();
}
-
-
+/**
+ * Main loop function
+ */
void
RGBLED::led()
{
switch (_mode) {
- case RGBLED_MODE_BLINK_SLOW:
- case RGBLED_MODE_BLINK_NORMAL:
- case RGBLED_MODE_BLINK_FAST:
- if(_counter % 2 == 0)
- set_on(true);
- else
- set_on(false);
- break;
- case RGBLED_MODE_BREATHE:
- if (_counter >= 30)
- _counter = 0;
- if (_counter <= 15) {
- set_brightness(((float)_counter)*((float)_counter)/(15.0f*15.0f));
- } else {
- set_brightness(((float)(30-_counter))*((float)(30-_counter))/(15.0f*15.0f));
- }
- break;
- case RGBLED_MODE_PATTERN:
- /* don't run out of the pattern array and stop if the next frame is 0 */
- if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
- _counter = 0;
+ case RGBLED_MODE_BLINK_SLOW:
+ case RGBLED_MODE_BLINK_NORMAL:
+ case RGBLED_MODE_BLINK_FAST:
+ if (_counter >= 2)
+ _counter = 0;
- set_color(_pattern.color[_counter]);
- _led_interval = _pattern.duration[_counter];
- break;
- default:
- break;
+ send_led_enable(_counter == 0);
+
+ break;
+
+ case RGBLED_MODE_BREATHE:
+
+ if (_counter >= 62)
+ _counter = 0;
+
+ int n;
+
+ if (_counter < 32) {
+ n = _counter;
+
+ } else {
+ n = 62 - _counter;
+ }
+
+ _brightness = n * n / (31.0f * 31.0f);
+ send_led_rgb();
+ break;
+
+ case RGBLED_MODE_PATTERN:
+
+ /* don't run out of the pattern array and stop if the next frame is 0 */
+ if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
+ _counter = 0;
+
+ set_color(_pattern.color[_counter]);
+ send_led_rgb();
+ _led_interval = _pattern.duration[_counter];
+ break;
+
+ default:
+ break;
}
_counter++;
@@ -282,181 +298,231 @@ RGBLED::led()
work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval);
}
+/**
+ * Parse color constant and set _r _g _b values
+ */
void
-RGBLED::set_color(rgbled_color_t color) {
+RGBLED::set_color(rgbled_color_t color)
+{
+ switch (color) {
+ case RGBLED_COLOR_OFF:
+ _r = 0;
+ _g = 0;
+ _b = 0;
+ break;
- _color = color;
+ case RGBLED_COLOR_RED:
+ _r = 255;
+ _g = 0;
+ _b = 0;
+ break;
- switch (color) {
- case RGBLED_COLOR_OFF: // off
- set_rgb(0,0,0);
- break;
- case RGBLED_COLOR_RED: // red
- set_rgb(255,0,0);
- break;
- case RGBLED_COLOR_YELLOW: // yellow
- set_rgb(255,70,0);
- break;
- case RGBLED_COLOR_PURPLE: // purple
- set_rgb(255,0,255);
- break;
- case RGBLED_COLOR_GREEN: // green
- set_rgb(0,255,0);
- break;
- case RGBLED_COLOR_BLUE: // blue
- set_rgb(0,0,255);
- break;
- case RGBLED_COLOR_WHITE: // white
- set_rgb(255,255,255);
- break;
- case RGBLED_COLOR_AMBER: // amber
- set_rgb(255,20,0);
- break;
- case RGBLED_COLOR_DIM_RED: // red
- set_rgb(90,0,0);
- break;
- case RGBLED_COLOR_DIM_YELLOW: // yellow
- set_rgb(80,30,0);
- break;
- case RGBLED_COLOR_DIM_PURPLE: // purple
- set_rgb(45,0,45);
- break;
- case RGBLED_COLOR_DIM_GREEN: // green
- set_rgb(0,90,0);
- break;
- case RGBLED_COLOR_DIM_BLUE: // blue
- set_rgb(0,0,90);
- break;
- case RGBLED_COLOR_DIM_WHITE: // white
- set_rgb(30,30,30);
- break;
- case RGBLED_COLOR_DIM_AMBER: // amber
- set_rgb(80,20,0);
- break;
- default:
- warnx("color unknown");
- break;
+ case RGBLED_COLOR_YELLOW:
+ _r = 255;
+ _g = 200;
+ _b = 0;
+ break;
+
+ case RGBLED_COLOR_PURPLE:
+ _r = 255;
+ _g = 0;
+ _b = 255;
+ break;
+
+ case RGBLED_COLOR_GREEN:
+ _r = 0;
+ _g = 255;
+ _b = 0;
+ break;
+
+ case RGBLED_COLOR_BLUE:
+ _r = 0;
+ _g = 0;
+ _b = 255;
+ break;
+
+ case RGBLED_COLOR_WHITE:
+ _r = 255;
+ _g = 255;
+ _b = 255;
+ break;
+
+ case RGBLED_COLOR_AMBER:
+ _r = 255;
+ _g = 80;
+ _b = 0;
+ break;
+
+ case RGBLED_COLOR_DIM_RED:
+ _r = 90;
+ _g = 0;
+ _b = 0;
+ break;
+
+ case RGBLED_COLOR_DIM_YELLOW:
+ _r = 80;
+ _g = 30;
+ _b = 0;
+ break;
+
+ case RGBLED_COLOR_DIM_PURPLE:
+ _r = 45;
+ _g = 0;
+ _b = 45;
+ break;
+
+ case RGBLED_COLOR_DIM_GREEN:
+ _r = 0;
+ _g = 90;
+ _b = 0;
+ break;
+
+ case RGBLED_COLOR_DIM_BLUE:
+ _r = 0;
+ _g = 0;
+ _b = 90;
+ break;
+
+ case RGBLED_COLOR_DIM_WHITE:
+ _r = 30;
+ _g = 30;
+ _b = 30;
+ break;
+
+ case RGBLED_COLOR_DIM_AMBER:
+ _r = 80;
+ _g = 20;
+ _b = 0;
+ break;
+
+ default:
+ warnx("color unknown");
+ break;
}
}
+/**
+ * Set mode, if mode not changed has no any effect (doesn't reset blinks phase)
+ */
void
RGBLED::set_mode(rgbled_mode_t mode)
{
- _mode = mode;
+ if (mode != _mode) {
+ _mode = mode;
+ bool should_run = false;
- switch (mode) {
+ switch (mode) {
case RGBLED_MODE_OFF:
- _should_run = false;
- set_on(false);
+ send_led_enable(false);
break;
+
case RGBLED_MODE_ON:
- _should_run = false;
- set_on(true);
+ _brightness = 1.0f;
+ send_led_rgb();
+ send_led_enable(true);
break;
+
case RGBLED_MODE_BLINK_SLOW:
- _should_run = true;
+ should_run = true;
+ _counter = 0;
_led_interval = 2000;
+ _brightness = 1.0f;
+ send_led_rgb();
break;
+
case RGBLED_MODE_BLINK_NORMAL:
- _should_run = true;
+ should_run = true;
+ _counter = 0;
_led_interval = 500;
+ _brightness = 1.0f;
+ send_led_rgb();
break;
+
case RGBLED_MODE_BLINK_FAST:
- _should_run = true;
+ should_run = true;
+ _counter = 0;
_led_interval = 100;
+ _brightness = 1.0f;
+ send_led_rgb();
break;
+
case RGBLED_MODE_BREATHE:
- _should_run = true;
- set_on(true);
+ should_run = true;
_counter = 0;
- _led_interval = 1000/15;
+ _led_interval = 25;
+ send_led_enable(true);
break;
+
case RGBLED_MODE_PATTERN:
- _should_run = true;
- set_on(true);
+ should_run = true;
_counter = 0;
+ _brightness = 1.0f;
+ send_led_enable(true);
break;
+
default:
warnx("mode unknown");
break;
- }
+ }
- /* if it should run now, start the workq */
- if (_should_run && !_running) {
- _running = true;
- work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
- }
- /* if it should stop, then cancel the workq */
- if (!_should_run && _running) {
- _running = false;
- work_cancel(LPWORK, &_work);
+ /* if it should run now, start the workq */
+ if (should_run && !_running) {
+ _running = true;
+ work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
+ }
+
+ /* if it should stop, then cancel the workq */
+ if (!should_run && _running) {
+ _running = false;
+ work_cancel(LPWORK, &_work);
+ }
}
}
+/**
+ * Set pattern for PATTERN mode, but don't change current mode
+ */
void
RGBLED::set_pattern(rgbled_pattern_t *pattern)
{
memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t));
-
- set_mode(RGBLED_MODE_PATTERN);
-}
-
-void
-RGBLED::set_brightness(float brightness) {
-
- _brightness = brightness;
- set_rgb(_r, _g, _b);
-}
-
-int
-RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b)
-{
- uint8_t settings_byte = 0;
-
- if (on)
- settings_byte |= SETTING_ENABLE;
-/* powersave not used */
-// if (not_powersave)
- settings_byte |= SETTING_NOT_POWERSAVE;
-
- const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte};
-
- return transfer(msg, sizeof(msg), nullptr, 0);
}
+/**
+ * Sent ENABLE flag to LED driver
+ */
int
-RGBLED::set_on(bool on)
+RGBLED::send_led_enable(bool enable)
{
uint8_t settings_byte = 0;
- if (on)
+ if (enable)
settings_byte |= SETTING_ENABLE;
-/* powersave not used */
-// if (not_powersave)
- settings_byte |= SETTING_NOT_POWERSAVE;
+ settings_byte |= SETTING_NOT_POWERSAVE;
const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte};
return transfer(msg, sizeof(msg), nullptr, 0);
}
+/**
+ * Send RGB PWM settings to LED driver according to current color and brightness
+ */
int
-RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b)
+RGBLED::send_led_rgb()
{
- /* save the RGB values in case we want to change the brightness later */
- _r = r;
- _g = g;
- _b = b;
-
- const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)((float)b/255.0f*15.0f*_brightness), SUB_ADDR_PWM1, (uint8_t)((float)g/255.0f*15.0f*_brightness), SUB_ADDR_PWM2, (uint8_t)((float)r/255.0f*15.0f*_brightness)};
-
+ /* To scale from 0..255 -> 0..15 shift right by 4 bits */
+ const uint8_t msg[6] = {
+ SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4),
+ SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4),
+ SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4)
+ };
return transfer(msg, sizeof(msg), nullptr, 0);
}
-
int
-RGBLED::get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
+RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
{
uint8_t result[2];
int ret;
@@ -465,24 +531,23 @@ RGBLED::get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
if (ret == OK) {
on = result[0] & SETTING_ENABLE;
- not_powersave = result[0] & SETTING_NOT_POWERSAVE;
+ powersave = !(result[0] & SETTING_NOT_POWERSAVE);
/* XXX check, looks wrong */
- r = (result[0] & 0x0f)*255/15;
- g = (result[1] & 0xf0)*255/15;
- b = (result[1] & 0x0f)*255/15;
+ r = (result[0] & 0x0f) << 4;
+ g = (result[1] & 0xf0);
+ b = (result[1] & 0x0f) << 4;
}
return ret;
}
-void rgbled_usage();
-
-
-void rgbled_usage() {
- warnx("missing command: try 'start', 'test', 'info', 'stop'/'off', 'rgb 30 40 50'");
+void
+rgbled_usage()
+{
+ warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'");
warnx("options:");
warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED);
- errx(0, " -a addr (0x%x)", ADDR);
+ warnx(" -a addr (0x%x)", ADDR);
}
int
@@ -492,17 +557,21 @@ rgbled_main(int argc, char *argv[])
int rgbledadr = ADDR; /* 7bit */
int ch;
+
/* jump over start/off/etc and look at options first */
- while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) {
+ while ((ch = getopt(argc - 1, &argv[1], "a:b:")) != EOF) {
switch (ch) {
case 'a':
rgbledadr = strtol(optarg, NULL, 0);
break;
+
case 'b':
i2cdevice = strtol(optarg, NULL, 0);
break;
+
default:
rgbled_usage();
+ exit(0);
}
}
@@ -519,17 +588,21 @@ rgbled_main(int argc, char *argv[])
// try the external bus first
i2cdevice = PX4_I2C_BUS_EXPANSION;
g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr);
+
if (g_rgbled != nullptr && OK != g_rgbled->init()) {
delete g_rgbled;
g_rgbled = nullptr;
}
+
if (g_rgbled == nullptr) {
// fall back to default bus
i2cdevice = PX4_I2C_BUS_LED;
}
}
+
if (g_rgbled == nullptr) {
g_rgbled = new RGBLED(i2cdevice, rgbledadr);
+
if (g_rgbled == nullptr)
errx(1, "new failed");
@@ -545,21 +618,24 @@ rgbled_main(int argc, char *argv[])
/* need the driver past this point */
if (g_rgbled == nullptr) {
- warnx("not started");
- rgbled_usage();
- exit(0);
+ warnx("not started");
+ rgbled_usage();
+ exit(0);
}
if (!strcmp(verb, "test")) {
fd = open(RGBLED_DEVICE_PATH, 0);
+
if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
}
- rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_OFF},
- {200, 200, 200, 400 } };
+ rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF},
+ {500, 500, 500, 500, 1000, 0 } // "0" indicates end of pattern
+ };
ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern);
+ ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN);
close(fd);
exit(ret);
@@ -570,33 +646,39 @@ rgbled_main(int argc, char *argv[])
exit(0);
}
- if (!strcmp(verb, "stop") || !strcmp(verb, "off")) {
- /* although technically it doesn't stop, this is the excepted syntax */
+ if (!strcmp(verb, "off")) {
fd = open(RGBLED_DEVICE_PATH, 0);
+
if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
}
+
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
close(fd);
exit(ret);
}
if (!strcmp(verb, "rgb")) {
+ if (argc < 5) {
+ errx(1, "Usage: rgbled rgb <red> <green> <blue>");
+ }
+
fd = open(RGBLED_DEVICE_PATH, 0);
+
if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
}
- if (argc < 5) {
- errx(1, "Usage: rgbled rgb <red> <green> <blue>");
- }
+
rgbled_rgbset_t v;
v.red = strtol(argv[2], NULL, 0);
v.green = strtol(argv[3], NULL, 0);
v.blue = strtol(argv[4], NULL, 0);
ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
+ ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
close(fd);
exit(ret);
}
rgbled_usage();
+ exit(0);
}
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index a582ece17..f36f2091e 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -317,7 +317,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
ToneAlarm::ToneAlarm() :
- CDev("tone_alarm", "/dev/tone_alarm"),
+ CDev("tone_alarm", TONEALARM_DEVICE_PATH),
_default_tune_number(0),
_user_tune(nullptr),
_tune(nullptr),
@@ -333,6 +333,7 @@ ToneAlarm::ToneAlarm() :
_default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning
_default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow
_default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast
+ _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow
_tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune
_tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone
@@ -342,6 +343,7 @@ ToneAlarm::ToneAlarm() :
_tune_names[TONE_ARMING_WARNING_TUNE] = "arming"; // arming warning
_tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat"; // battery warning slow
_tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat"; // battery warning fast
+ _tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning"; // gps warning
}
ToneAlarm::~ToneAlarm()
@@ -820,10 +822,10 @@ play_tune(unsigned tune)
{
int fd, ret;
- fd = open("/dev/tone_alarm", 0);
+ fd = open(TONEALARM_DEVICE_PATH, 0);
if (fd < 0)
- err(1, "/dev/tone_alarm");
+ err(1, TONEALARM_DEVICE_PATH);
ret = ioctl(fd, TONE_SET_ALARM, tune);
close(fd);
@@ -839,10 +841,10 @@ play_string(const char *str, bool free_buffer)
{
int fd, ret;
- fd = open("/dev/tone_alarm", O_WRONLY);
+ fd = open(TONEALARM_DEVICE_PATH, O_WRONLY);
if (fd < 0)
- err(1, "/dev/tone_alarm");
+ err(1, TONEALARM_DEVICE_PATH);
ret = write(fd, str, strlen(str) + 1);
close(fd);
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-19 08:54:45 +0000