diff --git a/conf/boards/bebop.makefile b/conf/boards/bebop.makefile index 6f00e27cff..6d9b714bd8 100644 --- a/conf/boards/bebop.makefile +++ b/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 diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index 9b6857bf81..0043002910 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/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 diff --git a/conf/modules/sonar_bebop.xml b/conf/modules/sonar_bebop.xml new file mode 100644 index 0000000000..754f9d1bbe --- /dev/null +++ b/conf/modules/sonar_bebop.xml @@ -0,0 +1,31 @@ + + + + + + Bebop Sonar driver. + Reads an anlog sonar sensor and outputs sonar distance in [m] + + + + +
+ +
+ + + + + + + + + + + + + include $(CFG_SHARED)/spi_master.makefile + + + + diff --git a/sw/airborne/arch/linux/mcu_periph/adc_arch.c b/sw/airborne/arch/linux/mcu_periph/adc_arch.c new file mode 100644 index 0000000000..8407379f00 --- /dev/null +++ b/sw/airborne/arch/linux/mcu_periph/adc_arch.c @@ -0,0 +1,188 @@ +/* + * Copyright (C) 2015 Freek van Tienen + * + * 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 +#include +#include +#include +#include +#include +#include + + +/******************************/ +/*** 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); +} diff --git a/sw/airborne/arch/linux/mcu_periph/adc_arch.h b/sw/airborne/arch/linux/mcu_periph/adc_arch.h new file mode 100644 index 0000000000..824dfb4c29 --- /dev/null +++ b/sw/airborne/arch/linux/mcu_periph/adc_arch.h @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2015 Freek van Tienen + * + * 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 */ diff --git a/sw/airborne/arch/linux/mcu_periph/spi_arch.c b/sw/airborne/arch/linux/mcu_periph/spi_arch.c index e4d6446f5f..1fe3dd0927 100644 --- a/sw/airborne/arch/linux/mcu_periph/spi_arch.c +++ b/sw/airborne/arch/linux/mcu_periph/spi_arch.c @@ -33,6 +33,7 @@ #include #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 */ diff --git a/sw/airborne/boards/bebop.h b/sw/airborne/boards/bebop.h index 340585df4f..45b72faf7b 100644 --- a/sw/airborne/boards/bebop.h +++ b/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 */ diff --git a/sw/airborne/math/pprz_algebra_float.c b/sw/airborne/math/pprz_algebra_float.c index ff978ca0a2..66dbf16050 100644 --- a/sw/airborne/math/pprz_algebra_float.c +++ b/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; diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 3e9b940357..4e3a0fa9ae 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/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); diff --git a/sw/airborne/modules/sonar/sonar_bebop.c b/sw/airborne/modules/sonar/sonar_bebop.c new file mode 100644 index 0000000000..3fb122498e --- /dev/null +++ b/sw/airborne/modules/sonar/sonar_bebop.c @@ -0,0 +1,141 @@ +/* + * Copyright (C) 2015 Freek van Tienen + * + * 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 +#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; +} + diff --git a/sw/airborne/modules/sonar/sonar_bebop.h b/sw/airborne/modules/sonar/sonar_bebop.h new file mode 100644 index 0000000000..5d3752354b --- /dev/null +++ b/sw/airborne/modules/sonar/sonar_bebop.h @@ -0,0 +1,42 @@ +/* + * Copyright (C) 2015 Freek van Tienen + * + * 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 */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c index 2b5e0e8d05..8e12989e71 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c +++ b/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};