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_COUNTER_FAILURE_INCREMENT 10
#ifndef HAL_GPS_COM_PORT_DEFAULT
#define HAL_GPS_COM_PORT_DEFAULT 1
#endif
extern const AP_HAL::HAL &hal;
// baudrates to try to detect GPSes with
@ -130,21 +126,10 @@ AP_GPS *AP_GPS::_singleton;
// table of user settable parameters
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
// @Param: _TYPE2
// @CopyFieldsFrom: GPS_TYPE
// @DisplayName: 2nd GPS type
// @Description: GPS type of 2nd GPS
AP_GROUPINFO("_TYPE2", 1, AP_GPS, _type[1], 0),
#endif
// 0 was GPS_TYPE
// 1 was GPS_TYPE2
// @Param: _NAVFILTER
// @DisplayName: Navigation filter setting
@ -203,12 +188,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @User: Advanced
AP_GROUPINFO("_RAW_DATA", 9, AP_GPS, _raw_data, 0),
// @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", 10, AP_GPS, _gnss_mode[0], 0),
// 10 was GPS_GNSS_MODE
// @Param: _SAVE_CFG
// @DisplayName: Save GPS configuration
@ -217,14 +197,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @User: Advanced
AP_GROUPINFO("_SAVE_CFG", 11, AP_GPS, _save_config, 2),
#if GPS_MAX_RECEIVERS > 1
// @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
// 12 was GPS_GNSS_MODE2
// @Param: _AUTO_CONFIG
// @DisplayName: Automatic GPS configuration
@ -233,97 +206,17 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @User: Advanced
AP_GROUPINFO("_AUTO_CONFIG", 13, AP_GPS, _auto_config, 1),
// @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", 14, AP_GPS, _rate_ms[0], 200),
// 14 was GPS_RATE_MS
#if GPS_MAX_RECEIVERS > 1
// @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
// 15 was GPS_RATE_MS2
// @Param: _POS1_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
// 16 was GPS_POS1
// @Param: _POS1_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
// 17 was GPS_POS2
// @Param: _POS1_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("_POS1", 16, AP_GPS, _antenna_offset[0], 0.0f),
// 18 was GPS_DELAY_MS
#if GPS_MAX_RECEIVERS > 1
// @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
// 19 was GPS_DELAY_MS2
#if defined(GPS_BLENDED_INSTANCE)
// @Param: _BLEND_MASK
@ -349,42 +242,13 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @User: Advanced
AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 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", 23, AP_GPS, _com_port[0], HAL_GPS_COM_PORT_DEFAULT),
// 23 was GPS_COM_PORT
#if GPS_MAX_RECEIVERS > 1
// @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
// 24 was GPS_COM_PORT2
#if GPS_MOVING_BASELINE
// 25 was GPS_MB1_*
// @Group: _MB1_
// @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
// 26 was GPS_MB2_*
#if GPS_MAX_RECEIVERS > 1
// @Param: _PRIMARY
@ -396,36 +260,23 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
AP_GROUPINFO("_PRIMARY", 27, AP_GPS, _primary, 0),
#endif
#if HAL_ENABLE_DRONECAN_DRIVERS
// @Param: _CAN_NODEID1
// @DisplayName: GPS Node ID 1
// @Description: GPS Node id for first-discovered GPS.
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("_CAN_NODEID1", 28, AP_GPS, _node_id[0], 0),
// 28 was GPS_CAN_NODEID1
// 29 was GPS_CAN_NODEID2
// 30 was GPS1_CAN_OVRIDE
// 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
// @Param: _CAN_NODEID2
// @DisplayName: GPS Node ID 2
// @Description: GPS Node id for second-discovered GPS.
// @ReadOnly: True
// @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
// @Group: 2_
// @Path: AP_GPS_Params.cpp
AP_SUBGROUPINFO(params[1], "2_", 33, AP_GPS, AP_GPS::Params),
#endif
AP_GROUPEND
};
@ -466,6 +317,9 @@ bool AP_GPS::needs_uart(GPS_Type type) const
/// Startup initialisation.
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
if ((_auto_switch.get() == 3) && !_primary.configured()) {
_primary.set_and_save(1);
@ -475,8 +329,8 @@ void AP_GPS::init()
// search for serial ports with gps protocol
const auto &serial_manager = AP::serialmanager();
uint8_t uart_idx = 0;
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (needs_uart((GPS_Type)_type[i].get())) {
for (uint8_t i=0; i<ARRAY_SIZE(params); i++) {
if (needs_uart(params[i].type)) {
_port[i] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, uart_idx);
uart_idx++;
}
@ -489,9 +343,10 @@ void AP_GPS::init()
}
// sanity check update rate
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (_rate_ms[i] <= 0 || _rate_ms[i] > GPS_MAX_RATE_MS) {
_rate_ms[i].set(GPS_MAX_RATE_MS);
for (uint8_t i=0; i<ARRAY_SIZE(params); i++) {
auto &rate_ms = params[i].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)
{
const auto type = _type[instance];
const auto type = params[instance].type;
#if AP_GPS_UBLOX_ENABLED
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:
const char *blob = nullptr;
uint32_t blob_size = 0;
switch (type) {
switch (GPS_Type(type)) {
#if AP_GPS_SBF_ENABLED
case GPS_TYPE_SBF:
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];
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
// do not try to detect the MAV type, assume it's there
case GPS_TYPE_MAV:
#if AP_GPS_MAV_ENABLED
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
// 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
case GPS_TYPE_MSP:
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
#if HAL_EXTERNAL_AHRS_ENABLED
case GPS_TYPE_EXTERNAL_AHRS:
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
#if AP_GPS_GSOF_ENABLED
case GPS_TYPE_GSOF:
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
default:
break;
@ -808,21 +663,21 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance)
send_blob_update(instance);
}
switch (type) {
switch (GPS_Type(type)) {
#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]);
return new AP_GPS_SBF(*this, params[instance], state[instance], _port[instance]);
#endif //AP_GPS_SBF_ENABLED
#if AP_GPS_NOVA_ENABLED
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
#if HAL_SIM_GPS_ENABLED
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
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] == 230400) &&
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;
@ -861,31 +716,31 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance)
} else {
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
#if AP_GPS_SBP2_ENABLED
if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_SBP) &&
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
#if AP_GPS_SBP_ENABLED
if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_SBP) &&
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
#if AP_GPS_SIRF_ENABLED
if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_SIRF) &&
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
#if AP_GPS_ERB_ENABLED
if ((type == GPS_TYPE_AUTO || type == GPS_TYPE_ERB) &&
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
#if AP_GPS_NMEA_ENABLED
@ -897,7 +752,7 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance)
#endif
type == GPS_TYPE_ALLYSTAR) &&
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
}
@ -936,7 +791,7 @@ bool AP_GPS::should_log() const
*/
void AP_GPS::update_instance(uint8_t instance)
{
const auto type = _type[instance];
const auto type = params[instance].type;
if (type == GPS_TYPE_HIL) {
// in HIL, leave info alone
return;
@ -1031,7 +886,7 @@ void AP_GPS::update_instance(uint8_t instance)
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 && _type[i] == GPS_TYPE_UBLOX_RTK_ROVER) {
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();
@ -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)
{
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);
}
}
@ -1109,7 +964,7 @@ bool AP_GPS::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
void AP_GPS::clear_RTCMV3()
{
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();
}
}
@ -1121,7 +976,7 @@ void AP_GPS::clear_RTCMV3()
void AP_GPS::inject_MBL_data(uint8_t* data, uint16_t length)
{
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
inject_data(i, data, length);
break;
@ -1356,7 +1211,7 @@ void AP_GPS::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg)
default: {
uint8_t 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);
}
}
@ -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)
{
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);
}
}
@ -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
{
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;
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)
{
// always send the message if 2nd GPS is configured
if (_type[1] == GPS_TYPE_NONE) {
if (params[1].type == GPS_TYPE_NONE) {
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
{
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;
return true;
}
@ -1580,7 +1435,7 @@ bool AP_GPS::all_consistent(float &distance) const
{
// return true immediately if only one valid receiver
if (num_instances <= 1 ||
drivers[0] == nullptr || _type[0] == GPS_TYPE_NONE) {
drivers[0] == nullptr || params[0].type == GPS_TYPE_NONE) {
distance = 0;
return true;
}
@ -1809,14 +1664,14 @@ bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const
}
#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
lag_sec = 0.001f * (float)_delay_ms[instance];
lag_sec = 0.001f * (float)params[instance].delay_ms;
// the user is always right !!
return true;
} else if (drivers[instance] == nullptr || state[instance].status == NO_GPS) {
// 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) {
lag_sec = 0.0f;
return true;
@ -1833,7 +1688,7 @@ const Vector3f &AP_GPS::get_antenna_offset(uint8_t instance) const
{
if (instance >= GPS_MAX_INSTANCES) {
// we have to return a reference so use instance 0
return _antenna_offset[0];
return params[0].antenna_offset;
}
#if defined(GPS_BLENDED_INSTANCE)
@ -1843,7 +1698,7 @@ const Vector3f &AP_GPS::get_antenna_offset(uint8_t instance) const
}
#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
{
// 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 MIN(_rate_ms[instance], GPS_MAX_RATE_MS);
return MIN(params[instance].rate_ms, GPS_MAX_RATE_MS);
}
#if defined(GPS_BLENDED_INSTANCE)
@ -2124,7 +1979,7 @@ void AP_GPS::calc_blended_state(void)
}
// 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];
_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) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
#if AP_GPS_DRONECAN_ENABLED
const auto type = _type[i];
const auto type = params[i].type;
if (type == GPS_TYPE_UAVCAN ||
type == GPS_TYPE_UAVCAN_RTK_BASE ||
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_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
/// static_assert in AP_GPS.cpp
enum GPS_Status {
@ -570,7 +593,7 @@ public:
// get configured type by instance
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
@ -596,7 +619,7 @@ public:
protected:
// configuration parameters
AP_Int8 _type[GPS_MAX_RECEIVERS];
Params params[GPS_MAX_RECEIVERS];
AP_Int8 _navfilter;
AP_Int8 _auto_switch;
AP_Int16 _sbp_logmask;
@ -605,23 +628,11 @@ protected:
AP_Enum<SBAS_Mode> _sbas_mode;
AP_Int8 _min_elevation;
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 _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_Int16 _driver_options;
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;

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;
// Member Methods
AP_GPS_DroneCAN::AP_GPS_DroneCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role _role) :
AP_GPS_Backend(_gps, _state, nullptr),
AP_GPS_DroneCAN::AP_GPS_DroneCAN(AP_GPS &_gps,
AP_GPS::Params &_params,
AP_GPS::GPS_State &_state,
AP_GPS::GPS_Role _role) :
AP_GPS_Backend(_gps, _params, _state, nullptr),
interim_state(_state),
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;
for (int8_t i = GPS_MAX_RECEIVERS - 1; i >= 0; i--) {
if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_dronecan != nullptr) {
if (_gps._override_node_id[_state.instance] != 0 &&
_gps._override_node_id[_state.instance] != _detected_modules[i].node_id) {
if (_gps.params[_state.instance].override_node_id != 0 &&
_gps.params[_state.instance].override_node_id != _detected_modules[i].node_id) {
continue; // This device doesn't match the correct node
}
last_match = found_match;
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)) {
//wrong instance
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
for (uint8_t j = 0; j < GPS_MAX_RECEIVERS; j++) {
if (_gps._override_node_id[i] != 0 && (i != j) &&
_gps._override_node_id[i] == _gps._override_node_id[j]) {
if (_gps.params[i].override_node_id != 0 && (i != j) &&
_gps.params[i].override_node_id == _gps.params[j].override_node_id) {
bad_override_config = true;
}
}
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;
}
@ -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
switch (_gps.get_type(_state.instance)) {
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;
#if GPS_MOVING_BASELINE
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;
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;
#endif
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);
_detected_modules[found_match].instance = _state.instance;
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) {
// Nothing to do here
break;
}
// else swap
uint8_t tmp = AP::gps()._node_id[_state.instance].get();
AP::gps()._node_id[_state.instance].set_and_notify(_detected_modules[found_match].node_id);
AP::gps()._node_id[i].set_and_notify(tmp);
uint8_t tmp = AP::gps().params[_state.instance].node_id.get();
AP::gps().params[_state.instance].node_id.set_and_notify(_detected_modules[found_match].node_id);
AP::gps().params[i].node_id.set_and_notify(tmp);
}
}
#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)
{
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
const auto &params_i = AP::gps().params[i];
bool overriden_node_found = false;
bool bad_override_config = false;
if (AP::gps()._override_node_id[i] == 0) {
if (params_i.override_node_id == 0) {
//anything goes
continue;
}
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;
break;
}
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;
break;
}
}
}
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;
}
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;
}
}
@ -287,7 +292,7 @@ AP_GPS_DroneCAN* AP_GPS_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan,
_detected_modules[i].node_id = node_id;
// Just set the Node ID in order of appearance
// 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;
}
}
@ -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)
{
#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
// the user to more easily control the relative antenna positions
return;

2
libraries/AP_GPS/AP_GPS_DroneCAN.h
View File

@ -31,7 +31,7 @@
class AP_GPS_DroneCAN : public AP_GPS_Backend {
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();
bool read() override;

16
libraries/AP_GPS/AP_GPS_GSOF.cpp
View File

@ -50,9 +50,11 @@ do { \
# define Debug(fmt, args ...)
#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_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
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;
constexpr uint8_t default_com_port = static_cast<uint8_t>(HW_Port::COM2);
gps._com_port[state.instance].set_default(default_com_port);
const auto com_port = gps._com_port[state.instance].get();
params.com_port.set_default(default_com_port);
const auto com_port = params.com_port;
if (!validate_com_port(com_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;
}
requestBaud(static_cast<HW_Port>(com_port));
requestBaud(static_cast<HW_Port>(unsigned(com_port)));
const uint32_t now = AP_HAL::millis();
gsofmsg_time = now + 110;
@ -81,7 +83,7 @@ AP_GPS_GSOF::read(void)
const uint32_t now = AP_HAL::millis();
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)) {
// The user parameter for COM port is not a valid GSOF port
return false;

2
libraries/AP_GPS/AP_GPS_GSOF.h
View File

@ -27,7 +27,7 @@
class AP_GPS_GSOF : public AP_GPS_Backend
{
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 {
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;
}
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
const float rate_s = rate_ms * 0.001;
#endif

8
libraries/AP_GPS/AP_GPS_NOVA.cpp
View File

@ -40,9 +40,11 @@ do { \
# define Debug(fmt, args ...)
#endif
AP_GPS_NOVA::AP_GPS_NOVA(AP_GPS &_gps, AP_GPS::GPS_State &_state,
AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _state, _port)
AP_GPS_NOVA::AP_GPS_NOVA(AP_GPS &_gps,
AP_GPS::Params &_params,
AP_GPS::GPS_State &_state,
AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _params, _state, _port)
{
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
{
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; }

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::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_GPS_Backend(_gps, _state, _port)
AP_GPS_Backend(_gps, _params, _state, _port)
{
sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
@ -112,7 +114,7 @@ AP_GPS_SBF::read(void)
switch (config_step) {
case Config_State::Baud_Rate:
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,
port->get_flow_control() != AP_HAL::UARTDriver::flow_control::FLOW_CONTROL_ENABLE ? "none" : "RTS|CTS") == -1) {
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",
(int)GPS_SBF_STREAM_NUMBER,
(int)gps._com_port[state.instance],
(int)params.com_port,
extra_config) == -1) {
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
{
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::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 ...)
#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_GPS_Backend(_gps, _state, _port)
AP_GPS_Backend(_gps, _params, _state, _port)
{
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
{
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; }

8
libraries/AP_GPS/AP_GPS_SBP2.cpp
View File

@ -61,9 +61,11 @@ do { \
#endif
AP_GPS_SBP2::AP_GPS_SBP2(AP_GPS &_gps, AP_GPS::GPS_State &_state,
AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _state, _port)
AP_GPS_SBP2::AP_GPS_SBP2(AP_GPS &_gps,
AP_GPS::Params &_params,
AP_GPS::GPS_State &_state,
AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _params, _state, _port)
{
Debug("SBP Driver Initialized");
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
{
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; }

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
};
AP_GPS_SIRF::AP_GPS_SIRF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _state, _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, _params, _state, _port)
{
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 {
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;

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 ...)
#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_Backend(_gps, _state, _port),
AP_GPS_UBLOX::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_Backend(_gps, _params, _state, _port),
_next_message(STEP_PVT),
_ublox_port(255),
_unconfigured_messages(CONFIG_ALL),
@ -1110,7 +1114,7 @@ AP_GPS_UBLOX::_parse_gps(void)
#if UBLOX_GNSS_SETTINGS
case MSG_CFG_GNSS:
if (gps._gnss_mode[state.instance] != 0) {
if (params.gnss_mode != 0) {
struct ubx_cfg_gnss start_gnss = _buffer.gnss;
uint8_t gnssCount = 0;
Debug("Got GNSS Settings %u %u %u %u:\n",
@ -1129,13 +1133,13 @@ AP_GPS_UBLOX::_parse_gps(void)
#endif
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++;
}
}
for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
// 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)) {
_buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
_buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw;
@ -1210,7 +1214,7 @@ AP_GPS_UBLOX::_parse_gps(void)
_ublox_port = _buffer.prt.portID;
return false;
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.timeref != 0) {
_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
{
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;
// Methods

4
libraries/AP_GPS/AP_GPS_config.h
View File

@ -66,3 +66,7 @@
#ifndef AP_GPS_RTCM_DECODE_ENABLED
#define AP_GPS_RTCM_DECODE_ENABLED BOARD_FLASH_SIZE > 1024
#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;
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),
gps(_gps),
state(_state)
state(_state),
params(_params)
{
state.have_speed_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
bool selectedOffset = false;
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:
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;
break;
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;
break;
}

3
libraries/AP_GPS/GPS_Backend.h
View File

@ -49,7 +49,7 @@
class AP_GPS_Backend
{
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
// override with a custom destructor if need be.
@ -119,6 +119,7 @@ protected:
AP_HAL::UARTDriver *port; ///< UART we are attached to
AP_GPS &gps; ///< access to frontend (for parameters)
AP_GPS::GPS_State &state; ///< public state for this instance
AP_GPS::Params &params;
uint64_t _last_pps_time_us;
JitterCorrection jitter_correction;

2
libraries/AP_GPS/MovingBase.h
View File

@ -17,7 +17,7 @@ public:
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
};