aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/boards/px4fmu-v1/board_config.h3
-rw-r--r--src/drivers/boards/px4fmu-v2/board_config.h7
-rw-r--r--src/drivers/boards/px4fmu-v2/px4fmu2_init.c12
-rw-r--r--src/drivers/boards/px4fmu-v2/px4fmu_spi.c35
-rw-r--r--src/drivers/drv_io_expander.h71
-rw-r--r--src/drivers/ets_airspeed/ets_airspeed.cpp10
-rw-r--r--src/drivers/gps/gps.cpp6
-rw-r--r--src/drivers/gps/gps_helper.cpp4
-rw-r--r--src/drivers/gps/gps_helper.h8
-rw-r--r--src/drivers/gps/ubx.cpp51
-rw-r--r--src/drivers/gps/ubx.h28
-rw-r--r--src/drivers/pca8574/module.mk6
-rw-r--r--src/drivers/pca8574/pca8574.cpp554
-rw-r--r--src/drivers/px4fmu/fmu.cpp9
-rw-r--r--src/drivers/px4io/px4io.cpp6
15 files changed, 776 insertions, 34 deletions
diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h
index 58273f2d2..c944007a5 100644
--- a/src/drivers/boards/px4fmu-v1/board_config.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -86,6 +86,7 @@ __BEGIN_DECLS
#define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
#define PX4_SPI_BUS_SENSORS 1
+#define PX4_SPI_BUS_EXT 2
/*
* Use these in place of the spi_dev_e enumeration to
@@ -98,7 +99,7 @@ __BEGIN_DECLS
/*
* Optional devices on IO's external port
*/
-#define PX4_SPIDEV_ACCEL_MAG 2
+#define PX4_SPIDEV_ACCEL_MAG 2
/*
* I2C busses
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index c2de1bfba..36eb7bec4 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -106,8 +106,11 @@ __BEGIN_DECLS
#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
+#define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
+#define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
#define PX4_SPI_BUS_SENSORS 1
+#define PX4_SPI_BUS_EXT 4
/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
#define PX4_SPIDEV_GYRO 1
@@ -115,6 +118,10 @@ __BEGIN_DECLS
#define PX4_SPIDEV_BARO 3
#define PX4_SPIDEV_MPU 4
+/* External bus */
+#define PX4_SPIDEV_EXT0 1
+#define PX4_SPIDEV_EXT1 2
+
/* I2C busses */
#define PX4_I2C_BUS_EXPANSION 1
#define PX4_I2C_BUS_LED 2
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
index 71414d62c..bf41bb1fe 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -192,6 +192,7 @@ stm32_boardinitialize(void)
static struct spi_dev_s *spi1;
static struct spi_dev_s *spi2;
+static struct spi_dev_s *spi4;
static struct sdio_dev_s *sdio;
#include <math.h>
@@ -305,6 +306,17 @@ __EXPORT int nsh_archinitialize(void)
message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n");
+ spi4 = up_spiinitialize(4);
+
+ /* Default SPI4 to 1MHz and de-assert the known chip selects. */
+ SPI_SETFREQUENCY(spi4, 10000000);
+ SPI_SETBITS(spi4, 8);
+ SPI_SETMODE(spi4, SPIDEV_MODE3);
+ SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false);
+ SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false);
+
+ message("[boot] Initialized SPI port 4\n");
+
#ifdef CONFIG_MMCSD
/* First, get an instance of the SDIO interface */
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
index c66c490a7..01dbd6e77 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
@@ -94,6 +94,13 @@ __EXPORT void weak_function stm32_spiinitialize(void)
stm32_configgpio(GPIO_SPI_CS_FRAM);
stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
#endif
+
+#ifdef CONFIG_STM32_SPI4
+ stm32_configgpio(GPIO_SPI_CS_EXT0);
+ stm32_configgpio(GPIO_SPI_CS_EXT1);
+ stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+ stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+#endif
}
__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
@@ -157,3 +164,31 @@ __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
return SPI_STATUS_PRESENT;
}
#endif
+
+__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+ /* SPI select is active low, so write !selected to select the device */
+
+ switch (devid) {
+ case PX4_SPIDEV_EXT0:
+ /* Making sure the other peripherals are not selected */
+ stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected);
+ stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
+ break;
+
+ case PX4_SPIDEV_EXT1:
+ /* Making sure the other peripherals are not selected */
+ stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected);
+ stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
+ break;
+
+ default:
+ break;
+
+ }
+}
+
+__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+ return SPI_STATUS_PRESENT;
+}
diff --git a/src/drivers/drv_io_expander.h b/src/drivers/drv_io_expander.h
new file mode 100644
index 000000000..106354377
--- /dev/null
+++ b/src/drivers/drv_io_expander.h
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_io_expander.h
+ *
+ * IO expander device API
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+/*
+ * ioctl() definitions
+ */
+
+#define _IOXIOCBASE (0x2800)
+#define _IOXIOC(_n) (_IOC(_IOXIOCBASE, _n))
+
+/** set a bitmask (non-blocking) */
+#define IOX_SET_MASK _IOXIOC(1)
+
+/** get a bitmask (blocking) */
+#define IOX_GET_MASK _IOXIOC(2)
+
+/** set device mode (non-blocking) */
+#define IOX_SET_MODE _IOXIOC(3)
+
+/** set constant values (non-blocking) */
+#define IOX_SET_VALUE _IOXIOC(4)
+
+/* ... to IOX_SET_VALUE + 8 */
+
+/* enum passed to RGBLED_SET_MODE ioctl()*/
+enum IOX_MODE {
+ IOX_MODE_OFF,
+ IOX_MODE_ON,
+ IOX_MODE_TEST_OUT
+};
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index 146a06e7c..2de7063ea 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -154,8 +154,9 @@ ETSAirspeed::collect()
return ret;
}
- uint16_t diff_pres_pa = val[1] << 8 | val[0];
- if (diff_pres_pa == 0) {
+ uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
+ uint16_t diff_pres_pa;
+ if (diff_pres_pa_raw == 0) {
// a zero value means the pressure sensor cannot give us a
// value. We need to return, and not report a value or the
// caller could end up using this value as part of an
@@ -165,10 +166,10 @@ ETSAirspeed::collect()
return -1;
}
- if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
+ if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
diff_pres_pa = 0;
} else {
- diff_pres_pa -= _diff_pres_offset;
+ diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
}
// Track maximum differential pressure measured (so we can work out top speed).
@@ -183,6 +184,7 @@ ETSAirspeed::collect()
// XXX we may want to smooth out the readings to remove noise.
report.differential_pressure_filtered_pa = (float)diff_pres_pa;
+ report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
report.temperature = -1000.0f;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 6195cd6ea..5342ccf78 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -448,10 +448,10 @@ GPS::print_info()
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
if (_report.timestamp_position != 0) {
- 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("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
+ _report.satellites_visible, (double)(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("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_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);
diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp
index 2360ff39b..3b92f1bf4 100644
--- a/src/drivers/gps/gps_helper.cpp
+++ b/src/drivers/gps/gps_helper.cpp
@@ -56,7 +56,7 @@ GPS_Helper::get_velocity_update_rate()
return _rate_vel;
}
-float
+void
GPS_Helper::reset_update_rates()
{
_rate_count_vel = 0;
@@ -64,7 +64,7 @@ GPS_Helper::reset_update_rates()
_interval_rate_start = hrt_absolute_time();
}
-float
+void
GPS_Helper::store_update_rates()
{
_rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
index cfb9e0d43..d14a95afe 100644
--- a/src/drivers/gps/gps_helper.h
+++ b/src/drivers/gps/gps_helper.h
@@ -46,13 +46,17 @@
class GPS_Helper
{
public:
+
+ GPS_Helper() {};
+ virtual ~GPS_Helper() {};
+
virtual int configure(unsigned &baud) = 0;
virtual int receive(unsigned timeout) = 0;
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();
+ void reset_update_rates();
+ void store_update_rates();
protected:
uint8_t _rate_count_lat_lon;
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 8a2afecb7..7502809bd 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -164,7 +164,7 @@ UBX::configure(unsigned &baudrate)
send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: configuration failed: RATE");
+ warnx("CFG FAIL: RATE");
return 1;
}
@@ -185,7 +185,7 @@ UBX::configure(unsigned &baudrate)
send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
- warnx("ubx: configuration failed: NAV5");
+ warnx("CFG FAIL: NAV5");
return 1;
}
@@ -194,35 +194,42 @@ 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");
+ warnx("MSG CFG FAIL: 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");
+ warnx("MSG CFG FAIL: 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");
+ warnx("MSG CFG FAIL: 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");
+ warnx("MSG CFG FAIL: 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");
+ warnx("MSG CFG FAIL: NAV SVINFO");
+ return 1;
+ }
+
+ configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
+
+ if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+ warnx("MSG CFG FAIL: MON HW");
return 1;
}
@@ -274,7 +281,7 @@ UBX::receive(unsigned timeout)
if (ret < 0) {
/* something went wrong when polling */
- warnx("ubx: poll error");
+ warnx("poll error");
return -1;
} else if (ret == 0) {
@@ -310,7 +317,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");
+ warnx("timeout - no useful messages");
return -1;
}
}
@@ -383,7 +390,7 @@ UBX::parse_char(uint8_t b)
return 1; // message received successfully
} else {
- warnx("ubx: checksum wrong");
+ warnx("checksum wrong");
decode_init();
return -1;
}
@@ -392,7 +399,7 @@ UBX::parse_char(uint8_t b)
_rx_count++;
} else {
- warnx("ubx: buffer full");
+ warnx("buffer full");
decode_init();
return -1;
}
@@ -440,8 +447,8 @@ UBX::handle_message()
gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
_gps_position->fix_type = packet->gpsFix;
- _gps_position->s_variance_m_s = packet->sAcc;
- _gps_position->p_variance_m = packet->pAcc;
+ _gps_position->s_variance_m_s = (float)packet->sAcc * 1e-2f; // from cm/s to m/s
+ _gps_position->p_variance_m = (float)packet->pAcc * 1e-2f; // from cm to m
_gps_position->timestamp_variance = hrt_absolute_time();
ret = 1;
@@ -566,6 +573,24 @@ UBX::handle_message()
break;
}
+ case UBX_CLASS_MON: {
+ switch (_message_id) {
+ case UBX_MESSAGE_MON_HW: {
+
+ struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
+
+ _gps_position->noise_per_ms = p->noisePerMS;
+ _gps_position->jamming_indicator = p->jamInd;
+
+ ret = 1;
+ break;
+ }
+
+ default:
+ break;
+ }
+ }
+
default:
break;
}
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 79a904f4a..5cf47b60b 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -56,6 +56,7 @@
//#define UBX_CLASS_RXM 0x02
#define UBX_CLASS_ACK 0x05
#define UBX_CLASS_CFG 0x06
+#define UBX_CLASS_MON 0x0A
/* MessageIDs (the ones that are used) */
#define UBX_MESSAGE_NAV_POSLLH 0x02
@@ -72,6 +73,8 @@
#define UBX_MESSAGE_CFG_RATE 0x08
#define UBX_MESSAGE_CFG_NAV5 0x24
+#define UBX_MESSAGE_MON_HW 0x09
+
#define UBX_CFG_PRT_LENGTH 20
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
@@ -210,6 +213,27 @@ typedef struct {
uint8_t ck_b;
} gps_bin_nav_velned_packet_t;
+struct gps_bin_mon_hw_packet {
+ uint32_t pinSel;
+ uint32_t pinBank;
+ uint32_t pinDir;
+ uint32_t pinVal;
+ uint16_t noisePerMS;
+ uint16_t agcCnt;
+ uint8_t aStatus;
+ uint8_t aPower;
+ uint8_t flags;
+ uint8_t __reserved1;
+ uint32_t usedMask;
+ uint8_t VP[25];
+ uint8_t jamInd;
+ uint16_t __reserved3;
+ uint32_t pinIrq;
+ uint32_t pulLH;
+ uint32_t pullL;
+};
+
+
//typedef struct {
// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */
// int16_t week; /**< Measurement GPS week number */
@@ -319,7 +343,7 @@ typedef enum {
//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
#pragma pack(pop)
-#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer
+#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer
class UBX : public GPS_Helper
{
@@ -383,7 +407,7 @@ private:
uint8_t _message_class;
uint8_t _message_id;
unsigned _payload_size;
- uint8_t _disable_cmd_last;
+ hrt_abstime _disable_cmd_last;
};
#endif /* UBX_H_ */
diff --git a/src/drivers/pca8574/module.mk b/src/drivers/pca8574/module.mk
new file mode 100644
index 000000000..825ee9bb7
--- /dev/null
+++ b/src/drivers/pca8574/module.mk
@@ -0,0 +1,6 @@
+#
+# PCA8574 driver for RGB LED
+#
+
+MODULE_COMMAND = pca8574
+SRCS = pca8574.cpp
diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp
new file mode 100644
index 000000000..904ce18e8
--- /dev/null
+++ b/src/drivers/pca8574/pca8574.cpp
@@ -0,0 +1,554 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2012-2014 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 pca8574.cpp
+ *
+ * Driver for an 8 I/O controller (PC8574) connected via I2C.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <nuttx/wqueue.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <board_config.h>
+
+#include <drivers/drv_io_expander.h>
+
+#define PCA8574_ONTIME 120
+#define PCA8574_OFFTIME 120
+#define PCA8574_DEVICE_PATH "/dev/pca8574"
+
+#define ADDR 0x20 ///< I2C adress of PCA8574 (default, A0-A2 pulled to GND)
+
+class PCA8574 : public device::I2C
+{
+public:
+ PCA8574(int bus, int pca8574);
+ virtual ~PCA8574();
+
+
+ virtual int init();
+ virtual int probe();
+ virtual int info();
+ virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
+ bool is_running() { return _running; }
+
+private:
+ work_s _work;
+
+ uint8_t _values_out;
+ uint8_t _values_in;
+ uint8_t _blinking;
+ uint8_t _blink_phase;
+
+ enum IOX_MODE _mode;
+ bool _running;
+ int _led_interval;
+ bool _should_run;
+ bool _update_out;
+ int _counter;
+
+ static void led_trampoline(void *arg);
+ void led();
+
+ int send_led_enable(uint8_t arg);
+ int send_led_values();
+
+ int get(uint8_t &vals);
+};
+
+/* for now, we only support one PCA8574 */
+namespace
+{
+PCA8574 *g_pca8574;
+}
+
+void pca8574_usage();
+
+extern "C" __EXPORT int pca8574_main(int argc, char *argv[]);
+
+PCA8574::PCA8574(int bus, int pca8574) :
+ I2C("pca8574", PCA8574_DEVICE_PATH, bus, pca8574, 100000),
+ _values_out(0),
+ _values_in(0),
+ _blinking(0),
+ _blink_phase(0),
+ _mode(IOX_MODE_OFF),
+ _running(false),
+ _led_interval(80),
+ _should_run(false),
+ _update_out(false),
+ _counter(0)
+{
+ memset(&_work, 0, sizeof(_work));
+}
+
+PCA8574::~PCA8574()
+{
+}
+
+int
+PCA8574::init()
+{
+ int ret;
+ ret = I2C::init();
+
+ if (ret != OK) {
+ return ret;
+ }
+
+ return OK;
+}
+
+int
+PCA8574::probe()
+{
+ uint8_t val;
+ return get(val);
+}
+
+int
+PCA8574::info()
+{
+ int ret = OK;
+
+ return ret;
+}
+
+int
+PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ int ret = ENOTTY;
+
+ switch (cmd) {
+ case IOX_SET_VALUE ...(IOX_SET_VALUE + 8): {
+ // set the specified on / off state
+ uint8_t position = (1 << (cmd - IOX_SET_VALUE));
+ uint8_t prev = _values_out;
+
+ if (arg) {
+ _values_out |= position;
+
+ } else {
+ _values_out &= ~(position);
+ }
+
+ if (_values_out != prev) {
+ if (_values_out) {
+ _mode = IOX_MODE_ON;
+ }
+ send_led_values();
+ }
+
+ return OK;
+ }
+
+ case IOX_SET_MASK:
+ send_led_enable(arg);
+ return OK;
+
+ case IOX_GET_MASK: {
+ uint8_t val;
+ ret = get(val);
+
+ if (ret == OK) {
+ return val;
+
+ } else {
+ return -1;
+ }
+ }
+
+ case IOX_SET_MODE:
+
+ if (_mode != (IOX_MODE)arg) {
+
+ switch ((IOX_MODE)arg) {
+ case IOX_MODE_OFF:
+ _values_out = 0xFF;
+ break;
+
+ case IOX_MODE_ON:
+ _values_out = 0;
+ break;
+
+ case IOX_MODE_TEST_OUT:
+ break;
+
+ default:
+ return -1;
+ }
+
+ _mode = (IOX_MODE)arg;
+ send_led_values();
+ }
+
+ return OK;
+
+ default:
+ // see if the parent class can make any use of it
+ ret = CDev::ioctl(filp, cmd, arg);
+ break;
+ }
+
+ return ret;
+}
+
+
+void
+PCA8574::led_trampoline(void *arg)
+{
+ PCA8574 *rgbl = reinterpret_cast<PCA8574 *>(arg);
+
+ rgbl->led();
+}
+
+/**
+ * Main loop function
+ */
+void
+PCA8574::led()
+{
+ if (_mode == IOX_MODE_TEST_OUT) {
+
+ // we count only seven states
+ _counter &= 0xF;
+ _counter++;
+
+ for (int i = 0; i < 8; i++) {
+ if (i < _counter) {
+ _values_out |= (1 << i);
+
+ } else {
+ _values_out &= ~(1 << i);
+ }
+ }
+
+ _update_out = true;
+ _should_run = true;
+ } else if (_mode == IOX_MODE_OFF) {
+ _update_out = true;
+ _should_run = false;
+ } else {
+
+ // Any of the normal modes
+ if (_blinking > 0) {
+ /* we need to be running to blink */
+ _should_run = true;
+ } else {
+ _should_run = false;
+ }
+ }
+
+ if (_update_out) {
+ uint8_t msg;
+
+ if (_blinking) {
+ msg = (_values_out & _blinking & _blink_phase);
+
+ // wipe out all positions that are marked as blinking
+ msg &= ~(_blinking);
+
+ // fill blink positions
+ msg |= ((_blink_phase) ? _blinking : 0);
+
+ _blink_phase = !_blink_phase;
+ } else {
+ msg = _values_out;
+ }
+
+ int ret = transfer(&msg, sizeof(msg), nullptr, 0);
+
+ if (!ret) {
+ _update_out = false;
+ }
+ }
+
+ // check if any activity remains, else stp
+ if (!_should_run) {
+ _running = false;
+ return;
+ }
+
+ // re-queue ourselves to run again later
+ _running = true;
+ work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, _led_interval);
+}
+
+/**
+ * Sent ENABLE flag to LED driver
+ */
+int
+PCA8574::send_led_enable(uint8_t arg)
+{
+
+ int ret = transfer(&arg, sizeof(arg), nullptr, 0);
+
+ return ret;
+}
+
+/**
+ * Send 8 outputs
+ */
+int
+PCA8574::send_led_values()
+{
+ _update_out = true;
+
+ // if not active, kick it
+ if (!_running) {
+ _running = true;
+ work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1);
+ }
+
+ return 0;
+}
+
+int
+PCA8574::get(uint8_t &vals)
+{
+ uint8_t result;
+ int ret;
+
+ ret = transfer(nullptr, 0, &result, 1);
+
+ if (ret == OK) {
+ _values_in = result;
+ vals = result;
+ }
+
+ return ret;
+}
+
+void
+pca8574_usage()
+{
+ warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'val 0 1'");
+ warnx("options:");
+ warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED);
+ warnx(" -a addr (0x%x)", ADDR);
+}
+
+int
+pca8574_main(int argc, char *argv[])
+{
+ int i2cdevice = -1;
+ int pca8574adr = ADDR; // 7bit
+
+ int ch;
+
+ // jump over start/off/etc and look at options first
+ while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
+ switch (ch) {
+ case 'a':
+ pca8574adr = strtol(optarg, NULL, 0);
+ break;
+
+ case 'b':
+ i2cdevice = strtol(optarg, NULL, 0);
+ break;
+
+ default:
+ pca8574_usage();
+ exit(0);
+ }
+ }
+
+ if (optind >= argc) {
+ pca8574_usage();
+ exit(1);
+ }
+
+ const char *verb = argv[optind];
+
+ int fd;
+ int ret;
+
+ if (!strcmp(verb, "start")) {
+ if (g_pca8574 != nullptr) {
+ errx(1, "already started");
+ }
+
+ if (i2cdevice == -1) {
+ // try the external bus first
+ i2cdevice = PX4_I2C_BUS_EXPANSION;
+ g_pca8574 = new PCA8574(PX4_I2C_BUS_EXPANSION, pca8574adr);
+
+ if (g_pca8574 != nullptr && OK != g_pca8574->init()) {
+ delete g_pca8574;
+ g_pca8574 = nullptr;
+ }
+
+ if (g_pca8574 == nullptr) {
+ // fall back to default bus
+ if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
+ errx(1, "init failed");
+ }
+
+ i2cdevice = PX4_I2C_BUS_LED;
+ }
+ }
+
+ if (g_pca8574 == nullptr) {
+ g_pca8574 = new PCA8574(i2cdevice, pca8574adr);
+
+ if (g_pca8574 == nullptr) {
+ errx(1, "new failed");
+ }
+
+ if (OK != g_pca8574->init()) {
+ delete g_pca8574;
+ g_pca8574 = nullptr;
+ errx(1, "init failed");
+ }
+ }
+
+ exit(0);
+ }
+
+ // need the driver past this point
+ if (g_pca8574 == nullptr) {
+ warnx("not started, run pca8574 start");
+ exit(1);
+ }
+
+ if (!strcmp(verb, "test")) {
+ fd = open(PCA8574_DEVICE_PATH, 0);
+
+ if (fd == -1) {
+ errx(1, "Unable to open " PCA8574_DEVICE_PATH);
+ }
+
+ ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT);
+
+ close(fd);
+ exit(ret);
+ }
+
+ if (!strcmp(verb, "info")) {
+ g_pca8574->info();
+ exit(0);
+ }
+
+ if (!strcmp(verb, "off")) {
+ fd = open(PCA8574_DEVICE_PATH, 0);
+
+ if (fd < 0) {
+ errx(1, "Unable to open " PCA8574_DEVICE_PATH);
+ }
+
+ ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF);
+ close(fd);
+ exit(ret);
+ }
+
+ if (!strcmp(verb, "stop")) {
+ fd = open(PCA8574_DEVICE_PATH, 0);
+
+ if (fd == -1) {
+ errx(1, "Unable to open " PCA8574_DEVICE_PATH);
+ }
+
+ ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF);
+ close(fd);
+
+ // wait until we're not running any more
+ for (unsigned i = 0; i < 15; i++) {
+ if (!g_pca8574->is_running()) {
+ break;
+ }
+
+ usleep(50000);
+ printf(".");
+ fflush(stdout);
+ }
+ printf("\n");
+ fflush(stdout);
+
+ if (!g_pca8574->is_running()) {
+ delete g_pca8574;
+ g_pca8574 = nullptr;
+ exit(0);
+ } else {
+ warnx("stop failed.");
+ exit(1);
+ }
+ }
+
+ if (!strcmp(verb, "val")) {
+ if (argc < 4) {
+ errx(1, "Usage: pca8574 val <channel> <0 or 1>");
+ }
+
+ fd = open(PCA8574_DEVICE_PATH, 0);
+
+ if (fd == -1) {
+ errx(1, "Unable to open " PCA8574_DEVICE_PATH);
+ }
+
+ unsigned channel = strtol(argv[2], NULL, 0);
+ unsigned val = strtol(argv[3], NULL, 0);
+
+ if (channel < 8) {
+ ret = ioctl(fd, (IOX_SET_VALUE + channel), val);
+ } else {
+ ret = -1;
+ }
+ close(fd);
+ exit(ret);
+ }
+
+ pca8574_usage();
+ exit(0);
+}
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index fd69cf795..8a4bfa18c 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -648,11 +648,9 @@ PX4FMU::task_main()
/* iterate actuators */
for (unsigned 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) {
+ /* last resort: catch NaN and INF */
+ if ((i >= outputs.noutputs) ||
+ !isfinite(outputs.output[i])) {
/*
* 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
@@ -664,6 +662,7 @@ PX4FMU::task_main()
uint16_t pwm_limited[num_outputs];
+ /* the PWM limit call takes care of out of band errors and constrains */
pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
/* output to the servos */
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 4099e5522..972f45148 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -581,8 +581,10 @@ PX4IO::init()
ASSERT(_task == -1);
sys_restart_param = param_find("SYS_RESTART_TYPE");
- /* Indicate restart type is unknown */
- param_set(sys_restart_param, &sys_restart_val);
+ if (sys_restart_param != PARAM_INVALID) {
+ /* Indicate restart type is unknown */
+ param_set(sys_restart_param, &sys_restart_val);
+ }
/* do regular cdev init */
ret = CDev::init();
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-16 09:45:54 +0000