[bebop] Sonar

Conflicts:

	sw/airborne/arch/linux/mcu_periph/spi_arch.c

conf/boards/bebop.makefile

@@ -29,7 +29,7 @@ GPS_PORT           ?= UART1
 GPS_BAUD           ?= B230400
 
 # handle linux signals by hand
-$(TARGET).CFLAGS += -DUSE_LINUX_SIGNAL
+$(TARGET).CFLAGS += -DUSE_LINUX_SIGNAL -D_GNU_SOURCE
 
 # Compile the video specific parts
 $(TARGET).srcs +=  $(SRC_BOARD)/video.c

conf/firmwares/rotorcraft.makefile

@@ -121,9 +121,9 @@ $(TARGET).srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c
 #
 # Electrical subsystem / Analog Backend
 #
-ifneq ($(ARCH), linux)
 $(TARGET).CFLAGS += -DUSE_ADC
 $(TARGET).srcs   += $(SRC_ARCH)/mcu_periph/adc_arch.c
+ifneq ($(ARCH), linux)
 $(TARGET).srcs   += subsystems/electrical.c
 endif
 

conf/modules/sonar_bebop.xml

@@ -0,0 +1,31 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="sonar">
+  <doc>
+    <description>
+     Bebop Sonar driver.
+     Reads an anlog sonar sensor and outputs sonar distance in [m]
+    </description>
+    <define name="USE_SONAR" value="" description="activate use of sonar in INS extended filter (only rotorcraft)"/>
+  </doc>
+
+  <header>
+    <file name="sonar_bebop.h"/>
+  </header>
+
+  <init fun="sonar_bebop_init()"/>
+
+  <makefile target="ap|sim">
+    <file name="sonar_bebop.c"/>
+  </makefile>
+  <makefile target="ap">
+    <define name="USE_SPI0" value="1"/>
+    <define name="USE_ADC0" value="1"/>
+    <define name="USE_SONAR" value="1"/>
+
+    <raw>
+      include $(CFG_SHARED)/spi_master.makefile
+    </raw>
+  </makefile>
+
+</module>

sw/airborne/arch/linux/mcu_periph/adc_arch.c

@@ -0,0 +1,188 @@
+/*
+ * Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file arch/linux/mcu_periph/adc_arch.c
+ * @ingroup linux_arch
+ *
+ * Driver for the analog to digital converters in Linux based systems.
+ */
+
+#include "mcu_periph/adc.h"
+
+#include BOARD_CONFIG
+#include <stdlib.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <poll.h>
+
+
+/******************************/
+/***   INTERNAL VARIABLES   ***/
+/******************************/
+
+/* ADC0 */
+#if USE_ADC0
+static uint8_t adc0_channels[] = {ADC0_CHANNELS};
+struct adc_t adc0 = {
+  .dev_id = ADC0_ID,
+  .channels = adc0_channels,
+  .channels_cnt = ADC0_CHANNELS_CNT,
+  .buf_length = ADC0_BUF_LENGTH
+};
+#endif
+
+/* ADC1 */
+#if USE_ADC1
+static uint8_t adc1_channels[] = {ADC1_CHANNELS};
+struct adc_t adc1 = {
+  .dev_id = ADC1_ID,
+  .channels = adc1_channels,
+  .channels_cnt = ADC1_CHANNELS_CNT,
+  .buf_length = ADC1_BUF_LENGTH
+};
+#endif
+
+/* Private functions */
+static inline void adc_dev_init(struct adc_t *adc);
+static void write_sysfs_int(uint8_t dev_id, char *filename, int val);
+
+/***************************************/
+/***   PUBLIC FUNCTION DEFINITIONS   ***/
+/***************************************/
+
+/**
+ * Initialize the ADC
+ */
+void adc_init(void)
+{
+#if USE_ADC0
+  adc_dev_init(&adc0);
+#endif
+#if USE_ADC1
+  adc_dev_init(&adc1);
+#endif
+}
+
+/**
+ * @todo: fx a more general ADC
+ */
+void adc_buf_channel(uint8_t adc_channel, struct adc_buf *s, uint8_t av_nb_sample)
+{
+
+}
+
+/**
+ * Start or stop the ADC readings
+ * @param[in] *adc The ADC to start the readings for
+ * @param[in] value 1 to enable and 0 to disable
+ */
+void adc_enable(struct adc_t *adc, uint8_t value)
+{
+  /* Write 1 or 0 to enable/disable the ADC */
+  write_sysfs_int(adc->dev_id, "buffer/enable", value);
+}
+
+/**
+ * Read the ADC buffer from the driver
+ * @param[in] *adc The adc you want to read from
+ * @param[out] *buf Output values
+ * @param[in] size The amount of bytes you want to read
+ */
+int adc_read(struct adc_t *adc, uint16_t *buf, uint16_t size)
+{
+  /* Allocate dev_id + name */
+  char *temp;
+  if(asprintf(&temp, "/dev/iio:device%d", adc->dev_id) < 0) {
+    return -1;
+  }
+
+  /* Open the file */
+  int fd = open(temp, O_RDONLY | O_NONBLOCK);
+  free(temp);
+
+  if(fd < 0) {
+    return -2;
+  }
+
+  struct pollfd pfd;
+  pfd.fd = fd;
+  pfd.events = POLLIN;
+  poll(&pfd, 1, -1);
+
+  /* Read the file */
+  int ret = read(fd, buf, size);
+  close(fd);
+  return ret;
+}
+
+/****************************************/
+/***   PRIVATE FUNCTION DEFINITIONS   ***/
+/****************************************/
+
+/**
+ * Initialize an ADC device
+ * @param[in] *adc The ADC device to initialize
+ */
+static inline void adc_dev_init(struct adc_t *adc)
+{
+  char filename[32];
+  uint8_t i;
+
+  /* Enable all linked channels */
+  for(i = 0; i < adc->channels_cnt; i++) {
+    sprintf(filename, "scan_elemens/in_voltage%d_en", adc->channels[i]);
+    write_sysfs_int(adc->dev_id, filename, 1);
+  }
+
+  /* Set the buffer length */
+  write_sysfs_int(adc->dev_id, "buffer/length", adc->buf_length);
+}
+
+/**
+ * Write an int to a sysfs file
+ * @param[in] dev_id The device id
+ * @param[in] *filename The file to write to
+ * @param[in] val The value to write
+ */
+static void write_sysfs_int(uint8_t dev_id, char *filename, int val)
+{
+  /* Allocate dev_id + filename */
+  char *temp;
+  if (asprintf(&temp, "/sys/bus/iio/devices/iio:device%d/%s", dev_id, filename) < 0) {
+    return;
+  }
+
+  /* Open the file */
+  FILE *fd = fopen(temp, "w");
+  free(temp);
+
+  if (fd == NULL) {
+    return;
+  }
+
+  /* Write the value to the file */
+  fprintf(fd, "%d", val);
+  fclose(fd);
+}

sw/airborne/arch/linux/mcu_periph/adc_arch.h

@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file arch/linux/mcu_periph/adc_arch.h
+ * @ingroup linux_arch
+ *
+ * Driver for the analog to digital converters in Linux based systems..
+ */
+
+#ifndef ADC_ARCH_H
+#define ADC_ARCH_H
+
+/* Main ADC structure */
+struct adc_t {
+  uint8_t dev_id;         ///< The iio device ID
+  uint8_t *channels;      ///< Channels used in the iio device
+  uint8_t channels_cnt;   ///< Amount of channels
+  uint16_t buf_length;    ///< ADC buffer length
+};
+
+#if USE_ADC0
+extern struct adc_t adc0;
+#endif
+
+#if USE_ADC1
+extern struct adc_t adc1;
+#endif
+
+void adc_enable(struct adc_t *adc, uint8_t value);
+int adc_read(struct adc_t *adc, uint16_t *buf, uint16_t size);
+
+#endif /* ADC_ARCH_H */

sw/airborne/arch/linux/mcu_periph/spi_arch.c

@@ -33,6 +33,7 @@
 #include <linux/spi/spidev.h>
 
 #include "mcu_periph/spi.h"
+#include BOARD_CONFIG
 
 
 void spi_init_slaves(void)
@@ -42,6 +43,8 @@ void spi_init_slaves(void)
    */
 }
 
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wcast-qual"
 bool_t spi_submit(struct spi_periph *p, struct spi_transaction *t)
 {
   int fd = (int)p->reg_addr;
@@ -73,7 +76,7 @@ bool_t spi_submit(struct spi_periph *p, struct spi_transaction *t)
 
   xfer.len = buf_len;
   /* fixed speed of 1Mhz for now, use SPIClockDiv?? */
-  xfer.speed_hz = 1000000;
+  xfer.speed_hz = (uint32_t)p->init_struct;
   xfer.delay_usecs = 0;
   if (t->dss == SPIDss16bit) {
     xfer.bits_per_word = 16;
@@ -84,19 +87,20 @@ bool_t spi_submit(struct spi_periph *p, struct spi_transaction *t)
     xfer.cs_change = 1;
   }
 
-  if (ioctl(fd, SPI_IOC_MESSAGE(1), xfer) < 0) {
+  if (ioctl(fd, SPI_IOC_MESSAGE(1), &xfer) < 0) {
     t->status = SPITransFailed;
     return FALSE;
   }
 
   /* copy recieved data if we had to use an extra rx_buffer */
   if (buf_len > t->input_length) {
-    memcpy((void *)t->input_buf, (void *)xfer.rx_buf, t->input_length);
+    memcpy((void *)t->input_buf, (void *)((uint32_t)xfer.rx_buf), t->input_length);
   }
 
   t->status = SPITransSuccess;
   return TRUE;
 }
+#pragma GCC diagnostic pop
 
 bool_t spi_lock(struct spi_periph *p, uint8_t slave)
 {
@@ -112,6 +116,23 @@ bool_t spi_resume(struct spi_periph *p, uint8_t slave)
 
 
 #if USE_SPI0
+
+#ifndef SPI0_MODE
+#define SPI0_MODE (SPI_CPOL | SPI_CPHA)
+#endif
+
+#ifndef SPI0_LSB_FIRST
+#define SPI0_LSB_FIRST 0
+#endif
+
+#ifndef SPI0_BITS_PER_WORD
+#define SPI0_BITS_PER_WORD 8
+#endif
+
+#ifndef SPI0_MAX_SPEED_HZ
+#define SPI0_MAX_SPEED_HZ 1000000
+#endif
+
 void spi0_arch_init(void)
 {
   int fd = open("/dev/spidev1.0", O_RDWR);
@@ -124,32 +145,50 @@ void spi0_arch_init(void)
   spi0.reg_addr = (void *)fd;
 
   /* spi mode */
-  unsigned char spi_mode = (SPI_CPOL | SPI_CPHA);
+  unsigned char spi_mode = SPI0_MODE;
   if (ioctl(fd, SPI_IOC_WR_MODE, &spi_mode) < 0) {
     perror("SPI0: can't set spi mode");
   }
 
   /* set to MSB first */
-  unsigned char spi_order = 0;
+  unsigned char spi_order = SPI0_LSB_FIRST;
   if (ioctl(fd, SPI_IOC_WR_LSB_FIRST, &spi_order) < 0) {
     perror("SPI0: can't set spi bit justification");
   }
 
   /* bits per word default to 8 */
-  unsigned char spi_bits_per_word = 8;
+  unsigned char spi_bits_per_word = SPI0_BITS_PER_WORD;
   if (ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word) < 0) {
     perror("SPI0: can't set bits per word");
   }
 
   /* max speed in hz, 1MHz for now */
-  unsigned int spi_speed = 1000000;
+  unsigned int spi_speed = SPI0_MAX_SPEED_HZ;
   if (ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed) < 0) {
     perror("SPI0: can't set max speed hz");
   }
+  spi0.init_struct = (void *)SPI0_MAX_SPEED_HZ;
 }
 #endif /* USE_SPI0 */
 
 #if USE_SPI1
+
+#ifndef SPI1_MODE
+#define SPI1_MODE (SPI_CPOL | SPI_CPHA)
+#endif
+
+#ifndef SPI1_LSB_FIRST
+#define SPI1_LSB_FIRST 0
+#endif
+
+#ifndef SPI1_BITS_PER_WORD
+#define SPI1_BITS_PER_WORD 8
+#endif
+
+#ifndef SPI1_MAX_SPEED_HZ
+#define SPI1_MAX_SPEED_HZ 1000000
+#endif
+
 void spi1_arch_init(void)
 {
   int fd = open("/dev/spidev1.1", O_RDWR);
@@ -162,27 +201,28 @@ void spi1_arch_init(void)
   spi1.reg_addr = (void *)fd;
 
   /* spi mode */
-  unsigned char spi_mode = (SPI_CPOL | SPI_CPHA);
+  unsigned char spi_mode = SPI1_MODE;
   if (ioctl(fd, SPI_IOC_WR_MODE, &spi_mode) < 0) {
     perror("SPI1: can't set spi mode");
   }
 
   /* set to MSB first */
-  unsigned char spi_order = 0;
+  unsigned char spi_order = SPI1_LSB_FIRST;
   if (ioctl(fd, SPI_IOC_WR_LSB_FIRST, &spi_order) < 0) {
     perror("SPI1: can't set spi bit justification");
   }
 
   /* bits per word default to 8 */
-  unsigned char spi_bits_per_word = 8;
-  if (ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word) < 0) {
+  unsigned char spi_bits_per_word = SPI1_BITS_PER_WORD;
+  if (ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word) < 0) {r
     perror("SPI1: can't set bits per word");
   }
 
   /* max speed in hz, 1MHz for now */
-  unsigned int spi_speed = 1000000;
+  unsigned int spi_speed = SPI1_MAX_SPEED_HZ;
   if (ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed) < 0) {
     perror("SPI1: can't set max speed hz");
   }
+  spi1.init_struct = (void *)SPI1_MAX_SPEED_HZ;
 }
 #endif /* USE_SPI1 */

sw/airborne/boards/bebop.h

@@ -40,4 +40,19 @@
 #define USE_BARO_BOARD 1
 #endif
 
+/* The ADC from the sonar */
+#if USE_ADC0
+#define ADC0_ID             0
+#define ADC0_CHANNELS       2
+#define ADC0_CHANNELS_CNT   1
+#define ADC0_BUF_LENGTH     8192
+#endif
+
+/* The SPI from the sonar */
+#if USE_SPI0
+#define SPI0_MODE           0
+#define SPI0_BITS_PER_WORD  8
+#define SPI0_MAX_SPEED_HZ   320000
+#endif
+
 #endif /* CONFIG_BEBOP */

sw/airborne/math/pprz_algebra_float.c

@@ -398,7 +398,7 @@ void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float d
   }
 }
 
-void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, struct FloatVect3 *v_in)
+void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
 {
   const float qi2_M1_2  = q->qi * q->qi - 0.5;
   const float qiqx = q->qi * q->qx;

sw/airborne/math/pprz_algebra_float.h

@@ -433,7 +433,7 @@ extern void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega,
 /** rotate 3D vector by quaternion.
  * vb = q_a2b * va * q_a2b^-1
  */
-extern void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, struct FloatVect3 *v_in);
+extern void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in);
 
 /// Quaternion from Euler angles.
 extern void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e);

sw/airborne/modules/sonar/sonar_bebop.c

@@ -0,0 +1,141 @@
+/*
+ * Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file modules/sonar/sonar_bebop.c
+ *  @brief Parrot Bebop Sonar driver
+ */
+
+#include "sonar_bebop.h"
+#include "generated/airframe.h"
+#include "mcu_periph/adc.h"
+#include "mcu_periph/spi.h"
+#include "subsystems/abi.h"
+#include <pthread.h>
+#include "subsystems/datalink/downlink.h"
+
+#ifdef SITL
+#include "state.h"
+#endif
+
+
+struct SonarBebop sonar_bebop;
+static uint8_t sonar_bebop_spi_d[16] = { 0xF0,0xF0,0xF0,0xF0,0xF0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 };
+static struct spi_transaction sonar_bebop_spi_t;
+static pthread_t sonar_bebop_thread;
+static void *sonar_bebop_read(void *data);
+
+void sonar_bebop_init(void)
+{
+  sonar_bebop.meas = 0;
+  sonar_bebop.offset = 0;
+
+  sonar_bebop_spi_t.status        = SPITransDone;
+  sonar_bebop_spi_t.select        = SPISelectUnselect;
+  sonar_bebop_spi_t.dss           = SPIDss8bit;
+  sonar_bebop_spi_t.output_buf    = sonar_bebop_spi_d;
+  sonar_bebop_spi_t.output_length = 16;
+  sonar_bebop_spi_t.input_buf     = NULL;
+  sonar_bebop_spi_t.input_length  = 0;
+
+  int rc = pthread_create(&sonar_bebop_thread, NULL, sonar_bebop_read, NULL);
+  if (rc < 0) {
+    return;
+  }
+}
+
+/**
+ * Read ADC value to update sonar measurement
+ */
+static void *sonar_bebop_read(void *data __attribute__((unused)))
+{
+  while(true) {
+
+#ifndef SITL
+    uint16_t i;
+    uint16_t adc_buffer[8192];
+
+
+    /* Start ADC and send sonar output */
+    adc_enable(&adc0, 1);
+    sonar_bebop_spi_t.status = SPITransDone;
+    spi_submit(&spi0, &sonar_bebop_spi_t);
+    while(sonar_bebop_spi_t.status != SPITransSuccess);
+    adc_read(&adc0, adc_buffer, 8192);
+    adc_enable(&adc0, 0);
+
+    /* Find the peeks */
+    uint16_t start_send = 0;
+    uint16_t stop_send = 0;
+    uint16_t first_peek = 0;
+    uint16_t lowest_value = 4095;
+    for(i = 0; i < 8192; i++){
+      uint16_t adc_val = adc_buffer[i]>>4;
+      if(start_send == 0 && adc_val == 4095)
+        start_send = i;
+      else if(start_send != 0 && stop_send == 0 && adc_val != 4095)
+      {
+        stop_send = i-1;
+        i += 300;
+        continue;
+      }
+
+      if(start_send != 0 && stop_send != 0 && first_peek == 0 && adc_val < lowest_value)
+        lowest_value = adc_val;
+      else if (start_send != 0 && stop_send != 0 && adc_val > lowest_value + 100) {
+        first_peek = i;
+        lowest_value = adc_val - 100;
+      }
+      else if(start_send != 0 && stop_send != 0 && first_peek != 0 && adc_val+100 < (adc_buffer[first_peek]>>4)) {
+        break;
+      }
+    }
+
+    /* Calculate the distance from the peeks */
+    uint16_t diff = stop_send - start_send;
+    int16_t peek_distance = first_peek - (stop_send - diff/2);
+    if(first_peek <= stop_send || diff > 250)
+      peek_distance = 0;
+
+    sonar_bebop.distance = peek_distance / 1000.0;
+#else // SITL
+    sonar_bebop.distance = stateGetPositionEnu_f()->z;
+    Bound(sonar_bebop.distance, 0.1f, 7.0f);
+    uint16_t peek_distance = 1;
+#endif // SITL
+
+    usleep(10000);
+
+    if(peek_distance > 0)
+    {
+      // Send ABI message
+      AbiSendMsgAGL(AGL_SONAR_ADC_ID, sonar_bebop.distance);
+
+#ifdef SENSOR_SYNC_SEND_SONAR
+      // Send Telemetry report
+      DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_bebop.meas, &sonar_bebop.distance);
+#endif
+    }
+  }
+
+  return NULL;
+}
+

sw/airborne/modules/sonar/sonar_bebop.h

@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file modules/sonar/sonar_bebop.h
+ *  @brief Parrot Bebop Sonar driver
+ */
+
+#ifndef SONAR_BEBOP_H
+#define SONAR_BEBOP_H
+
+#include "std.h"
+
+struct SonarBebop {
+  uint16_t meas;          ///< Raw ADC value
+  uint16_t offset;        ///< Sonar offset in ADC units
+  float distance;         ///< Distance measured in meters
+};
+
+extern struct SonarBebop sonar_bebop;
+
+extern void sonar_bebop_init(void);
+
+#endif /* SONAR_BEBOP_H */

sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c

@@ -235,7 +235,7 @@ static inline void update_state(const struct FloatVect3 *i_expected, struct Floa
 
   /* converted expected measurement from inertial to body frame */
   struct FloatVect3 b_expected;
-  float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, (struct FloatVect3 *)i_expected);
+  float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, i_expected);
 
   // S = HPH' + JRJ
   float H[3][6] = {{           0., -b_expected.z,  b_expected.y, 0., 0., 0.},
@@ -304,7 +304,7 @@ static inline void update_state_heading(const struct FloatVect3 *i_expected,
 
   /* converted expected measurement from inertial to body frame */
   struct FloatVect3 b_expected;
-  float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, (struct FloatVect3 *)i_expected);
+  float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, i_expected);
 
   /* set roll/pitch errors to zero to only correct heading */
   struct FloatVect3 i_h_2d = {i_expected->y, -i_expected->x, 0.f};