aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.mc_defaults3
-rw-r--r--ROMFS/px4fmu_common/init.d/rcS90
-rw-r--r--makefiles/config_px4fmu-v2_default.mk1
-rw-r--r--makefiles/config_px4fmu-v2_test.mk1
-rw-r--r--src/drivers/drv_io_expander.h71
-rw-r--r--src/drivers/gps/ubx.cpp4
-rw-r--r--src/drivers/pca8574/module.mk6
-rw-r--r--src/drivers/pca8574/pca8574.cpp554
-rw-r--r--src/modules/commander/commander.cpp2
-rw-r--r--src/modules/mavlink/mavlink_main.cpp45
-rw-r--r--src/modules/navigator/navigator_main.cpp42
-rw-r--r--src/modules/uORB/topics/actuator_armed.h20
12 files changed, 749 insertions, 90 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults
index b6b0a5b4e..127e42164 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults
@@ -37,10 +37,11 @@ then
param set MPC_LAND_SPEED 1.0
param set PE_VELNE_NOISE 0.5
- param set PE_VELNE_NOISE 0.7
+ param set PE_VELD_NOISE 0.7
param set PE_POSNE_NOISE 0.5
param set PE_POSD_NOISE 1.0
+ param set NAV_ACCEPT_RAD 2.0
fi
set PWM_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 8c413e087..6d06f897a 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -65,12 +65,12 @@ then
# Start CDC/ACM serial driver
#
sercon
-
+
#
# Start the ORB (first app to start)
#
uorb start
-
+
#
# Load parameters
#
@@ -79,7 +79,7 @@ then
then
set PARAM_FILE /fs/mtd_params
fi
-
+
param select $PARAM_FILE
if param load
then
@@ -87,21 +87,25 @@ then
else
echo "[init] ERROR: Params loading failed: $PARAM_FILE"
fi
-
+
#
# Start system state indicator
#
if rgbled start
then
- echo "[init] Using external RGB Led"
+ echo "[init] RGB Led"
else
if blinkm start
then
- echo "[init] Using blinkm"
+ echo "[init] BlinkM"
blinkm systemstate
fi
fi
-
+
+ if pca8574 start
+ then
+ fi
+
#
# Set default values
#
@@ -122,7 +126,7 @@ then
set LOAD_DEFAULT_APPS yes
set GPS yes
set GPS_FAKE no
-
+
#
# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
#
@@ -132,7 +136,7 @@ then
else
set DO_AUTOCONFIG no
fi
-
+
#
# Set USE_IO flag
#
@@ -142,7 +146,7 @@ then
else
set USE_IO no
fi
-
+
#
# Set parameters and env variables for selected AUTOSTART
#
@@ -172,9 +176,9 @@ then
param set SYS_AUTOCONFIG 0
param save
fi
-
+
set IO_PRESENT no
-
+
if [ $USE_IO == yes ]
then
#
@@ -186,19 +190,19 @@ then
else
set IO_FILE /etc/extras/px4io-v1_default.bin
fi
-
+
if px4io checkcrc $IO_FILE
then
echo "[init] PX4IO CRC OK"
echo "PX4IO CRC OK" >> $LOG_FILE
-
+
set IO_PRESENT yes
else
echo "[init] Trying to update"
echo "PX4IO Trying to update" >> $LOG_FILE
-
+
tone_alarm MLL32CP8MB
-
+
if px4io forceupdate 14662 $IO_FILE
then
usleep 500000
@@ -207,7 +211,7 @@ then
echo "[init] PX4IO CRC OK, update successful"
echo "PX4IO CRC OK after updating" >> $LOG_FILE
tone_alarm MLL8CDE
-
+
set IO_PRESENT yes
else
echo "[init] ERROR: PX4IO update failed"
@@ -220,14 +224,14 @@ then
tone_alarm $TUNE_OUT_ERROR
fi
fi
-
+
if [ $IO_PRESENT == no ]
then
echo "[init] ERROR: PX4IO not found"
tone_alarm $TUNE_OUT_ERROR
fi
fi
-
+
#
# Set default output if not set
#
@@ -246,7 +250,7 @@ then
# Need IO for output but it not present, disable output
set OUTPUT_MODE none
echo "[init] ERROR: PX4IO not found, disabling output"
-
+
# Avoid using ttyS0 for MAVLink on FMUv1
if ver hwcmp PX4FMU_V1
then
@@ -270,17 +274,17 @@ then
# Try to get an USB console
nshterm /dev/ttyACM0 &
-
+
#
# Start the Commander (needs to be this early for in-air-restarts)
#
commander start
-
+
#
# Start primary output
#
set TTYS1_BUSY no
-
+
# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
if [ $OUTPUT_MODE != none ]
then
@@ -296,7 +300,7 @@ then
tone_alarm $TUNE_OUT_ERROR
fi
fi
-
+
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
then
echo "[init] Use FMU as primary output"
@@ -307,7 +311,7 @@ then
echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
tone_alarm $TUNE_OUT_ERROR
fi
-
+
if ver hwcmp PX4FMU_V1
then
if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
@@ -320,7 +324,7 @@ then
fi
fi
fi
-
+
if [ $OUTPUT_MODE == mkblctrl ]
then
echo "[init] Use MKBLCTRL as primary output"
@@ -333,7 +337,7 @@ then
then
set MKBLCTRL_ARG "-mkmode +"
fi
-
+
if mkblctrl $MKBLCTRL_ARG
then
echo "[init] MKBLCTRL started"
@@ -341,9 +345,9 @@ then
echo "[init] ERROR: MKBLCTRL start failed"
tone_alarm $TUNE_OUT_ERROR
fi
-
+
fi
-
+
if [ $OUTPUT_MODE == hil ]
then
echo "[init] Use HIL as primary output"
@@ -355,7 +359,7 @@ then
tone_alarm $TUNE_OUT_ERROR
fi
fi
-
+
#
# Start IO or FMU for RC PPM input if needed
#
@@ -382,7 +386,7 @@ then
echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
tone_alarm $TUNE_OUT_ERROR
fi
-
+
if ver hwcmp PX4FMU_V1
then
if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
@@ -397,7 +401,7 @@ then
fi
fi
fi
-
+
#
# MAVLink
#
@@ -418,7 +422,7 @@ then
fi
mavlink start $MAVLINK_FLAGS
-
+
#
# Sensors, Logging, GPS
#
@@ -429,7 +433,7 @@ then
echo "[init] Start logging"
sh /etc/init.d/rc.logging
fi
-
+
if [ $GPS == yes ]
then
echo "[init] Start GPS"
@@ -439,7 +443,7 @@ then
gps start -f
else
gps start
- fi
+ fi
fi
#
@@ -456,24 +460,24 @@ then
if [ $VEHICLE_TYPE == fw ]
then
echo "[init] Vehicle type: FIXED WING"
-
+
if [ $MIXER == none ]
then
# Set default mixer for fixed wing if not defined
set MIXER FMU_AERT
fi
-
+
if [ $MAV_TYPE == none ]
then
# Use MAV_TYPE = 1 (fixed wing) if not defined
set MAV_TYPE 1
fi
-
+
param set MAV_TYPE $MAV_TYPE
-
+
# Load mixer and configure outputs
sh /etc/init.d/rc.interface
-
+
# Start standard fixedwing apps
if [ $LOAD_DEFAULT_APPS == yes ]
then
@@ -521,7 +525,7 @@ then
set MAV_TYPE 14
fi
fi
-
+
# Still no MAV_TYPE found
if [ $MAV_TYPE == none ]
then
@@ -529,10 +533,10 @@ then
else
param set MAV_TYPE $MAV_TYPE
fi
-
+
# Load mixer and configure outputs
sh /etc/init.d/rc.interface
-
+
# Start standard multicopter apps
if [ $LOAD_DEFAULT_APPS == yes ]
then
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index d6e88859c..a68cc32f4 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -40,6 +40,7 @@ MODULES += drivers/meas_airspeed
MODULES += drivers/frsky_telemetry
MODULES += modules/sensors
MODULES += drivers/mkblctrl
+MODULES += drivers/pca8574
# Needs to be burned to the ground and re-written; for now,
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 84274bf75..66b2157ed 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -24,6 +24,7 @@ MODULES += drivers/lsm303d
MODULES += drivers/l3gd20
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
+MODULES += drivers/pca8574
MODULES += systemcmds/perf
MODULES += systemcmds/reboot
MODULES += systemcmds/tests
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/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 19cf5beec..7502809bd 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -447,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;
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/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 5c0628a16..65922b2a5 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1408,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[])
home.alt = global_position.alt;
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
- mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
+ mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
/* announce new home position */
if (home_pub > 0) {
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index bb1ad86ef..304a02c29 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -105,7 +105,8 @@ static struct file_operations fops;
*/
extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
-static uint64_t last_write_times[6] = {0};
+static uint64_t last_write_success_times[6] = {0};
+static uint64_t last_write_try_times[6] = {0};
/*
* Internal function to send the bytes through the right serial port
@@ -166,38 +167,40 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
if (instance->get_flow_control_enabled()
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
- if (buf_free == 0) {
-
- if (last_write_times[(unsigned)channel] != 0 &&
- hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
-
- warnx("DISABLING HARDWARE FLOW CONTROL");
- instance->enable_flow_control(false);
- }
-
- } else {
-
- /* apparently there is space left, although we might be
- * partially overflooding the buffer already */
- last_write_times[(unsigned)channel] = hrt_absolute_time();
+ /* Disable hardware flow control:
+ * if no successful write since a defined time
+ * and if the last try was not the last successful write
+ */
+ if (last_write_try_times[(unsigned)channel] != 0 &&
+ hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
+ last_write_success_times[(unsigned)channel] !=
+ last_write_try_times[(unsigned)channel])
+ {
+ warnx("DISABLING HARDWARE FLOW CONTROL");
+ instance->enable_flow_control(false);
}
+
}
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (instance->should_transmit()) {
+ last_write_try_times[(unsigned)channel] = hrt_absolute_time();
/* check if there is space in the buffer, let it overflow else */
if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
- if (desired > buf_free) {
- desired = buf_free;
+ if (buf_free < desired) {
+ /* we don't want to send anything just in half, so return */
+ return;
}
}
ssize_t ret = write(uart, ch, desired);
if (ret != desired) {
warnx("TX FAIL");
+ } else {
+ last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
}
}
@@ -222,6 +225,8 @@ Mavlink::Mavlink() :
_subscriptions(nullptr),
_streams(nullptr),
_mission_pub(-1),
+ _mode(MAVLINK_MODE_NORMAL),
+ _total_counter(0),
_verbose(false),
_forwarding_on(false),
_passing_on(false),
@@ -891,6 +896,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
+ mission_item->time_inside = mavlink_mission_item->param1;
break;
}
@@ -899,7 +905,6 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
- mission_item->time_inside = mavlink_mission_item->param1;
mission_item->autocontinue = mavlink_mission_item->autocontinue;
// mission_item->index = mavlink_mission_item->seq;
mission_item->origin = ORIGIN_MAVLINK;
@@ -918,11 +923,12 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
switch (mission_item->nav_cmd) {
case NAV_CMD_TAKEOFF:
- mavlink_mission_item->param2 = mission_item->pitch_min;
+ mavlink_mission_item->param1 = mission_item->pitch_min;
break;
default:
mavlink_mission_item->param2 = mission_item->acceptance_radius;
+ mavlink_mission_item->param1 = mission_item->time_inside;
break;
}
@@ -933,7 +939,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
mavlink_mission_item->command = mission_item->nav_cmd;
- mavlink_mission_item->param1 = mission_item->time_inside;
mavlink_mission_item->autocontinue = mission_item->autocontinue;
// mavlink_mission_item->seq = mission_item->index;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 401d50f7e..06e0c5904 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1458,7 +1458,6 @@ Navigator::check_mission_item_reached()
/* XXX TODO count turns */
if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
- _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
_mission_item.loiter_radius > 0.01f) {
@@ -1477,27 +1476,27 @@ Navigator::check_mission_item_reached()
acceptance_radius = _parameters.acceptance_radius;
}
- float dist = -1.0f;
- float dist_xy = -1.0f;
- float dist_z = -1.0f;
-
- /* calculate AMSL altitude for this waypoint */
- float wp_alt_amsl = _mission_item.altitude;
-
- if (_mission_item.altitude_is_relative)
- wp_alt_amsl += _home_pos.alt;
-
- dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
- (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
- &dist_xy, &dist_z);
-
if (_do_takeoff) {
- if (_global_pos.alt > wp_alt_amsl - acceptance_radius) {
- /* require only altitude for takeoff */
+ /* require only altitude for takeoff */
+ if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) {
_waypoint_position_reached = true;
}
} else {
+ float dist = -1.0f;
+ float dist_xy = -1.0f;
+ float dist_z = -1.0f;
+
+ /* calculate AMSL altitude for this waypoint */
+ float wp_alt_amsl = _mission_item.altitude;
+
+ if (_mission_item.altitude_is_relative)
+ wp_alt_amsl += _home_pos.alt;
+
+ dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
+ (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
+ &dist_xy, &dist_z);
+
if (dist >= 0.0f && dist <= acceptance_radius) {
_waypoint_position_reached = true;
}
@@ -1567,7 +1566,14 @@ Navigator::on_mission_item_reached()
}
if (_mission.current_mission_available()) {
- set_mission_item();
+ if (_mission_item.autocontinue) {
+ /* continue mission */
+ set_mission_item();
+
+ } else {
+ /* autocontinue disabled for this item */
+ request_loiter_or_ready();
+ }
} else {
/* if no more mission items available then finish mission */
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h
index 6e944ffee..a98d3fc3a 100644
--- a/src/modules/uORB/topics/actuator_armed.h
+++ b/src/modules/uORB/topics/actuator_armed.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * 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
@@ -44,15 +44,25 @@
#include <stdint.h>
#include "../uORB.h"
+/**
+ * @addtogroup topics
+ * @{
+ */
+
/** global 'actuator output is live' control. */
struct actuator_armed_s {
- uint64_t timestamp;
- bool armed; /**< Set to true if system is armed */
- bool ready_to_arm; /**< Set to true if system is ready to be armed */
- bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
+ uint64_t timestamp; /**< Microseconds since system boot */
+ bool armed; /**< Set to true if system is armed */
+ bool ready_to_arm; /**< Set to true if system is ready to be armed */
+ bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
};
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
ORB_DECLARE(actuator_armed);
#endif \ No newline at end of file
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-21 09:05:56 +0000