int main(int argc, char **argv) {
long n, i;
double *X, *X0;
printf("Test SwapDouble: ");
n=1000 ; /* must be even */
X = (double *) malloc(n* sizeof(double)) ;
X0 = (double *) malloc(n* sizeof(double)) ;
if ( X == NULL || X0 == NULL ){
printf("Error\n") ;
exit(1);
}
for (i=0; i<n; i++){
X[i] = i ;
X0[i] = i ; /* keep original values */
}
for (i=0; i<n; i +=2)
SwapDouble(X,i,i+1) ;
/* Check result values */
for (i=0; i<n; i +=2){
if (X[i] != X0[i+1] || X[i+1] != X0[i]) {
printf("Error\n") ;
exit(1);
}
}
printf("OK\n") ;
exit(0);
} /* end main */
bool
AP_GPS_GSOF::process_message(void)
{
//http://www.trimble.com/OEM_ReceiverHelp/V4.81/en/default.html#welcome.html
if (gsof_msg.packettype == 0x40) { // GSOF
#if gsof_DEBUGGING
uint8_t trans_number = gsof_msg.data[0];
uint8_t pageidx = gsof_msg.data[1];
uint8_t maxpageidx = gsof_msg.data[2];
Debug("GSOF page: %u of %u (trans_number=%u)",
pageidx, maxpageidx, trans_number);
#endif
int valid = 0;
// want 1 2 8 9 12
for (uint32_t a = 3; a < gsof_msg.length; a++)
{
uint8_t output_type = gsof_msg.data[a];
a++;
uint8_t output_length = gsof_msg.data[a];
a++;
//Debug("GSOF type: " + output_type + " len: " + output_length);
if (output_type == 1) // pos time
{
state.time_week_ms = SwapUint32(gsof_msg.data, a);
state.time_week = SwapUint16(gsof_msg.data, a + 4);
state.num_sats = gsof_msg.data[a + 6];
uint8_t posf1 = gsof_msg.data[a + 7];
uint8_t posf2 = gsof_msg.data[a + 8];
//Debug("POSTIME: " + posf1 + " " + posf2);
if ((posf1 & 1)) { // New position
state.status = AP_GPS::GPS_OK_FIX_3D;
if ((posf2 & 1)) { // Differential position
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
if (posf2 & 2) { // Differential position method
if (posf2 & 4) {// Differential position method
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
} else {
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
}
}
}
} else {
state.status = AP_GPS::NO_FIX;
}
valid++;
}
else if (output_type == 2) // position
{
state.location.lat = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a)) * (double)1e7);
state.location.lng = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a + 8)) * (double)1e7);
state.location.alt = (int32_t)(SwapDouble(gsof_msg.data, a + 16) * 100);
state.last_gps_time_ms = AP_HAL::millis();
valid++;
}
else if (output_type == 8) // velocity
{
uint8_t vflag = gsof_msg.data[a];
if ((vflag & 1) == 1)
{
state.ground_speed = SwapFloat(gsof_msg.data, a + 1);
state.ground_course = degrees(SwapFloat(gsof_msg.data, a + 5));
fill_3d_velocity();
state.velocity.z = -SwapFloat(gsof_msg.data, a + 9);
state.have_vertical_velocity = true;
}
valid++;
}
else if (output_type == 9) //dop
{
state.hdop = (uint16_t)(SwapFloat(gsof_msg.data, a + 4) * 100);
valid++;
}
else if (output_type == 12) // position sigma
{
state.horizontal_accuracy = (SwapFloat(gsof_msg.data, a + 4) + SwapFloat(gsof_msg.data, a + 8)) / 2;
state.vertical_accuracy = SwapFloat(gsof_msg.data, a + 16);
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
valid++;
}
a += output_length-1u;
}
if (valid == 5) {
return true;
} else {
state.status = AP_GPS::NO_FIX;
}
}
return false;
}
bool apm::AP_GPS_GSOF::process_message(void)
{
//http://www.trimble.com/OEM_ReceiverHelp/V4.81/en/default.html#welcome.html
if (gsof_msg.packettype == 0x40) { // GSOF
int valid = 0;
// want 1 2 8 9 12
for (uint32_t a = 3; a < gsof_msg.length; a++) {
uint8_t output_type = gsof_msg.data[a];
a++;
uint8_t output_length = gsof_msg.data[a];
a++;
//Debug("GSOF type: " + output_type + " len: " + output_length);
if (output_type == 1) { // pos time
gps.state.time_week_ms = SwapUint32(gsof_msg.data, a);
gps.state.time_week = SwapUint16(gsof_msg.data, a + 4);
gps.state.num_sats = gsof_msg.data[a + 6];
uint8_t posf1 = gsof_msg.data[a + 7];
uint8_t posf2 = gsof_msg.data[a + 8];
//Debug("POSTIME: " + posf1 + " " + posf2);
if ((posf1 & 1) == 1) {
gps.state.status = gps_t::FIX_3D;
if ((posf2 & 1) == 1) {
gps.state.status = gps_t::FIX_3D_DGPS;
if ((posf2 & 4) == 4) {
gps.state.status = gps_t::FIX_3D_RTK;
}
}
} else {
gps.state.status = gps_t::NO_FIX;
}
valid++;
} else if (output_type == 2) { // position
gps.state.location.lat = gps_t::lat_lon_type {RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a)) * 1e7}; // deg1e7
gps.state.location.lon = gps_t::lat_lon_type {RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a + 8)) * 1e7}; // deg1e7
gps.state.location.alt = gps_t::altitude_type {SwapDouble(gsof_msg.data, a + 16) * 1e2}; //cm
gps.state.last_gps_time_ms = gps.state.time_week_ms;
valid++;
} else if (output_type == 8) { // velocity
uint8_t vflag = gsof_msg.data[a];
if ((vflag & 1) == 1) {
gps.state.ground_speed = SwapFloat(gsof_msg.data, a + 1);
gps.state.ground_course_cd = (int32_t)(ToDeg(SwapFloat(gsof_msg.data, a + 5)) * 100);
fill_3d_velocity();
gps.state.velocity.z = gps_t::velocity_type {-SwapFloat(gsof_msg.data, a + 9)};
gps.state.have_vertical_velocity = true;
}
valid++;
} else if (output_type == 9) { //dop
gps.state.hdop = (uint16_t)(SwapFloat(gsof_msg.data, a + 4) * 100);
valid++;
} else if (output_type == 12) { // position sigma
gps.state.horizontal_accuracy = (SwapFloat(gsof_msg.data, a + 4) + SwapFloat(gsof_msg.data, a + 8)) / 2;
gps.state.vertical_accuracy = SwapFloat(gsof_msg.data, a + 16);
gps.state.have_horizontal_accuracy = true;
gps.state.have_vertical_accuracy = true;
valid++;
}
a += output_length-1u;
}
if (valid == 5) {
return true;
} else {
gps.state.status = gps_t::NO_FIX;
}
}
return false;
}
void
DoSwap(double val)
{
SwapDouble(val);
printDouble(val);
}
