HAL_PX4: try to reopen UARTs if safety switch is disarmed

this allows attaching to the USB port after power on

Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>

libraries/AP_HAL_PX4/UARTDriver.cpp

@@ -23,8 +23,13 @@ extern const AP_HAL::HAL& hal;
 
 PX4UARTDriver::PX4UARTDriver(const char *devpath, const char *perf_name) :
 	_devpath(devpath),
-    _perf_uart(perf_alloc(PC_ELAPSED, perf_name))
-{}
+    _fd(-1),
+    _baudrate(57600),
+    _perf_uart(perf_alloc(PC_ELAPSED, perf_name)),
+    _initialised(false),
+    _in_timer(false)
+{
+}
 
 
 extern const AP_HAL::HAL& hal;
@@ -35,58 +40,28 @@ extern const AP_HAL::HAL& hal;
 
 void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) 
 {
-	if (!_initialised) {
-        uint8_t retries = 0;
-        while (retries < 5) {
-            _fd = open(_devpath, O_RDWR);
-            if (_fd != -1) {
-                break;
-            }
-            // sleep a bit and retry. There seems to be a NuttX bug
-            // that can cause ttyACM0 to not be available immediately,
-            // but a small delay can fix it
-            hal.scheduler->delay(100);
-            retries++;
-        }
-		if (_fd == -1) {
-			fprintf(stdout, "Failed to open UART device %s - %s\n",
-				_devpath, strerror(errno));
-			return;
-		}
-		if (retries != 0) {
-			fprintf(stdout, "WARNING: took %u retries to open UART %s\n", 
-                    (unsigned)retries, _devpath);
-			return;
-		}
-
-        if (rxS == 0) {
-            rxS = 128;
-        }
-        // on PX4 we have enough memory to have a larger transmit
-        // buffer for all ports. This means we don't get delays while
-        // waiting to write GPS config packets
-        if (txS < 512) {
-            txS = 512;
-        }
-	}
-
-    _initialised = false;
-    while (_in_timer) hal.scheduler->delay(1);
-
-	if (b != 0) {
-		// set the baud rate
-		struct termios t;
-		tcgetattr(_fd, &t);
-		cfsetspeed(&t, b);
-		// disable LF -> CR/LF
-		t.c_oflag &= ~ONLCR;
-		tcsetattr(_fd, TCSANOW, &t);
-	}
+    // on PX4 we have enough memory to have a larger transmit and
+    // receive buffer for all ports. This means we don't get delays
+    // while waiting to write GPS config packets
+    if (txS < 512) {
+        txS = 512;
+    }
+    if (rxS < 512) {
+        rxS = 512;
+    }
 
     /*
       allocate the read buffer
+      we allocate buffers before we successfully open the device as we
+      want to allocate in the early stages of boot, and cause minimum
+      thrashing of the heap once we are up. The ttyACM0 driver may not
+      connect for some time after boot
      */
 	if (rxS != 0 && rxS != _readbuf_size) {
+        _initialised = false;
+        while (_in_timer) {
+            hal.scheduler->delay(1);
+        }
 		_readbuf_size = rxS;
 		if (_readbuf != NULL) {
 			free(_readbuf);
@@ -96,10 +71,18 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
 		_readbuf_tail = 0;
 	}
 
+    if (b != 0) {
+        _baudrate = b;
+    }
+
     /*
       allocate the write buffer
      */
 	if (txS != 0 && txS != _writebuf_size) {
+        _initialised = false;
+        while (_in_timer) {
+            hal.scheduler->delay(1);
+        }
 		_writebuf_size = txS;
 		if (_writebuf != NULL) {
 			free(_writebuf);
@@ -109,7 +92,24 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
 		_writebuf_tail = 0;
 	}
 
-    if (_writebuf_size != 0 && _readbuf_size != 0) {
+	if (_fd == -1) {
+        _fd = open(_devpath, O_RDWR);
+		if (_fd == -1) {
+			return;
+		}
+	}
+
+	if (_baudrate != 0) {
+		// set the baud rate
+		struct termios t;
+		tcgetattr(_fd, &t);
+		cfsetspeed(&t, _baudrate);
+		// disable LF -> CR/LF
+		t.c_oflag &= ~ONLCR;
+		tcsetattr(_fd, TCSANOW, &t);
+	}
+
+    if (_writebuf_size != 0 && _readbuf_size != 0 && _fd != -1) {
         _initialised = true;
     }
 }
@@ -120,6 +120,27 @@ void PX4UARTDriver::begin(uint32_t b)
 }
 
 
+/*
+  try to initialise the UART. This is used to cope with the way NuttX
+  handles /dev/ttyACM0 (the USB port). The port appears in /dev on
+  boot, but cannot be opened until a USB cable is connected and the
+  host starts the CDCACM communication.
+ */
+void PX4UARTDriver::try_initialise(void)
+{
+    if (_initialised) {
+        return;
+    }
+    if ((hal.scheduler->millis() - _last_initialise_attempt_ms) < 2000) {
+        return;
+    }
+    _last_initialise_attempt_ms = hal.scheduler->millis();
+    if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED) {
+        begin(0);
+    }
+}
+
+
 void PX4UARTDriver::end() 
 {
     _initialised = false;
@@ -147,6 +168,7 @@ void PX4UARTDriver::flush() {}
 
 bool PX4UARTDriver::is_initialized() 
 { 
+    try_initialise();
     return _initialised; 
 }
 
@@ -172,6 +194,7 @@ bool PX4UARTDriver::tx_pending() { return false; }
 int16_t PX4UARTDriver::available() 
 { 
 	if (!_initialised) {
+        try_initialise();
 		return 0;
 	}
     uint16_t _tail;
@@ -184,6 +207,7 @@ int16_t PX4UARTDriver::available()
 int16_t PX4UARTDriver::txspace() 
 { 
 	if (!_initialised) {
+        try_initialise();
 		return 0;
 	}
     uint16_t _head;
@@ -196,7 +220,11 @@ int16_t PX4UARTDriver::txspace()
 int16_t PX4UARTDriver::read() 
 { 
 	uint8_t c;
-	if (!_initialised || _readbuf == NULL) {
+    if (!_initialised) {
+        try_initialise();
+        return -1;
+    }
+	if (_readbuf == NULL) {
 		return -1;
 	}
     if (BUF_EMPTY(_readbuf)) {
@@ -213,6 +241,7 @@ int16_t PX4UARTDriver::read()
 size_t PX4UARTDriver::write(uint8_t c) 
 { 
     if (!_initialised) {
+        try_initialise();
         return 0;
     }
     if (hal.scheduler->in_timerprocess()) {
@@ -238,6 +267,7 @@ size_t PX4UARTDriver::write(uint8_t c)
 size_t PX4UARTDriver::write(const uint8_t *buffer, size_t size)
 {
 	if (!_initialised) {
+        try_initialise();
 		return 0;
 	}
     if (hal.scheduler->in_timerprocess()) {

libraries/AP_HAL_PX4/UARTDriver.h

@@ -26,9 +26,6 @@ public:
     size_t write(uint8_t c);
     size_t write(const uint8_t *buffer, size_t size);
 
-    volatile bool _initialised;
-    volatile bool _in_timer;
-
     void set_device_path(const char *path) {
 	    _devpath = path;
     }
@@ -42,6 +39,9 @@ public:
 private:
     const char *_devpath;
     int _fd;
+    uint32_t _baudrate;
+    volatile bool _initialised;
+    volatile bool _in_timer;
 
     bool _nonblocking_writes;
 
@@ -64,6 +64,9 @@ private:
     int _write_fd(const uint8_t *buf, uint16_t n);
     int _read_fd(uint8_t *buf, uint16_t n);
     uint64_t _last_write_time;
+
+    void try_initialise(void);
+    uint32_t _last_initialise_attempt_ms;
 };
 
 #endif // __AP_HAL_PX4_UARTDRIVER_H__

libraries/AP_HAL_PX4/Util.cpp

@@ -9,6 +9,8 @@
 #include <apps/nsh.h>
 #include <fcntl.h>
 #include "UARTDriver.h"
+#include <uORB/uORB.h>
+#include <uORB/topics/safety.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -17,6 +19,15 @@ using namespace PX4;
 
 extern bool _px4_thread_should_exit;
 
+/*
+  constructor
+ */
+PX4Util::PX4Util(void) 
+{
+    _safety_handle = orb_subscribe(ORB_ID(safety));
+}
+
+
 /*
   start an instance of nsh
  */
@@ -54,4 +65,28 @@ bool PX4Util::run_debug_shell(AP_HAL::BetterStream *stream)
 	return true;
 }
 
+/*
+  return state of safety switch
+ */
+enum PX4Util::safety_state PX4Util::safety_switch_state(void)
+{
+    if (_safety_handle == -1) {
+        _safety_handle = orb_subscribe(ORB_ID(safety));
+    }
+    if (_safety_handle == -1) {
+        return AP_HAL::Util::SAFETY_NONE;
+    }
+    struct safety_s safety;
+    if (orb_copy(ORB_ID(safety), _safety_handle, &safety) != OK) {
+        return AP_HAL::Util::SAFETY_NONE;
+    }
+    if (!safety.safety_switch_available) {
+        return AP_HAL::Util::SAFETY_NONE;
+    }
+    if (safety.safety_off) {
+        return AP_HAL::Util::SAFETY_ARMED;
+    }
+    return AP_HAL::Util::SAFETY_DISARMED;
+}
+
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4

libraries/AP_HAL_PX4/Util.h

@@ -7,7 +7,13 @@
 
 class PX4::PX4Util : public AP_HAL::Util {
 public:
+    PX4Util(void);
     bool run_debug_shell(AP_HAL::BetterStream *stream);
+
+    enum safety_state safety_switch_state(void);
+
+private:
+    int _safety_handle;
 };
 
 #endif // __AP_HAL_PX4_UTIL_H__