Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 05:42:49 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[style] fix code style

Browse Source
This commit is contained in:
Gautier Hattenberger
2014-12-12 15:30:26 +01:00
parent 41593685fe
commit 93fe826776
1 changed files with 117 additions and 137 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/boards/ardrone/navdata.c
+117 -137
View File
@@ -75,18 +75,16 @@ static int imu_lost_counter = 0;
#define SONAR_SCALE 0.00047 #define SONAR_SCALE 0.00047
#endif #endif
// FIXME(ben): there must be a better home for these
ssize_t full_write(int fd, const uint8_t *buf, size_t count) ssize_t full_write(int fd, const uint8_t *buf, size_t count)
{ {
size_t written = 0; size_t written = 0;
while(written < count) while (written < count) {
{
ssize_t n = write(fd, buf + written, count - written); ssize_t n = write(fd, buf + written, count - written);
if (n < 0) if (n < 0) {
{ if (errno == EAGAIN || errno == EWOULDBLOCK) {
if (errno == EAGAIN || errno == EWOULDBLOCK)
continue; continue;
}
return n; return n;
} }
written += 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) 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; size_t readed = 0;
while(readed < count) while (readed < count) {
{
ssize_t n = read(fd, buf + readed, count - readed); ssize_t n = read(fd, buf + readed, count - readed);
if (n < 0) if (n < 0) {
{ if (errno == EAGAIN || errno == EWOULDBLOCK) {
if (errno == EAGAIN || errno == EWOULDBLOCK)
continue; continue;
}
return n; return n;
} }
readed += 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) 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"); perror("navdata_write: Write failed");
}
} }
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h" #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, pprz_msg_send_ARDRONE_NAVDATA(trans, dev, AC_ID,
&navdata.taille, &navdata.taille,
&navdata.nu_trame, &navdata.nu_trame,
&navdata.ax, &navdata.ax,
&navdata.ay, &navdata.ay,
&navdata.az, &navdata.az,
&navdata.vx, &navdata.vx,
&navdata.vy, &navdata.vy,
&navdata.vz, &navdata.vz,
&navdata.temperature_acc, &navdata.temperature_acc,
&navdata.temperature_gyro, &navdata.temperature_gyro,
&navdata.ultrasound, &navdata.ultrasound,
&navdata.us_debut_echo, &navdata.us_debut_echo,
&navdata.us_fin_echo, &navdata.us_fin_echo,
&navdata.us_association_echo, &navdata.us_association_echo,
&navdata.us_distance_echo, &navdata.us_distance_echo,
&navdata.us_curve_time, &navdata.us_curve_time,
&navdata.us_curve_value, &navdata.us_curve_value,
&navdata.us_curve_ref, &navdata.us_curve_ref,
&navdata.nb_echo, &navdata.nb_echo,
&navdata.sum_echo, &navdata.sum_echo,
&navdata.gradient, &navdata.gradient,
&navdata.flag_echo_ini, &navdata.flag_echo_ini,
&navdata.pressure, &navdata.pressure,
&navdata.temperature_pressure, &navdata.temperature_pressure,
&navdata.mx, &navdata.mx,
&navdata.my, &navdata.my,
&navdata.mz, &navdata.mz,
&navdata.chksum, &navdata.chksum,
&nav_port.checksum_errors); &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; uint8_t mde = 3;
if (ahrs.status == AHRS_UNINIT) mde = 2; if (ahrs.status == AHRS_UNINIT) { mde = 2; }
if (imu_lost) mde = 5; if (imu_lost) { mde = 5; }
uint16_t val = imu_lost_counter; uint16_t val = imu_lost_counter;
pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val); 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); tcsetattr(nav_fd, TCSANOW, &options);
// stop acquisition // stop acquisition
uint8_t cmd=0x02; uint8_t cmd = 0x02;
navdata_write(&cmd, 1); navdata_write(&cmd, 1);
// read some potential dirt // read some potential dirt
// wait 10 milliseconds // wait 10 milliseconds
char tmp[100]; 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); uint16_t dirt = read(nav_fd, tmp, sizeof tmp);
(void) dirt; (void) dirt;
@@ -208,8 +208,9 @@ bool_t navdata_init()
} }
baro_calibrated = FALSE; baro_calibrated = FALSE;
if(!acquire_baro_calibration()) if (!acquire_baro_calibration()) {
return FALSE; return FALSE;
}
// start acquisition // start acquisition
cmd = 0x01; cmd = 0x01;
@@ -228,7 +229,7 @@ bool_t navdata_init()
// set navboard gpio control // set navboard gpio control
gpio_setup_output(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); 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 #if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "ARDRONE_NAVDATA", send_navdata); register_periodic_telemetry(DefaultPeriodic, "ARDRONE_NAVDATA", send_navdata);
@@ -241,19 +242,17 @@ bool_t navdata_init()
static inline bool_t acquire_baro_calibration(void) static inline bool_t acquire_baro_calibration(void)
{ {
// start baro calibration acquisition // start baro calibration acquisition
uint8_t cmd=0x17; // send cmd 23 uint8_t cmd = 0x17; // send cmd 23
navdata_write(&cmd, 1); navdata_write(&cmd, 1);
// wait 20ms to retrieve data // wait 20ms to retrieve data
for (int i=0;i<22;i++) for (int i = 0; i < 22; i++) {
{
usleep(1000); usleep(1000);
} }
uint8_t calibBuffer[22]; 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"); perror("acquire_baro_calibration: read failed");
return FALSE; return FALSE;
} }
@@ -290,11 +289,10 @@ static inline bool_t acquire_baro_calibration(void)
void navdata_read() 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 // because non-blocking read returns -1 when no bytes available
if (newbytes > 0) if (newbytes > 0) {
{
nav_port.bytesRead += newbytes; nav_port.bytesRead += newbytes;
nav_port.totalBytesRead += newbytes; nav_port.totalBytesRead += newbytes;
} }
@@ -302,8 +300,9 @@ void navdata_read()
#define MAG_FREEZE_MAX_RETRY 10 #define MAG_FREEZE_MAX_RETRY 10
static void mag_freeze_check(void) { static void mag_freeze_check(void)
// from daren.g.lee paparazzi-l 20140530 {
// Thanks to Daren.G.Lee for initial fix on 20140530
static int16_t LastMagValue = 0; static int16_t LastMagValue = 0;
static int MagFreezeCounter = 0; static int MagFreezeCounter = 0;
static int mag_freeze_retry = 0; static int mag_freeze_retry = 0;
@@ -312,8 +311,8 @@ static void mag_freeze_check(void) {
if (LastMagValue == navdata.mx) { if (LastMagValue == navdata.mx) {
MagFreezeCounter++; 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) { if (MagFreezeCounter > 30) {
printf("Mag needs resetting, Values are frozen!!! %d , %d \n", LastMagValue, navdata.mx); printf("Mag needs resetting, Values are frozen!!! %d , %d \n", LastMagValue, navdata.mx);
// set imu_lost flag // set imu_lost flag
@@ -343,8 +342,11 @@ static void mag_freeze_check(void) {
//// restart acquisition //// restart acquisition
cmd = 0x01; cmd = 0x01;
usleep(5000); 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); usleep(1000);
navdata_write(&cmd, 1); navdata_write(&cmd, 1);
} }
@@ -352,10 +354,9 @@ static void mag_freeze_check(void) {
MagFreezeCounter = 0; // reset counter back to zero MagFreezeCounter = 0; // reset counter back to zero
} }
} } else {
else {
imu_lost = 0; imu_lost = 0;
// remember to reset if value _does_ change // Reset counter if value _does_ change
MagFreezeCounter = 0; MagFreezeCounter = 0;
mag_freeze_retry = 0; mag_freeze_retry = 0;
} }
@@ -375,42 +376,34 @@ static void baro_update_logic(void)
static int spikes = 0; static int spikes = 0;
static int spike_detected = 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 was updated
temp_or_press_was_updated_last = TRUE; temp_or_press_was_updated_last = TRUE;
// This means that press must remain constant // This means that press must remain constant
if (lastpressval != 0) if (lastpressval != 0) {
{
// If pressure was updated: this is a sync error // If pressure was updated: this is a sync error
if (lastpressval != navdata.pressure) if (lastpressval != navdata.pressure) {
{
// wait for temp again // wait for temp again
temp_or_press_was_updated_last = FALSE; temp_or_press_was_updated_last = FALSE;
sync_errors++; 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 // press was updated
temp_or_press_was_updated_last = FALSE; temp_or_press_was_updated_last = FALSE;
// This means that temp must remain constant // This means that temp must remain constant
if (lasttempval != 0) if (lasttempval != 0) {
{
// If temp was updated: this is a sync error // If temp was updated: this is a sync error
if (lasttempval != navdata.temperature_pressure) if (lasttempval != navdata.temperature_pressure) {
{
// wait for press again // wait for press again
temp_or_press_was_updated_last = TRUE; temp_or_press_was_updated_last = TRUE;
sync_errors++; 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 // We now got valid pressure and temperature
navdata_baro_available = TRUE; navdata_baro_available = TRUE;
} }
@@ -418,12 +411,12 @@ static void baro_update_logic(void)
} }
// Detected a pressure switch // 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; navdata_baro_available = FALSE;
} }
// Detected a temprature switch // 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; 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 * 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. * press==temp yields very good results as a detector, although theoretically not perfect.
#samp press temp. #samp press temp.
50925 39284 34501 50925 39284 34501
50926 39287 34501 50926 39287 34501
50927 39287 34501 50927 39287 34501
50928 39283 34501 // *press good -> baro 50928 39283 34501 // *press good -> baro
50929 39283 34501 50929 39283 34501
50930 39285 34501 // *press good -> baro 50930 39285 34501 // *press good -> baro
50931 39285 34500 50931 39285 34500
50932 34500 34500 // press read too soon from chip (<4.5ms) -> ADC register still previous temp value 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 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 50934 39284 36618 // new press read, but temp still outdated
50935 39284 34501 50935 39284 34501
50936 39284 34501 // *press good -> baro 50936 39284 34501 // *press good -> baro
50937 39284 34500 50937 39284 34500
50938 39281 34500 50938 39281 34500
50939 39281 34500 50939 39281 34500
50940 39280 34500 50940 39280 34500
50941 39280 34502 50941 39280 34502
50942 39280 34502 50942 39280 34502
50943 39280 34501 50943 39280 34501
*/ */
// if press and temp are same and temp has jump: neglect the next frame // 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 // dont use next 3 packets
spike_detected = 3; spike_detected = 3;
spikes++;
printf("Spike! # %d\n",spikes);
} }
if (spike_detected > 0) if (spike_detected > 0) {
{
// disable kalman filter use // disable kalman filter use
navdata_baro_available = FALSE; navdata_baro_available = FALSE;
@@ -490,15 +478,13 @@ static void baro_update_logic(void)
// Countdown // Countdown
spike_detected--; spike_detected--;
} } else { // both are good
else // both are good
{
lastpressval_nospike = navdata.pressure; lastpressval_nospike = navdata.pressure;
lasttempval_nospike = navdata.temperature_pressure; lasttempval_nospike = navdata.temperature_pressure;
} }
// printf("%d %d %d\r\n", navdata.temperature_pressure, navdata.pressure, spike_detected); // 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() void navdata_update()
@@ -514,29 +500,26 @@ void navdata_update()
navdata_read(); navdata_read();
// while there is something interesting to do... // while there is something interesting to do...
while (nav_port.bytesRead >= NAVDATA_PACKET_SIZE) while (nav_port.bytesRead >= NAVDATA_PACKET_SIZE) {
{ if (nav_port.buffer[0] == NAVDATA_START_BYTE) {
if (nav_port.buffer[0] == NAVDATA_START_BYTE)
{
assert(sizeof navdata == NAVDATA_PACKET_SIZE); assert(sizeof navdata == NAVDATA_PACKET_SIZE);
memcpy(&navdata, nav_port.buffer, NAVDATA_PACKET_SIZE); memcpy(&navdata, nav_port.buffer, NAVDATA_PACKET_SIZE);
// Calculating the checksum // Calculating the checksum
uint16_t checksum = 0; uint16_t checksum = 0;
for(int i = 2; i < NAVDATA_PACKET_SIZE-2; i += 2) { for (int i = 2; i < NAVDATA_PACKET_SIZE - 2; i += 2) {
checksum += nav_port.buffer[i] + (nav_port.buffer[i+1] << 8); checksum += nav_port.buffer[i] + (nav_port.buffer[i + 1] << 8);
} }
// When checksum is incorrect // When checksum is incorrect
if(navdata.chksum != checksum) { if (navdata.chksum != checksum) {
printf("Checksum error [calculated: %d] [packet: %d] [diff: %d]\n",checksum , navdata.chksum, checksum-navdata.chksum); printf("Checksum error [calculated: %d] [packet: %d] [diff: %d]\n", checksum , navdata.chksum, checksum - navdata.chksum);
nav_port.checksum_errors++; nav_port.checksum_errors++;
} }
nav_port.last_packet_number++; nav_port.last_packet_number++;
if (nav_port.last_packet_number != navdata.nu_trame) 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);
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.lost_imu_frames++;
} }
nav_port.last_packet_number = navdata.nu_trame; nav_port.last_packet_number = navdata.nu_trame;
@@ -546,11 +529,11 @@ void navdata_update()
if(navdata.chksum == checksum) { if(navdata.chksum == checksum) {
// Invert byte order so that TELEMETRY works better // Invert byte order so that TELEMETRY works better
uint8_t tmp; uint8_t tmp;
uint8_t* p = (uint8_t*) &(navdata.pressure); uint8_t *p = (uint8_t *) & (navdata.pressure);
tmp = p[0]; tmp = p[0];
p[0] = p[1]; p[0] = p[1];
p[1] = tmp; p[1] = tmp;
p = (uint8_t*) &(navdata.temperature_pressure); p = (uint8_t *) & (navdata.temperature_pressure);
tmp = p[0]; tmp = p[0];
p[0] = p[1]; p[0] = p[1];
p[1] = tmp; p[1] = tmp;
@@ -565,8 +548,7 @@ void navdata_update()
#ifdef USE_SONAR #ifdef USE_SONAR
// Check if there is a new sonar measurement and update the 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; float sonar_meas = (float)((navdata.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE;
AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, &sonar_meas); AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, &sonar_meas);
} }
@@ -578,11 +560,9 @@ void navdata_update()
// Crop the buffer // Crop the buffer
navdata_cropbuffer(NAVDATA_PACKET_SIZE); navdata_cropbuffer(NAVDATA_PACKET_SIZE);
} } else {
else
{
// find start byte, copy all data from startbyte to buffer origin, update bytesread // 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); pint = memchr(nav_port.buffer, NAVDATA_START_BYTE, nav_port.bytesRead);
if (pint != NULL) { if (pint != NULL) {
@@ -604,6 +584,6 @@ static void navdata_cropbuffer(int cropsize)
return; 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; nav_port.bytesRead -= cropsize;
} }