Merge pull request #535 from benlaurie/tidy

AR Drone2 Tidy up

fix warnings and correct some potential and actual problems

sw/airborne/boards/ardrone/actuators_ardrone2_raw.c

@@ -33,6 +33,7 @@
 #include "mcu_periph/gpio.h"
 #include "led_hw.h"
 #include "mcu_periph/sys_time.h"
+#include "navdata.h" // for full_write
 
 #include <stdio.h>   /* Standard input/output definitions */
 #include <string.h>  /* String function definitions */
@@ -157,8 +158,12 @@ void actuators_ardrone_init(void)
 }
 
 int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen) {
-  write(actuator_ardrone2_raw_fd, &cmd, 1);
-  return read(actuator_ardrone2_raw_fd, reply, replylen);
+  if (full_write(actuator_ardrone2_raw_fd, &cmd, 1) < 0)
+  {
+    perror("actuators_ardrone_cmd: write failed");
+    return -1;
+  }
+  return full_read(actuator_ardrone2_raw_fd, reply, replylen);
 }
 
 #include "autopilot.h"
@@ -241,7 +246,7 @@ void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint
   cmd[2] = ((pwm1&0x1ff)<<3) | ((pwm2&0x1ff)>>6);
   cmd[3] = ((pwm2&0x1ff)<<2) | ((pwm3&0x1ff)>>7);
   cmd[4] = ((pwm3&0x1ff)<<1);
-  write(actuator_ardrone2_raw_fd, cmd, 5);
+  full_write(actuator_ardrone2_raw_fd, cmd, 5);
   RunOnceEvery(20,actuators_ardrone_led_run());
 }
 
@@ -270,7 +275,7 @@ void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_
   cmd[0]=0x60 | ((led0&1)<<4) | ((led1&1)<<3) | ((led2&1)<<2) | ((led3&1) <<1);
   cmd[1]=((led0&2)<<3) | ((led1&2)<<2) | ((led2&2)<<1) | ((led3&2)<<0);
 
-  write(actuator_ardrone2_raw_fd, cmd, 2);
+  full_write(actuator_ardrone2_raw_fd, cmd, 2);
 }
 
 void actuators_ardrone_close(void)

sw/airborne/boards/ardrone/baro_board.c

@@ -77,8 +77,8 @@ void process_ardrone_baro(void)
 {
   if(baro.status == BS_RUNNING) {
     // first read temperature because pressure calibration depends on temperature
-    baro.differential = baro_apply_calibration_temp(navdata->temperature_pressure);   // We store the temperature in Baro-Diff
-    baro.absolute = baro_apply_calibration(navdata->pressure);
+    baro.differential = baro_apply_calibration_temp(navdata.temperature_pressure);   // We store the temperature in Baro-Diff
+    baro.absolute = baro_apply_calibration(navdata.pressure);
   }
   else {
     if (baro_calibrated == TRUE) {

sw/airborne/boards/ardrone/navdata.c

@@ -34,21 +34,81 @@
 #include <unistd.h>
 #include <string.h>
 #include <math.h>
+#include <errno.h>
+#include <assert.h>
 #include "navdata.h"
 
-int nav_fd;
+#define NAVDATA_PACKET_SIZE 60
+#define NAVDATA_BUFFER_SIZE 80
+#define NAVDATA_START_BYTE 0x3a
+
+typedef struct {
+	uint8_t isInitialized;
+	uint8_t isOpen;
+	uint16_t bytesRead;
+	uint32_t totalBytesRead;
+	uint32_t packetsRead;
+	uint8_t buffer[NAVDATA_BUFFER_SIZE];
+} navdata_port;
+
+static navdata_port port;
+static int nav_fd;
+
+measures_t navdata;
+
+// FIXME(ben): there must be a better home for these
+ssize_t full_write(int fd, const uint8_t *buf, size_t count)
+{
+  size_t written = 0;
+
+  while(written < count)
+  {
+    ssize_t n = write(fd, buf + written, count - written);
+    if (n < 0)
+    {
+      if (errno == EAGAIN || errno == EWOULDBLOCK)
+	continue;
+      return n;
+    }
+    written += n;
+  }
+  return written;
+}
+
+ssize_t full_read(int fd, uint8_t *buf, size_t count)
+{
+  // Apologies for illiteracy, but we can't overload |read|.
+  size_t readed = 0;
+
+  while(readed < count)
+  {
+    ssize_t n = read(fd, buf + readed, count - readed);
+    if (n < 0)
+    {
+      if (errno == EAGAIN || errno == EWOULDBLOCK)
+	continue;
+      return n;
+    }
+    readed += n;
+  }
+  return readed;
+}
+
+static void navdata_write(const uint8_t *buf, size_t count)
+{
+  if (full_write(nav_fd, buf, count) < 0)
+    perror("navdata_write: Write failed");
+}
 
 int navdata_init()
 {
-  port = malloc(sizeof(navdata_port));
-
   nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK);
   if (nav_fd == -1)
   {
     perror("navdata_init: Unable to open /dev/ttyO1 - ");
     return 1;
   } else {
-    port->isOpen = 1;
+    port.isOpen = 1;
   }
 
   fcntl(nav_fd, F_SETFL, 0); //read calls are non blocking
@@ -70,23 +130,22 @@ int navdata_init()
 
   // stop acquisition
   uint8_t cmd=0x02;
-  write(nav_fd, &cmd, 1);
+  navdata_write(&cmd, 1);
 
   baro_calibrated = 0;
   acquire_baro_calibration();
 
   // start acquisition
   cmd=0x01;
-  write(nav_fd, &cmd, 1);
+  navdata_write(&cmd, 1);
 
-  navdata = malloc(sizeof(measures_t));
   navdata_imu_available = 0;
   navdata_baro_available = 0;
 
-  port->bytesRead = 0;
-  port->totalBytesRead = 0;
-  port->packetsRead = 0;
-  port->isInitialized = 1;
+  port.bytesRead = 0;
+  port.totalBytesRead = 0;
+  port.packetsRead = 0;
+  port.isInitialized = 1;
 
   previousUltrasoundHeight = 0;
 
@@ -95,24 +154,19 @@ int navdata_init()
 
 void acquire_baro_calibration()
 {
-  read(nav_fd, NULL, 100); // read some potential dirt
+  char tmp[100];
+  read(nav_fd, tmp, sizeof tmp); // read some potential dirt
 
   // start baro calibration acquisition
   uint8_t cmd=0x17; // send cmd 23
-  write(nav_fd, &cmd, 1);
+  navdata_write(&cmd, 1);
 
   uint8_t calibBuffer[22];
-  int calib_bytes_read, calib_bytes_left, readcount;
-  calib_bytes_read = 0;
-  calib_bytes_left = 22;
-  readcount = 0;
 
-  while(calib_bytes_read != 22) {
-    readcount = read(nav_fd, calibBuffer+(22-calib_bytes_left), calib_bytes_left);
-    if (readcount > 0) {
-      calib_bytes_read += readcount;
-      calib_bytes_left -= readcount;
-    }
+  if (full_read(nav_fd, calibBuffer, sizeof calibBuffer) < 0)
+  {
+    perror("acquire_baro_calibration: read failed");
+    return;
   }
 
   baro_calibration.ac1 = calibBuffer[0]  << 8 | calibBuffer[1];
@@ -127,8 +181,6 @@ void acquire_baro_calibration()
   baro_calibration.mc  = calibBuffer[18] << 8 | calibBuffer[19];
   baro_calibration.md  = calibBuffer[20] << 8 | calibBuffer[21];
 
-  printf("Calibration bytes read: %d\n", calib_bytes_read);
-
   printf("Calibration AC1: %d\n", baro_calibration.ac1);
   printf("Calibration AC2: %d\n", baro_calibration.ac2);
   printf("Calibration AC3: %d\n", baro_calibration.ac3);
@@ -148,7 +200,7 @@ void acquire_baro_calibration()
 
 void navdata_close()
 {
-  port->isOpen = 0;
+  port.isOpen = 0;
   close(nav_fd);
 }
 
@@ -156,24 +208,23 @@ void navdata_read()
 {
   int newbytes = 0;
 
-  if (port->isInitialized != 1)
+  if (port.isInitialized != 1)
     navdata_init();
 
-  if (port->isOpen != 1)
+  if (port.isOpen != 1)
     return;
 
-  newbytes = read(nav_fd, port->buffer+port->bytesRead, NAVDATA_BUFFER_SIZE-port->bytesRead);
+  newbytes = read(nav_fd, port.buffer+port.bytesRead, NAVDATA_BUFFER_SIZE-port.bytesRead);
 
   // because non-blocking read returns -1 when no bytes available
   if (newbytes > 0)
   {
-    port->bytesRead += newbytes;
-    port->totalBytesRead += newbytes;
+    port.bytesRead += newbytes;
+    port.totalBytesRead += newbytes;
   }
 }
 
-void baro_update_logic(void);
-void baro_update_logic(void)
+static void baro_update_logic(void)
 {
   static int32_t lastpressval = 0;
   static uint16_t lasttempval = 0;
@@ -190,7 +241,7 @@ void baro_update_logic(void)
     if (lastpressval != 0)
     {
       // If pressure was updated: this is a sync error
-      if (lastpressval != navdata->pressure)
+      if (lastpressval != navdata.pressure)
       {
         // wait for temp again
         temp_or_press_was_updated_last = 0;
@@ -208,7 +259,7 @@ void baro_update_logic(void)
     if (lasttempval != 0)
     {
       // If temp was updated: this is a sync error
-      if (lasttempval != navdata->temperature_pressure)
+      if (lasttempval != navdata.temperature_pressure)
       {
         // wait for press again
         temp_or_press_was_updated_last = 1;
@@ -219,8 +270,8 @@ void baro_update_logic(void)
     navdata_baro_available = 1;
   }
 
-  lastpressval = navdata->pressure;
-  lasttempval = navdata->temperature_pressure;
+  lastpressval = navdata.pressure;
+  lasttempval = navdata.temperature_pressure;
 
   // debug
   // navdata->temperature_pressure = sync_errors;
@@ -231,23 +282,24 @@ void navdata_update()
   navdata_read();
 
   // while there is something interesting to do...
-  while (port->bytesRead >= 60)
+  while (port.bytesRead >= NAVDATA_PACKET_SIZE)
   {
-    if (port->buffer[0] == NAVDATA_START_BYTE)
+    if (port.buffer[0] == NAVDATA_START_BYTE)
     {
       // if checksum is OK
       if ( 1 ) // we dont know how to calculate the checksum
 //      if ( navdata_checksum() == 0 )
       {
-        memcpy(navdata, port->buffer, NAVDATA_PACKET_SIZE);
+	assert(sizeof navdata == NAVDATA_PACKET_SIZE);
+        memcpy(&navdata, port.buffer, NAVDATA_PACKET_SIZE);
 
         // Invert byte order so that TELEMETRY works better
         uint8_t tmp;
-        uint8_t* p = (uint8_t*) &(navdata->pressure);
+        uint8_t* p = (uint8_t*) &(navdata.pressure);
         tmp = p[0];
         p[0] = p[1];
         p[1] = tmp;
-        p = (uint8_t*) &(navdata->temperature_pressure);
+        p = (uint8_t*) &(navdata.temperature_pressure);
         tmp = p[0];
         p[0] = p[1];
         p[1] = tmp;
@@ -256,23 +308,23 @@ void navdata_update()
 
         navdata_imu_available = 1;
 
-        port->packetsRead++;
+        port.packetsRead++;
 //        printf("CCRC=%d, GCRC=%d, error=%d\n", crc, navdata->chksum, abs(crc-navdata->chksum));
         //navdata_getHeight();
       }
-      navdata_CropBuffer(60);
+      navdata_CropBuffer(NAVDATA_PACKET_SIZE);
     }
     else
     {
       // find start byte, copy all data from startbyte to buffer origin, update bytesread
       uint8_t * pint;
-      pint = memchr(port->buffer, NAVDATA_START_BYTE, port->bytesRead);
+      pint = memchr(port.buffer, NAVDATA_START_BYTE, port.bytesRead);
 
       if (pint != NULL) {
-        navdata_CropBuffer(pint - port->buffer);
+        navdata_CropBuffer(pint - port.buffer);
       } else {
         // if the start byte was not found, it means there is junk in the buffer
-        port->bytesRead = 0;
+        port.bytesRead = 0;
       }
     }
   }
@@ -280,25 +332,25 @@ void navdata_update()
 
 void navdata_CropBuffer(int cropsize)
 {
-  if (port->bytesRead - cropsize < 0) {
+  if (port.bytesRead - cropsize < 0) {
     // TODO think about why the amount of bytes read minus the cropsize gets below zero
-    printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", port->bytesRead, cropsize, port->buffer);
+    printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", port.bytesRead, cropsize, port.buffer);
     return;
   }
 
-  memmove(port->buffer, port->buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize);
-  port->bytesRead -= cropsize;
+  memmove(port.buffer, port.buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize);
+  port.bytesRead -= cropsize;
 }
 
 int16_t navdata_getHeight() {
 
-  if (navdata->ultrasound > 10000) {
+  if (navdata.ultrasound > 10000) {
     return previousUltrasoundHeight;
   }
 
   int16_t ultrasoundHeight = 0;
 
-  ultrasoundHeight = (navdata->ultrasound - 880) / 26.553;
+  ultrasoundHeight = (navdata.ultrasound - 880) / 26.553;
 
   previousUltrasoundHeight = ultrasoundHeight;
 
@@ -308,32 +360,32 @@ int16_t navdata_getHeight() {
 // The checksum should be calculated here: we don't know the algorithm
 uint16_t navdata_checksum() {
   navdata_cks = 0;
-  navdata_cks += navdata->nu_trame;
-  navdata_cks += navdata->ax;
-  navdata_cks += navdata->ay;
-  navdata_cks += navdata->az;
-  navdata_cks += navdata->vx;
-  navdata_cks += navdata->vy;
-  navdata_cks += navdata->vz;
-  navdata_cks += navdata->temperature_acc;
-  navdata_cks += navdata->temperature_gyro;
-  navdata_cks += navdata->ultrasound;
-  navdata_cks += navdata->us_debut_echo;
-  navdata_cks += navdata->us_fin_echo;
-  navdata_cks += navdata->us_association_echo;
-  navdata_cks += navdata->us_distance_echo;
-  navdata_cks += navdata->us_curve_time;
-  navdata_cks += navdata->us_curve_value;
-  navdata_cks += navdata->us_curve_ref;
-  navdata_cks += navdata->nb_echo;
-  navdata_cks += navdata->sum_echo;
-  navdata_cks += navdata->gradient;
-  navdata_cks += navdata->flag_echo_ini;
-  navdata_cks += navdata->pressure;
-  navdata_cks += navdata->temperature_pressure;
-  navdata_cks += navdata->mx;
-  navdata_cks += navdata->my;
-  navdata_cks += navdata->mz;
+  navdata_cks += navdata.nu_trame;
+  navdata_cks += navdata.ax;
+  navdata_cks += navdata.ay;
+  navdata_cks += navdata.az;
+  navdata_cks += navdata.vx;
+  navdata_cks += navdata.vy;
+  navdata_cks += navdata.vz;
+  navdata_cks += navdata.temperature_acc;
+  navdata_cks += navdata.temperature_gyro;
+  navdata_cks += navdata.ultrasound;
+  navdata_cks += navdata.us_debut_echo;
+  navdata_cks += navdata.us_fin_echo;
+  navdata_cks += navdata.us_association_echo;
+  navdata_cks += navdata.us_distance_echo;
+  navdata_cks += navdata.us_curve_time;
+  navdata_cks += navdata.us_curve_value;
+  navdata_cks += navdata.us_curve_ref;
+  navdata_cks += navdata.nb_echo;
+  navdata_cks += navdata.sum_echo;
+  navdata_cks += navdata.gradient;
+  navdata_cks += navdata.flag_echo_ini;
+  navdata_cks += navdata.pressure;
+  navdata_cks += navdata.temperature_pressure;
+  navdata_cks += navdata.mx;
+  navdata_cks += navdata.my;
+  navdata_cks += navdata.mz;
 //  navdata_cks += navdata->chksum;
 
   return 0; // we dont know how to calculate the checksum

sw/airborne/boards/ardrone/navdata.h

@@ -31,19 +31,7 @@
 #define NAVDATA_H_
 
 #include <stdint.h>
-
-#define NAVDATA_PACKET_SIZE 60
-#define NAVDATA_BUFFER_SIZE 80
-#define NAVDATA_START_BYTE 0x3a
-
-typedef struct {
-	uint8_t isInitialized;
-	uint8_t isOpen;
-	uint16_t bytesRead;
-	uint32_t totalBytesRead;
-	uint32_t packetsRead;
-	uint8_t buffer[NAVDATA_BUFFER_SIZE];
-} navdata_port;
+#include <sys/types.h>
 
 typedef struct
 {
@@ -107,9 +95,8 @@ struct bmp180_baro_calibration
   int32_t b5;
 };
 
-measures_t* navdata;
+extern measures_t navdata;
 struct bmp180_baro_calibration baro_calibration;
-navdata_port* port;
 uint16_t navdata_cks;
 uint8_t navdata_imu_available;
 uint8_t navdata_baro_available;
@@ -128,4 +115,7 @@ int16_t navdata_getHeight(void);
 
 void acquire_baro_calibration(void);
 
+ssize_t full_write(int fd, const uint8_t *buf, size_t count);
+ssize_t full_read(int fd, uint8_t *buf, size_t count);
+
 #endif /* NAVDATA_H_ */

sw/airborne/firmwares/rotorcraft/telemetry.h

@@ -956,34 +956,34 @@
 #ifdef ARDRONE2_RAW
 #include "navdata.h"
 #define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) DOWNLINK_SEND_ARDRONE_NAVDATA(_trans, _dev, \
-	&navdata->taille, \
-	&navdata->nu_trame, \
-	&navdata->ax, \
-	&navdata->ay, \
-	&navdata->az, \
-	&navdata->vx, \
-	&navdata->vy, \
-	&navdata->vz, \
-	&navdata->temperature_acc, \
-	&navdata->temperature_gyro, \
-	&navdata->ultrasound, \
-	&navdata->us_debut_echo, \
-	&navdata->us_fin_echo, \
-	&navdata->us_association_echo, \
-	&navdata->us_distance_echo, \
-	&navdata->us_curve_time, \
-	&navdata->us_curve_value, \
-	&navdata->us_curve_ref, \
-	&navdata->nb_echo, \
-	&navdata->sum_echo, \
-	&navdata->gradient, \
-	&navdata->flag_echo_ini, \
-	&navdata->pressure, \
-	&navdata->temperature_pressure, \
-	&navdata->mx, \
-	&navdata->my, \
-	&navdata->mz, \
-	&navdata->chksum \
+	&navdata.taille, \
+	&navdata.nu_trame, \
+	&navdata.ax, \
+	&navdata.ay, \
+	&navdata.az, \
+	&navdata.vx, \
+	&navdata.vy, \
+	&navdata.vz, \
+	&navdata.temperature_acc, \
+	&navdata.temperature_gyro, \
+	&navdata.ultrasound, \
+	&navdata.us_debut_echo, \
+	&navdata.us_fin_echo, \
+	&navdata.us_association_echo, \
+	&navdata.us_distance_echo, \
+	&navdata.us_curve_time, \
+	&navdata.us_curve_value, \
+	&navdata.us_curve_ref, \
+	&navdata.nb_echo, \
+	&navdata.sum_echo, \
+	&navdata.gradient, \
+	&navdata.flag_echo_ini, \
+	&navdata.pressure, \
+	&navdata.temperature_pressure, \
+	&navdata.mx, \
+	&navdata.my, \
+	&navdata.mz, \
+	&navdata.chksum \
 	)
 #else
 #define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) {}

sw/airborne/subsystems/imu/imu_ardrone2_raw.h

@@ -116,9 +116,9 @@ static inline void imu_ardrone2_event ( void (* _gyro_handler)(void), void (* _a
   //checks if the navboard has a new dataset ready
   if (navdata_imu_available == TRUE) {
     navdata_imu_available = FALSE;
-    RATES_ASSIGN(imu.gyro_unscaled, navdata->vx, -navdata->vy, -navdata->vz);
-    VECT3_ASSIGN(imu.accel_unscaled, navdata->ax, 4096-navdata->ay, 4096-navdata->az);
-    VECT3_ASSIGN(imu.mag_unscaled, -navdata->mx, -navdata->my, -navdata->mz);
+    RATES_ASSIGN(imu.gyro_unscaled, navdata.vx, -navdata.vy, -navdata.vz);
+    VECT3_ASSIGN(imu.accel_unscaled, navdata.ax, 4096-navdata.ay, 4096-navdata.az);
+    VECT3_ASSIGN(imu.mag_unscaled, -navdata.mx, -navdata.my, -navdata.mz);
 
     _gyro_handler();
     _accel_handler();