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

AP_GPS: SBF supports yaw from dual antennas

Browse Source 
Co-authored-by: Andrew Tridgell <andrew@tridgell.net>
Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
Michael du Breuil 2021-11-05 13:00:51 -07:00 committed by Andrew Tridgell
parent 9f3595d573
commit 772dbfb04f
4 changed files with 133 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_GPS/AP_GPS.cpp
View File

@ -129,18 +129,16 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Param: _TYPE
// @DisplayName: 1st GPS type
// @Description: GPS type of 1st GPS
// @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA
// @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA,26:SBF-DualAntenna
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("_TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT),
#if GPS_MAX_RECEIVERS > 1
// @Param: _TYPE2
// @CopyFieldsFrom: GPS_TYPE
// @DisplayName: 2nd GPS type
// @Description: GPS type of 2nd GPS
// @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("_TYPE2", 1, AP_GPS, _type[1], 0),
#endif
@ -645,6 +643,7 @@ void AP_GPS::send_blob_start(uint8_t instance)
switch (_type[instance]) {
#if AP_GPS_SBF_ENABLED
case GPS_TYPE_SBF:
case GPS_TYPE_SBF_DUAL_ANTENNA:
#endif //AP_GPS_SBF_ENABLED
#if AP_GPS_GSOF_ENABLED
case GPS_TYPE_GSOF:
@ -806,6 +805,7 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance)
#if AP_GPS_SBF_ENABLED
// by default the sbf/trimble gps outputs no data on its port, until configured.
case GPS_TYPE_SBF:
case GPS_TYPE_SBF_DUAL_ANTENNA:
return new AP_GPS_SBF(*this, state[instance], _port[instance]);
#endif //AP_GPS_SBF_ENABLED
#if AP_GPS_NOVA_ENABLED

1
libraries/AP_GPS/AP_GPS.h
View File

@ -131,6 +131,7 @@ public:
GPS_TYPE_UAVCAN_RTK_ROVER = 23,
GPS_TYPE_UNICORE_NMEA = 24,
GPS_TYPE_UNICORE_MOVINGBASE_NMEA = 25,
GPS_TYPE_SBF_DUAL_ANTENNA = 26,
#if HAL_SIM_GPS_ENABLED
GPS_TYPE_SITL = 100,
#endif

92
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -68,7 +68,10 @@ 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 (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw)) {
// yaw available when option bit set or using dual antenna
if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw) ||
(get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA)) {
state.gps_yaw_configured = true;
}
}
@ -92,9 +95,9 @@ AP_GPS_SBF::read(void)
ret |= parse(temp);
}
const uint32_t now = AP_HAL::millis();
if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
if (config_step != Config_State::Complete) {
uint32_t now = AP_HAL::millis();
if (now > _init_blob_time) {
if (now > _config_last_ack_time + 2000) {
const size_t port_enable_len = strlen(_port_enable);
@ -116,9 +119,20 @@ AP_GPS_SBF::read(void)
}
break;
case Config_State::SSO:
if (asprintf(&config_string, "sso,Stream%d,COM%d,PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod,msec100\n",
const char *extra_config;
switch (get_type()) {
case AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA:
extra_config = "+AttCovEuler+AuxAntPositions";
break;
case AP_GPS::GPS_Type::GPS_TYPE_SBF:
default:
extra_config = "";
break;
}
if (asprintf(&config_string, "sso,Stream%d,COM%d,PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod%s,msec100\n",
(int)GPS_SBF_STREAM_NUMBER,
(int)gps._com_port[state.instance]) == -1) {
(int)gps._com_port[state.instance],
extra_config) == -1) {
config_string = nullptr;
}
break;
@ -145,6 +159,17 @@ AP_GPS_SBF::read(void)
break;
}
break;
case Config_State::SGA:
{
const char *targetGA = "none";
if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) {
targetGA = "MultiAntenna";
}
if (asprintf(&config_string, "sga, %s\n", targetGA)) {
config_string = nullptr;
}
break;
}
case Config_State::Complete:
// should never reach here, why search for a config if we have fully configured already
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
@ -168,7 +193,6 @@ AP_GPS_SBF::read(void)
} else if (_has_been_armed && (RxState & SBF_DISK_MOUNTED)) {
// since init is done at this point and unmounting should be rate limited,
// take over the _init_blob_time variable
uint32_t now = AP_HAL::millis();
if (now > _init_blob_time) {
unmount_disk();
_init_blob_time = now + 1000;
@ -177,6 +201,12 @@ AP_GPS_SBF::read(void)
}
}
// yaw timeout after 300 milliseconds
if ((now - state.gps_yaw_time_ms) > 300) {
state.have_gps_yaw = false;
state.have_gps_yaw_accuracy = false;
}
return ret;
}
@ -345,9 +375,12 @@ AP_GPS_SBF::parse(uint8_t temp)
_init_blob_index++;
if ((gps._sbas_mode == AP_GPS::SBAS_Mode::Disabled)
||_init_blob_index >= ARRAY_SIZE(sbas_on_blob)) {
config_step = Config_State::Complete;
config_step = Config_State::SGA;
}
break;
case Config_State::SGA:
config_step = Config_State::Complete;
break;
case Config_State::Complete:
// should never reach here, this implies that we validated a config string when we hadn't sent any
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
@ -500,6 +533,51 @@ AP_GPS_SBF::process_message(void)
}
break;
}
case AttEulerCov:
{
// yaw accuracy is taken from this message even though we actually calculate the yaw ourself (see AuxAntPositions below)
// this is OK based on the assumption that the calculation methods are similar and that inaccuracy arises from the sensor readings
if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) {
const msg5939 &temp = sbf_msg.data.msg5939u;
check_new_itow(temp.TOW, sbf_msg.length);
constexpr double floatDNU = -2e-10f;
constexpr uint8_t errorBits = 0x8F; // Bits 0-1 are aux 1 baseline
// Bits 2-3 are aux 2 baseline
// Bit 7 is attitude not requested
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal" // suppress -Wfloat-equal as it's false positive when testing for DNU values
if (((temp.Error & errorBits) == 0)
&& (temp.Cov_HeadHead != floatDNU)) {
#pragma GCC diagnostic pop
state.gps_yaw_accuracy = sqrtf(temp.Cov_HeadHead);
state.have_gps_yaw_accuracy = true;
} else {
state.gps_yaw_accuracy = false;
}
}
break;
}
case AuxAntPositions:
{
#if GPS_MOVING_BASELINE
if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) {
// calculate yaw using reported antenna positions in earth-frame
// note that this calculation does not correct for the vehicle's roll and pitch meaning it is inaccurate at very high lean angles
const msg5942 &temp = sbf_msg.data.msg5942u;
check_new_itow(temp.TOW, sbf_msg.length);
if (temp.N > 0 && temp.ant1.Error == 0 && temp.ant1.AmbiguityType == 0) {
// valid RTK integer fix
const float rel_heading_deg = degrees(atan2f(temp.ant1.DeltaEast, temp.ant1.DeltaNorth));
calculate_moving_base_yaw(rel_heading_deg,
Vector3f(temp.ant1.DeltaNorth, temp.ant1.DeltaEast, temp.ant1.DeltaUp).length(),
-temp.ant1.DeltaUp);
}
}
#endif
break;
}
case BaseVectorGeod:
{
#pragma GCC diagnostic push
@ -542,7 +620,7 @@ AP_GPS_SBF::process_message(void)
}
#endif // GPS_MOVING_BASELINE
} else {
} else if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw)) {
state.rtk_baseline_y_mm = 0;
state.rtk_baseline_x_mm = 0;
state.rtk_baseline_z_mm = 0;

44
libraries/AP_GPS/AP_GPS_SBF.h
View File

@ -75,6 +75,7 @@ private:
SSO,
Blob,
SBAS,
SGA,
Complete
};
Config_State config_step;
@ -111,7 +112,9 @@ private:
PVTGeodetic = 4007,
ReceiverStatus = 4014,
BaseVectorGeod = 4028,
VelCovGeodetic = 5908
VelCovGeodetic = 5908,
AttEulerCov = 5939,
AuxAntPositions = 5942,
};
struct PACKED msg4007 // PVTGeodetic
@ -219,12 +222,51 @@ private:
float Cov_VuDt;
};
struct PACKED msg5939 // AttEulerCoV
{
uint32_t TOW; // receiver time stamp, 0.001s
uint16_t WNc; // receiver time stamp, 1 week
uint8_t Reserved; // unused
uint8_t Error; // error code. bit 0-1:antenna 1, bit 2-3:antenna2, bit 7: when att not requested
// 00b:no error, 01b:not enough meausurements, 10b:antennas are on one line, 11b:inconsistent with manual anntena pos info
float Cov_HeadHead; // heading estimate variance
float Cov_PitchPitch; // pitch estimate variance
float Cov_RollRoll; // roll estimate variance
float Cov_HeadPitch; // covariance between Euler angle estimates. Always set to Do-No-Use values
float Cov_HeadRoll;
float Cov_PitchRoll;
};
struct PACKED AuxAntPositionSubBlock {
uint8_t NrSV; // total number of satellites tracked by the antenna
uint8_t Error; // aux antenna position error code
uint8_t AmbiguityType; // aux antenna positions obtained with 0: fixed ambiguities, 1: float ambiguities
uint8_t AuxAntID; // aux antenna ID: 1 for the first auxiliary antenna, 2 for the second, etc.
double DeltaEast; // position in East direction (relative to main antenna)
double DeltaNorth; // position in North direction (relative to main antenna)
double DeltaUp; // position in Up direction (relative to main antenna)
double EastVel; // velocity in East direction (relative to main antenna)
double NorthVel; // velocity in North direction (relative to main antenna)
double UpVel; // velocity in Up direction (relative to main antenna)
};
struct PACKED msg5942 // AuxAntPositions
{
uint32_t TOW;
uint16_t WNc;
uint8_t N; // number of AuxAntPosition sub-blocks in this AuxAntPositions block
uint8_t SBLength; // length of one sub-block in bytes
AuxAntPositionSubBlock ant1; // first aux antennas position
};
union PACKED msgbuffer {
msg4007 msg4007u;
msg4001 msg4001u;
msg4014 msg4014u;
msg4028 msg4028u;
msg5908 msg5908u;
msg5939 msg5939u;
msg5942 msg5942u;
uint8_t bytes[256];
};