Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Support yaw from moving base station on SBF drivers

This commit is contained in:
Michael du Breuil 2020-09-29 13:34:57 -07:00 committed by Andrew Tridgell
parent 4161e2eb28
commit b30c55b3f3
4 changed files with 37 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_GPS/AP_GPS.cpp
View File

@ -297,7 +297,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Param: DRV_OPTIONS
// @DisplayName: driver options
// @Description: Additional backend specific options
// @Bitmask: 0:Use UART2 for moving baseline on ublox
// @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF
// @User: Advanced
AP_GROUPINFO("DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),
#endif

32
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -63,6 +63,9 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
// if we ever parse RTK observations it will always be of type NED, so set it once
state.rtk_baseline_coords_type = RTK_BASELINE_COORDINATE_SYSTEM_NED;
if (driver_options() & DriverOptions::SBF_UseBaseForYaw) {
state.gps_yaw_configured = true;
}
}
AP_GPS_SBF::~AP_GPS_SBF (void) {
@ -420,6 +423,7 @@ AP_GPS_SBF::process_message(void)
// just breakout any consts we need for Do Not Use (DNU) reasons
constexpr double doubleDNU = -2e-10;
constexpr uint16_t uint16DNU = 65535;
check_new_itow(temp.TOW, sbf_msg.length);
@ -436,10 +440,30 @@ AP_GPS_SBF::process_message(void)
state.rtk_age_ms = (temp.info.CorrAge != 65535) ? ((uint32_t)temp.info.CorrAge) * 10 : 0;
// copy the position as long as the data isn't DNU
state.rtk_baseline_y_mm = (temp.info.DeltaEast != doubleDNU) ? temp.info.DeltaEast * 1e3 : 0;
state.rtk_baseline_x_mm = (temp.info.DeltaNorth != doubleDNU) ? temp.info.DeltaNorth * 1e3 : 0;
state.rtk_baseline_z_mm = (temp.info.DeltaUp != doubleDNU) ? temp.info.DeltaUp * -1e3 : 0;
// copy the position as long as the data isn't DNU, we require NED, and heading before accepting any of it
if ((temp.info.DeltaEast != doubleDNU) && (temp.info.DeltaNorth != doubleDNU) && (temp.info.DeltaUp != doubleDNU) &&
(temp.info.Azimuth != uint16DNU)) {
state.rtk_baseline_y_mm = temp.info.DeltaEast * 1e3;
state.rtk_baseline_x_mm = temp.info.DeltaNorth * 1e3;
state.rtk_baseline_z_mm = temp.info.DeltaUp * -1e3;
#if GPS_MOVING_BASELINE
// copy the baseline data as a yaw source
if (driver_options() & DriverOptions::SBF_UseBaseForYaw) {
calculate_moving_base_yaw(temp.info.Azimuth * 0.01f + 180.0f,
Vector3f(temp.info.DeltaNorth, temp.info.DeltaEast, temp.info.DeltaUp).length(),
-temp.info.DeltaUp);
}
#endif // GPS_MOVING_BASELINE
} else {
state.rtk_baseline_y_mm = 0;
state.rtk_baseline_x_mm = 0;
state.rtk_baseline_z_mm = 0;
state.have_gps_yaw = false;
}
#pragma GCC diagnostic pop
break;
}

7
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -659,11 +659,6 @@ private:
STEP_LAST
};
// GPS_DRV_OPTIONS bits
enum class DRV_OPTIONS {
MB_USE_UART2 = 1U<<0,
};
// Packet checksum accumulators
uint8_t _ck_a;
uint8_t _ck_b;
@ -745,7 +740,7 @@ private:
#if GPS_MOVING_BASELINE
// see if we should use uart2 for moving baseline config
bool mb_use_uart2(void) const {
return (driver_options() & unsigned(DRV_OPTIONS::MB_USE_UART2))?true:false;
return (driver_options() & DriverOptions::UBX_MBUseUart2)?true:false;
}
#endif

9
libraries/AP_GPS/GPS_Backend.h
View File

@ -111,11 +111,16 @@ protected:
void check_new_itow(uint32_t itow, uint32_t msg_length);
enum DriverOptions : int16_t {
UBX_MBUseUart2 = (1 << 0U),
SBF_UseBaseForYaw = (1 << 1U),
};
/*
access to driver option bits
*/
uint16_t driver_options(void) const {
return uint16_t(gps._driver_options.get());
DriverOptions driver_options(void) const {
return DriverOptions(gps._driver_options.get());
}
#if GPS_MOVING_BASELINE