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 25 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
#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()
@@ -514,29 +500,26 @@ 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;
@@ -546,11 +529,11 @@ void navdata_update()
if(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;
@@ -565,8 +548,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);
}
@@ -578,11 +560,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) {
@@ -604,6 +584,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;
}