From 21fc0130166918cb517b77c2e226f52e42c03dc5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 12 Mar 2024 14:00:05 +1100 Subject: [PATCH] AP_GPS: use subgroup to hold GPS instance parameters --- libraries/AP_GPS/AP_GPS.cpp | 301 +++++++-------------------- libraries/AP_GPS/AP_GPS.h | 39 ++-- libraries/AP_GPS/AP_GPS_DroneCAN.cpp | 49 +++-- libraries/AP_GPS/AP_GPS_DroneCAN.h | 2 +- libraries/AP_GPS/AP_GPS_GSOF.cpp | 16 +- libraries/AP_GPS/AP_GPS_GSOF.h | 2 +- libraries/AP_GPS/AP_GPS_NMEA.cpp | 2 +- libraries/AP_GPS/AP_GPS_NOVA.cpp | 8 +- libraries/AP_GPS/AP_GPS_NOVA.h | 2 +- libraries/AP_GPS/AP_GPS_Params.cpp | 124 +++++++++++ libraries/AP_GPS/AP_GPS_SBF.cpp | 10 +- libraries/AP_GPS/AP_GPS_SBF.h | 2 +- libraries/AP_GPS/AP_GPS_SBP.cpp | 6 +- libraries/AP_GPS/AP_GPS_SBP.h | 2 +- libraries/AP_GPS/AP_GPS_SBP2.cpp | 8 +- libraries/AP_GPS/AP_GPS_SBP2.h | 2 +- libraries/AP_GPS/AP_GPS_SIRF.cpp | 4 +- libraries/AP_GPS/AP_GPS_SIRF.h | 2 +- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 16 +- libraries/AP_GPS/AP_GPS_UBLOX.h | 2 +- libraries/AP_GPS/AP_GPS_config.h | 4 + libraries/AP_GPS/GPS_Backend.cpp | 11 +- libraries/AP_GPS/GPS_Backend.h | 3 +- libraries/AP_GPS/MovingBase.h | 2 +- 24 files changed, 317 insertions(+), 302 deletions(-) create mode 100644 libraries/AP_GPS/AP_GPS_Params.cpp diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 8ab52caa71..d0255a7121 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -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_RATE_MS) { - _rate_ms[i].set(GPS_MAX_RATE_MS); + for (uint8_t i=0; i 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 ×tamp, 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; ihandle_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; ihandle_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; iis_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) { diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index d5921f04c3..97817031fc 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -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 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; 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; diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp index 8515f5d9ee..972cc83938 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp @@ -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 ¶ms_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 ¶ms_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; diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.h b/libraries/AP_GPS/AP_GPS_DroneCAN.h index 10f6d71954..200d8ae11c 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.h +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.h @@ -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; diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp index 42616a6af8..4fbfef5833 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.cpp +++ b/libraries/AP_GPS/AP_GPS_GSOF.cpp @@ -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(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(com_port)); + requestBaud(static_cast(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; diff --git a/libraries/AP_GPS/AP_GPS_GSOF.h b/libraries/AP_GPS/AP_GPS_GSOF.h index a55d91c0a9..c1bf4a3fbd 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.h +++ b/libraries/AP_GPS/AP_GPS_GSOF.h @@ -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; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 0ec48ce9ae..8d8c0db185 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -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 diff --git a/libraries/AP_GPS/AP_GPS_NOVA.cpp b/libraries/AP_GPS/AP_GPS_NOVA.cpp index bea9bfd675..e03640188a 100644 --- a/libraries/AP_GPS/AP_GPS_NOVA.cpp +++ b/libraries/AP_GPS/AP_GPS_NOVA.cpp @@ -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; diff --git a/libraries/AP_GPS/AP_GPS_NOVA.h b/libraries/AP_GPS/AP_GPS_NOVA.h index 3a288d276c..6f10806af5 100644 --- a/libraries/AP_GPS/AP_GPS_NOVA.h +++ b/libraries/AP_GPS/AP_GPS_NOVA.h @@ -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; } diff --git a/libraries/AP_GPS/AP_GPS_Params.cpp b/libraries/AP_GPS/AP_GPS_Params.cpp new file mode 100644 index 0000000000..7f227dbace --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_Params.cpp @@ -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 . + */ + +#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 diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 569d5b6910..9981344baa 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -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; } diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index e8773cf42c..8817653f42 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -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; } diff --git a/libraries/AP_GPS/AP_GPS_SBP.cpp b/libraries/AP_GPS/AP_GPS_SBP.cpp index c96262a3e2..4009ad8200 100644 --- a/libraries/AP_GPS/AP_GPS_SBP.cpp +++ b/libraries/AP_GPS/AP_GPS_SBP.cpp @@ -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"); diff --git a/libraries/AP_GPS/AP_GPS_SBP.h b/libraries/AP_GPS/AP_GPS_SBP.h index a553588931..e4ab3c7394 100644 --- a/libraries/AP_GPS/AP_GPS_SBP.h +++ b/libraries/AP_GPS/AP_GPS_SBP.h @@ -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; } diff --git a/libraries/AP_GPS/AP_GPS_SBP2.cpp b/libraries/AP_GPS/AP_GPS_SBP2.cpp index e13490daea..83c3f5c81b 100644 --- a/libraries/AP_GPS/AP_GPS_SBP2.cpp +++ b/libraries/AP_GPS/AP_GPS_SBP2.cpp @@ -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; diff --git a/libraries/AP_GPS/AP_GPS_SBP2.h b/libraries/AP_GPS/AP_GPS_SBP2.h index 85ae81e04a..66e2d36eac 100644 --- a/libraries/AP_GPS/AP_GPS_SBP2.h +++ b/libraries/AP_GPS/AP_GPS_SBP2.h @@ -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; } diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index da37a94aff..dd6eb901c8 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -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)); } diff --git a/libraries/AP_GPS/AP_GPS_SIRF.h b/libraries/AP_GPS/AP_GPS_SIRF.h index d3c1b8ee7a..7f4127ba1e 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.h +++ b/libraries/AP_GPS/AP_GPS_SIRF.h @@ -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; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 9d9604f21f..179c5d34d4 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -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(); diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 738dac3b42..617a072e79 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -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 diff --git a/libraries/AP_GPS/AP_GPS_config.h b/libraries/AP_GPS/AP_GPS_config.h index 779879c4fd..9d1767504b 100644 --- a/libraries/AP_GPS/AP_GPS_config.h +++ b/libraries/AP_GPS/AP_GPS_config.h @@ -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 diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index a976ea280c..eb55bd76e4 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -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; } diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 2544fcdefb..70f4cb36fc 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -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 ¶ms; uint64_t _last_pps_time_us; JitterCorrection jitter_correction; diff --git a/libraries/AP_GPS/MovingBase.h b/libraries/AP_GPS/MovingBase.h index 7df25fe0a2..dd4e0f97eb 100644 --- a/libraries/AP_GPS/MovingBase.h +++ b/libraries/AP_GPS/MovingBase.h @@ -17,7 +17,7 @@ public: RelativeToCustomBase = 1, }; - AP_Int8 type; // an option from MovingBaseType + AP_Enum type; // an option from MovingBaseType AP_Vector3f base_offset; // base position offset from the selected GPS receiver };