AP_HAL_Linux: SPIUARTDriver: add some cosmetic changes

Mainly changes to follow the coding style and reduce the complexity a
little bit.

libraries/AP_HAL_Linux/SPIUARTDriver.cpp

@@ -1,18 +1,16 @@
 #include "SPIUARTDriver.h"
 
+#include <assert.h>
 #include <stdlib.h>
 #include <cstdio>
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/utility/RingBuffer.h>
 
-extern const AP_HAL::HAL& hal;
+extern const AP_HAL::HAL &hal;
 
-#define SPIUART_DEBUG 0
-
-#include <cassert>
-
-#if SPIUART_DEBUG
+#ifdef SPIUART_DEBUG
+#include <stdio.h>
 #define debug(fmt, args ...)  do {hal.console->printf("[SPIUARTDriver]: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
 #define error(fmt, args ...)  do {fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
 #else
@@ -22,43 +20,21 @@ extern const AP_HAL::HAL& hal;
 
 using namespace Linux;
 
-SPIUARTDriver::SPIUARTDriver() :
-    UARTDriver(false),
-    _dev(nullptr),
-    _last_update_timestamp(0),
-    _buffer(NULL),
-    _external(false)
-{
-    _readbuf = NULL;
-    _writebuf = NULL;
-}
-
-bool SPIUARTDriver::sem_take_nonblocking()
+SPIUARTDriver::SPIUARTDriver()
+    : UARTDriver(false)
 {
-    return _dev->get_semaphore()->take_nonblocking();
-}
-
-void SPIUARTDriver::sem_give()
-{
-    _dev->get_semaphore()->give();
 }
 
 void SPIUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
 {
     if (device_path != NULL) {
-        UARTDriver::begin(b,rxS,txS);
-        if ( is_initialized()) {
+        UARTDriver::begin(b, rxS, txS);
+        if (is_initialized()) {
             _external = true;
             return;
         }
     }
 
-   if (rxS < 1024) {
-       rxS = 2048;
-   }
-   if (txS < 1024) {
-       txS = 2048;
-   }
     if (!is_initialized()) {
         _dev = hal.spi->get_device("ublox");
         if (!_dev) {
@@ -66,65 +42,70 @@ void SPIUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
         }
     }
 
-   /*
+    if (rxS < 1024) {
+        rxS = 2048;
+    }
+    if (txS < 1024) {
+        txS = 2048;
+    }
+
+    /*
      allocate the read buffer
    */
-   if (rxS != 0 && rxS != _readbuf_size) {
-       _readbuf_size = rxS;
-       if (_readbuf != NULL) {
-           free(_readbuf);
-       }
-       _readbuf = (uint8_t *)malloc(_readbuf_size);
-       _readbuf_head = 0;
-       _readbuf_tail = 0;
-   }
-
-   /*
+    if (rxS != 0 && rxS != _readbuf_size) {
+        _readbuf_size = rxS;
+        if (_readbuf != NULL) {
+            free(_readbuf);
+        }
+        _readbuf = (uint8_t *)malloc(_readbuf_size);
+        _readbuf_head = 0;
+        _readbuf_tail = 0;
+    }
+
+    /*
      allocate the write buffer
    */
-   if (txS != 0 && txS != _writebuf_size) {
-       _writebuf_size = txS;
-       if (_writebuf != NULL) {
-           free(_writebuf);
-       }
-       _writebuf = (uint8_t *)malloc(_writebuf_size);
-       _writebuf_head = 0;
-       _writebuf_tail = 0;
-   }
-
-   if (_buffer == NULL) {
-       /* Do not allocate new buffer, if we're just changing speed */
-       _buffer = new uint8_t[rxS];
-       if (_buffer == NULL) {
-           hal.console->printf("Not enough memory\n");
-           AP_HAL::panic("Not enough memory\n");
-       }
-   }
-
-   _dev = hal.spi->get_device("ublox");
-
-   switch (b) {
-       case 4000000U:
-           if (is_initialized()) {
-               /* Do not allow speed changes before device is initialized, because
+    if (txS != 0 && txS != _writebuf_size) {
+        _writebuf_size = txS;
+        if (_writebuf != NULL) {
+            free(_writebuf);
+        }
+        _writebuf = (uint8_t *)malloc(_writebuf_size);
+        _writebuf_head = 0;
+        _writebuf_tail = 0;
+    }
+
+    if (_buffer == NULL) {
+        /* Do not allocate new buffer, if we're just changing speed */
+        _buffer = new uint8_t[rxS];
+        if (_buffer == NULL) {
+            hal.console->printf("Not enough memory\n");
+            AP_HAL::panic("Not enough memory\n");
+        }
+    }
+
+    switch (b) {
+    case 4000000U:
+        if (is_initialized()) {
+            /* Do not allow speed changes before device is initialized, because
                 * it can lead to misconfiguraration. Once the device is initialized,
                 * it's sage to update speed
                 */
-               _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
-               debug("Set higher SPI-frequency");
-           } else {
-               _dev->set_speed(AP_HAL::Device::SPEED_LOW);
-               debug("Set lower SPI-frequency");
-           }
-           break;
-       default:
-           _dev->set_speed(AP_HAL::Device::SPEED_LOW);
-           debug("Set lower SPI-frequency");
-           debug("%s: wrong baudrate (%u) for SPI-driven device. setting default speed", __func__, b);
-           break;
-   }
-
-   _initialised = true;
+            _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
+            debug("Set higher SPI-frequency");
+        } else {
+            _dev->set_speed(AP_HAL::Device::SPEED_LOW);
+            debug("Set lower SPI-frequency");
+        }
+        break;
+    default:
+        _dev->set_speed(AP_HAL::Device::SPEED_LOW);
+        debug("Set lower SPI-frequency");
+        debug("%s: wrong baudrate (%u) for SPI-driven device. setting default speed", __func__, b);
+        break;
+    }
+
+    _initialised = true;
 }
 
 int SPIUARTDriver::_write_fd(const uint8_t *buf, uint16_t size)
@@ -133,13 +114,13 @@ int SPIUARTDriver::_write_fd(const uint8_t *buf, uint16_t size)
         return UARTDriver::_write_fd(buf,size);
     }
 
-    if (!sem_take_nonblocking()) {
+    if (!_dev->get_semaphore()->take_nonblocking()) {
         return 0;
     }
 
     _dev->transfer_fullduplex(buf, _buffer, size);
 
-    sem_give();
+    _dev->get_semaphore()->give();
 
     BUF_ADVANCEHEAD(_writebuf, size);
 
@@ -186,9 +167,7 @@ int SPIUARTDriver::_write_fd(const uint8_t *buf, uint16_t size)
         BUF_ADVANCETAIL(_readbuf, n);
     }
 
-
     return ret;
-
 }
 
 static uint8_t ff_stub[300] = {0xff};
@@ -199,23 +178,21 @@ int SPIUARTDriver::_read_fd(uint8_t *buf, uint16_t n)
         return UARTDriver::_read_fd(buf, n);
     }
 
-    if (!sem_take_nonblocking()) {
-        return 0;
-    }
-
     /* Make SPI transactions shorter. It can save SPI bus from keeping too
      * long. It's essential for NavIO as MPU9250 is on the same bus and
      * doesn't like to be waiting. Making transactions more frequent but shorter
      * is a win.
      */
-
     if (n > 100) {
         n = 100;
     }
 
-    _dev->transfer_fullduplex(ff_stub, buf, n);
+    if (!_dev->get_semaphore()->take_nonblocking()) {
+        return 0;
+    }
 
-    sem_give();
+    _dev->transfer_fullduplex(ff_stub, buf, n);
+    _dev->get_semaphore()->give();
 
     BUF_ADVANCETAIL(_readbuf, n);
 
@@ -228,6 +205,7 @@ void SPIUARTDriver::_timer_tick(void)
         UARTDriver::_timer_tick();
         return;
     }
+
     /* lower the update rate */
     if (AP_HAL::micros() - _last_update_timestamp < 10000) {
         return;

libraries/AP_HAL_Linux/SPIUARTDriver.h

@@ -15,14 +15,11 @@ protected:
     int _write_fd(const uint8_t *buf, uint16_t n);
     int _read_fd(uint8_t *buf, uint16_t n);
 
-private:
-    bool sem_take_nonblocking();
-    void sem_give();
-
     AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
 
+    uint8_t *_buffer;
+
     uint32_t _last_update_timestamp;
 
-    uint8_t *_buffer;
     bool _external;
 };