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

AP_GPS: give yaw information for AP_Periph from Unicore GPS

Browse Source 
this allows for CAN GPS yaw with length and height checks with unicore
GPS
This commit is contained in:
Andrew Tridgell 2022-12-12 18:16:59 +11:00
parent 2fd575aca2
commit 898187950e
2 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_GPS/AP_GPS.cpp
View File

@ -1010,7 +1010,9 @@ void AP_GPS::update_instance(uint8_t instance)
void AP_GPS::get_RelPosHeading(uint32_t &timestamp, float &relPosHeading, float &relPosLength, float &relPosD, float &accHeading)
{
for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
if (drivers[i] && _type[i] == GPS_TYPE_UBLOX_RTK_ROVER) {
if (drivers[i] &&
state[i].relposheading_ts != 0 &&
AP_HAL::millis() - state[i].relposheading_ts < 500) {
relPosHeading = state[i].relPosHeading;
relPosLength = state[i].relPosLength;
relPosD = state[i].relPosD;

8
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -380,7 +380,7 @@ bool AP_GPS_NMEA::_term_complete()
_last_yaw_ms = now;
state.gps_yaw = wrap_360(_new_gps_yaw*0.01f);
state.have_gps_yaw = true;
state.gps_yaw_time_ms = AP_HAL::millis();
state.gps_yaw_time_ms = now;
// remember that we are setup to provide yaw. With
// a NMEA GPS we can only tell if the GPS is
// configured to provide yaw when it first sends a
@ -437,7 +437,7 @@ bool AP_GPS_NMEA::_term_complete()
_last_yaw_ms = now;
state.gps_yaw = _ksxt.fields[4];
state.have_gps_yaw = true;
state.gps_yaw_time_ms = AP_HAL::millis();
state.gps_yaw_time_ms = now;
state.gps_yaw_configured = true;
}
break;
@ -469,6 +469,10 @@ bool AP_GPS_NMEA::_term_complete()
const float dist = state.location.get_distance_NED(slave).length();
const float bearing = degrees(state.location.get_bearing(slave));
const float alt_diff = (state.location.alt - slave.alt)*0.01;
state.relPosHeading = bearing;
state.relPosLength = dist;
state.relPosD = alt_diff;
state.relposheading_ts = now;
if (calculate_moving_base_yaw(bearing, dist, alt_diff)) {
_last_yaw_ms = now;
}