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 <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)
{
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,
float64_t *result)
double *result)
{
float64_t f = 0.0, div = 1.0;
double f = 0.0, div = 1.0;
int32_t d_int;
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++;
while (*pos >= '0' && *pos <= '9') {
f = f * (10.0) + (float64_t)(*(pos++) - '0');
f = f * (10.0) + (double)(*(pos++) - '0');
div *= (0.1);
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); }
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);
}
@ -129,7 +126,7 @@ int ASHTECH::handle_message(int len)
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.
*/
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;
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_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
*/
@ -203,9 +200,9 @@ int ASHTECH::handle_message(int len)
The checksum data, always begins with *
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;
float64_t hdop = 99.9;
double hdop = 99.9;
char ns = '?', ew = '?';
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); }
@ -290,10 +287,10 @@ int ASHTECH::handle_message(int len)
*cc Checksum
*/
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;
float64_t 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 track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0;
double hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0;
char ns = '?', ew = '?';
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();
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_north = velocity_ms * cos(track_rad);
@ -394,8 +389,8 @@ int ASHTECH::handle_message(int len)
8 Height 1 sigma error, in meters
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;
float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0;
double ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_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); }
@ -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,GGA,B,OFF\r\n"\
"$PASHS,NME,GST,B,ON,3\r\n"\