aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS: ensure all GPS drivers give headings as 0..360 degrees

Browse Source 
this prevents inconsistency between interfaces, and fixes a MAVLink
reporting bug with UAVCAN GPS
This commit is contained in:
Andrew Tridgell 2015-08-31 08:24:35 +10:00
parent 15b242832e
commit 2c7a113790
8 changed files with 9 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_GPS/AP_GPS.cpp
View File

@ -454,7 +454,7 @@ AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms,
istate.location.options = 0;
istate.velocity = _velocity;
istate.ground_speed = pythagorous2(istate.velocity.x, istate.velocity.y);
istate.ground_course_cd = degrees(atan2f(istate.velocity.y, istate.velocity.x)) * 100UL;
istate.ground_course_cd = wrap_360_cd(degrees(atan2f(istate.velocity.y, istate.velocity.x)) * 100UL);
istate.hdop = hdop;
istate.num_sats = _num_sats;
istate.have_vertical_velocity |= _have_vertical_velocity;

2
libraries/AP_GPS/AP_GPS_MTK.cpp
View File

@ -147,7 +147,7 @@ restart:
state.location.lng = swap_int32(_buffer.msg.longitude) * 10;
state.location.alt = swap_int32(_buffer.msg.altitude);
state.ground_speed = swap_int32(_buffer.msg.ground_speed) * 0.01f;
state.ground_course_cd = swap_int32(_buffer.msg.ground_course) / 10000;
state.ground_course_cd = wrap_360_cd(swap_int32(_buffer.msg.ground_course) / 10000);
state.num_sats = _buffer.msg.satellites;
if (state.status >= AP_GPS::GPS_OK_FIX_2D) {

2
libraries/AP_GPS/AP_GPS_MTK19.cpp
View File

@ -144,7 +144,7 @@ restart:
}
state.location.alt = _buffer.msg.altitude;
state.ground_speed = _buffer.msg.ground_speed*0.01f;
state.ground_course_cd = _buffer.msg.ground_course;
state.ground_course_cd = wrap_360_cd(_buffer.msg.ground_course);
state.num_sats = _buffer.msg.satellites;
state.hdop = _buffer.msg.hdop;

4
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -296,7 +296,7 @@ bool AP_GPS_NMEA::_term_complete()
state.location.lat = _new_latitude;
state.location.lng = _new_longitude;
state.ground_speed = _new_speed*0.01f;
state.ground_course_cd = _new_course;
state.ground_course_cd = wrap_360_cd(_new_course);
make_gps_time(_new_date, _new_time * 10);
state.last_gps_time_ms = hal.scheduler->millis();
// To-Do: add support for proper reporting of 2D and 3D fix
@ -314,7 +314,7 @@ bool AP_GPS_NMEA::_term_complete()
break;
case _GPS_SENTENCE_GPVTG:
state.ground_speed = _new_speed*0.01f;
state.ground_course_cd = _new_course;
state.ground_course_cd = wrap_360_cd(_new_course);
// VTG has no fix indicator, can't change fix status
break;
}

2
libraries/AP_GPS/AP_GPS_PX4.cpp
View File

@ -63,7 +63,7 @@ AP_GPS_PX4::read(void)
state.location.alt = _gps_pos.alt/10;
state.ground_speed = _gps_pos.vel_m_s;
state.ground_course_cd = int32_t(double(_gps_pos.cog_rad) / M_PI * 18000. +.5);
state.ground_course_cd = wrap_360_cd(degrees(_gps_pos.cog_rad)*100);
state.hdop = _gps_pos.eph*100;
// convert epoch timestamp back to gps epoch - evil hack until we get the genuine

5
libraries/AP_GPS/AP_GPS_SBP.cpp
View File

@ -277,10 +277,7 @@ AP_GPS_SBP::_attempt_state_update()
float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1];
state.ground_speed = safe_sqrt(ground_vector_sq);
state.ground_course_cd = (int32_t) 100*ToDeg(atan2f(state.velocity[1], state.velocity[0]));
if (state.ground_course_cd < 0) {
state.ground_course_cd += 36000;
}
state.ground_course_cd = wrap_360_cd((int32_t) 100*ToDeg(atan2f(state.velocity[1], state.velocity[0])));
// Update position state

2
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -186,7 +186,7 @@ AP_GPS_SIRF::_parse_gps(void)
state.location.lng = swap_int32(_buffer.nav.longitude);
state.location.alt = swap_int32(_buffer.nav.altitude_msl);
state.ground_speed = swap_int32(_buffer.nav.ground_speed)*0.01f;
state.ground_course_cd = swap_int16(_buffer.nav.ground_course);
state.ground_course_cd = wrap_360_cd(swap_int16(_buffer.nav.ground_course));
state.num_sats = _buffer.nav.satellites;
fill_3d_velocity();
return true;

2
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -634,7 +634,7 @@ AP_GPS_UBLOX::_parse_gps(void)
Debug("MSG_VELNED");
_last_vel_time = _buffer.velned.time;
state.ground_speed = _buffer.velned.speed_2d*0.01f; // m/s
state.ground_course_cd = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
state.ground_course_cd = wrap_360_cd(_buffer.velned.heading_2d / 1000); // Heading 2D deg * 100000 rescaled to deg * 100
state.have_vertical_velocity = true;
state.velocity.x = _buffer.velned.ned_north * 0.01f;
state.velocity.y = _buffer.velned.ned_east * 0.01f;