dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Multiple fixes: 

- Fixed boad - board typo
- Ashtech initialization string is const char* now
- Using standard M_PI constant instead of locally defined one
- Removed float32_t and float64_t in favor of standard float and double
This commit is contained in:
Denis Yeldandi 2014-10-09 12:01:09 +04:00
parent e37b25fd58
commit c4e934c133
1 changed files with 17 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
src/drivers/gps/ashtech.cpp
View File

@ -15,9 +15,6 @@
#include <fcntl.h> #include <fcntl.h>
#include <math.h> #include <math.h>
typedef double float64_t;
typedef float float32_t;
char *str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) char *str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result)
{ {
int8_t n = 0; int8_t n = 0;
@ -47,9 +44,9 @@ char *str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *re
} }
char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac,
float64_t *result) double *result)
{ {
float64_t f = 0.0, div = 1.0; double f = 0.0, div = 1.0;
int32_t d_int; int32_t d_int;
int8_t n = 0, isneg = 0; int8_t n = 0, isneg = 0;
@ -61,7 +58,7 @@ char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_
pos++; pos++;
while (*pos >= '0' && *pos <= '9') { while (*pos >= '0' && *pos <= '9') {
f = f * (10.0) + (float64_t)(*(pos++) - '0'); f = f * (10.0) + (double)(*(pos++) - '0');
div *= (0.1); div *= (0.1);
n++; n++;
@ -70,9 +67,9 @@ char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_
} else if (n_max_frac > 0) { return (NULL); } } else if (n_max_frac > 0) { return (NULL); }
if (isneg) { *result = (float64_t)d_int - f * div; } if (isneg) { *result = (double)d_int - f * div; }
else { *result = (float64_t)d_int + f * div; } else { *result = (double)d_int + f * div; }
return ((char *)pos); return ((char *)pos);
} }
@ -129,7 +126,7 @@ int ASHTECH::handle_message(int len)
7 The checksum data, always begins with * 7 The checksum data, always begins with *
Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT.
*/ */
float64_t ashtech_time = 0.0; double ashtech_time = 0.0;
int day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0; int day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0;
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); }
@ -147,7 +144,7 @@ int ASHTECH::handle_message(int len)
int ashtech_hour = ashtech_time / 10000; int ashtech_hour = ashtech_time / 10000;
int ashtech_minute = (ashtech_time - ashtech_hour * 10000) / 100; int ashtech_minute = (ashtech_time - ashtech_hour * 10000) / 100;
float64_t ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100; double ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100;
/* /*
* convert to unix timestamp * convert to unix timestamp
*/ */
@ -203,9 +200,9 @@ int ASHTECH::handle_message(int len)
The checksum data, always begins with * The checksum data, always begins with *
Note - If a user-defined geoid model, or an inclined Note - If a user-defined geoid model, or an inclined
*/ */
float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
int num_of_sv = 0, fix_quality = 0; int num_of_sv = 0, fix_quality = 0;
float64_t hdop = 99.9; double hdop = 99.9;
char ns = '?', ew = '?'; char ns = '?', ew = '?';
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); }
@ -290,10 +287,10 @@ int ASHTECH::handle_message(int len)
*cc Checksum *cc Checksum
*/ */
bufptr = (char *)(_rx_buffer + 10); bufptr = (char *)(_rx_buffer + 10);
float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
int num_of_sv = 0, fix_quality = 0; int num_of_sv = 0, fix_quality = 0;
float64_t track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; double track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0;
float64_t hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0; double hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0;
char ns = '?', ew = '?'; char ns = '?', ew = '?';
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); }
@ -350,9 +347,7 @@ int ASHTECH::handle_message(int len)
_gps_position->timestamp_position = hrt_absolute_time(); _gps_position->timestamp_position = hrt_absolute_time();
const float64_t m_pi = 3.14159265; double track_rad = track_true * M_PI / 180.0;
double track_rad = track_true * m_pi / 180.0;
double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */ double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */
double velocity_north = velocity_ms * cos(track_rad); double velocity_north = velocity_ms * cos(track_rad);
@ -394,8 +389,8 @@ int ASHTECH::handle_message(int len)
8 Height 1 sigma error, in meters 8 Height 1 sigma error, in meters
9 The checksum data, always begins with * 9 The checksum data, always begins with *
*/ */
float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; double ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0;
float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; double min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0;
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); }
@ -645,10 +640,10 @@ void ASHTECH::decode_init(void)
} }
/* /*
* ashtech boad configuration script * ashtech board configuration script
*/ */
char comm[] = "$PASHS,POP,20\r\n"\ const char comm[] = "$PASHS,POP,20\r\n"\
"$PASHS,NME,ZDA,B,ON,3\r\n"\ "$PASHS,NME,ZDA,B,ON,3\r\n"\
"$PASHS,NME,GGA,B,OFF\r\n"\ "$PASHS,NME,GGA,B,OFF\r\n"\
"$PASHS,NME,GST,B,ON,3\r\n"\ "$PASHS,NME,GST,B,ON,3\r\n"\