From b577a568c15fafb6b5bc985b104899fa58834d61 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 12 Dec 2014 15:30:26 +0100 Subject: [PATCH] [style] fix code style --- sw/airborne/boards/ardrone/navdata.c | 256 ++++++++++++--------------- 1 file changed, 118 insertions(+), 138 deletions(-) diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 949f80f972b..8e7f12ac80a 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -75,18 +75,16 @@ static int imu_lost_counter = 0; #define SONAR_SCALE 0.00047 #endif -// FIXME(ben): there must be a better home for these ssize_t full_write(int fd, const uint8_t *buf, size_t count) { size_t written = 0; - while(written < count) - { + while (written < count) { ssize_t n = write(fd, buf + written, count - written); - if (n < 0) - { - if (errno == EAGAIN || errno == EWOULDBLOCK) + if (n < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK) { continue; + } return n; } written += n; @@ -96,16 +94,15 @@ 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) { - // Apologies for illiteracy, but we can't overload |read|. + /* Apologies for illiteracy, but we can't overload |read|.*/ size_t readed = 0; - while(readed < count) - { + while (readed < count) { ssize_t n = read(fd, buf + readed, count - readed); - if (n < 0) - { - if (errno == EAGAIN || errno == EWOULDBLOCK) + if (n < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK) { continue; + } return n; } readed += n; @@ -115,50 +112,53 @@ ssize_t full_read(int fd, uint8_t *buf, size_t count) static void navdata_write(const uint8_t *buf, size_t count) { - if (full_write(nav_fd, buf, count) < 0) + 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) { +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.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) { +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; + if (ahrs.status == AHRS_UNINIT) { 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); } @@ -194,13 +194,13 @@ bool_t navdata_init() tcsetattr(nav_fd, TCSANOW, &options); // stop acquisition - uint8_t cmd=0x02; + 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++) { + for (int i = 0; i < 12; i++) { uint16_t dirt = read(nav_fd, tmp, sizeof tmp); (void) dirt; @@ -208,8 +208,9 @@ bool_t navdata_init() } baro_calibrated = FALSE; - if(!acquire_baro_calibration()) + if (!acquire_baro_calibration()) { return FALSE; + } // start acquisition cmd = 0x01; @@ -228,7 +229,7 @@ bool_t navdata_init() // set navboard gpio control gpio_setup_output(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); - gpio_set(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); @@ -241,19 +242,17 @@ bool_t navdata_init() static inline bool_t acquire_baro_calibration(void) { // start baro calibration acquisition - uint8_t cmd=0x17; // send cmd 23 + uint8_t cmd = 0x17; // send cmd 23 navdata_write(&cmd, 1); // wait 20ms to retrieve data - for (int i=0;i<22;i++) - { + for (int i = 0; i < 22; i++) { usleep(1000); } uint8_t calibBuffer[22]; - if (full_read(nav_fd, calibBuffer, sizeof calibBuffer) < 0) - { + if (full_read(nav_fd, calibBuffer, sizeof calibBuffer) < 0) { perror("acquire_baro_calibration: read failed"); return FALSE; } @@ -290,11 +289,10 @@ static inline bool_t acquire_baro_calibration(void) void navdata_read() { - int newbytes = read(nav_fd, nav_port.buffer+nav_port.bytesRead, NAVDATA_BUFFER_SIZE-nav_port.bytesRead); + 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) - { + if (newbytes > 0) { nav_port.bytesRead += newbytes; nav_port.totalBytesRead += newbytes; } @@ -302,8 +300,9 @@ void navdata_read() #define MAG_FREEZE_MAX_RETRY 10 -static void mag_freeze_check(void) { - // from daren.g.lee paparazzi-l 20140530 +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; static int mag_freeze_retry = 0; @@ -312,8 +311,8 @@ static void mag_freeze_check(void) { if (LastMagValue == navdata.mx) { MagFreezeCounter++; - // Re-initialize the serial port here, in paparazzi this should be ~150 ms - // considering it updates at 200 Hz + + // has to have at least 30 times the same value to consider it a frozen magnetometer value if (MagFreezeCounter > 30) { printf("Mag needs resetting, Values are frozen!!! %d , %d \n", LastMagValue, navdata.mx); // set imu_lost flag @@ -343,8 +342,11 @@ static void mag_freeze_check(void) { //// restart acquisition cmd = 0x01; usleep(5000); - for (int i=0;i<10;i++) - { + + /* 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); } @@ -352,10 +354,9 @@ static void mag_freeze_check(void) { MagFreezeCounter = 0; // reset counter back to zero } - } - else { + } else { imu_lost = 0; - // remember to reset if value _does_ change + // Reset counter if value _does_ change MagFreezeCounter = 0; mag_freeze_retry = 0; } @@ -375,42 +376,34 @@ static void baro_update_logic(void) static int spikes = 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 - { + 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 (lastpressval != 0) { // If pressure was updated: this is a sync error - if (lastpressval != navdata.pressure) - { + if (lastpressval != navdata.pressure) { // wait for temp again temp_or_press_was_updated_last = FALSE; sync_errors++; - printf("Baro-Logic-Error (expected updated temp, got press)\n"); + //printf("Baro-Logic-Error (expected updated temp, got press)\n"); } } - } - else - { + } else { // press was updated temp_or_press_was_updated_last = FALSE; // This means that temp must remain constant - if (lasttempval != 0) - { + if (lasttempval != 0) { // If temp was updated: this is a sync error - if (lasttempval != navdata.temperature_pressure) - { + if (lasttempval != navdata.temperature_pressure) { // wait for press again temp_or_press_was_updated_last = TRUE; sync_errors++; - printf("Baro-Logic-Error (expected updated press, got temp)\n"); + //printf("Baro-Logic-Error (expected updated press, got temp)\n"); - } - else { + } else { // We now got valid pressure and temperature navdata_baro_available = TRUE; } @@ -418,12 +411,12 @@ static void baro_update_logic(void) } // Detected a pressure switch - if(lastpressval != 0 && lasttempval != 0 && ABS(lastpressval - navdata.pressure) > ABS(lasttempval - navdata.pressure)) { + 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)) { + if (lastpressval != 0 && lasttempval != 0 && ABS(lasttempval - navdata.temperature_pressure) > ABS(lastpressval - navdata.temperature_pressure)) { navdata_baro_available = FALSE; } @@ -446,41 +439,36 @@ static void baro_update_logic(void) * 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 + #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)) - { + if (navdata.temperature_pressure == navdata.pressure) { // && (abs((int32_t)navdata.temperature_pressure - (int32_t)lasttempval) > 40)) // dont use next 3 packets spike_detected = 3; - - spikes++; - printf("Spike! # %d\n",spikes); } - if (spike_detected > 0) - { + if (spike_detected > 0) { // disable kalman filter use navdata_baro_available = FALSE; @@ -490,15 +478,13 @@ static void baro_update_logic(void) // Countdown spike_detected--; - } - else // both are good - { + } 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); +// printf(",%d,%d",spike_detected,spikes); } void navdata_update() @@ -515,43 +501,40 @@ void navdata_update() navdata_read(); // while there is something interesting to do... - while (nav_port.bytesRead >= NAVDATA_PACKET_SIZE) - { - if (nav_port.buffer[0] == NAVDATA_START_BYTE) - { + 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); + 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); + 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); + 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 we already dropped a packet or checksum is correct - if(last_checksum_wrong || navdata.chksum == checksum) { + if (last_checksum_wrong || navdata.chksum == checksum) { // Invert byte order so that TELEMETRY works better uint8_t tmp; - uint8_t* p = (uint8_t*) &(navdata.pressure); + uint8_t *p = (uint8_t *) & (navdata.pressure); tmp = p[0]; p[0] = p[1]; p[1] = tmp; - p = (uint8_t*) &(navdata.temperature_pressure); + p = (uint8_t *) & (navdata.temperature_pressure); tmp = p[0]; p[0] = p[1]; p[1] = tmp; @@ -566,8 +549,7 @@ void navdata_update() #ifdef USE_SONAR // Check if there is a new sonar measurement and update the sonar - if (navdata.ultrasound >> 15) - { + if (navdata.ultrasound >> 15) { float sonar_meas = (float)((navdata.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE; AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, &sonar_meas); } @@ -580,11 +562,9 @@ void navdata_update() // Crop the buffer navdata_cropbuffer(NAVDATA_PACKET_SIZE); - } - else - { + } else { // find start byte, copy all data from startbyte to buffer origin, update bytesread - uint8_t * pint; + uint8_t *pint; pint = memchr(nav_port.buffer, NAVDATA_START_BYTE, nav_port.bytesRead); if (pint != NULL) { @@ -606,6 +586,6 @@ static void navdata_cropbuffer(int cropsize) return; } - memmove(nav_port.buffer, nav_port.buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize); + memmove(nav_port.buffer, nav_port.buffer + cropsize, NAVDATA_BUFFER_SIZE - cropsize); nav_port.bytesRead -= cropsize; }