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 24 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
+52 -72
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,14 +112,16 @@ 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,
@@ -155,10 +154,11 @@ static void send_navdata(struct transport_tx *trans, struct link_device *dev) {
&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);
}
@@ -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;
@@ -245,15 +246,13 @@ static inline bool_t acquire_baro_calibration(void)
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;
}
@@ -293,8 +292,7 @@ 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)
{
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;
}
@@ -470,17 +463,12 @@ 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.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,9 +478,7 @@ 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;
}
@@ -514,10 +500,8 @@ 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);
@@ -534,9 +518,8 @@ void navdata_update()
}
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;
@@ -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,9 +560,7 @@ 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;
pint = memchr(nav_port.buffer, NAVDATA_START_BYTE, nav_port.bytesRead);