forked from Archive/PX4-Autopilot
Code style fix
This commit is contained in:
parent
1bf1cec567
commit
09b5206404
|
@ -25,17 +25,24 @@ char* str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *re
|
||||||
int8_t neg = 0;
|
int8_t neg = 0;
|
||||||
|
|
||||||
if (*pos == '-') { neg = 1; pos++; }
|
if (*pos == '-') { neg = 1; pos++; }
|
||||||
|
|
||||||
else if (*pos == '+') { pos++; }
|
else if (*pos == '+') { pos++; }
|
||||||
else if(sign) return(NULL);
|
|
||||||
while(*pos >= '0' && *pos <= '9')
|
else if (sign) { return (NULL); }
|
||||||
{
|
|
||||||
|
while (*pos >= '0' && *pos <= '9') {
|
||||||
d = d * 10 + (*(pos++) - '0');
|
d = d * 10 + (*(pos++) - '0');
|
||||||
n++;
|
n++;
|
||||||
if(n_max_digit>0 && n==n_max_digit) break;
|
|
||||||
|
if (n_max_digit > 0 && n == n_max_digit) { break; }
|
||||||
}
|
}
|
||||||
if(n==0 || n>10) return(NULL);
|
|
||||||
if(neg) *result = -d;
|
if (n == 0 || n > 10) { return (NULL); }
|
||||||
else *result = d;
|
|
||||||
|
if (neg) { *result = -d; }
|
||||||
|
|
||||||
|
else { *result = d; }
|
||||||
|
|
||||||
return ((char *)pos);
|
return ((char *)pos);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -46,22 +53,27 @@ char* scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_
|
||||||
int32_t d_int;
|
int32_t d_int;
|
||||||
int8_t n = 0, isneg = 0;
|
int8_t n = 0, isneg = 0;
|
||||||
|
|
||||||
if( *pos == '-' ) isneg= 1;
|
if (*pos == '-') { isneg = 1; }
|
||||||
if((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) return(NULL);
|
|
||||||
if(*(pos) == '.')
|
if ((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) { return (NULL); }
|
||||||
{
|
|
||||||
|
if (*(pos) == '.') {
|
||||||
pos++;
|
pos++;
|
||||||
while(*pos >= '0' && *pos <= '9')
|
|
||||||
{
|
while (*pos >= '0' && *pos <= '9') {
|
||||||
f = f * (10.0) + (float64_t)(*(pos++) - '0');
|
f = f * (10.0) + (float64_t)(*(pos++) - '0');
|
||||||
div *= (0.1);
|
div *= (0.1);
|
||||||
n++;
|
n++;
|
||||||
if(n_max_frac>0 && n==n_max_frac) break;
|
|
||||||
|
if (n_max_frac > 0 && n == n_max_frac) { break; }
|
||||||
}
|
}
|
||||||
}
|
|
||||||
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;
|
|
||||||
else *result = (float64_t)d_int + f*div;
|
if (isneg) { *result = (float64_t)d_int - f * div; }
|
||||||
|
|
||||||
|
else { *result = (float64_t)d_int + f * div; }
|
||||||
|
|
||||||
return ((char *)pos);
|
return ((char *)pos);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -77,16 +89,25 @@ _gps_position(gps_position)
|
||||||
_rx_buffer_bytes = 0;
|
_rx_buffer_bytes = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
ASHTECH::~ASHTECH(){
|
ASHTECH::~ASHTECH()
|
||||||
|
{
|
||||||
}
|
}
|
||||||
//All NMEA descriptions are taken from
|
|
||||||
//http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html
|
/*
|
||||||
int ASHTECH::handle_message(int len){
|
* All NMEA descriptions are taken from
|
||||||
if(len < 7) return 0;
|
* http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html
|
||||||
|
*/
|
||||||
|
|
||||||
|
int ASHTECH::handle_message(int len)
|
||||||
|
{
|
||||||
|
if (len < 7) { return 0; }
|
||||||
|
|
||||||
int uiCalcComma = 0;
|
int uiCalcComma = 0;
|
||||||
|
|
||||||
for (int i = 0 ; i < len; i++) {
|
for (int i = 0 ; i < len; i++) {
|
||||||
if(_rx_buffer[i] == ',')uiCalcComma++;
|
if (_rx_buffer[i] == ',') { uiCalcComma++; }
|
||||||
}
|
}
|
||||||
|
|
||||||
char *bufptr = (char *)(_rx_buffer + 6);
|
char *bufptr = (char *)(_rx_buffer + 6);
|
||||||
|
|
||||||
if ((memcmp(_rx_buffer + 3, "ZDA,", 3) == 0) && (uiCalcComma == 6)) {
|
if ((memcmp(_rx_buffer + 3, "ZDA,", 3) == 0) && (uiCalcComma == 6)) {
|
||||||
|
@ -110,18 +131,26 @@ int ASHTECH::handle_message(int len){
|
||||||
*/
|
*/
|
||||||
float64_t ashtech_time = 0.0;
|
float64_t 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 = str_scanDec(bufptr, 0, 9, &day);
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); }
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &month);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &year);
|
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &day); }
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min);
|
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &month); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &year); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min); }
|
||||||
|
|
||||||
|
|
||||||
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;
|
float64_t ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100;
|
||||||
//convert to unix timestamp
|
/*
|
||||||
|
* convert to unix timestamp
|
||||||
|
*/
|
||||||
struct tm timeinfo;
|
struct tm timeinfo;
|
||||||
timeinfo.tm_year = year - 1900;
|
timeinfo.tm_year = year - 1900;
|
||||||
timeinfo.tm_mon = month - 1;
|
timeinfo.tm_mon = month - 1;
|
||||||
|
@ -179,20 +208,31 @@ int ASHTECH::handle_message(int len){
|
||||||
float64_t hdop = 99.9;
|
float64_t 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); }
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat);
|
|
||||||
if(bufptr && *(++bufptr) != ',') ns = *(bufptr++);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon);
|
|
||||||
if(bufptr && *(++bufptr) != ',') ew = *(bufptr++);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt);
|
|
||||||
|
|
||||||
if(ns == 'S')
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &hdop); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt); }
|
||||||
|
|
||||||
|
if (ns == 'S') {
|
||||||
lat = -lat;
|
lat = -lat;
|
||||||
if(ew == 'W')
|
}
|
||||||
|
|
||||||
|
if (ew == 'W') {
|
||||||
lon = -lon;
|
lon = -lon;
|
||||||
|
}
|
||||||
|
|
||||||
_gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000;
|
_gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000;
|
||||||
_gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000;
|
_gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000;
|
||||||
|
@ -201,9 +241,10 @@ int ASHTECH::handle_message(int len){
|
||||||
|
|
||||||
if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) {
|
if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) {
|
||||||
_gps_position->fix_type = 0;
|
_gps_position->fix_type = 0;
|
||||||
}
|
|
||||||
else
|
} else {
|
||||||
_gps_position->fix_type = 3 + fix_quality;
|
_gps_position->fix_type = 3 + fix_quality;
|
||||||
|
}
|
||||||
|
|
||||||
_gps_position->timestamp_position = hrt_absolute_time();
|
_gps_position->timestamp_position = hrt_absolute_time();
|
||||||
|
|
||||||
|
@ -211,13 +252,14 @@ int ASHTECH::handle_message(int len){
|
||||||
_gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */
|
_gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */
|
||||||
_gps_position->vel_e_m_s = 0; /**< GPS ground speed in m/s */
|
_gps_position->vel_e_m_s = 0; /**< GPS ground speed in m/s */
|
||||||
_gps_position->vel_d_m_s = 0; /**< GPS ground speed in m/s */
|
_gps_position->vel_d_m_s = 0; /**< GPS ground speed in m/s */
|
||||||
_gps_position->cog_rad = 0; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
|
_gps_position->cog_rad =
|
||||||
|
0; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
|
||||||
_gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */
|
_gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */
|
||||||
_gps_position->c_variance_rad = 0.1;
|
_gps_position->c_variance_rad = 0.1;
|
||||||
_gps_position->timestamp_velocity = hrt_absolute_time();
|
_gps_position->timestamp_velocity = hrt_absolute_time();
|
||||||
return 1;
|
return 1;
|
||||||
}
|
|
||||||
else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) {
|
} else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) {
|
||||||
/*
|
/*
|
||||||
Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34
|
Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34
|
||||||
|
|
||||||
|
@ -254,27 +296,45 @@ int ASHTECH::handle_message(int len){
|
||||||
float64_t hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0;
|
float64_t 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); }
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat);
|
|
||||||
if(bufptr && *(++bufptr) != ',') ns = *(bufptr++);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon);
|
|
||||||
if(bufptr && *(++bufptr) != ',') ew = *(bufptr++);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&age_of_corr);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vertic_vel);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&pdop);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vdop);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&tdop);
|
|
||||||
|
|
||||||
if(ns == 'S')
|
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &age_of_corr); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &track_true); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ground_speed); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &vertic_vel); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &pdop); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &hdop); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &vdop); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &tdop); }
|
||||||
|
|
||||||
|
if (ns == 'S') {
|
||||||
lat = -lat;
|
lat = -lat;
|
||||||
if(ew == 'W')
|
}
|
||||||
|
|
||||||
|
if (ew == 'W') {
|
||||||
lon = -lon;
|
lon = -lon;
|
||||||
|
}
|
||||||
|
|
||||||
_gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000;
|
_gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000;
|
||||||
_gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000;
|
_gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000;
|
||||||
|
@ -283,9 +343,10 @@ int ASHTECH::handle_message(int len){
|
||||||
|
|
||||||
if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) {
|
if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) {
|
||||||
_gps_position->fix_type = 0;
|
_gps_position->fix_type = 0;
|
||||||
}
|
|
||||||
else
|
} else {
|
||||||
_gps_position->fix_type = 3 + fix_quality;
|
_gps_position->fix_type = 3 + fix_quality;
|
||||||
|
}
|
||||||
|
|
||||||
_gps_position->timestamp_position = hrt_absolute_time();
|
_gps_position->timestamp_position = hrt_absolute_time();
|
||||||
|
|
||||||
|
@ -301,14 +362,14 @@ int ASHTECH::handle_message(int len){
|
||||||
_gps_position->vel_n_m_s = velocity_north; /**< GPS ground speed in m/s */
|
_gps_position->vel_n_m_s = velocity_north; /**< GPS ground speed in m/s */
|
||||||
_gps_position->vel_e_m_s = velocity_east; /**< GPS ground speed in m/s */
|
_gps_position->vel_e_m_s = velocity_east; /**< GPS ground speed in m/s */
|
||||||
_gps_position->vel_d_m_s = -vertic_vel; /**< GPS ground speed in m/s */
|
_gps_position->vel_d_m_s = -vertic_vel; /**< GPS ground speed in m/s */
|
||||||
_gps_position->cog_rad = track_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
|
_gps_position->cog_rad =
|
||||||
|
track_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
|
||||||
_gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */
|
_gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */
|
||||||
_gps_position->c_variance_rad = 0.1;
|
_gps_position->c_variance_rad = 0.1;
|
||||||
_gps_position->timestamp_velocity = hrt_absolute_time();
|
_gps_position->timestamp_velocity = hrt_absolute_time();
|
||||||
return 1;
|
return 1;
|
||||||
|
|
||||||
}
|
} else if ((memcmp(_rx_buffer + 3, "GST,", 3) == 0) && (uiCalcComma == 8)) {
|
||||||
else if((memcmp(_rx_buffer+3, "GST,",3) == 0)&&(uiCalcComma == 8)){
|
|
||||||
/*
|
/*
|
||||||
Position error statistics
|
Position error statistics
|
||||||
An example of the GST message string is:
|
An example of the GST message string is:
|
||||||
|
@ -336,22 +397,29 @@ int ASHTECH::handle_message(int len){
|
||||||
float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0;
|
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;
|
float64_t 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); }
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&rms_err);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&maj_err);
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &rms_err); }
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&min_err);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,°_from_north);
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &maj_err); }
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat_err);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon_err);
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &min_err); }
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt_err);
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, °_from_north); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat_err); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon_err); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt_err); }
|
||||||
|
|
||||||
_gps_position->eph = sqrt(lat_err * lat_err + lon_err * lon_err);
|
_gps_position->eph = sqrt(lat_err * lat_err + lon_err * lon_err);
|
||||||
_gps_position->epv = alt_err;
|
_gps_position->epv = alt_err;
|
||||||
|
|
||||||
_gps_position->s_variance_m_s = 0;
|
_gps_position->s_variance_m_s = 0;
|
||||||
_gps_position->timestamp_variance = hrt_absolute_time();
|
_gps_position->timestamp_variance = hrt_absolute_time();
|
||||||
}
|
|
||||||
else if((memcmp(_rx_buffer+3, "VTG,",3) == 0)&&(uiCalcComma == 9)){
|
} else if ((memcmp(_rx_buffer + 3, "VTG,", 3) == 0) && (uiCalcComma == 9)) {
|
||||||
/*
|
/*
|
||||||
Track made good and speed over ground
|
Track made good and speed over ground
|
||||||
An example of the VTG message string is:
|
An example of the VTG message string is:
|
||||||
|
@ -387,8 +455,7 @@ int ASHTECH::handle_message(int len){
|
||||||
float64_t tan_C = tan(track_true * dPI / 180.0);
|
float64_t tan_C = tan(track_true * dPI / 180.0);
|
||||||
float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour
|
float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour
|
||||||
float64_t lon_ = lat_*tan_C; // //km/hour*/
|
float64_t lon_ = lat_*tan_C; // //km/hour*/
|
||||||
}
|
} else if ((memcmp(_rx_buffer + 3, "GSV,", 3) == 0)) {
|
||||||
else if((memcmp(_rx_buffer+3, "GSV,",3) == 0)){
|
|
||||||
/*
|
/*
|
||||||
The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is:
|
The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is:
|
||||||
|
|
||||||
|
@ -409,13 +476,18 @@ int ASHTECH::handle_message(int len){
|
||||||
16-19 Information about fourth SV, same format as fields 4 through 7
|
16-19 Information about fourth SV, same format as fields 4 through 7
|
||||||
20 The checksum data, always begins with *
|
20 The checksum data, always begins with *
|
||||||
*/
|
*/
|
||||||
// currently process only gps, because do not know what
|
/*
|
||||||
// Global satellite ID I should use for non GPS sats
|
* currently process only gps, because do not know what
|
||||||
|
* Global satellite ID I should use for non GPS sats
|
||||||
|
*/
|
||||||
bool bGPS = false;
|
bool bGPS = false;
|
||||||
if(memcmp(_rx_buffer, "$GP",3) != 0)
|
|
||||||
|
if (memcmp(_rx_buffer, "$GP", 3) != 0) {
|
||||||
return 0;
|
return 0;
|
||||||
else
|
|
||||||
|
} else {
|
||||||
bGPS = true;
|
bGPS = true;
|
||||||
|
}
|
||||||
|
|
||||||
int all_msg_num, this_msg_num, tot_sv_visible;
|
int all_msg_num, this_msg_num, tot_sv_visible;
|
||||||
struct gsv_sat {
|
struct gsv_sat {
|
||||||
|
@ -426,12 +498,16 @@ int ASHTECH::handle_message(int len){
|
||||||
} sat[4];
|
} sat[4];
|
||||||
memset(sat, 0, sizeof(sat));
|
memset(sat, 0, sizeof(sat));
|
||||||
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num);
|
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num); }
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible);
|
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible); }
|
||||||
|
|
||||||
if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) {
|
if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
if ((this_msg_num == 0) && (bGPS == true)) {
|
if ((this_msg_num == 0) && (bGPS == true)) {
|
||||||
memset(_satellite_info->svid, 0, sizeof(_satellite_info->svid));
|
memset(_satellite_info->svid, 0, sizeof(_satellite_info->svid));
|
||||||
memset(_satellite_info->used, 0, sizeof(_satellite_info->used));
|
memset(_satellite_info->used, 0, sizeof(_satellite_info->used));
|
||||||
|
@ -441,17 +517,22 @@ int ASHTECH::handle_message(int len){
|
||||||
}
|
}
|
||||||
|
|
||||||
int end = 4;
|
int end = 4;
|
||||||
|
|
||||||
if (this_msg_num == all_msg_num) {
|
if (this_msg_num == all_msg_num) {
|
||||||
end = tot_sv_visible - (this_msg_num - 1) * 4;
|
end = tot_sv_visible - (this_msg_num - 1) * 4;
|
||||||
_gps_position->satellites_used = tot_sv_visible;
|
_gps_position->satellites_used = tot_sv_visible;
|
||||||
_satellite_info->count = SAT_INFO_MAX_SATELLITES;
|
_satellite_info->count = SAT_INFO_MAX_SATELLITES;
|
||||||
_satellite_info->timestamp = hrt_absolute_time();
|
_satellite_info->timestamp = hrt_absolute_time();
|
||||||
}
|
}
|
||||||
|
|
||||||
for (int y = 0 ; y < end ; y++) {
|
for (int y = 0 ; y < end ; y++) {
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid);
|
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid); }
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation);
|
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth);
|
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation); }
|
||||||
if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr);
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth); }
|
||||||
|
|
||||||
|
if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr); }
|
||||||
|
|
||||||
_satellite_info->svid[y + (this_msg_num - 1) * 4] = sat[y].svid;
|
_satellite_info->svid[y + (this_msg_num - 1) * 4] = sat[y].svid;
|
||||||
_satellite_info->used[y + (this_msg_num - 1) * 4] = ((sat[y].snr > 0) ? true : false);
|
_satellite_info->used[y + (this_msg_num - 1) * 4] = ((sat[y].snr > 0) ? true : false);
|
||||||
|
@ -465,7 +546,8 @@ int ASHTECH::handle_message(int len){
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
int ASHTECH::receive(unsigned timeout){
|
int ASHTECH::receive(unsigned timeout)
|
||||||
|
{
|
||||||
{
|
{
|
||||||
/* poll descriptor */
|
/* poll descriptor */
|
||||||
pollfd fds[1];
|
pollfd fds[1];
|
||||||
|
@ -485,16 +567,20 @@ int ASHTECH::receive(unsigned timeout){
|
||||||
/* pass received bytes to the packet decoder */
|
/* pass received bytes to the packet decoder */
|
||||||
while (j < count) {
|
while (j < count) {
|
||||||
int l = 0;
|
int l = 0;
|
||||||
|
|
||||||
if ((l = parse_char(buf[j])) > 0) {
|
if ((l = parse_char(buf[j])) > 0) {
|
||||||
/* return to configure during configuration or to the gps driver during normal work
|
/* return to configure during configuration or to the gps driver during normal work
|
||||||
* if a packet has arrived */
|
* if a packet has arrived */
|
||||||
if (handle_message(l) > 0)
|
if (handle_message(l) > 0) {
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/* in case we keep trying but only get crap from GPS */
|
/* in case we keep trying but only get crap from GPS */
|
||||||
if (time_started + timeout * 1000 * 2 < hrt_absolute_time()) {
|
if (time_started + timeout * 1000 * 2 < hrt_absolute_time()) {
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
j++;
|
j++;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -557,8 +643,9 @@ int ASHTECH::parse_char(uint8_t b)
|
||||||
_decode_state = NME_DECODE_UNINIT;
|
_decode_state = NME_DECODE_UNINIT;
|
||||||
_rx_buffer_bytes = 0;
|
_rx_buffer_bytes = 0;
|
||||||
|
|
||||||
} else
|
} else {
|
||||||
_rx_buffer[_rx_buffer_bytes++] = b;
|
_rx_buffer[_rx_buffer_bytes++] = b;
|
||||||
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
@ -573,7 +660,7 @@ int ASHTECH::parse_char(uint8_t b)
|
||||||
uint8_t *buffer = _rx_buffer + 1;
|
uint8_t *buffer = _rx_buffer + 1;
|
||||||
uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3;
|
uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3;
|
||||||
|
|
||||||
for (; buffer < bufend; buffer++) checksum ^= *buffer;
|
for (; buffer < bufend; buffer++) { checksum ^= *buffer; }
|
||||||
|
|
||||||
if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) &&
|
if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) &&
|
||||||
(HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) {
|
(HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) {
|
||||||
|
@ -588,12 +675,15 @@ int ASHTECH::parse_char(uint8_t b)
|
||||||
return iRet;
|
return iRet;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ASHTECH::decode_init(void){
|
void ASHTECH::decode_init(void)
|
||||||
|
{
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//ashtech boad configuration script
|
/*
|
||||||
//char comm[] = "$PASHS,NME,GGA,A,ON,0.1\r\n"
|
* ashtech boad configuration script
|
||||||
|
*/
|
||||||
|
|
||||||
char comm[] = "$PASHS,POP,20\r\n"\
|
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"\
|
||||||
|
@ -601,9 +691,10 @@ char comm[] = "$PASHS,POP,20\r\n"\
|
||||||
"$PASHS,NME,POS,B,ON,0.05\r\n"\
|
"$PASHS,NME,POS,B,ON,0.05\r\n"\
|
||||||
"$PASHS,NME,GSV,B,ON,3\r\n"\
|
"$PASHS,NME,GSV,B,ON,3\r\n"\
|
||||||
"$PASHS,SPD,A,8\r\n"\
|
"$PASHS,SPD,A,8\r\n"\
|
||||||
"$PASHS,SPD,B,9\r\n"; // default baud is 7
|
"$PASHS,SPD,B,9\r\n";
|
||||||
|
|
||||||
int ASHTECH::configure(unsigned &baudrate){
|
int ASHTECH::configure(unsigned &baudrate)
|
||||||
|
{
|
||||||
/* try different baudrates */
|
/* try different baudrates */
|
||||||
const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
|
const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
|
||||||
|
|
||||||
|
|
|
@ -67,8 +67,8 @@ class ASHTECH : public GPS_Helper
|
||||||
bool _parse_error; // parse error flag
|
bool _parse_error; // parse error flag
|
||||||
char *_parse_pos; // parse position
|
char *_parse_pos; // parse position
|
||||||
|
|
||||||
bool _gsv_in_progress; // Indicates that gsv data parsing is in progress
|
bool _gsv_in_progress; /**< Indicates that gsv data parsing is in progress */
|
||||||
//int _satellites_count; // Number of satellites info parsed.
|
//int _satellites_count; /**< Number of satellites info parsed. */
|
||||||
uint8_t count; /**< Number of satellites in satellite info */
|
uint8_t count; /**< Number of satellites in satellite info */
|
||||||
uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */
|
uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */
|
||||||
uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */
|
uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */
|
||||||
|
|
|
@ -343,7 +343,6 @@ GPS::task_main()
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case GPS_DRIVER_MODE_ASHTECH:
|
case GPS_DRIVER_MODE_ASHTECH:
|
||||||
|
|
||||||
_Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info);
|
_Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue