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 28 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 #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,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) 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,
@@ -155,10 +154,11 @@ static void send_navdata(struct transport_tx *trans, struct link_device *dev) {
&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);
} }
@@ -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;
@@ -245,15 +246,13 @@ static inline bool_t acquire_baro_calibration(void)
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;
} }
@@ -293,8 +292,7 @@ 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;
} }
@@ -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 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,9 +478,7 @@ 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;
} }
@@ -514,10 +500,8 @@ 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);
@@ -534,9 +518,8 @@ void navdata_update()
} }
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;
@@ -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,9 +560,7 @@ 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);