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

AP_GPS: use subgroup to hold GPS instance parameters

This commit is contained in:
Peter Barker 2024-03-12 14:00:05 +11:00 committed by Peter Barker
parent 8763084a9e
commit 21fc013016
24 changed files with 317 additions and 302 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

301
libraries/AP_GPS/AP_GPS.cpp
View File

@ -72,10 +72,6 @@
#define BLEND_MASK_USE_SPD_ACC 4 #define BLEND_MASK_USE_SPD_ACC 4
#define BLEND_COUNTER_FAILURE_INCREMENT 10 #define BLEND_COUNTER_FAILURE_INCREMENT 10
#ifndef HAL_GPS_COM_PORT_DEFAULT
#define HAL_GPS_COM_PORT_DEFAULT 1
#endif
extern const AP_HAL::HAL &hal; extern const AP_HAL::HAL &hal;
// baudrates to try to detect GPSes with // baudrates to try to detect GPSes with
@ -130,21 +126,10 @@ AP_GPS *AP_GPS::_singleton;
// table of user settable parameters // table of user settable parameters
const AP_Param::GroupInfo AP_GPS::var_info[] = { 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,26:SBF-DualAntenna
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("_TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT),
#if GPS_MAX_RECEIVERS > 1 // 0 was GPS_TYPE
// @Param: _TYPE2
// @CopyFieldsFrom: GPS_TYPE // 1 was GPS_TYPE2
// @DisplayName: 2nd GPS type
// @Description: GPS type of 2nd GPS
AP_GROUPINFO("_TYPE2", 1, AP_GPS, _type[1], 0),
#endif
// @Param: _NAVFILTER // @Param: _NAVFILTER
// @DisplayName: Navigation filter setting // @DisplayName: Navigation filter setting
@ -203,12 +188,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("_RAW_DATA", 9, AP_GPS, _raw_data, 0), AP_GROUPINFO("_RAW_DATA", 9, AP_GPS, _raw_data, 0),
// @Param: _GNSS_MODE // 10 was GPS_GNSS_MODE
// @DisplayName: GNSS system configuration
// @Description: Bitmask for what GNSS system to use on the first GPS (all unchecked or zero to leave GPS as configured)
// @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLONASS
// @User: Advanced
AP_GROUPINFO("_GNSS_MODE", 10, AP_GPS, _gnss_mode[0], 0),
// @Param: _SAVE_CFG // @Param: _SAVE_CFG
// @DisplayName: Save GPS configuration // @DisplayName: Save GPS configuration
@ -217,14 +197,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("_SAVE_CFG", 11, AP_GPS, _save_config, 2), AP_GROUPINFO("_SAVE_CFG", 11, AP_GPS, _save_config, 2),
#if GPS_MAX_RECEIVERS > 1 // 12 was GPS_GNSS_MODE2
// @Param: _GNSS_MODE2
// @DisplayName: GNSS system configuration
// @Description: Bitmask for what GNSS system to use on the second GPS (all unchecked or zero to leave GPS as configured)
// @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLONASS
// @User: Advanced
AP_GROUPINFO("_GNSS_MODE2", 12, AP_GPS, _gnss_mode[1], 0),
#endif
// @Param: _AUTO_CONFIG // @Param: _AUTO_CONFIG
// @DisplayName: Automatic GPS configuration // @DisplayName: Automatic GPS configuration
@ -233,97 +206,17 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("_AUTO_CONFIG", 13, AP_GPS, _auto_config, 1), AP_GROUPINFO("_AUTO_CONFIG", 13, AP_GPS, _auto_config, 1),
// @Param: _RATE_MS // 14 was GPS_RATE_MS
// @DisplayName: GPS update rate in milliseconds
// @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz(default) is not allowed. Raising the rate above 5Hz usually provides little benefit and for some GPS (eg Ublox M9N) can severely impact performance.
// @Units: ms
// @Values: 100:10Hz,125:8Hz,200:5Hz
// @Range: 50 200
// @User: Advanced
AP_GROUPINFO("_RATE_MS", 14, AP_GPS, _rate_ms[0], 200),
#if GPS_MAX_RECEIVERS > 1 // 15 was GPS_RATE_MS2
// @Param: _RATE_MS2
// @DisplayName: GPS 2 update rate in milliseconds
// @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz(default) is not allowed. Raising the rate above 5Hz usually provides little benefit and for some GPS (eg Ublox M9N) can severely impact performance.
// @Units: ms
// @Values: 100:10Hz,125:8Hz,200:5Hz
// @Range: 50 200
// @User: Advanced
AP_GROUPINFO("_RATE_MS2", 15, AP_GPS, _rate_ms[1], 200),
#endif
// @Param: _POS1_X // 16 was GPS_POS1
// @DisplayName: Antenna X position offset
// @Description: X position of the first GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer.
// @Units: m
// @Range: -5 5
// @Increment: 0.01
// @User: Advanced
// @Param: _POS1_Y // 17 was GPS_POS2
// @DisplayName: Antenna Y position offset
// @Description: Y position of the first GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer.
// @Units: m
// @Range: -5 5
// @Increment: 0.01
// @User: Advanced
// @Param: _POS1_Z // 18 was GPS_DELAY_MS
// @DisplayName: Antenna Z position offset
// @Description: Z position of the first GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer.
// @Units: m
// @Range: -5 5
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("_POS1", 16, AP_GPS, _antenna_offset[0], 0.0f),
#if GPS_MAX_RECEIVERS > 1 // 19 was GPS_DELAY_MS2
// @Param: _POS2_X
// @DisplayName: Antenna X position offset
// @Description: X position of the second GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer.
// @Units: m
// @Range: -5 5
// @Increment: 0.01
// @User: Advanced
// @Param: _POS2_Y
// @DisplayName: Antenna Y position offset
// @Description: Y position of the second GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer.
// @Units: m
// @Range: -5 5
// @Increment: 0.01
// @User: Advanced
// @Param: _POS2_Z
// @DisplayName: Antenna Z position offset
// @Description: Z position of the second GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer.
// @Units: m
// @Range: -5 5
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("_POS2", 17, AP_GPS, _antenna_offset[1], 0.0f),
#endif
// @Param: _DELAY_MS
// @DisplayName: GPS delay in milliseconds
// @Description: Controls the amount of GPS measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.
// @Units: ms
// @Range: 0 250
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("_DELAY_MS", 18, AP_GPS, _delay_ms[0], 0),
#if GPS_MAX_RECEIVERS > 1
// @Param: _DELAY_MS2
// @DisplayName: GPS 2 delay in milliseconds
// @Description: Controls the amount of GPS measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.
// @Units: ms
// @Range: 0 250
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("_DELAY_MS2", 19, AP_GPS, _delay_ms[1], 0),
#endif
#if defined(GPS_BLENDED_INSTANCE) #if defined(GPS_BLENDED_INSTANCE)
// @Param: _BLEND_MASK // @Param: _BLEND_MASK
@ -349,42 +242,13 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0), AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),
#if AP_GPS_SBF_ENABLED // 23 was GPS_COM_PORT
// @Param: _COM_PORT
// @DisplayName: GPS physical COM port
// @Description: The physical COM port on the connected device, currently only applies to SBF and GSOF GPS
// @Range: 0 10
// @Increment: 1
// @User: Advanced
// @Values: 0:COM1(RS232) on GSOF, 1:COM2(TTL) on GSOF
// @RebootRequired: True
AP_GROUPINFO("_COM_PORT", 23, AP_GPS, _com_port[0], HAL_GPS_COM_PORT_DEFAULT),
#if GPS_MAX_RECEIVERS > 1 // 24 was GPS_COM_PORT2
// @Param: _COM_PORT2
// @DisplayName: GPS physical COM port
// @Description: The physical COM port on the connected device, currently only applies to SBF and GSOF GPS
// @Range: 0 10
// @Increment: 1
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("_COM_PORT2", 24, AP_GPS, _com_port[1], HAL_GPS_COM_PORT_DEFAULT),
#endif
#endif //AP_GPS_SBF_ENABLED
#if GPS_MOVING_BASELINE // 25 was GPS_MB1_*
// @Group: _MB1_ // 26 was GPS_MB2_*
// @Path: MovingBase.cpp
AP_SUBGROUPINFO(mb_params[0], "_MB1_", 25, AP_GPS, MovingBase),
#if GPS_MAX_RECEIVERS > 1
// @Group: _MB2_
// @Path: MovingBase.cpp
AP_SUBGROUPINFO(mb_params[1], "_MB2_", 26, AP_GPS, MovingBase),
#endif // GPS_MAX_RECEIVERS > 1
#endif // GPS_MOVING_BASELINE
#if GPS_MAX_RECEIVERS > 1 #if GPS_MAX_RECEIVERS > 1
// @Param: _PRIMARY // @Param: _PRIMARY
@ -396,36 +260,23 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
AP_GROUPINFO("_PRIMARY", 27, AP_GPS, _primary, 0), AP_GROUPINFO("_PRIMARY", 27, AP_GPS, _primary, 0),
#endif #endif
#if HAL_ENABLE_DRONECAN_DRIVERS // 28 was GPS_CAN_NODEID1
// @Param: _CAN_NODEID1
// @DisplayName: GPS Node ID 1 // 29 was GPS_CAN_NODEID2
// @Description: GPS Node id for first-discovered GPS.
// @ReadOnly: True // 30 was GPS1_CAN_OVRIDE
// @User: Advanced
AP_GROUPINFO("_CAN_NODEID1", 28, AP_GPS, _node_id[0], 0), // 31 was GPS2_CAN_OVRIDE
// @Group: _
// @Path: AP_GPS_Params.cpp
AP_SUBGROUPINFO(params[0], "_", 32, AP_GPS, AP_GPS::Params),
#if GPS_MAX_RECEIVERS > 1 #if GPS_MAX_RECEIVERS > 1
// @Param: _CAN_NODEID2 // @Group: 2_
// @DisplayName: GPS Node ID 2 // @Path: AP_GPS_Params.cpp
// @Description: GPS Node id for second-discovered GPS. AP_SUBGROUPINFO(params[1], "2_", 33, AP_GPS, AP_GPS::Params),
// @ReadOnly: True #endif
// @User: Advanced
AP_GROUPINFO("_CAN_NODEID2", 29, AP_GPS, _node_id[1], 0),
#endif // GPS_MAX_RECEIVERS > 1
// @Param: 1_CAN_OVRIDE
// @DisplayName: First DroneCAN GPS NODE ID
// @Description: GPS Node id for first GPS. If 0 the gps will be automatically selected on a first-come-first-GPS basis.
// @User: Advanced
AP_GROUPINFO("1_CAN_OVRIDE", 30, AP_GPS, _override_node_id[0], 0),
#if GPS_MAX_RECEIVERS > 1
// @Param: 2_CAN_OVRIDE
// @DisplayName: Second DroneCAN GPS NODE ID
// @Description: GPS Node id for second GPS. If 0 the gps will be automatically selected on a second-come-second-GPS basis.
// @User: Advanced
AP_GROUPINFO("2_CAN_OVRIDE", 31, AP_GPS, _override_node_id[1], 0),
#endif // GPS_MAX_RECEIVERS > 1
#endif // HAL_ENABLE_DRONECAN_DRIVERS
AP_GROUPEND AP_GROUPEND
}; };
@ -466,6 +317,9 @@ bool AP_GPS::needs_uart(GPS_Type type) const
/// Startup initialisation. /// Startup initialisation.
void AP_GPS::init() void AP_GPS::init()
{ {
// set the default for the first GPS according to define:
params[0].type.set_default(HAL_GPS_TYPE_DEFAULT);
// Set new primary param based on old auto_switch use second option // Set new primary param based on old auto_switch use second option
if ((_auto_switch.get() == 3) && !_primary.configured()) { if ((_auto_switch.get() == 3) && !_primary.configured()) {
_primary.set_and_save(1); _primary.set_and_save(1);
@ -475,8 +329,8 @@ void AP_GPS::init()
// search for serial ports with gps protocol // search for serial ports with gps protocol
const auto &serial_manager = AP::serialmanager(); const auto &serial_manager = AP::serialmanager();
uint8_t uart_idx = 0; uint8_t uart_idx = 0;
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) { for (uint8_t i=0; i<ARRAY_SIZE(params); i++) {
if (needs_uart((GPS_Type)_type[i].get())) { if (needs_uart(params[i].type)) {
_port[i] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, uart_idx); _port[i] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, uart_idx);
uart_idx++; uart_idx++;
} }
@ -489,9 +343,10 @@ void AP_GPS::init()
} }
// sanity check update rate // sanity check update rate
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) { for (uint8_t i=0; i<ARRAY_SIZE(params); i++) {
if (_rate_ms[i] <= 0 || _rate_ms[i] > GPS_MAX_RATE_MS) { auto &rate_ms = params[i].rate_ms;
_rate_ms[i].set(GPS_MAX_RATE_MS); if (rate_ms <= 0 || rate_ms > GPS_MAX_RATE_MS) {
rate_ms.set(GPS_MAX_RATE_MS);
} }
} }
} }
@ -614,7 +469,7 @@ void AP_GPS::send_blob_start(uint8_t instance, const char *_blob, uint16_t size)
*/ */
void AP_GPS::send_blob_start(uint8_t instance) void AP_GPS::send_blob_start(uint8_t instance)
{ {
const auto type = _type[instance]; const auto type = params[instance].type;
#if AP_GPS_UBLOX_ENABLED #if AP_GPS_UBLOX_ENABLED
if (type == GPS_TYPE_UBLOX && option_set(DriverOptions::UBX_Use115200)) { if (type == GPS_TYPE_UBLOX && option_set(DriverOptions::UBX_Use115200)) {
@ -641,7 +496,7 @@ void AP_GPS::send_blob_start(uint8_t instance)
// the following devices don't have init blobs: // the following devices don't have init blobs:
const char *blob = nullptr; const char *blob = nullptr;
uint32_t blob_size = 0; uint32_t blob_size = 0;
switch (type) { switch (GPS_Type(type)) {
#if AP_GPS_SBF_ENABLED #if AP_GPS_SBF_ENABLED
case GPS_TYPE_SBF: case GPS_TYPE_SBF:
case GPS_TYPE_SBF_DUAL_ANTENNA: case GPS_TYPE_SBF_DUAL_ANTENNA:
@ -726,15 +581,15 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance)
{ {
struct detect_state *dstate = &detect_state[instance]; struct detect_state *dstate = &detect_state[instance];
const auto type = _type[instance]; const auto type = params[instance].type;
switch (type) { switch (GPS_Type(type)) {
// user has to explicitly set the MAV type, do not use AUTO // user has to explicitly set the MAV type, do not use AUTO
// do not try to detect the MAV type, assume it's there // do not try to detect the MAV type, assume it's there
case GPS_TYPE_MAV: case GPS_TYPE_MAV:
#if AP_GPS_MAV_ENABLED #if AP_GPS_MAV_ENABLED
dstate->auto_detected_baud = false; // specified, not detected dstate->auto_detected_baud = false; // specified, not detected
return new AP_GPS_MAV(*this, state[instance], nullptr); return new AP_GPS_MAV(*this, params[instance], state[instance], nullptr);
#endif //AP_GPS_MAV_ENABLED #endif //AP_GPS_MAV_ENABLED
// user has to explicitly set the UAVCAN type, do not use AUTO // user has to explicitly set the UAVCAN type, do not use AUTO
@ -749,17 +604,17 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance)
#if HAL_MSP_GPS_ENABLED #if HAL_MSP_GPS_ENABLED
case GPS_TYPE_MSP: case GPS_TYPE_MSP:
dstate->auto_detected_baud = false; // specified, not detected dstate->auto_detected_baud = false; // specified, not detected
return new AP_GPS_MSP(*this, state[instance], nullptr); return new AP_GPS_MSP(*this, params[instance], state[instance], nullptr);
#endif #endif
#if HAL_EXTERNAL_AHRS_ENABLED #if HAL_EXTERNAL_AHRS_ENABLED
case GPS_TYPE_EXTERNAL_AHRS: case GPS_TYPE_EXTERNAL_AHRS:
dstate->auto_detected_baud = false; // specified, not detected dstate->auto_detected_baud = false; // specified, not detected
return new AP_GPS_ExternalAHRS(*this, state[instance], nullptr); return new AP_GPS_ExternalAHRS(*this, params[instance], state[instance], nullptr);
#endif #endif
#if AP_GPS_GSOF_ENABLED #if AP_GPS_GSOF_ENABLED
case GPS_TYPE_GSOF: case GPS_TYPE_GSOF:
dstate->auto_detected_baud = false; // specified, not detected dstate->auto_detected_baud = false; // specified, not detected
return new AP_GPS_GSOF(*this, state[instance], _port[instance]); return new AP_GPS_GSOF(*this, params[instance], state[instance], _port[instance]);
#endif //AP_GPS_GSOF_ENABLED #endif //AP_GPS_GSOF_ENABLED
default: default:
break; break;
@ -808,21 +663,21 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance)
send_blob_update(instance); send_blob_update(instance);
} }
switch (type) { switch (GPS_Type(type)) {
#if AP_GPS_SBF_ENABLED #if AP_GPS_SBF_ENABLED
// by default the sbf/trimble gps outputs no data on its port, until configured. // by default the sbf/trimble gps outputs no data on its port, until configured.
case GPS_TYPE_SBF: case GPS_TYPE_SBF:
case GPS_TYPE_SBF_DUAL_ANTENNA: case GPS_TYPE_SBF_DUAL_ANTENNA:
return new AP_GPS_SBF(*this, state[instance], _port[instance]); return new AP_GPS_SBF(*this, params[instance], state[instance], _port[instance]);
#endif //AP_GPS_SBF_ENABLED #endif //AP_GPS_SBF_ENABLED
#if AP_GPS_NOVA_ENABLED #if AP_GPS_NOVA_ENABLED
case GPS_TYPE_NOVA: case GPS_TYPE_NOVA:
return new AP_GPS_NOVA(*this, state[instance], _port[instance]); return new AP_GPS_NOVA(*this, params[instance], state[instance], _port[instance]);
#endif //AP_GPS_NOVA_ENABLED #endif //AP_GPS_NOVA_ENABLED
#if HAL_SIM_GPS_ENABLED #if HAL_SIM_GPS_ENABLED
case GPS_TYPE_SITL: case GPS_TYPE_SITL:
return new AP_GPS_SITL(*this, state[instance], _port[instance]); return new AP_GPS_SITL(*this, params[instance], state[instance], _port[instance]);
#endif // HAL_SIM_GPS_ENABLED #endif // HAL_SIM_GPS_ENABLED
default: default:
@ -847,7 +702,7 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance)
(_baudrates[dstate->current_baud] >= 115200 && option_set(DriverOptions::UBX_Use115200)) || (_baudrates[dstate->current_baud] >= 115200 && option_set(DriverOptions::UBX_Use115200)) ||
_baudrates[dstate->current_baud] == 230400) && _baudrates[dstate->current_baud] == 230400) &&
AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) { AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
return new AP_GPS_UBLOX(*this, state[instance], _port[instance], GPS_ROLE_NORMAL); return new AP_GPS_UBLOX(*this, params[instance], state[instance], _port[instance], GPS_ROLE_NORMAL);
} }
const uint32_t ublox_mb_required_baud = option_set(DriverOptions::UBX_MBUseUart2)?230400:460800; const uint32_t ublox_mb_required_baud = option_set(DriverOptions::UBX_MBUseUart2)?230400:460800;
@ -861,31 +716,31 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance)
} else { } else {
role = GPS_ROLE_MB_ROVER; role = GPS_ROLE_MB_ROVER;
} }
return new AP_GPS_UBLOX(*this, state[instance], _port[instance], role); return new AP_GPS_UBLOX(*this, params[instance], state[instance], _port[instance], role);
} }
#endif // AP_GPS_UBLOX_ENABLED #endif // AP_GPS_UBLOX_ENABLED
#if AP_GPS_SBP2_ENABLED #if AP_GPS_SBP2_ENABLED
if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_SBP) && if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_SBP) &&
AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) { AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) {
return new AP_GPS_SBP2(*this, state[instance], _port[instance]); return new AP_GPS_SBP2(*this, params[instance], state[instance], _port[instance]);
} }
#endif //AP_GPS_SBP2_ENABLED #endif //AP_GPS_SBP2_ENABLED
#if AP_GPS_SBP_ENABLED #if AP_GPS_SBP_ENABLED
if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_SBP) && if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_SBP) &&
AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) { AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) {
return new AP_GPS_SBP(*this, state[instance], _port[instance]); return new AP_GPS_SBP(*this, params[instance], state[instance], _port[instance]);
} }
#endif //AP_GPS_SBP_ENABLED #endif //AP_GPS_SBP_ENABLED
#if AP_GPS_SIRF_ENABLED #if AP_GPS_SIRF_ENABLED
if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_SIRF) && if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_SIRF) &&
AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) { AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) {
return new AP_GPS_SIRF(*this, state[instance], _port[instance]); return new AP_GPS_SIRF(*this, params[instance], state[instance], _port[instance]);
} }
#endif #endif
#if AP_GPS_ERB_ENABLED #if AP_GPS_ERB_ENABLED
if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_ERB) && if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_ERB) &&
AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) { AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) {
return new AP_GPS_ERB(*this, state[instance], _port[instance]); return new AP_GPS_ERB(*this, params[instance], state[instance], _port[instance]);
} }
#endif // AP_GPS_ERB_ENABLED #endif // AP_GPS_ERB_ENABLED
#if AP_GPS_NMEA_ENABLED #if AP_GPS_NMEA_ENABLED
@ -897,7 +752,7 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance)
#endif #endif
type == GPS_TYPE_ALLYSTAR) && type == GPS_TYPE_ALLYSTAR) &&
AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) { AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
return new AP_GPS_NMEA(*this, state[instance], _port[instance]); return new AP_GPS_NMEA(*this, params[instance], state[instance], _port[instance]);
} }
#endif //AP_GPS_NMEA_ENABLED #endif //AP_GPS_NMEA_ENABLED
} }
@ -936,7 +791,7 @@ bool AP_GPS::should_log() const
*/ */
void AP_GPS::update_instance(uint8_t instance) void AP_GPS::update_instance(uint8_t instance)
{ {
const auto type = _type[instance]; const auto type = params[instance].type;
if (type == GPS_TYPE_HIL) { if (type == GPS_TYPE_HIL) {
// in HIL, leave info alone // in HIL, leave info alone
return; return;
@ -1031,7 +886,7 @@ void AP_GPS::update_instance(uint8_t instance)
uint16_t rtcm_len; uint16_t rtcm_len;
if (drivers[instance]->get_RTCMV3(rtcm_data, rtcm_len)) { if (drivers[instance]->get_RTCMV3(rtcm_data, rtcm_len)) {
for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) { for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
if (i != instance && _type[i] == GPS_TYPE_UBLOX_RTK_ROVER) { if (i != instance && params[i].type == GPS_TYPE_UBLOX_RTK_ROVER) {
// pass the data to the rover // pass the data to the rover
inject_data(i, rtcm_data, rtcm_len); inject_data(i, rtcm_data, rtcm_len);
drivers[instance]->clear_RTCMV3(); drivers[instance]->clear_RTCMV3();
@ -1099,7 +954,7 @@ void AP_GPS::get_RelPosHeading(uint32_t &timestamp, float &relPosHeading, float
bool AP_GPS::get_RTCMV3(const uint8_t *&bytes, uint16_t &len) bool AP_GPS::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
{ {
for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) { for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
if (drivers[i] && _type[i] == GPS_TYPE_UBLOX_RTK_BASE) { if (drivers[i] && params[i].type == GPS_TYPE_UBLOX_RTK_BASE) {
return drivers[i]->get_RTCMV3(bytes, len); return drivers[i]->get_RTCMV3(bytes, len);
} }
} }
@ -1109,7 +964,7 @@ bool AP_GPS::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
void AP_GPS::clear_RTCMV3() void AP_GPS::clear_RTCMV3()
{ {
for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) { for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
if (drivers[i] && _type[i] == GPS_TYPE_UBLOX_RTK_BASE) { if (drivers[i] && params[i].type == GPS_TYPE_UBLOX_RTK_BASE) {
drivers[i]->clear_RTCMV3(); drivers[i]->clear_RTCMV3();
} }
} }
@ -1121,7 +976,7 @@ void AP_GPS::clear_RTCMV3()
void AP_GPS::inject_MBL_data(uint8_t* data, uint16_t length) void AP_GPS::inject_MBL_data(uint8_t* data, uint16_t length)
{ {
for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) { for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER) { if (params[i].type == GPS_TYPE_UBLOX_RTK_ROVER) {
// pass the data to the rover // pass the data to the rover
inject_data(i, data, length); inject_data(i, data, length);
break; break;
@ -1356,7 +1211,7 @@ void AP_GPS::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg)
default: { default: {
uint8_t i; uint8_t i;
for (i=0; i<num_instances; i++) { for (i=0; i<num_instances; i++) {
if ((drivers[i] != nullptr) && (_type[i] != GPS_TYPE_NONE)) { if ((drivers[i] != nullptr) && (params[i].type != GPS_TYPE_NONE)) {
drivers[i]->handle_msg(msg); drivers[i]->handle_msg(msg);
} }
} }
@ -1370,7 +1225,7 @@ void AP_GPS::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg)
void AP_GPS::handle_msp(const MSP::msp_gps_data_message_t &pkt) void AP_GPS::handle_msp(const MSP::msp_gps_data_message_t &pkt)
{ {
for (uint8_t i=0; i<num_instances; i++) { for (uint8_t i=0; i<num_instances; i++) {
if (drivers[i] != nullptr && _type[i] == GPS_TYPE_MSP) { if (drivers[i] != nullptr && params[i].type == GPS_TYPE_MSP) {
drivers[i]->handle_msp(pkt); drivers[i]->handle_msp(pkt);
} }
} }
@ -1382,7 +1237,7 @@ void AP_GPS::handle_msp(const MSP::msp_gps_data_message_t &pkt)
bool AP_GPS::get_first_external_instance(uint8_t& instance) const bool AP_GPS::get_first_external_instance(uint8_t& instance) const
{ {
for (uint8_t i=0; i<num_instances; i++) { for (uint8_t i=0; i<num_instances; i++) {
if (drivers[i] != nullptr && _type[i] == GPS_TYPE_EXTERNAL_AHRS) { if (drivers[i] != nullptr && params[i].type == GPS_TYPE_EXTERNAL_AHRS) {
instance = i; instance = i;
return true; return true;
} }
@ -1501,7 +1356,7 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
{ {
// always send the message if 2nd GPS is configured // always send the message if 2nd GPS is configured
if (_type[1] == GPS_TYPE_NONE) { if (params[1].type == GPS_TYPE_NONE) {
return; return;
} }
@ -1555,7 +1410,7 @@ void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst)
bool AP_GPS::first_unconfigured_gps(uint8_t &instance) const bool AP_GPS::first_unconfigured_gps(uint8_t &instance) const
{ {
for (int i = 0; i < GPS_MAX_RECEIVERS; i++) { for (int i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_type[i] != GPS_TYPE_NONE && (drivers[i] == nullptr || !drivers[i]->is_configured())) { if (params[i].type != GPS_TYPE_NONE && (drivers[i] == nullptr || !drivers[i]->is_configured())) {
instance = i; instance = i;
return true; return true;
} }
@ -1580,7 +1435,7 @@ bool AP_GPS::all_consistent(float &distance) const
{ {
// return true immediately if only one valid receiver // return true immediately if only one valid receiver
if (num_instances <= 1 || if (num_instances <= 1 ||
drivers[0] == nullptr || _type[0] == GPS_TYPE_NONE) { drivers[0] == nullptr || params[0].type == GPS_TYPE_NONE) {
distance = 0; distance = 0;
return true; return true;
} }
@ -1809,14 +1664,14 @@ bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const
} }
#endif #endif
if (_delay_ms[instance] > 0) { if (params[instance].delay_ms > 0) {
// if the user has specified a non zero time delay, always return that value // if the user has specified a non zero time delay, always return that value
lag_sec = 0.001f * (float)_delay_ms[instance]; lag_sec = 0.001f * (float)params[instance].delay_ms;
// the user is always right !! // the user is always right !!
return true; return true;
} else if (drivers[instance] == nullptr || state[instance].status == NO_GPS) { } else if (drivers[instance] == nullptr || state[instance].status == NO_GPS) {
// no GPS was detected in this instance so return the worst possible lag term // no GPS was detected in this instance so return the worst possible lag term
const auto type = _type[instance]; const auto type = params[instance].type;
if (type == GPS_TYPE_NONE) { if (type == GPS_TYPE_NONE) {
lag_sec = 0.0f; lag_sec = 0.0f;
return true; return true;
@ -1833,7 +1688,7 @@ const Vector3f &AP_GPS::get_antenna_offset(uint8_t instance) const
{ {
if (instance >= GPS_MAX_INSTANCES) { if (instance >= GPS_MAX_INSTANCES) {
// we have to return a reference so use instance 0 // we have to return a reference so use instance 0
return _antenna_offset[0]; return params[0].antenna_offset;
} }
#if defined(GPS_BLENDED_INSTANCE) #if defined(GPS_BLENDED_INSTANCE)
@ -1843,7 +1698,7 @@ const Vector3f &AP_GPS::get_antenna_offset(uint8_t instance) const
} }
#endif #endif
return _antenna_offset[instance]; return params[instance].antenna_offset;
} }
/* /*
@ -1855,10 +1710,10 @@ const Vector3f &AP_GPS::get_antenna_offset(uint8_t instance) const
uint16_t AP_GPS::get_rate_ms(uint8_t instance) const uint16_t AP_GPS::get_rate_ms(uint8_t instance) const
{ {
// sanity check // sanity check
if (instance >= num_instances || _rate_ms[instance] <= 0) { if (instance >= num_instances || params[instance].rate_ms <= 0) {
return GPS_MAX_RATE_MS; return GPS_MAX_RATE_MS;
} }
return MIN(_rate_ms[instance], GPS_MAX_RATE_MS); return MIN(params[instance].rate_ms, GPS_MAX_RATE_MS);
} }
#if defined(GPS_BLENDED_INSTANCE) #if defined(GPS_BLENDED_INSTANCE)
@ -2124,7 +1979,7 @@ void AP_GPS::calc_blended_state(void)
} }
// report a blended average GPS antenna position // report a blended average GPS antenna position
Vector3f temp_antenna_offset = _antenna_offset[i]; Vector3f temp_antenna_offset = params[i].antenna_offset;
temp_antenna_offset *= _blend_weights[i]; temp_antenna_offset *= _blend_weights[i];
_blended_antenna_offset += temp_antenna_offset; _blended_antenna_offset += temp_antenna_offset;
@ -2285,7 +2140,7 @@ bool AP_GPS::prepare_for_arming(void) {
bool AP_GPS::backends_healthy(char failure_msg[], uint16_t failure_msg_len) { bool AP_GPS::backends_healthy(char failure_msg[], uint16_t failure_msg_len) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
#if AP_GPS_DRONECAN_ENABLED #if AP_GPS_DRONECAN_ENABLED
const auto type = _type[i]; const auto type = params[i].type;
if (type == GPS_TYPE_UAVCAN || if (type == GPS_TYPE_UAVCAN ||
type == GPS_TYPE_UAVCAN_RTK_BASE || type == GPS_TYPE_UAVCAN_RTK_BASE ||
type == GPS_TYPE_UAVCAN_RTK_ROVER) { type == GPS_TYPE_UAVCAN_RTK_ROVER) {

39
libraries/AP_GPS/AP_GPS.h
View File

@ -142,6 +142,29 @@ public:
bool is_rtk_base(uint8_t instance) const; bool is_rtk_base(uint8_t instance) const;
bool is_rtk_rover(uint8_t instance) const; bool is_rtk_rover(uint8_t instance) const;
// params for an instance:
class Params {
public:
// Constructor
Params(void);
AP_Enum<GPS_Type> type;
AP_Int8 gnss_mode;
AP_Int16 rate_ms; // this parameter should always be accessed using get_rate_ms()
AP_Vector3f antenna_offset;
AP_Int16 delay_ms;
AP_Int8 com_port;
#if HAL_ENABLE_DRONECAN_DRIVERS
AP_Int32 node_id;
AP_Int32 override_node_id;
#endif
#if GPS_MOVING_BASELINE
MovingBase mb_params;
#endif // GPS_MOVING_BASELINE
static const struct AP_Param::GroupInfo var_info[];
};
/// GPS status codes. These are kept aligned with MAVLink by /// GPS status codes. These are kept aligned with MAVLink by
/// static_assert in AP_GPS.cpp /// static_assert in AP_GPS.cpp
enum GPS_Status { enum GPS_Status {
@ -570,7 +593,7 @@ public:
// get configured type by instance // get configured type by instance
GPS_Type get_type(uint8_t instance) const { GPS_Type get_type(uint8_t instance) const {
return instance>=GPS_MAX_RECEIVERS? GPS_Type::GPS_TYPE_NONE : GPS_Type(_type[instance].get()); return instance>=ARRAY_SIZE(params) ? GPS_Type::GPS_TYPE_NONE : params[instance].type;
} }
// get iTOW, if supported, zero otherwie // get iTOW, if supported, zero otherwie
@ -596,7 +619,7 @@ public:
protected: protected:
// configuration parameters // configuration parameters
AP_Int8 _type[GPS_MAX_RECEIVERS]; Params params[GPS_MAX_RECEIVERS];
AP_Int8 _navfilter; AP_Int8 _navfilter;
AP_Int8 _auto_switch; AP_Int8 _auto_switch;
AP_Int16 _sbp_logmask; AP_Int16 _sbp_logmask;
@ -605,23 +628,11 @@ protected:
AP_Enum<SBAS_Mode> _sbas_mode; AP_Enum<SBAS_Mode> _sbas_mode;
AP_Int8 _min_elevation; AP_Int8 _min_elevation;
AP_Int8 _raw_data; AP_Int8 _raw_data;
AP_Int8 _gnss_mode[GPS_MAX_RECEIVERS];
AP_Int16 _rate_ms[GPS_MAX_RECEIVERS]; // this parameter should always be accessed using get_rate_ms()
AP_Int8 _save_config; AP_Int8 _save_config;
AP_Int8 _auto_config; AP_Int8 _auto_config;
AP_Vector3f _antenna_offset[GPS_MAX_RECEIVERS];
AP_Int16 _delay_ms[GPS_MAX_RECEIVERS];
AP_Int8 _com_port[GPS_MAX_RECEIVERS];
AP_Int8 _blend_mask; AP_Int8 _blend_mask;
AP_Int16 _driver_options; AP_Int16 _driver_options;
AP_Int8 _primary; AP_Int8 _primary;
#if HAL_ENABLE_DRONECAN_DRIVERS
AP_Int32 _node_id[GPS_MAX_RECEIVERS];
AP_Int32 _override_node_id[GPS_MAX_RECEIVERS];
#endif
#if GPS_MOVING_BASELINE
MovingBase mb_params[GPS_MAX_RECEIVERS];
#endif // GPS_MOVING_BASELINE
uint32_t _log_gps_bit = -1; uint32_t _log_gps_bit = -1;

49
libraries/AP_GPS/AP_GPS_DroneCAN.cpp
View File

@ -63,8 +63,11 @@ AP_GPS_DroneCAN::DetectedModules AP_GPS_DroneCAN::_detected_modules[];
HAL_Semaphore AP_GPS_DroneCAN::_sem_registry; HAL_Semaphore AP_GPS_DroneCAN::_sem_registry;
// Member Methods // Member Methods
AP_GPS_DroneCAN::AP_GPS_DroneCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role _role) : AP_GPS_DroneCAN::AP_GPS_DroneCAN(AP_GPS &_gps,
AP_GPS_Backend(_gps, _state, nullptr), AP_GPS::Params &_params,
AP_GPS::GPS_State &_state,
AP_GPS::GPS_Role _role) :
AP_GPS_Backend(_gps, _params, _state, nullptr),
interim_state(_state), interim_state(_state),
role(_role) role(_role)
{ {
@ -126,13 +129,13 @@ AP_GPS_Backend* AP_GPS_DroneCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
bool bad_override_config = false; bool bad_override_config = false;
for (int8_t i = GPS_MAX_RECEIVERS - 1; i >= 0; i--) { for (int8_t i = GPS_MAX_RECEIVERS - 1; i >= 0; i--) {
if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_dronecan != nullptr) { if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_dronecan != nullptr) {
if (_gps._override_node_id[_state.instance] != 0 && if (_gps.params[_state.instance].override_node_id != 0 &&
_gps._override_node_id[_state.instance] != _detected_modules[i].node_id) { _gps.params[_state.instance].override_node_id != _detected_modules[i].node_id) {
continue; // This device doesn't match the correct node continue; // This device doesn't match the correct node
} }
last_match = found_match; last_match = found_match;
for (uint8_t j = 0; j < GPS_MAX_RECEIVERS; j++) { for (uint8_t j = 0; j < GPS_MAX_RECEIVERS; j++) {
if (_detected_modules[i].node_id == _gps._override_node_id[j] && if (_detected_modules[i].node_id == _gps.params[j].override_node_id &&
(j != _state.instance)) { (j != _state.instance)) {
//wrong instance //wrong instance
found_match = -1; found_match = -1;
@ -143,13 +146,13 @@ AP_GPS_Backend* AP_GPS_DroneCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
// Handle Duplicate overrides // Handle Duplicate overrides
for (uint8_t j = 0; j < GPS_MAX_RECEIVERS; j++) { for (uint8_t j = 0; j < GPS_MAX_RECEIVERS; j++) {
if (_gps._override_node_id[i] != 0 && (i != j) && if (_gps.params[i].override_node_id != 0 && (i != j) &&
_gps._override_node_id[i] == _gps._override_node_id[j]) { _gps.params[i].override_node_id == _gps.params[j].override_node_id) {
bad_override_config = true; bad_override_config = true;
} }
} }
if (bad_override_config) { if (bad_override_config) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Same Node Id %lu set for multiple GPS", (unsigned long int)_gps._override_node_id[i].get()); GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Same Node Id %lu set for multiple GPS", (unsigned long int)_gps.params[i].override_node_id.get());
last_match = i; last_match = i;
} }
@ -167,14 +170,14 @@ AP_GPS_Backend* AP_GPS_DroneCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
// initialise the backend based on the UAVCAN Moving baseline selection // initialise the backend based on the UAVCAN Moving baseline selection
switch (_gps.get_type(_state.instance)) { switch (_gps.get_type(_state.instance)) {
case AP_GPS::GPS_TYPE_UAVCAN: case AP_GPS::GPS_TYPE_UAVCAN:
backend = new AP_GPS_DroneCAN(_gps, _state, AP_GPS::GPS_ROLE_NORMAL); backend = new AP_GPS_DroneCAN(_gps, _gps.params[_state.instance], _state, AP_GPS::GPS_ROLE_NORMAL);
break; break;
#if GPS_MOVING_BASELINE #if GPS_MOVING_BASELINE
case AP_GPS::GPS_TYPE_UAVCAN_RTK_BASE: case AP_GPS::GPS_TYPE_UAVCAN_RTK_BASE:
backend = new AP_GPS_DroneCAN(_gps, _state, AP_GPS::GPS_ROLE_MB_BASE); backend = new AP_GPS_DroneCAN(_gps, _gps.params[_state.instance], _state, AP_GPS::GPS_ROLE_MB_BASE);
break; break;
case AP_GPS::GPS_TYPE_UAVCAN_RTK_ROVER: case AP_GPS::GPS_TYPE_UAVCAN_RTK_ROVER:
backend = new AP_GPS_DroneCAN(_gps, _state, AP_GPS::GPS_ROLE_MB_ROVER); backend = new AP_GPS_DroneCAN(_gps, _gps.params[_state.instance], _state, AP_GPS::GPS_ROLE_MB_ROVER);
break; break;
#endif #endif
default: default:
@ -198,15 +201,15 @@ AP_GPS_Backend* AP_GPS_DroneCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
snprintf(backend->_name, ARRAY_SIZE(backend->_name), "DroneCAN%u-%u", _detected_modules[found_match].ap_dronecan->get_driver_index()+1, _detected_modules[found_match].node_id); snprintf(backend->_name, ARRAY_SIZE(backend->_name), "DroneCAN%u-%u", _detected_modules[found_match].ap_dronecan->get_driver_index()+1, _detected_modules[found_match].node_id);
_detected_modules[found_match].instance = _state.instance; _detected_modules[found_match].instance = _state.instance;
for (uint8_t i=0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i=0; i < GPS_MAX_RECEIVERS; i++) {
if (_detected_modules[found_match].node_id == AP::gps()._node_id[i]) { if (_detected_modules[found_match].node_id == AP::gps().params[i].node_id) {
if (i == _state.instance) { if (i == _state.instance) {
// Nothing to do here // Nothing to do here
break; break;
} }
// else swap // else swap
uint8_t tmp = AP::gps()._node_id[_state.instance].get(); uint8_t tmp = AP::gps().params[_state.instance].node_id.get();
AP::gps()._node_id[_state.instance].set_and_notify(_detected_modules[found_match].node_id); AP::gps().params[_state.instance].node_id.set_and_notify(_detected_modules[found_match].node_id);
AP::gps()._node_id[i].set_and_notify(tmp); AP::gps().params[i].node_id.set_and_notify(tmp);
} }
} }
#if GPS_MOVING_BASELINE #if GPS_MOVING_BASELINE
@ -225,31 +228,33 @@ AP_GPS_Backend* AP_GPS_DroneCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
bool AP_GPS_DroneCAN::backends_healthy(char failure_msg[], uint16_t failure_msg_len) bool AP_GPS_DroneCAN::backends_healthy(char failure_msg[], uint16_t failure_msg_len)
{ {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
const auto &params_i = AP::gps().params[i];
bool overriden_node_found = false; bool overriden_node_found = false;
bool bad_override_config = false; bool bad_override_config = false;
if (AP::gps()._override_node_id[i] == 0) { if (params_i.override_node_id == 0) {
//anything goes //anything goes
continue; continue;
} }
for (uint8_t j = 0; j < GPS_MAX_RECEIVERS; j++) { for (uint8_t j = 0; j < GPS_MAX_RECEIVERS; j++) {
if (AP::gps()._override_node_id[i] == AP::gps()._override_node_id[j] && (i != j)) { const auto &params_j = AP::gps().params[j];
if (params_i.override_node_id == params_j.override_node_id && (i != j)) {
bad_override_config = true; bad_override_config = true;
break; break;
} }
if (i == _detected_modules[j].instance && _detected_modules[j].driver) { if (i == _detected_modules[j].instance && _detected_modules[j].driver) {
if (AP::gps()._override_node_id[i] == _detected_modules[j].node_id) { if (params_i.override_node_id == _detected_modules[j].node_id) {
overriden_node_found = true; overriden_node_found = true;
break; break;
} }
} }
} }
if (bad_override_config) { if (bad_override_config) {
snprintf(failure_msg, failure_msg_len, "Same Node Id %lu set for multiple GPS", (unsigned long int)AP::gps()._override_node_id[i].get()); snprintf(failure_msg, failure_msg_len, "Same Node Id %lu set for multiple GPS", (unsigned long int)params_i.override_node_id.get());
return false; return false;
} }
if (!overriden_node_found) { if (!overriden_node_found) {
snprintf(failure_msg, failure_msg_len, "Selected GPS Node %lu not set as instance %d", (unsigned long int)AP::gps()._override_node_id[i].get(), i + 1); snprintf(failure_msg, failure_msg_len, "Selected GPS Node %lu not set as instance %d", (unsigned long int)params_i.override_node_id.get(), i + 1);
return false; return false;
} }
} }
@ -287,7 +292,7 @@ AP_GPS_DroneCAN* AP_GPS_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan,
_detected_modules[i].node_id = node_id; _detected_modules[i].node_id = node_id;
// Just set the Node ID in order of appearance // Just set the Node ID in order of appearance
// This will be used to set select ids // This will be used to set select ids
AP::gps()._node_id[i].set_and_notify(node_id); AP::gps().params[i].node_id.set_and_notify(node_id);
break; break;
} }
} }
@ -494,7 +499,7 @@ void AP_GPS_DroneCAN::handle_aux_msg(const uavcan_equipment_gnss_Auxiliary& msg)
void AP_GPS_DroneCAN::handle_heading_msg(const ardupilot_gnss_Heading& msg) void AP_GPS_DroneCAN::handle_heading_msg(const ardupilot_gnss_Heading& msg)
{ {
#if GPS_MOVING_BASELINE #if GPS_MOVING_BASELINE
if (seen_relposheading && gps.mb_params[interim_state.instance].type.get() != 0) { if (seen_relposheading && gps.params[interim_state.instance].mb_params.type.get() != 0) {
// we prefer to use the relposheading to get yaw as it allows // we prefer to use the relposheading to get yaw as it allows
// the user to more easily control the relative antenna positions // the user to more easily control the relative antenna positions
return; return;

2
libraries/AP_GPS/AP_GPS_DroneCAN.h
View File

@ -31,7 +31,7 @@
class AP_GPS_DroneCAN : public AP_GPS_Backend { class AP_GPS_DroneCAN : public AP_GPS_Backend {
public: public:
AP_GPS_DroneCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role role); AP_GPS_DroneCAN(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role role);
~AP_GPS_DroneCAN(); ~AP_GPS_DroneCAN();
bool read() override; bool read() override;

16
libraries/AP_GPS/AP_GPS_GSOF.cpp
View File

@ -50,9 +50,11 @@ do { \
# define Debug(fmt, args ...) # define Debug(fmt, args ...)
#endif #endif
AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps,
AP_GPS::Params &_params,
AP_GPS::GPS_State &_state,
AP_HAL::UARTDriver *_port) : AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _state, _port) AP_GPS_Backend(_gps, _params, _state, _port)
{ {
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_Overview.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257COverview%257C_____0 // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_Overview.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257COverview%257C_____0
static_assert(ARRAY_SIZE(gsofmsgreq) <= 10, "The maximum number of outputs allowed with GSOF is 10."); static_assert(ARRAY_SIZE(gsofmsgreq) <= 10, "The maximum number of outputs allowed with GSOF is 10.");
@ -60,14 +62,14 @@ AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
msg.state = Msg_Parser::State::STARTTX; msg.state = Msg_Parser::State::STARTTX;
constexpr uint8_t default_com_port = static_cast<uint8_t>(HW_Port::COM2); constexpr uint8_t default_com_port = static_cast<uint8_t>(HW_Port::COM2);
gps._com_port[state.instance].set_default(default_com_port); params.com_port.set_default(default_com_port);
const auto com_port = gps._com_port[state.instance].get(); const auto com_port = params.com_port;
if (!validate_com_port(com_port)) { if (!validate_com_port(com_port)) {
// The user parameter for COM port is not a valid GSOF port // The user parameter for COM port is not a valid GSOF port
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "GSOF instance %d has invalid COM port setting of %d", state.instance, com_port); GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "GSOF instance %d has invalid COM port setting of %d", state.instance, (unsigned)com_port);
return; return;
} }
requestBaud(static_cast<HW_Port>(com_port)); requestBaud(static_cast<HW_Port>(unsigned(com_port)));
const uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
gsofmsg_time = now + 110; gsofmsg_time = now + 110;
@ -81,7 +83,7 @@ AP_GPS_GSOF::read(void)
const uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
if (gsofmsgreq_index < (sizeof(gsofmsgreq))) { if (gsofmsgreq_index < (sizeof(gsofmsgreq))) {
const auto com_port = gps._com_port[state.instance].get(); const auto com_port = params.com_port.get();
if (!validate_com_port(com_port)) { if (!validate_com_port(com_port)) {
// The user parameter for COM port is not a valid GSOF port // The user parameter for COM port is not a valid GSOF port
return false; return false;

2
libraries/AP_GPS/AP_GPS_GSOF.h
View File

@ -27,7 +27,7 @@
class AP_GPS_GSOF : public AP_GPS_Backend class AP_GPS_GSOF : public AP_GPS_Backend
{ {
public: public:
AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
AP_GPS::GPS_Status highest_supported_status(void) override WARN_IF_UNUSED { AP_GPS::GPS_Status highest_supported_status(void) override WARN_IF_UNUSED {
return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;

2
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -850,7 +850,7 @@ void AP_GPS_NMEA::send_config(void)
return; return;
} }
last_config_ms = now_ms; last_config_ms = now_ms;
const uint16_t rate_ms = gps._rate_ms[state.instance]; const uint16_t rate_ms = params.rate_ms;
#if AP_GPS_NMEA_UNICORE_ENABLED #if AP_GPS_NMEA_UNICORE_ENABLED
const float rate_s = rate_ms * 0.001; const float rate_s = rate_ms * 0.001;
#endif #endif

8
libraries/AP_GPS/AP_GPS_NOVA.cpp
View File

@ -40,9 +40,11 @@ do { \
# define Debug(fmt, args ...) # define Debug(fmt, args ...)
#endif #endif
AP_GPS_NOVA::AP_GPS_NOVA(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS_NOVA::AP_GPS_NOVA(AP_GPS &_gps,
AP_HAL::UARTDriver *_port) : AP_GPS::Params &_params,
AP_GPS_Backend(_gps, _state, _port) AP_GPS::GPS_State &_state,
AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _params, _state, _port)
{ {
nova_msg.nova_state = nova_msg_parser::PREAMBLE1; nova_msg.nova_state = nova_msg_parser::PREAMBLE1;

2
libraries/AP_GPS/AP_GPS_NOVA.h
View File

@ -26,7 +26,7 @@
class AP_GPS_NOVA : public AP_GPS_Backend class AP_GPS_NOVA : public AP_GPS_Backend
{ {
public: public:
AP_GPS_NOVA(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); AP_GPS_NOVA(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; } AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; }

124
libraries/AP_GPS/AP_GPS_Params.cpp Normal file
View File

@ -0,0 +1,124 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_GPS_config.h"
#if AP_GPS_ENABLED
#include "AP_GPS.h"
// table of user settable parameters
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: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_FLAGS("TYPE", 1, AP_GPS::Params, type, 0, AP_PARAM_FLAG_ENABLE),
// @Param: GNSS_MODE
// @DisplayName: GNSS system configuration
// @Description: Bitmask for what GNSS system to use on the first GPS (all unchecked or zero to leave GPS as configured)
// @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLONASS
// @User: Advanced
AP_GROUPINFO("GNSS_MODE", 2, AP_GPS::Params, gnss_mode, 0),
// @Param: RATE_MS
// @DisplayName: GPS update rate in milliseconds
// @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz(default) is not allowed. Raising the rate above 5Hz usually provides little benefit and for some GPS (eg Ublox M9N) can severely impact performance.
// @Units: ms
// @Values: 100:10Hz,125:8Hz,200:5Hz
// @Range: 50 200
// @User: Advanced
AP_GROUPINFO("RATE_MS", 3, AP_GPS::Params, rate_ms, 200),
// @Param: POS_X
// @DisplayName: Antenna X position offset
// @Description: X position of the first GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer.
// @Units: m
// @Range: -5 5
// @Increment: 0.01
// @User: Advanced
// @Param: POS_Y
// @DisplayName: Antenna Y position offset
// @Description: Y position of the first GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer.
// @Units: m
// @Range: -5 5
// @Increment: 0.01
// @User: Advanced
// @Param: POS_Z
// @DisplayName: Antenna Z position offset
// @Description: Z position of the first GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer.
// @Units: m
// @Range: -5 5
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("POS", 4, AP_GPS::Params, antenna_offset, 0.0f),
// @Param: DELAY_MS
// @DisplayName: GPS delay in milliseconds
// @Description: Controls the amount of GPS measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.
// @Units: ms
// @Range: 0 250
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("DELAY_MS", 5, AP_GPS::Params, delay_ms, 0),
#if AP_GPS_SBF_ENABLED
// @Param: COM_PORT
// @DisplayName: GPS physical COM port
// @Description: The physical COM port on the connected device, currently only applies to SBF and GSOF GPS
// @Range: 0 10
// @Increment: 1
// @User: Advanced
// @Values: 0:COM1(RS232) on GSOF, 1:COM2(TTL) on GSOF
// @RebootRequired: True
AP_GROUPINFO("COM_PORT", 6, AP_GPS::Params, com_port, HAL_GPS_COM_PORT_DEFAULT),
#endif
#if GPS_MOVING_BASELINE
// @Group: MB_
// @Path: MovingBase.cpp
AP_SUBGROUPINFO(mb_params, "MB_", 7, AP_GPS::Params, MovingBase),
#endif
#if HAL_ENABLE_DRONECAN_DRIVERS
// @Param: CAN_NODEID
// @DisplayName: Detected CAN Node ID for GPS
// @Description: GPS Node id for GPS. Detected node unless CAN_OVRIDE is set
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("CAN_NODEID", 8, AP_GPS::Params, node_id, 0),
// @Param: CAN_OVRIDE
// @DisplayName: DroneCAN GPS NODE ID
// @Description: GPS Node id for GPS. If 0 the gps will be automatically selected on a first-come-first-GPS basis.
// @User: Advanced
AP_GROUPINFO("CAN_OVRIDE", 9, AP_GPS::Params, override_node_id, 0),
#endif
AP_GROUPEND
};
AP_GPS::Params::Params(void)
{
AP_Param::setup_object_defaults(this, var_info);
}
#endif // AP_GPS_ENABLED

10
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -58,9 +58,11 @@ constexpr const char *AP_GPS_SBF::portIdentifiers[];
constexpr const char* AP_GPS_SBF::_initialisation_blob[]; constexpr const char* AP_GPS_SBF::_initialisation_blob[];
constexpr const char* AP_GPS_SBF::sbas_on_blob[]; constexpr const char* AP_GPS_SBF::sbas_on_blob[];
AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps,
AP_GPS::Params &_params,
AP_GPS::GPS_State &_state,
AP_HAL::UARTDriver *_port) : AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _state, _port) AP_GPS_Backend(_gps, _params, _state, _port)
{ {
sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1; sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
@ -112,7 +114,7 @@ AP_GPS_SBF::read(void)
switch (config_step) { switch (config_step) {
case Config_State::Baud_Rate: case Config_State::Baud_Rate:
if (asprintf(&config_string, "scs,COM%d,baud%d,bits8,No,bit1,%s\n", if (asprintf(&config_string, "scs,COM%d,baud%d,bits8,No,bit1,%s\n",
(int)gps._com_port[state.instance], (int)params.com_port,
230400, 230400,
port->get_flow_control() != AP_HAL::UARTDriver::flow_control::FLOW_CONTROL_ENABLE ? "none" : "RTS|CTS") == -1) { port->get_flow_control() != AP_HAL::UARTDriver::flow_control::FLOW_CONTROL_ENABLE ? "none" : "RTS|CTS") == -1) {
config_string = nullptr; config_string = nullptr;
@ -131,7 +133,7 @@ AP_GPS_SBF::read(void)
} }
if (asprintf(&config_string, "sso,Stream%d,COM%d,PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod%s,msec100\n", if (asprintf(&config_string, "sso,Stream%d,COM%d,PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod%s,msec100\n",
(int)GPS_SBF_STREAM_NUMBER, (int)GPS_SBF_STREAM_NUMBER,
(int)gps._com_port[state.instance], (int)params.com_port,
extra_config) == -1) { extra_config) == -1) {
config_string = nullptr; config_string = nullptr;
} }

2
libraries/AP_GPS/AP_GPS_SBF.h
View File

@ -31,7 +31,7 @@
class AP_GPS_SBF : public AP_GPS_Backend class AP_GPS_SBF : public AP_GPS_Backend
{ {
public: public:
AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); AP_GPS_SBF(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
~AP_GPS_SBF(); ~AP_GPS_SBF();
AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; } AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; }

6
libraries/AP_GPS/AP_GPS_SBP.cpp
View File

@ -47,9 +47,11 @@ do { \
# define Debug(fmt, args ...) # define Debug(fmt, args ...)
#endif #endif
AP_GPS_SBP::AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS_SBP::AP_GPS_SBP(AP_GPS &_gps,
AP_GPS::Params &_params,
AP_GPS::GPS_State &_state,
AP_HAL::UARTDriver *_port) : AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _state, _port) AP_GPS_Backend(_gps, _params, _state, _port)
{ {
Debug("SBP Driver Initialized"); Debug("SBP Driver Initialized");

2
libraries/AP_GPS/AP_GPS_SBP.h
View File

@ -28,7 +28,7 @@
class AP_GPS_SBP : public AP_GPS_Backend class AP_GPS_SBP : public AP_GPS_Backend
{ {
public: public:
AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); AP_GPS_SBP(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; } AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; }

8
libraries/AP_GPS/AP_GPS_SBP2.cpp
View File

@ -61,9 +61,11 @@ do { \
#endif #endif
AP_GPS_SBP2::AP_GPS_SBP2(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS_SBP2::AP_GPS_SBP2(AP_GPS &_gps,
AP_HAL::UARTDriver *_port) : AP_GPS::Params &_params,
AP_GPS_Backend(_gps, _state, _port) AP_GPS::GPS_State &_state,
AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _params, _state, _port)
{ {
Debug("SBP Driver Initialized"); Debug("SBP Driver Initialized");
parser_state.state = sbp_parser_state_t::WAITING; parser_state.state = sbp_parser_state_t::WAITING;

2
libraries/AP_GPS/AP_GPS_SBP2.h
View File

@ -28,7 +28,7 @@
class AP_GPS_SBP2 : public AP_GPS_Backend class AP_GPS_SBP2 : public AP_GPS_Backend
{ {
public: public:
AP_GPS_SBP2(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); AP_GPS_SBP2(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; } AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; }

4
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -37,8 +37,8 @@ const uint8_t AP_GPS_SIRF::_initialisation_blob[] = {
0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3
}; };
AP_GPS_SIRF::AP_GPS_SIRF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : AP_GPS_SIRF::AP_GPS_SIRF(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _state, _port) AP_GPS_Backend(_gps, _params, _state, _port)
{ {
gps.send_blob_start(state.instance, (const char *)_initialisation_blob, sizeof(_initialisation_blob)); gps.send_blob_start(state.instance, (const char *)_initialisation_blob, sizeof(_initialisation_blob));
} }

2
libraries/AP_GPS/AP_GPS_SIRF.h
View File

@ -31,7 +31,7 @@
class AP_GPS_SIRF : public AP_GPS_Backend { class AP_GPS_SIRF : public AP_GPS_Backend {
public: public:
AP_GPS_SIRF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); AP_GPS_SIRF(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
bool read() override; bool read() override;

16
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -94,8 +94,12 @@ extern const AP_HAL::HAL& hal;
# define CFG_Debug(fmt, args ...) # define CFG_Debug(fmt, args ...)
#endif #endif
AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port, AP_GPS::GPS_Role _role) : AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps,
AP_GPS_Backend(_gps, _state, _port), AP_GPS::Params &_params,
AP_GPS::GPS_State &_state,
AP_HAL::UARTDriver *_port,
AP_GPS::GPS_Role _role) :
AP_GPS_Backend(_gps, _params, _state, _port),
_next_message(STEP_PVT), _next_message(STEP_PVT),
_ublox_port(255), _ublox_port(255),
_unconfigured_messages(CONFIG_ALL), _unconfigured_messages(CONFIG_ALL),
@ -1110,7 +1114,7 @@ AP_GPS_UBLOX::_parse_gps(void)
#if UBLOX_GNSS_SETTINGS #if UBLOX_GNSS_SETTINGS
case MSG_CFG_GNSS: case MSG_CFG_GNSS:
if (gps._gnss_mode[state.instance] != 0) { if (params.gnss_mode != 0) {
struct ubx_cfg_gnss start_gnss = _buffer.gnss; struct ubx_cfg_gnss start_gnss = _buffer.gnss;
uint8_t gnssCount = 0; uint8_t gnssCount = 0;
Debug("Got GNSS Settings %u %u %u %u:\n", Debug("Got GNSS Settings %u %u %u %u:\n",
@ -1129,13 +1133,13 @@ AP_GPS_UBLOX::_parse_gps(void)
#endif #endif
for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) { for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
if((gps._gnss_mode[state.instance] & (1 << i)) && i != GNSS_SBAS) { if((params.gnss_mode & (1 << i)) && i != GNSS_SBAS) {
gnssCount++; gnssCount++;
} }
} }
for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) { for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
// Reserve an equal portion of channels for all enabled systems that supports it // Reserve an equal portion of channels for all enabled systems that supports it
if(gps._gnss_mode[state.instance] & (1 << _buffer.gnss.configBlock[i].gnssId)) { if(params.gnss_mode & (1 << _buffer.gnss.configBlock[i].gnssId)) {
if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId && (_hardware_generation > UBLOX_M8 || GNSS_GALILEO !=_buffer.gnss.configBlock[i].gnssId)) { if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId && (_hardware_generation > UBLOX_M8 || GNSS_GALILEO !=_buffer.gnss.configBlock[i].gnssId)) {
_buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2); _buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
_buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw; _buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw;
@ -1210,7 +1214,7 @@ AP_GPS_UBLOX::_parse_gps(void)
_ublox_port = _buffer.prt.portID; _ublox_port = _buffer.prt.portID;
return false; return false;
case MSG_CFG_RATE: case MSG_CFG_RATE:
if(_buffer.nav_rate.measure_rate_ms != gps._rate_ms[state.instance] || if(_buffer.nav_rate.measure_rate_ms != params.rate_ms ||
_buffer.nav_rate.nav_rate != 1 || _buffer.nav_rate.nav_rate != 1 ||
_buffer.nav_rate.timeref != 0) { _buffer.nav_rate.timeref != 0) {
_configure_rate(); _configure_rate();

2
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -124,7 +124,7 @@ class RTCM3_Parser;
class AP_GPS_UBLOX : public AP_GPS_Backend class AP_GPS_UBLOX : public AP_GPS_Backend
{ {
public: public:
AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port, AP_GPS::GPS_Role role); AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port, AP_GPS::GPS_Role role);
~AP_GPS_UBLOX() override; ~AP_GPS_UBLOX() override;
// Methods // Methods

4
libraries/AP_GPS/AP_GPS_config.h
View File

@ -66,3 +66,7 @@
#ifndef AP_GPS_RTCM_DECODE_ENABLED #ifndef AP_GPS_RTCM_DECODE_ENABLED
#define AP_GPS_RTCM_DECODE_ENABLED BOARD_FLASH_SIZE > 1024 #define AP_GPS_RTCM_DECODE_ENABLED BOARD_FLASH_SIZE > 1024
#endif #endif
#ifndef HAL_GPS_COM_PORT_DEFAULT
#define HAL_GPS_COM_PORT_DEFAULT 1
#endif

11
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -41,10 +41,11 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
port(_port), port(_port),
gps(_gps), gps(_gps),
state(_state) state(_state),
params(_params)
{ {
state.have_speed_accuracy = false; state.have_speed_accuracy = false;
state.have_horizontal_accuracy = false; state.have_horizontal_accuracy = false;
@ -332,13 +333,13 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state,
#endif #endif
bool selectedOffset = false; bool selectedOffset = false;
Vector3f offset; Vector3f offset;
switch (MovingBase::Type(gps.mb_params[interim_state.instance].type.get())) { switch (MovingBase::Type(gps.params[interim_state.instance].mb_params.type)) {
case MovingBase::Type::RelativeToAlternateInstance: case MovingBase::Type::RelativeToAlternateInstance:
offset = gps._antenna_offset[interim_state.instance^1].get() - gps._antenna_offset[interim_state.instance].get(); offset = gps.params[interim_state.instance^1].antenna_offset.get() - gps.params[interim_state.instance].antenna_offset.get();
selectedOffset = true; selectedOffset = true;
break; break;
case MovingBase::Type::RelativeToCustomBase: case MovingBase::Type::RelativeToCustomBase:
offset = gps.mb_params[interim_state.instance].base_offset.get(); offset = gps.params[interim_state.instance].mb_params.base_offset.get();
selectedOffset = true; selectedOffset = true;
break; break;
} }

3
libraries/AP_GPS/GPS_Backend.h
View File

@ -49,7 +49,7 @@
class AP_GPS_Backend class AP_GPS_Backend
{ {
public: public:
AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); AP_GPS_Backend(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
// we declare a virtual destructor so that GPS drivers can // we declare a virtual destructor so that GPS drivers can
// override with a custom destructor if need be. // override with a custom destructor if need be.
@ -119,6 +119,7 @@ protected:
AP_HAL::UARTDriver *port; ///< UART we are attached to AP_HAL::UARTDriver *port; ///< UART we are attached to
AP_GPS &gps; ///< access to frontend (for parameters) AP_GPS &gps; ///< access to frontend (for parameters)
AP_GPS::GPS_State &state; ///< public state for this instance AP_GPS::GPS_State &state; ///< public state for this instance
AP_GPS::Params &params;
uint64_t _last_pps_time_us; uint64_t _last_pps_time_us;
JitterCorrection jitter_correction; JitterCorrection jitter_correction;

2
libraries/AP_GPS/MovingBase.h
View File

@ -17,7 +17,7 @@ public:
RelativeToCustomBase = 1, RelativeToCustomBase = 1,
}; };
AP_Int8 type; // an option from MovingBaseType AP_Enum<Type> type; // an option from MovingBaseType
AP_Vector3f base_offset; // base position offset from the selected GPS receiver AP_Vector3f base_offset; // base position offset from the selected GPS receiver
}; };