From e9523b91383ce858b0131ddfae806242454bbd9f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 23 Feb 2015 17:19:47 +0100 Subject: [PATCH 1/7] [arch/linux] test using timer_fd instead of SIGALRM and create a separate thread for the sys_tick handling... seems that some timer event are missed... --- conf/Makefile.linux | 2 +- .../arch/linux/mcu_periph/sys_time_arch.c | 68 +++++++++++++++---- .../arch/linux/mcu_periph/sys_time_arch.h | 2 - 3 files changed, 54 insertions(+), 18 deletions(-) diff --git a/conf/Makefile.linux b/conf/Makefile.linux index 830d508683..e4fbd9b0a5 100644 --- a/conf/Makefile.linux +++ b/conf/Makefile.linux @@ -65,7 +65,7 @@ CFLAGS += $(CSTANDARD) CFLAGS += $($(TARGET).CFLAGS) CFLAGS += $(USER_CFLAGS) -LDFLAGS += -lm +LDFLAGS += -lm -pthread CXXFLAGS += -O$(OPT) -fPIC CXXFLAGS += -pipe -fshow-column -ffast-math diff --git a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c index 4d49a83d94..2026cd3bcb 100644 --- a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c +++ b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c @@ -29,11 +29,60 @@ #include #include #include +#include +#include #ifdef SYS_TIME_LED #include "led.h" #endif +pthread_t sys_time_thread; +static void sys_tick_handler(void); +void *sys_time_thread_main(void *data); + +#define NSEC_OF_SEC(sec) ((sec) * 1e9) + +void *sys_time_thread_main(void *data) +{ + int fd; + + /* Create the timer */ + fd = timerfd_create(CLOCK_MONOTONIC, 0); + if (fd == -1) { + perror("Could not set up timer."); + return NULL; + } + + /* Make the timer periodic */ + struct itimerspec timer; + /* timer expires after sys_time.resolution sec */ + timer.it_value.tv_sec = 0; + timer.it_value.tv_nsec = NSEC_OF_SEC(sys_time.resolution); + /* and every SYS_TIME_RESOLUTION sec after that */ + timer.it_interval.tv_sec = 0; + timer.it_interval.tv_nsec = NSEC_OF_SEC(sys_time.resolution); + + if (timerfd_settime(fd, 0, &timer, NULL) == -1) { + perror("Could not set up timer."); + return NULL; + } + + while (1) { + unsigned long long missed; + /* Wait for the next timer event. If we have missed any the + number is written to "missed" */ + int r = read(fd, &missed, sizeof(missed)); + if (r == -1) { + perror("Couldn't read timer!"); + } + if (missed > 1) { + printf("Missed %lld timer events!\n", missed); + } + /* advance sys_time */ + sys_tick_handler(); + } + return NULL; +} void sys_time_arch_init(void) { @@ -41,27 +90,16 @@ void sys_time_arch_init(void) sys_time.cpu_ticks_per_sec = 1e6; sys_time.resolution_cpu_ticks = (uint32_t)(sys_time.resolution * sys_time.cpu_ticks_per_sec + 0.5); - struct sigaction sa; - struct itimerval timer; - memset(&sa, 0, sizeof(sa)); - sa.sa_handler = &sys_tick_handler; - if (sigaction(SIGALRM, &sa, NULL) == -1) { - printf("Couldn't set up sys_time timer!\n"); + int ret = pthread_create(&sys_time_thread, NULL, sys_time_thread_main, NULL); + if (ret) { + perror("Could not setup sys_time_thread"); return; } - // timer expires after sys_time.resolution sec - timer.it_value.tv_sec = 0; - timer.it_value.tv_usec = USEC_OF_SEC(sys_time.resolution); - // and every SYS_TIME_RESOLUTION sec after that - timer.it_interval.tv_sec = 0; - timer.it_interval.tv_usec = USEC_OF_SEC(sys_time.resolution); - - setitimer(ITIMER_REAL, &timer, NULL); } -void sys_tick_handler(int signum) +static void sys_tick_handler(void) { sys_time.nb_tick++; diff --git a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.h b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.h index f7b5a612b2..0828993893 100644 --- a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.h +++ b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.h @@ -31,8 +31,6 @@ #include "std.h" #include -extern void sys_tick_handler(int signum); - /** * Get the time in microseconds since startup. * WARNING: overflows after 71min34seconds! From c18355d4a592c640c5e05deb0b301de67388f1ca Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 23 Feb 2015 17:54:10 +0100 Subject: [PATCH 2/7] [arch/linux] run sys_time thread with higher rt prio --- conf/Makefile.linux | 2 +- .../arch/linux/mcu_periph/sys_time_arch.c | 27 +++++++++++++++++++ 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/conf/Makefile.linux b/conf/Makefile.linux index e4fbd9b0a5..7fe01a03e3 100644 --- a/conf/Makefile.linux +++ b/conf/Makefile.linux @@ -56,7 +56,7 @@ CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow -Wunused #CFLAGS += -Wa,-adhlns=$(OBJDIR)/$(notdir $(subst $(suffix $<),.lst,$<)) #CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -CFLAGS += -lm +CFLAGS += -lm -pthread # flags only for C CFLAGS += -Wstrict-prototypes -Wmissing-declarations diff --git a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c index 2026cd3bcb..1f47616c3b 100644 --- a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c +++ b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c @@ -42,6 +42,31 @@ void *sys_time_thread_main(void *data); #define NSEC_OF_SEC(sec) ((sec) * 1e9) +static inline int get_rt_prio(int prio) +{ + struct sched_param param; + int policy; + pthread_getschedparam(pthread_self(), &policy, ¶m); + printf("Current shedparam: policy %d, prio %d\n", policy, param.sched_priority); + + //SCHED_RR, SCHED_FIFO, SCHED_OTHER (POSIX scheduling policies) + int sched = SCHED_FIFO; + int min = sched_get_priority_min(sched); + int max = sched_get_priority_max(sched); + printf("Current min/max prios: %d/%d\n", min, max); + param.sched_priority = prio; + if (pthread_setschedparam(pthread_self(), sched, ¶m)) { + perror("setchedparam failed!"); + return -1; + } + else { + pthread_getschedparam(pthread_self(), &policy, ¶m); + printf("New shedparam: policy %d, prio %d\n", policy, param.sched_priority); + } + + return 0; +} + void *sys_time_thread_main(void *data) { int fd; @@ -53,6 +78,8 @@ void *sys_time_thread_main(void *data) return NULL; } + get_rt_prio(29); + /* Make the timer periodic */ struct itimerspec timer; /* timer expires after sys_time.resolution sec */ From b2f1efbafaca68e74136a342d812d63eb4d544b6 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 23 Feb 2015 18:11:43 +0100 Subject: [PATCH 3/7] [arch/linux]call sys_tick_handler more than once if we missed some events --- sw/airborne/arch/linux/mcu_periph/sys_time_arch.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c index 1f47616c3b..7af8fd8615 100644 --- a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c +++ b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c @@ -105,8 +105,11 @@ void *sys_time_thread_main(void *data) if (missed > 1) { printf("Missed %lld timer events!\n", missed); } - /* advance sys_time */ - sys_tick_handler(); + /* advance sys_time, in case we missed some events: call it more than once */ + unsigned int i; + for (i = 0; i < missed; i++) { + sys_tick_handler(); + } } return NULL; } From f25770882f1fbc587373bb982f8835d5f41ac6ac Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 23 Feb 2015 19:12:45 +0100 Subject: [PATCH 4/7] [arch/linux] separate file for get_rt_prio and some cleanup --- .../arch/linux/mcu_periph/sys_time_arch.c | 41 ++---------- sw/airborne/arch/linux/rt_priority.h | 65 +++++++++++++++++++ 2 files changed, 72 insertions(+), 34 deletions(-) create mode 100644 sw/airborne/arch/linux/rt_priority.h diff --git a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c index 7af8fd8615..9c325fe4df 100644 --- a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c +++ b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c @@ -25,48 +25,25 @@ */ #include "mcu_periph/sys_time.h" -#include -#include -#include #include #include #include +#include "rt_priority.h" #ifdef SYS_TIME_LED #include "led.h" #endif +#ifndef SYS_TIME_THREAD_PRIO +#define SYS_TIME_THREAD_PRIO 29 +#endif + pthread_t sys_time_thread; static void sys_tick_handler(void); void *sys_time_thread_main(void *data); #define NSEC_OF_SEC(sec) ((sec) * 1e9) -static inline int get_rt_prio(int prio) -{ - struct sched_param param; - int policy; - pthread_getschedparam(pthread_self(), &policy, ¶m); - printf("Current shedparam: policy %d, prio %d\n", policy, param.sched_priority); - - //SCHED_RR, SCHED_FIFO, SCHED_OTHER (POSIX scheduling policies) - int sched = SCHED_FIFO; - int min = sched_get_priority_min(sched); - int max = sched_get_priority_max(sched); - printf("Current min/max prios: %d/%d\n", min, max); - param.sched_priority = prio; - if (pthread_setschedparam(pthread_self(), sched, ¶m)) { - perror("setchedparam failed!"); - return -1; - } - else { - pthread_getschedparam(pthread_self(), &policy, ¶m); - printf("New shedparam: policy %d, prio %d\n", policy, param.sched_priority); - } - - return 0; -} - void *sys_time_thread_main(void *data) { int fd; @@ -78,7 +55,7 @@ void *sys_time_thread_main(void *data) return NULL; } - get_rt_prio(29); + get_rt_prio(SYS_TIME_THREAD_PRIO); /* Make the timer periodic */ struct itimerspec timer; @@ -103,7 +80,7 @@ void *sys_time_thread_main(void *data) perror("Couldn't read timer!"); } if (missed > 1) { - printf("Missed %lld timer events!\n", missed); + fprintf(stderr, "Missed %lld timer events!\n", missed); } /* advance sys_time, in case we missed some events: call it more than once */ unsigned int i; @@ -116,22 +93,18 @@ void *sys_time_thread_main(void *data) void sys_time_arch_init(void) { - sys_time.cpu_ticks_per_sec = 1e6; sys_time.resolution_cpu_ticks = (uint32_t)(sys_time.resolution * sys_time.cpu_ticks_per_sec + 0.5); - int ret = pthread_create(&sys_time_thread, NULL, sys_time_thread_main, NULL); if (ret) { perror("Could not setup sys_time_thread"); return; } - } static void sys_tick_handler(void) { - sys_time.nb_tick++; sys_time.nb_sec_rem += sys_time.resolution_cpu_ticks;; if (sys_time.nb_sec_rem >= sys_time.cpu_ticks_per_sec) { diff --git a/sw/airborne/arch/linux/rt_priority.h b/sw/airborne/arch/linux/rt_priority.h new file mode 100644 index 0000000000..de6f8e0f11 --- /dev/null +++ b/sw/airborne/arch/linux/rt_priority.h @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2015 Felix Ruess + * + * 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, see + * . + */ + +/** + * @file rt_priority.h + * Function to obtain rt priority. + */ + +#ifndef RT_PRIORITY_H +#define RT_PRIORITY_H + +#include +#include + +static inline int get_rt_prio(int prio) +{ + struct sched_param param; + int policy; + pthread_getschedparam(pthread_self(), &policy, ¶m); + printf("Current schedparam: policy %d, prio %d\n", policy, param.sched_priority); + + //SCHED_RR, SCHED_FIFO, SCHED_OTHER (POSIX scheduling policies) + int sched = SCHED_FIFO; + int min = sched_get_priority_min(sched); + int max = sched_get_priority_max(sched); + param.sched_priority = prio; + if (prio > max || prio < min) { + printf("Requested prio %d outside current min/max prios: %d/%d\n", prio, min, max); + if (prio > max) { + param.sched_priority = max; + } else { + param.sched_priority = min; + } + } + + if (pthread_setschedparam(pthread_self(), sched, ¶m)) { + perror("setschedparam failed!"); + return -1; + } + else { + pthread_getschedparam(pthread_self(), &policy, ¶m); + printf("New schedparam: policy %d, prio %d\n", policy, param.sched_priority); + } + + return 0; +} + +#endif /* RT_PRIORITY_H */ From 85ffc78fb53ac3b83829f519ba46c33b594c917f Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Tue, 24 Feb 2015 14:20:02 +0100 Subject: [PATCH 5/7] [ardrone] Fix IMU cleaning bug (discovered with new sys_time implementation) --- sw/airborne/boards/ardrone/navdata.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index e791cdb8e6..0fd4d18939 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -198,14 +198,14 @@ bool_t navdata_init() navdata_write(&cmd, 1); // read some potential dirt - // wait 10 milliseconds - char tmp[100]; + // wait 10 milliseconds (doesn't work anymore because device doesn't support EAGAIN) + /*char tmp[100]; for (int i = 0; i < 12; i++) { uint16_t dirt = read(nav_fd, tmp, sizeof tmp); (void) dirt; usleep(1000); - } + }*/ baro_calibrated = FALSE; if (!acquire_baro_calibration()) { From e2b741be03cd4e5c5a81a88c525e7d30a9c508be Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Wed, 25 Feb 2015 16:40:06 +0100 Subject: [PATCH 6/7] [ardrone] Make navdata multi-threaded because of blocking read --- sw/airborne/boards/ardrone/baro_board.c | 32 +- sw/airborne/boards/ardrone/navdata.c | 516 ++++++++---------- sw/airborne/boards/ardrone/navdata.h | 58 +- sw/airborne/subsystems/imu/imu_ardrone2_raw.h | 10 +- 4 files changed, 299 insertions(+), 317 deletions(-) diff --git a/sw/airborne/boards/ardrone/baro_board.c b/sw/airborne/boards/ardrone/baro_board.c index 939106b936..876e3a33bb 100644 --- a/sw/airborne/boards/ardrone/baro_board.c +++ b/sw/airborne/boards/ardrone/baro_board.c @@ -57,15 +57,15 @@ void baro_periodic(void) {} */ static inline int32_t baro_apply_calibration(int32_t raw) { - int32_t b6 = ((int32_t)baro_calibration.b5) - 4000L; - int32_t x1 = (((int32_t)baro_calibration.b2) * (b6 * b6 >> 12)) >> 11; - int32_t x2 = ((int32_t)baro_calibration.ac2) * b6 >> 11; + int32_t b6 = ((int32_t)navdata.bmp180_calib.b5) - 4000L; + int32_t x1 = (((int32_t)navdata.bmp180_calib.b2) * (b6 * b6 >> 12)) >> 11; + int32_t x2 = ((int32_t)navdata.bmp180_calib.ac2) * b6 >> 11; int32_t x3 = x1 + x2; - int32_t b3 = (((((int32_t)baro_calibration.ac1) * 4 + x3) << BMP180_OSS) + 2) / 4; - x1 = ((int32_t)baro_calibration.ac3) * b6 >> 13; - x2 = (((int32_t)baro_calibration.b1) * (b6 * b6 >> 12)) >> 16; + int32_t b3 = (((((int32_t)navdata.bmp180_calib.ac1) * 4 + x3) << BMP180_OSS) + 2) / 4; + x1 = ((int32_t)navdata.bmp180_calib.ac3) * b6 >> 13; + x2 = (((int32_t)navdata.bmp180_calib.b1) * (b6 * b6 >> 12)) >> 16; x3 = ((x1 + x2) + 2) >> 2; - uint32_t b4 = (((int32_t)baro_calibration.ac4) * (uint32_t)(x3 + 32768L)) >> 15; + uint32_t b4 = (((int32_t)navdata.bmp180_calib.ac4) * (uint32_t)(x3 + 32768L)) >> 15; uint32_t b7 = (raw - b3) * (50000L >> BMP180_OSS); int32_t p = b7 < 0x80000000L ? (b7 * 2) / b4 : (b7 / b4) * 2; x1 = (p >> 8) * (p >> 8); @@ -83,26 +83,26 @@ static inline int32_t baro_apply_calibration(int32_t raw) */ static inline int32_t baro_apply_calibration_temp(int32_t tmp_raw) { - int32_t x1 = ((tmp_raw - ((int32_t)baro_calibration.ac6)) * ((int32_t)baro_calibration.ac5)) >> 15; - int32_t x2 = (((int32_t)baro_calibration.mc) << 11) / (x1 + ((int32_t)baro_calibration.md)); - baro_calibration.b5 = x1 + x2; - return (baro_calibration.b5 + 8) >> 4; + int32_t x1 = ((tmp_raw - ((int32_t)navdata.bmp180_calib.ac6)) * ((int32_t)navdata.bmp180_calib.ac5)) >> 15; + int32_t x2 = (((int32_t)navdata.bmp180_calib.mc) << 11) / (x1 + ((int32_t)navdata.bmp180_calib.md)); + navdata.bmp180_calib.b5 = x1 + x2; + return (navdata.bmp180_calib.b5 + 8) >> 4; } void ardrone_baro_event(void) { - if (navdata_baro_available) { - if (baro_calibrated) { + if (navdata.baro_available) { + if (navdata.baro_calibrated) { // first read temperature because pressure calibration depends on temperature - float temp_deg = 0.1 * baro_apply_calibration_temp(navdata.temperature_pressure); + float temp_deg = 0.1 * baro_apply_calibration_temp(navdata.measure.temperature_pressure); AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp_deg); - int32_t press_pascal = baro_apply_calibration(navdata.pressure); + int32_t press_pascal = baro_apply_calibration(navdata.measure.pressure); #if USE_BARO_MEDIAN_FILTER press_pascal = update_median_filter(&baro_median, press_pascal); #endif float pressure = (float)press_pascal; AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); } - navdata_baro_available = FALSE; + navdata.baro_available = FALSE; } } diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 0fd4d18939..bfce8c7629 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -1,5 +1,6 @@ /* * Copyright (C) 2013 Dino Hensen, Vincent van Hoek + * 2015 Freek van Tienen * * This file is part of Paparazzi. * @@ -29,13 +30,14 @@ #include #include -#include // for O_RDWR, O_NOCTTY, O_NONBLOCK +#include #include // for baud rates and options #include #include #include #include #include +#include #include "std.h" #include "navdata.h" @@ -44,21 +46,13 @@ #include "subsystems/abi.h" #include "mcu_periph/gpio.h" -#define NAVDATA_PACKET_SIZE 60 -#define NAVDATA_START_BYTE 0x3a +/* Internal used functions */ +static void *navdata_read(void *data __attribute__((unused))); +static void navdata_cmd_send(uint8_t cmd); +static inline bool_t navdata_baro_calib(void); -#define ARDRONE_GPIO_PORT 0x32524 -#define ARDRONE_GPIO_PIN_NAVDATA 177 - -static inline bool_t acquire_baro_calibration(void); -static void navdata_cropbuffer(int cropsize); - -navdata_port nav_port; -static int nav_fd = 0; -measures_t navdata; - -static int imu_lost = 0; -static int imu_lost_counter = 0; +/* Main navdata structure */ +struct navdata_t navdata; /** Sonar offset. * Offset value in ADC @@ -75,6 +69,9 @@ static int imu_lost_counter = 0; #define SONAR_SCALE 0.00047 #endif +/** + * Write to fd even while being interrupted + */ ssize_t full_write(int fd, const uint8_t *buf, size_t count) { size_t written = 0; @@ -92,6 +89,9 @@ ssize_t full_write(int fd, const uint8_t *buf, size_t count) return written; } +/** + * Read from fd even while being interrupted + */ ssize_t full_read(int fd, uint8_t *buf, size_t count) { /* Apologies for illiteracy, but we can't overload |read|.*/ @@ -110,217 +110,215 @@ ssize_t full_read(int fd, uint8_t *buf, size_t count) 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"); - } -} - #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" static void send_navdata(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_ARDRONE_NAVDATA(trans, dev, AC_ID, - &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, - &nav_port.checksum_errors); + &navdata.measure.taille, + &navdata.measure.nu_trame, + &navdata.measure.ax, + &navdata.measure.ay, + &navdata.measure.az, + &navdata.measure.vx, + &navdata.measure.vy, + &navdata.measure.vz, + &navdata.measure.temperature_acc, + &navdata.measure.temperature_gyro, + &navdata.measure.ultrasound, + &navdata.measure.us_debut_echo, + &navdata.measure.us_fin_echo, + &navdata.measure.us_association_echo, + &navdata.measure.us_distance_echo, + &navdata.measure.us_curve_time, + &navdata.measure.us_curve_value, + &navdata.measure.us_curve_ref, + &navdata.measure.nb_echo, + &navdata.measure.sum_echo, + &navdata.measure.gradient, + &navdata.measure.flag_echo_ini, + &navdata.measure.pressure, + &navdata.measure.temperature_pressure, + &navdata.measure.mx, + &navdata.measure.my, + &navdata.measure.mz, + &navdata.measure.chksum, + &navdata.checksum_errors); } static void send_filter_status(struct transport_tx *trans, struct link_device *dev) { uint8_t mde = 3; if (ahrs.status == AHRS_UNINIT) { mde = 2; } - if (imu_lost) { mde = 5; } - uint16_t val = imu_lost_counter; + if (navdata.imu_lost) { mde = 5; } + uint16_t val = navdata.lost_imu_frames; pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val); } #endif +/** + * Initialize the navdata board + */ bool_t navdata_init() { - if (nav_fd <= 0) { - nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK); + assert(sizeof(struct navdata_measure_t) == NAVDATA_PACKET_SIZE); - if (nav_fd == -1) { - perror("navdata_init: Unable to open /dev/ttyO1 - "); + // Check if the FD isn't already initialized + if (navdata.fd <= 0) { + navdata.fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY); //O_NONBLOCK doesn't work + + if (navdata.fd < 0) { + printf("[navdata] Unable to open navdata board connection(/dev/ttyO1)\n"); return FALSE; } + + // Update the settings of the UART connection + fcntl(navdata.fd, F_SETFL, 0); //read calls are non blocking + //set port options + struct termios options; + //Get the current options for the port + tcgetattr(navdata.fd, &options); + //Set the baud rates to 460800 + cfsetispeed(&options, B460800); + cfsetospeed(&options, B460800); + + options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode + options.c_iflag = 0; //clear input options + options.c_lflag = 0; //clear local options + options.c_oflag &= ~OPOST; //clear output options (raw output) + + //Set the new options for the port + tcsetattr(navdata.fd, TCSANOW, &options); } - fcntl(nav_fd, F_SETFL, 0); //read calls are non blocking - //set port options - struct termios options; - //Get the current options for the port - tcgetattr(nav_fd, &options); - //Set the baud rates to 460800 - cfsetispeed(&options, B460800); - cfsetospeed(&options, B460800); + // Reset available flags + navdata.baro_calibrated = FALSE; + navdata.imu_available = FALSE; + navdata.baro_available = FALSE; + navdata.imu_lost = FALSE; - options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode - options.c_iflag = 0; //clear input options - options.c_lflag = 0; //clear local options - options.c_oflag &= ~OPOST; //clear output options (raw output) + // Set all statistics to 0 + navdata.checksum_errors = 0; + navdata.lost_imu_frames = 0; + navdata.totalBytesRead = 0; + navdata.packetsRead = 0; + navdata.last_packet_number = 0; - //Set the new options for the port - tcsetattr(nav_fd, TCSANOW, &options); + // Stop acquisition + navdata_cmd_send(NAVDATA_CMD_STOP); - // stop acquisition - uint8_t cmd = 0x02; - navdata_write(&cmd, 1); - - // read some potential dirt - // wait 10 milliseconds (doesn't work anymore because device doesn't support EAGAIN) - /*char tmp[100]; - for (int i = 0; i < 12; i++) { - uint16_t dirt = read(nav_fd, tmp, sizeof tmp); - (void) dirt; - - usleep(1000); - }*/ - - baro_calibrated = FALSE; - if (!acquire_baro_calibration()) { + // Read the baro calibration(blocking) + if (!navdata_baro_calib()) { + printf("[navdata] Could not acquire baro calibration!\n"); return FALSE; } + navdata.baro_calibrated = TRUE; - // start acquisition - cmd = 0x01; - navdata_write(&cmd, 1); + // Start acquisition + navdata_cmd_send(NAVDATA_CMD_START); - navdata_imu_available = FALSE; - navdata_baro_available = FALSE; - - nav_port.checksum_errors = 0; - nav_port.lost_imu_frames = 0; - nav_port.bytesRead = 0; - nav_port.totalBytesRead = 0; - nav_port.packetsRead = 0; - nav_port.isInitialized = TRUE; - nav_port.last_packet_number = 0; - - // set navboard gpio control + // Set navboard gpio control gpio_setup_output(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); + // Start navdata reading thread + pthread_t navdata_thread; + if(pthread_create(&navdata_thread, NULL, navdata_read, NULL) != 0) { + printf("[navdata] Could not create navdata reading thread!\n"); + return FALSE; + } + #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "ARDRONE_NAVDATA", send_navdata); register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); #endif + // Set to initialized + navdata.is_initialized = TRUE; return TRUE; } -static inline bool_t acquire_baro_calibration(void) +/** + * Try to receive the baro calibration from the navdata board + */ +static inline bool_t navdata_baro_calib(void) { - // start baro calibration acquisition - uint8_t cmd = 0x17; // send cmd 23 - navdata_write(&cmd, 1); - - // wait 20ms to retrieve data - for (int i = 0; i < 22; i++) { - usleep(1000); - } + // Start baro calibration acquisition + navdata_cmd_send(NAVDATA_CMD_BARO_CALIB); + // Receive the calibration (blocking) uint8_t calibBuffer[22]; - - if (full_read(nav_fd, calibBuffer, sizeof calibBuffer) < 0) { - perror("acquire_baro_calibration: read failed"); + if (full_read(navdata.fd, calibBuffer, sizeof calibBuffer) < 0) { + printf("[navdata] Could not read calibration data."); return FALSE; } - baro_calibration.ac1 = calibBuffer[0] << 8 | calibBuffer[1]; - baro_calibration.ac2 = calibBuffer[2] << 8 | calibBuffer[3]; - baro_calibration.ac3 = calibBuffer[4] << 8 | calibBuffer[5]; - baro_calibration.ac4 = calibBuffer[6] << 8 | calibBuffer[7]; - baro_calibration.ac5 = calibBuffer[8] << 8 | calibBuffer[9]; - baro_calibration.ac6 = calibBuffer[10] << 8 | calibBuffer[11]; - baro_calibration.b1 = calibBuffer[12] << 8 | calibBuffer[13]; - baro_calibration.b2 = calibBuffer[14] << 8 | calibBuffer[15]; - baro_calibration.mb = calibBuffer[16] << 8 | calibBuffer[17]; - baro_calibration.mc = calibBuffer[18] << 8 | calibBuffer[19]; - baro_calibration.md = calibBuffer[20] << 8 | calibBuffer[21]; + //Convert the read bytes + navdata.bmp180_calib.ac1 = calibBuffer[0] << 8 | calibBuffer[1]; + navdata.bmp180_calib.ac2 = calibBuffer[2] << 8 | calibBuffer[3]; + navdata.bmp180_calib.ac3 = calibBuffer[4] << 8 | calibBuffer[5]; + navdata.bmp180_calib.ac4 = calibBuffer[6] << 8 | calibBuffer[7]; + navdata.bmp180_calib.ac5 = calibBuffer[8] << 8 | calibBuffer[9]; + navdata.bmp180_calib.ac6 = calibBuffer[10] << 8 | calibBuffer[11]; + navdata.bmp180_calib.b1 = calibBuffer[12] << 8 | calibBuffer[13]; + navdata.bmp180_calib.b2 = calibBuffer[14] << 8 | calibBuffer[15]; + navdata.bmp180_calib.mb = calibBuffer[16] << 8 | calibBuffer[17]; + navdata.bmp180_calib.mc = calibBuffer[18] << 8 | calibBuffer[19]; + navdata.bmp180_calib.md = calibBuffer[20] << 8 | calibBuffer[21]; - printf("Calibration AC1: %d\n", baro_calibration.ac1); - printf("Calibration AC2: %d\n", baro_calibration.ac2); - printf("Calibration AC3: %d\n", baro_calibration.ac3); - printf("Calibration AC4: %d\n", baro_calibration.ac4); - printf("Calibration AC5: %d\n", baro_calibration.ac5); - printf("Calibration AC6: %d\n", baro_calibration.ac6); - - printf("Calibration B1: %d\n", baro_calibration.b1); - printf("Calibration B2: %d\n", baro_calibration.b2); - - printf("Calibration MB: %d\n", baro_calibration.mb); - printf("Calibration MC: %d\n", baro_calibration.mc); - printf("Calibration MD: %d\n", baro_calibration.md); - - baro_calibrated = TRUE; return TRUE; } -void navdata_read() +/** + * Main reading thread + * This is done asynchronous because the navdata board doesn't support NON_BLOCKING + */ +static void *navdata_read(void *data __attribute__((unused))) { - int newbytes = read(nav_fd, nav_port.buffer + nav_port.bytesRead, NAVDATA_BUFFER_SIZE - nav_port.bytesRead); + while(TRUE) { + // Check if we need to read something or already have a packet + if((NAVDATA_PACKET_SIZE - navdata.buffer_idx) <= 0) + continue; - // because non-blocking read returns -1 when no bytes available - if (newbytes > 0) { - nav_port.bytesRead += newbytes; - nav_port.totalBytesRead += newbytes; + // Read new bytes + int newbytes = read(navdata.fd, navdata.buffer + navdata.buffer_idx, NAVDATA_PACKET_SIZE - navdata.buffer_idx); + + // When there was no signal interrupt + if (newbytes > 0) { + navdata.buffer_idx += newbytes; + navdata.totalBytesRead += newbytes; + } } + + return NULL; } - +/** + * Check if the magneto is frozen + * Unknown why this bug happens. + */ static void mag_freeze_check(void) { // Thanks to Daren.G.Lee for initial fix on 20140530 static int16_t LastMagValue = 0; static int MagFreezeCounter = 0; - if (LastMagValue == navdata.mx) { + if (LastMagValue == navdata.measure.mx) { MagFreezeCounter++; // has to have at least 30 times the same value to consider it a frozen magnetometer value if (MagFreezeCounter > 30) { - //printf("Magetometer is frozen. Lastvalue X: %d , currentvalue X: %d resetting...", LastMagValue, navdata.mx); // set imu_lost flag - imu_lost = 1; - imu_lost_counter++; + navdata.imu_lost = TRUE; + navdata.lost_imu_frames++; - // stop acquisition - uint8_t cmd = 0x02; - navdata_write(&cmd, 1); - // do the navboard reset via GPIOs + // Stop acquisition + navdata_cmd_send(NAVDATA_CMD_STOP); + + // Reset the hardware of the navboard gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); - // a delay added, otherwise gpio_set sometime does not work usleep(20000); gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); @@ -328,38 +326,27 @@ static void mag_freeze_check(void) //uint16_t val = 0; //DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &val); - // wait 40ms to retrieve data - // using 40 times a 1ms wait in case the usleep function - // is interupted by a signal - for (int i = 0; i < 40; i++) { - usleep(1000); - } - - // restart acquisition - cmd = 0x01; - - // Weird, not having one more a delay and fix does not work... thus pragmatic fix - usleep(5000); - - /* Due to the Ardrone2 NAVBoard design, one time restarting does not work - * in all cases, but multiple attempts do. - */ - for (int i = 0; i < 10; i++) { - usleep(1000); - navdata_write(&cmd, 1); - } + // Wait for 40ms for it to boot + usleep(40000); + // Start the navdata again and reset the counter + navdata_cmd_send(NAVDATA_CMD_START); MagFreezeCounter = 0; // reset counter back to zero } } else { - imu_lost = 0; + navdata.imu_lost = FALSE; // Reset counter if value _does_ change MagFreezeCounter = 0; } // set last value - LastMagValue = navdata.mx; + LastMagValue = navdata.measure.mx; } +/** + * Handle the baro(pressure/temperature) logic + * Sometimes the temperature and pressure are switched because of a bug in + * the navdata board firmware. + */ static void baro_update_logic(void) { static int32_t lastpressval = 0; @@ -379,7 +366,7 @@ static void baro_update_logic(void) // This means that press must remain constant if (lastpressval != 0) { // If pressure was updated: this is a sync error - if (lastpressval != navdata.pressure) { + if (lastpressval != navdata.measure.pressure) { // wait for temp again temp_or_press_was_updated_last = FALSE; sync_errors++; @@ -393,7 +380,7 @@ static void baro_update_logic(void) // This means that temp must remain constant if (lasttempval != 0) { // If temp was updated: this is a sync error - if (lasttempval != navdata.temperature_pressure) { + if (lasttempval != navdata.measure.temperature_pressure) { // wait for press again temp_or_press_was_updated_last = TRUE; sync_errors++; @@ -401,25 +388,25 @@ static void baro_update_logic(void) } else { // We now got valid pressure and temperature - navdata_baro_available = TRUE; + navdata.baro_available = TRUE; } } } // Detected a pressure switch if (lastpressval != 0 && lasttempval != 0 - && ABS(lastpressval - navdata.pressure) > ABS(lasttempval - navdata.pressure)) { - navdata_baro_available = FALSE; + && ABS(lastpressval - navdata.measure.pressure) > ABS(lasttempval - navdata.measure.pressure)) { + navdata.baro_available = FALSE; } // Detected a temprature switch if (lastpressval != 0 && lasttempval != 0 - && ABS(lasttempval - navdata.temperature_pressure) > ABS(lastpressval - navdata.temperature_pressure)) { - navdata_baro_available = FALSE; + && ABS(lasttempval - navdata.measure.temperature_pressure) > ABS(lastpressval - navdata.measure.temperature_pressure)) { + navdata.baro_available = FALSE; } - lasttempval = navdata.temperature_pressure; - lastpressval = navdata.pressure; + lasttempval = navdata.measure.temperature_pressure; + lastpressval = navdata.measure.pressure; /* * It turns out that a lot of navdata boards have a problem (probably interrupt related) @@ -461,130 +448,107 @@ static void baro_update_logic(void) */ // if press and temp are same and temp has jump: neglect the next frame - if (navdata.temperature_pressure == - navdata.pressure) { // && (abs((int32_t)navdata.temperature_pressure - (int32_t)lasttempval) > 40)) + if (navdata.measure.temperature_pressure == + navdata.measure.pressure) { // && (abs((int32_t)navdata.temperature_pressure - (int32_t)lasttempval) > 40)) // dont use next 3 packets spike_detected = 3; } if (spike_detected > 0) { // disable kalman filter use - navdata_baro_available = FALSE; + navdata.baro_available = FALSE; // override both to last good - navdata.pressure = lastpressval_nospike; - navdata.temperature_pressure = lasttempval_nospike; + navdata.measure.pressure = lastpressval_nospike; + navdata.measure.temperature_pressure = lasttempval_nospike; // Countdown spike_detected--; } else { // both are good - lastpressval_nospike = navdata.pressure; - lasttempval_nospike = navdata.temperature_pressure; + lastpressval_nospike = navdata.measure.pressure; + lasttempval_nospike = navdata.measure.temperature_pressure; } - -// printf("%d %d %d\r\n", navdata.temperature_pressure, navdata.pressure, spike_detected); -// printf(",%d,%d",spike_detected,spikes); } +/** + * Update the navdata (event loop) + */ void navdata_update() { // Check if initialized - if (!nav_port.isInitialized) { + if (!navdata.is_initialized) { navdata_init(); mag_freeze_check(); return; } - // Start reading - navdata_read(); + // If we got a full packet + if (navdata.buffer_idx >= NAVDATA_PACKET_SIZE) { + // check if the start byte is correct + if (navdata.buffer[0] != NAVDATA_START_BYTE) { + uint8_t *pint = memchr(navdata.buffer, NAVDATA_START_BYTE, navdata.buffer_idx); + navdata.buffer_idx = (pint != NULL)? (pint - navdata.buffer):0; + return; + } - // while there is something interesting to do... - while (nav_port.bytesRead >= NAVDATA_PACKET_SIZE) { - if (nav_port.buffer[0] == NAVDATA_START_BYTE) { - assert(sizeof navdata == NAVDATA_PACKET_SIZE); - memcpy(&navdata, nav_port.buffer, NAVDATA_PACKET_SIZE); + // Copy the navdata packet + memcpy(&navdata.measure, navdata.buffer, NAVDATA_PACKET_SIZE); - // Calculating the checksum - uint16_t checksum = 0; - for (int i = 2; i < NAVDATA_PACKET_SIZE - 2; i += 2) { - checksum += nav_port.buffer[i] + (nav_port.buffer[i + 1] << 8); - } + // Calculating the checksum + uint16_t checksum = 0; + for (int i = 2; i < NAVDATA_PACKET_SIZE - 2; i += 2) { + checksum += navdata.buffer[i] + (navdata.buffer[i + 1] << 8); + } - // When checksum is incorrect - if (navdata.chksum != checksum) { - printf("Checksum error [calculated: %d] [packet: %d] [diff: %d]\n", checksum , navdata.chksum, - checksum - navdata.chksum); - nav_port.checksum_errors++; - } + // Check if the checksum is ok + if (navdata.measure.chksum != checksum) { + printf("[navdata] Checksum error [calculated: %d] [packet: %d] [diff: %d]\n", checksum , navdata.measure.chksum, + checksum - navdata.measure.chksum); + navdata.checksum_errors++; + navdata.buffer_idx = 0; + return; + } - nav_port.last_packet_number++; - if (nav_port.last_packet_number != navdata.nu_trame) { - //printf("Lost Navdata frame: %d should have been %d\n",navdata.nu_trame, nav_port.last_packet_number); - nav_port.lost_imu_frames++; - } - nav_port.last_packet_number = navdata.nu_trame; - //printf("%d\r",navdata.nu_trame); + // Check if we missed a packet (our counter and the one from the navdata) + navdata.last_packet_number++; + if (navdata.last_packet_number != navdata.measure.nu_trame) { + printf("[navdata] Lost frame: %d should have been %d\n", navdata.measure.nu_trame, navdata.last_packet_number); + navdata.lost_imu_frames++; + } + navdata.last_packet_number = navdata.measure.nu_trame; - // When checksum is correct - if (navdata.chksum == checksum) { - // Invert byte order so that TELEMETRY works better - uint8_t tmp; - uint8_t *p = (uint8_t *) & (navdata.pressure); - tmp = p[0]; - p[0] = p[1]; - p[1] = tmp; - p = (uint8_t *) & (navdata.temperature_pressure); - tmp = p[0]; - p[0] = p[1]; - p[1] = tmp; + // Invert byte order so that TELEMETRY works better + uint8_t tmp; + uint8_t *p = (uint8_t *) & (navdata.measure.pressure); + tmp = p[0]; + p[0] = p[1]; + p[1] = tmp; + p = (uint8_t *) & (navdata.measure.temperature_pressure); + tmp = p[0]; + p[0] = p[1]; + p[1] = tmp; -// printf("%d,%d,%d",navdata.nu_trame, navdata.pressure, navdata.temperature_pressure); - - baro_update_logic(); - -// printf(",%d,%d,%d\n", navdata.pressure, navdata.temperature_pressure, (int)navdata_baro_available); - - mag_freeze_check(); + baro_update_logic(); + mag_freeze_check(); #ifdef USE_SONAR - // Check if there is a new sonar measurement and update the sonar - if (navdata.ultrasound >> 15) { - float sonar_meas = (float)((navdata.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE; - AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, sonar_meas); - } + // Check if there is a new sonar measurement and update the sonar + if (navdata.measure.ultrasound >> 15) { + float sonar_meas = (float)((navdata.measure.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE; + AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, sonar_meas); + } #endif - navdata_imu_available = TRUE; - nav_port.packetsRead++; - } - - // Crop the buffer - navdata_cropbuffer(NAVDATA_PACKET_SIZE); - } else { - // find start byte, copy all data from startbyte to buffer origin, update bytesread - uint8_t *pint; - pint = memchr(nav_port.buffer, NAVDATA_START_BYTE, nav_port.bytesRead); - - if (pint != NULL) { - navdata_cropbuffer(pint - nav_port.buffer); - } else { - // if the start byte was not found, it means there is junk in the buffer - nav_port.bytesRead = 0; - } - } + navdata.imu_available = TRUE; + navdata.packetsRead++; + navdata.buffer_idx = 0; } } -static void navdata_cropbuffer(int cropsize) -{ - if (nav_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", nav_port.bytesRead, cropsize, - nav_port.buffer); - return; - } - - memmove(nav_port.buffer, nav_port.buffer + cropsize, NAVDATA_BUFFER_SIZE - cropsize); - nav_port.bytesRead -= cropsize; +/** + * Sends a one byte command + */ +static void navdata_cmd_send(uint8_t cmd) { + full_write(navdata.fd, &cmd, 1); } diff --git a/sw/airborne/boards/ardrone/navdata.h b/sw/airborne/boards/ardrone/navdata.h index 2613bc70eb..94ad39faf9 100644 --- a/sw/airborne/boards/ardrone/navdata.h +++ b/sw/airborne/boards/ardrone/navdata.h @@ -30,10 +30,13 @@ #ifndef NAVDATA_H_ #define NAVDATA_H_ -#include -#include +#include "std.h" -typedef struct { +/** + * Main navdata structure from the navdata board + * This is received from the navdata board at ~200Hz + */ +struct navdata_measure_t { uint16_t taille; uint16_t nu_trame; @@ -74,9 +77,10 @@ typedef struct { uint16_t chksum; -} __attribute__((packed)) measures_t; +} __attribute__((packed)); -struct bmp180_baro_calibration { +/* The baro calibration received from the navboard */ +struct bmp180_calib_t { int16_t ac1; int16_t ac2; int16_t ac3; @@ -93,32 +97,46 @@ struct bmp180_baro_calibration { int32_t b5; }; -#define NAVDATA_BUFFER_SIZE 80 -typedef struct { - uint8_t isInitialized; - uint16_t bytesRead; +/* Navdata board defines */ +#define NAVDATA_PACKET_SIZE 60 +#define NAVDATA_START_BYTE 0x3A +#define NAVDATA_CMD_START 0x01 +#define NAVDATA_CMD_STOP 0x02 +#define NAVDATA_CMD_BARO_CALIB 0x17 + +#define ARDRONE_GPIO_PORT 0x32524 +#define ARDRONE_GPIO_PIN_NAVDATA 177 + +/* Main navdata structure */ +struct navdata_t { + bool_t is_initialized; //< Check if the navdata board is initialized + int fd; //< The navdata file pointer + uint32_t totalBytesRead; uint32_t packetsRead; uint32_t checksum_errors; uint32_t lost_imu_frames; uint16_t last_packet_number; - uint8_t buffer[NAVDATA_BUFFER_SIZE]; -} navdata_port; -extern measures_t navdata; -extern navdata_port nav_port; -struct bmp180_baro_calibration baro_calibration; -navdata_port *port; -uint16_t navdata_cks; -uint8_t navdata_imu_available; -uint8_t navdata_baro_available; -uint8_t baro_calibrated; + volatile uint8_t buffer_idx; //< Buffer insert index for reading/writing + uint8_t buffer[NAVDATA_PACKET_SIZE]; //< Buffer filled in the thread (maximum one navdata packet) + + struct navdata_measure_t measure; //< Main navdata packet receieved from navboard + struct bmp180_calib_t bmp180_calib; //< BMP180 calibration receieved from navboard + + bool_t baro_calibrated; //< Whenever the baro is calibrated + bool_t imu_available; //< Whenever the imu is available + bool_t baro_available; //< Whenever the baro is available + bool_t imu_lost; //< Whenever the imu is lost +}; +extern struct navdata_t navdata; + bool_t navdata_init(void); -void navdata_read(void); void navdata_update(void); int16_t navdata_height(void); +/* This should be moved to the uart handling part! */ 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); diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h index 69975d54ab..f01b241e1b 100644 --- a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h +++ b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h @@ -89,11 +89,11 @@ static inline void imu_ardrone2_event(void (* _gyro_handler)(void), void (* _acc { navdata_update(); //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); + if (navdata.imu_available == TRUE) { + navdata.imu_available = FALSE; + RATES_ASSIGN(imu.gyro_unscaled, navdata.measure.vx, -navdata.measure.vy, -navdata.measure.vz); + VECT3_ASSIGN(imu.accel_unscaled, navdata.measure.ax, 4096 - navdata.measure.ay, 4096 - navdata.measure.az); + VECT3_ASSIGN(imu.mag_unscaled, -navdata.measure.mx, -navdata.measure.my, -navdata.measure.mz); _gyro_handler(); _accel_handler(); From 973372733d0b8af8bb4c68e92a0ecd3a59023afe Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Wed, 25 Feb 2015 16:57:36 +0100 Subject: [PATCH 7/7] [ardrone] Fix small navdata bug and update changelog --- CHANGELOG.md | 2 ++ sw/airborne/boards/ardrone/navdata.c | 10 +++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3ca4249260..f264f1df75 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -3,6 +3,8 @@ Paparazzi 5.5_devel currently ongoing development, changes so far (no particular order, nor complete) +- arch/linux: change the sys timer to a multi threaded implementation + [#1117] (https://github.com/paparazzi/paparazzi/pull/1117) - python: generate paparazzi math wrappers with SWIG [#1066] (https://github.com/paparazzi/paparazzi/pull/1066) - ground segment: use pkg-config for ivy-c if available diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index bfce8c7629..b6af91b0b6 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -487,7 +487,15 @@ void navdata_update() // check if the start byte is correct if (navdata.buffer[0] != NAVDATA_START_BYTE) { uint8_t *pint = memchr(navdata.buffer, NAVDATA_START_BYTE, navdata.buffer_idx); - navdata.buffer_idx = (pint != NULL)? (pint - navdata.buffer):0; + + // Check if we found the start byte in the read data + if(pint != NULL) { + memmove(navdata.buffer, pint, NAVDATA_PACKET_SIZE - (pint - navdata.buffer)); + navdata.buffer_idx = pint - navdata.buffer; + } + else { + navdata.buffer_idx = 0; + } return; }