/* * Copyright (C) 2013 Dino Hensen, Vincent van Hoek * * 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 boards/ardrone/navdata.c * ardrone2 navdata aquisition driver. * * The ardrone2 provides a navdata stream of packets * containing info about all sensors at a rate of 200Hz. */ #include #include #include // for O_RDWR, O_NOCTTY, O_NONBLOCK #include // for baud rates and options #include #include #include #include #include #include "std.h" #include "navdata.h" #include "subsystems/ins.h" #include "subsystems/ahrs.h" #include "subsystems/abi.h" #include "mcu_periph/gpio.h" #define NAVDATA_PACKET_SIZE 60 #define NAVDATA_START_BYTE 0x3a #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; /** Sonar offset. * Offset value in ADC * equals to the ADC value so that height is zero */ #ifndef SONAR_OFFSET #define SONAR_OFFSET 880 #endif /** Sonar scale. * Sensor sensitivity in m/adc (float) */ #ifndef SONAR_SCALE #define SONAR_SCALE 0.00047 #endif 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"); } } #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); } static void send_filter_status(struct transport_tx *trans, struct link_device *dev) { uint8_t mde = 3; if (!DefaultAhrsImpl.is_aligned) { mde = 2; } if (imu_lost) { mde = 5; } uint16_t val = imu_lost_counter; pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val); } #endif bool_t navdata_init() { if (nav_fd <= 0) { nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK); if (nav_fd == -1) { perror("navdata_init: Unable to open /dev/ttyO1 - "); return FALSE; } } 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); 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(nav_fd, TCSANOW, &options); // stop acquisition uint8_t cmd = 0x02; navdata_write(&cmd, 1); // read some potential dirt // wait 10 milliseconds 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()) { return FALSE; } // start acquisition cmd = 0x01; navdata_write(&cmd, 1); 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 gpio_setup_output(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "ARDRONE_NAVDATA", send_navdata); register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); #endif return TRUE; } static inline bool_t acquire_baro_calibration(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); } uint8_t calibBuffer[22]; if (full_read(nav_fd, calibBuffer, sizeof calibBuffer) < 0) { perror("acquire_baro_calibration: read failed"); 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]; 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() { int newbytes = read(nav_fd, nav_port.buffer + nav_port.bytesRead, NAVDATA_BUFFER_SIZE - nav_port.bytesRead); // because non-blocking read returns -1 when no bytes available if (newbytes > 0) { nav_port.bytesRead += newbytes; nav_port.totalBytesRead += newbytes; } } 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) { 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++; // stop acquisition uint8_t cmd = 0x02; navdata_write(&cmd, 1); // do the navboard reset via GPIOs 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); //uint8_t mde = 5; //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); } MagFreezeCounter = 0; // reset counter back to zero } } else { imu_lost = 0; // Reset counter if value _does_ change MagFreezeCounter = 0; } // set last value LastMagValue = navdata.mx; } static void baro_update_logic(void) { static int32_t lastpressval = 0; static uint16_t lasttempval = 0; static int32_t lastpressval_nospike = 0; static uint16_t lasttempval_nospike = 0; static uint8_t temp_or_press_was_updated_last = 0; // 0 = press, so we now wait for temp, 1 = temp so we now wait for press static int sync_errors = 0; static int spike_detected = 0; if (temp_or_press_was_updated_last == 0) { // Last update was press so we are now waiting for temp // temp was updated temp_or_press_was_updated_last = TRUE; // This means that press must remain constant if (lastpressval != 0) { // If pressure was updated: this is a sync error if (lastpressval != navdata.pressure) { // wait for temp again temp_or_press_was_updated_last = FALSE; sync_errors++; //printf("Baro-Logic-Error (expected updated temp, got press)\n"); } } } else { // press was updated temp_or_press_was_updated_last = FALSE; // This means that temp must remain constant if (lasttempval != 0) { // If temp was updated: this is a sync error if (lasttempval != navdata.temperature_pressure) { // wait for press again temp_or_press_was_updated_last = TRUE; sync_errors++; //printf("Baro-Logic-Error (expected updated press, got temp)\n"); } else { // We now got valid pressure and temperature 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; } // Detected a temprature switch if (lastpressval != 0 && lasttempval != 0 && ABS(lasttempval - navdata.temperature_pressure) > ABS(lastpressval - navdata.temperature_pressure)) { navdata_baro_available = FALSE; } lasttempval = navdata.temperature_pressure; lastpressval = navdata.pressure; /* * It turns out that a lot of navdata boards have a problem (probably interrupt related) * in which reading I2C data and writing uart output data is interrupted very long (50% of 200Hz). * Afterwards, the 200Hz loop tries to catch up lost time but reads the baro too fast swapping the * pressure and temperature values by exceeding the minimal conversion time of the bosh baro. The * normal Parrot firmware seems to be perfectly capable to fly with this, either with excessive use of * the sonar or with software filtering or spike detection. Paparazzi with its tightly coupled baro-altitude * had problems. Since this problems looks not uncommon a detector was made. A lot of evidence is grabbed * with a logic analyzer on the navboard I2C and serial output. The UART CRC is still perfect, the baro * temp-press-temp-press logic is still perfect, so not easy to detect. Temp and pressure are swapped, * and since both can have almost the same value, the size of the spike is not predictable. However at * every spike of at least 3 broken boards the press and temp are byte-wise exactly the same due to * reading them too quickly (trying to catch up for delay that happened earlier due to still non understood * reasons. As pressure is more likely to quickly change, a small (yet unlikely) spike on temperature together with * press==temp yields very good results as a detector, although theoretically not perfect. #samp press temp. 50925 39284 34501 50926 39287 34501 50927 39287 34501 50928 39283 34501 // *press good -> baro 50929 39283 34501 50930 39285 34501 // *press good -> baro 50931 39285 34500 50932 34500 34500 // press read too soon from chip (<4.5ms) -> ADC register still previous temp value 50933 34500 36618 // press not updated, still wrong. Temp is weird: looks like the average of both 50934 39284 36618 // new press read, but temp still outdated 50935 39284 34501 50936 39284 34501 // *press good -> baro 50937 39284 34500 50938 39281 34500 50939 39281 34500 50940 39280 34500 50941 39280 34502 50942 39280 34502 50943 39280 34501 */ // 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)) // dont use next 3 packets spike_detected = 3; } if (spike_detected > 0) { // disable kalman filter use navdata_baro_available = FALSE; // override both to last good navdata.pressure = lastpressval_nospike; navdata.temperature_pressure = lasttempval_nospike; // Countdown spike_detected--; } else { // both are good lastpressval_nospike = navdata.pressure; lasttempval_nospike = navdata.temperature_pressure; } // printf("%d %d %d\r\n", navdata.temperature_pressure, navdata.pressure, spike_detected); // printf(",%d,%d",spike_detected,spikes); } void navdata_update() { // Check if initialized if (!nav_port.isInitialized) { navdata_init(); mag_freeze_check(); return; } // Start reading navdata_read(); // 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); // 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); } // 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++; } 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); // 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; // 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(); #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); } #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; } } } } 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; }