Skip to content

Commit

Permalink
AP_GPS: add Septentrio dual-receiver moving baseline heading support
Browse files Browse the repository at this point in the history 
Dual-receiver moving baseline heading allows users to use two Septentrio
GNSS receivers base on the mosaic-X5 for L5 frequency support.

wip
  • Loading branch information
@flyingthingsintothings
flyingthingsintothings committed Jun 12, 2024
1 parent 15bb86d commit 6d55b7f
Show file tree
Hide file tree
Showing 5 changed files with 214 additions and 45 deletions.
59 changes: 43 additions & 16 deletions libraries/AP_GPS/AP_GPS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -544,6 +544,8 @@ void AP_GPS::send_blob_start(uint8_t instance)
#if AP_GPS_SBF_ENABLED
case GPS_TYPE_SBF:
case GPS_TYPE_SBF_DUAL_ANTENNA:
case GPS_TYPE_SBF_RTK_BASE:
case GPS_TYPE_SBF_RTK_ROVER:
#endif //AP_GPS_SBF_ENABLED
#if AP_GPS_GSOF_ENABLED
case GPS_TYPE_GSOF:
Expand Down Expand Up @@ -712,6 +714,8 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance)
// by default the sbf/trimble gps outputs no data on its port, until configured.
case GPS_TYPE_SBF:
case GPS_TYPE_SBF_DUAL_ANTENNA:
case GPS_TYPE_SBF_RTK_BASE:
case GPS_TYPE_SBF_RTK_ROVER:
return new AP_GPS_SBF(*this, params[instance], state[instance], _port[instance]);
#endif //AP_GPS_SBF_ENABLED
#if AP_GPS_NOVA_ENABLED
Expand Down Expand Up @@ -923,21 +927,8 @@ void AP_GPS::update_instance(uint8_t instance)
}

#if GPS_MAX_RECEIVERS > 1
if (drivers[instance] && type == GPS_TYPE_UBLOX_RTK_BASE) {
// see if a moving baseline base has some RTCMv3 data
// which we need to pass along to the rover
const uint8_t *rtcm_data;
uint16_t rtcm_len;
if (drivers[instance]->get_RTCMV3(rtcm_data, rtcm_len)) {
for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
if (i != instance && params[i].type == GPS_TYPE_UBLOX_RTK_ROVER) {
// pass the data to the rover
inject_data(i, rtcm_data, rtcm_len);
drivers[instance]->clear_RTCMV3();
break;
}
}
}
if (provides_moving_base_corrections(instance)) {
forward_moving_base_corrections(instance);
}
#endif

Expand Down Expand Up @@ -978,6 +969,42 @@ void AP_GPS::update_instance(uint8_t instance)
#endif
}

/*
check whether receiver managed by driver `instance` is expected to provide RTCM corrections
*/
bool AP_GPS::provides_moving_base_corrections(uint8_t instance) const
{
const auto driver_type = params[instance].type;
return driver_type == GPS_TYPE_UBLOX_RTK_BASE || driver_type == GPS_TYPE_SBF_RTK_BASE;
}

/*
check whether receiver managed by driver `instance` accepts moving base RTCM corrections
*/
bool AP_GPS::accepts_moving_base_corrections(uint8_t instance) const
{
const auto driver_type = params[instance].type;
return driver_type == GPS_TYPE_UBLOX_RTK_ROVER || driver_type == GPS_TYPE_SBF_RTK_ROVER;
}

/*
check if driver `instance` has any moving base RTCMv3 corrections and if so, forward them to the other instances if they accept them
*/
void AP_GPS::forward_moving_base_corrections(uint8_t instance)
{
const uint8_t *rtcm_data;
uint16_t rtcm_len;
if (drivers[instance] && drivers[instance]->get_RTCMV3(rtcm_data, rtcm_len)) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (i != instance && accepts_moving_base_corrections(i)) {
// pass the data to the rover
inject_data(i, rtcm_data, rtcm_len);
drivers[instance]->clear_RTCMV3();
break;
}
}
}
}

#if GPS_MOVING_BASELINE
bool AP_GPS::get_RelPosHeading(uint32_t &timestamp, float &relPosHeading, float &relPosLength, float &relPosD, float &accHeading)
Expand Down Expand Up @@ -1998,7 +2025,7 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
// @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,26:SBF-DualAntenna
// @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,27:SBF-MovingBaseline-Base,28,SBF-MovingBaseline-Rover
// @RebootRequired: True
// @User: Advanced

Expand Down
11 changes: 11 additions & 0 deletions libraries/AP_GPS/AP_GPS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,8 @@ class AP_GPS
GPS_TYPE_UNICORE_NMEA = 24,
GPS_TYPE_UNICORE_MOVINGBASE_NMEA = 25,
GPS_TYPE_SBF_DUAL_ANTENNA = 26,
GPS_TYPE_SBF_RTK_BASE = 27,
GPS_TYPE_SBF_RTK_ROVER = 28,
#if HAL_SIM_GPS_ENABLED
GPS_TYPE_SITL = 100,
#endif
Expand Down Expand Up @@ -722,6 +724,15 @@ class AP_GPS

void update_instance(uint8_t instance);

// check whether receiver managed by driver `instance` is expected to provide RTCM corrections
bool provides_moving_base_corrections(uint8_t instance) const;

// check whether receiver managed by driver `instance` accepts moving base RTCM corrections
bool accepts_moving_base_corrections(uint8_t instace) const;

// check if driver `instance` has any moving base RTCMv3 corrections and if so, forward them to the other instances if they accept them
void forward_moving_base_corrections(uint8_t instance);

/*
buffer for re-assembling RTCM data for GPS injection.
The 8 bit flags field in GPS_RTCM_DATA is interpreted as:
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_GPS/AP_GPS_Params.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ const AP_Param::GroupInfo AP_GPS::Params::var_info[] = {
// @Param: TYPE
// @DisplayName: GPS type
// @Description: GPS type
// @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAVLink,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
// @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAVLink,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,27:SBF-MovingBaseline-Base,28:SBF-MovingBaseline-Rover
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO_FLAGS("TYPE", 1, AP_GPS::Params, type, 0, AP_PARAM_FLAG_ENABLE),
Expand Down
Loading

0 comments on commit 6d55b7f

Please sign in to comment.