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

AP_GPS: enabled reduced size for AP_Periph support

Browse Source
This commit is contained in:
Andrew Tridgell 2019-05-27 11:34:13 +10:00
parent f897eae89d
commit 291d72601b
4 changed files with 80 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
libraries/AP_GPS/AP_GPS.cpp
View File

@ -77,6 +77,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @User: Advanced
AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT),
#if GPS_MAX_RECEIVERS > 1
// @Param: TYPE2
// @DisplayName: 2nd GPS type
// @Description: GPS type of 2nd GPS
@ -84,6 +85,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0),
#endif
// @Param: NAVFILTER
// @DisplayName: Navigation filter setting
@ -92,12 +94,14 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @User: Advanced
AP_GROUPINFO("NAVFILTER", 2, AP_GPS, _navfilter, GPS_ENGINE_AIRBORNE_4G),
#if GPS_MAX_RECEIVERS > 1
// @Param: AUTO_SWITCH
// @DisplayName: Automatic Switchover Setting
// @Description: Automatic switchover to GPS reporting best lock
// @Values: 0:Disabled,1:UseBest,2:Blend,3:UseSecond
// @User: Advanced
AP_GROUPINFO("AUTO_SWITCH", 3, AP_GPS, _auto_switch, 1),
#endif
// @Param: MIN_DGPS
// @DisplayName: Minimum Lock Type Accepted for DGPS
@ -159,6 +163,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)
@ -166,6 +171,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS
// @User: Advanced
AP_GROUPINFO("GNSS_MODE2", 12, AP_GPS, _gnss_mode[1], 0),
#endif
// @Param: AUTO_CONFIG
// @DisplayName: Automatic GPS configuration
@ -183,6 +189,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @User: Advanced
AP_GROUPINFO("RATE_MS", 14, AP_GPS, _rate_ms[0], 200),
#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 is not allowed
@ -191,6 +198,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Range: 50 200
// @User: Advanced
AP_GROUPINFO("RATE_MS2", 15, AP_GPS, _rate_ms[1], 200),
#endif
// @Param: POS1_X
// @DisplayName: Antenna X position offset
@ -228,6 +236,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Range: -10 10
// @User: Advanced
#if GPS_MAX_RECEIVERS > 1
// @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.
@ -235,6 +244,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Range: -10 10
// @User: Advanced
AP_GROUPINFO("POS2", 17, AP_GPS, _antenna_offset[1], 0.0f),
#endif
// @Param: DELAY_MS
// @DisplayName: GPS delay in milliseconds
@ -245,6 +255,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @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.
@ -253,7 +264,9 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("DELAY_MS2", 19, AP_GPS, _delay_ms[1], 0),
#endif
#if defined(GPS_BLENDED_INSTANCE)
// @Param: BLEND_MASK
// @DisplayName: Multi GPS Blending Mask
// @Description: Determines which of the accuracy measures Horizontal position, Vertical Position and Speed are used to calculate the weighting on each GPS receiver when soft switching has been selected by setting GPS_AUTO_SWITCH to 2
@ -268,6 +281,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Range: 5.0 30.0
// @User: Advanced
AP_GROUPINFO("BLEND_TC", 21, AP_GPS, _blend_tc, 10.0f),
#endif
AP_GROUPEND
};
@ -292,8 +306,9 @@ void AP_GPS::init(const AP_SerialManager& serial_manager)
primary_instance = 0;
// search for serial ports with gps protocol
_port[0] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, 0);
_port[1] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, 1);
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
_port[i] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, i);
}
_last_instance_swap_ms = 0;
// Initialise class variables used to do GPS blending
@ -430,9 +445,11 @@ void AP_GPS::detect_instance(uint8_t instance)
// 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:
#ifndef HAL_BUILD_AP_PERIPH
dstate->auto_detected_baud = false; // specified, not detected
new_gps = new AP_GPS_MAV(*this, state[instance], nullptr);
goto found_gps;
#endif
break;
// user has to explicitly set the UAVCAN type, do not use AUTO
@ -456,6 +473,7 @@ void AP_GPS::detect_instance(uint8_t instance)
// the correct baud rate, and should have the selected baud broadcast
dstate->auto_detected_baud = true;
#ifndef HAL_BUILD_AP_PERIPH
switch (_type[instance]) {
// by default the sbf/trimble gps outputs no data on its port, until configured.
case GPS_TYPE_SBF:
@ -473,6 +491,7 @@ void AP_GPS::detect_instance(uint8_t instance)
default:
break;
}
#endif // HAL_BUILD_AP_PERIPH
if (now - dstate->last_baud_change_ms > GPS_BAUD_TIME_MS) {
// try the next baud rate
@ -516,6 +535,7 @@ void AP_GPS::detect_instance(uint8_t instance)
AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance]);
}
#ifndef HAL_BUILD_AP_PERIPH
#if !HAL_MINIMIZE_FEATURES
// we drop the MTK drivers when building a small build as they are so rarely used
// and are surprisingly large
@ -549,6 +569,10 @@ void AP_GPS::detect_instance(uint8_t instance)
AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]);
}
#endif // HAL_BUILD_AP_PERIPH
if (new_gps) {
goto found_gps;
}
}
found_gps:
@ -571,6 +595,7 @@ AP_GPS::GPS_Status AP_GPS::highest_supported_status(uint8_t instance) const
bool AP_GPS::should_log() const
{
#ifndef HAL_BUILD_AP_PERIPH
AP_Logger *logger = AP_Logger::get_singleton();
if (logger == nullptr) {
return false;
@ -582,6 +607,9 @@ bool AP_GPS::should_log() const
return false;
}
return true;
#else
return false;
#endif
}
@ -664,6 +692,7 @@ void AP_GPS::update_instance(uint8_t instance)
data_should_be_logged = true;
}
#ifndef HAL_BUILD_AP_PERIPH
if (data_should_be_logged &&
should_log() &&
!AP::ahrs().have_ekf_logging()) {
@ -676,6 +705,9 @@ void AP_GPS::update_instance(uint8_t instance)
AP::rtc().set_utc_usec(now, AP_RTC::SOURCE_GPS);
}
}
#else
(void)data_should_be_logged;
#endif
}
/*
@ -694,6 +726,7 @@ void AP_GPS::update(void)
}
}
#if defined(GPS_BLENDED_INSTANCE)
// if blending is requested, attempt to calculate weighting for each GPS
if (_auto_switch == 2) {
_output_is_blended = calc_blend_weights();
@ -776,11 +809,13 @@ void AP_GPS::update(void)
state[GPS_BLENDED_INSTANCE] = state[primary_instance];
_blended_antenna_offset = _antenna_offset[primary_instance];
}
#endif // GPS_BLENDED_INSTANCE
#ifndef HAL_BUILD_AP_PERIPH
// update notify with gps status. We always base this on the primary_instance
AP_Notify::flags.gps_status = state[primary_instance].status;
AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats;
#endif
}
void AP_GPS::handle_gps_inject(const mavlink_message_t &msg)
@ -947,6 +982,7 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
0); // TODO one-sigma heading accuracy standard deviation
}
#if GPS_MAX_RECEIVERS > 1
void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
{
static uint32_t last_send_time_ms[MAVLINK_COMM_NUM_BUFFERS];
@ -975,6 +1011,7 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
state[1].rtk_num_sats,
state[1].rtk_age_ms);
}
#endif // GPS_MAX_RECEIVERS
void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst)
{
@ -1124,6 +1161,7 @@ bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const
return false;
}
#if defined(GPS_BLENDED_INSTANCE)
// return lag of blended GPS
if (instance == GPS_BLENDED_INSTANCE) {
lag_sec = _blended_lag_sec;
@ -1131,6 +1169,7 @@ bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const
uint8_t inst; // we don't actually care what the number is, but must pass it
return first_unconfigured_gps(inst);
}
#endif
if (_delay_ms[instance] > 0) {
// if the user has specified a non zero time delay, always return that value
@ -1158,10 +1197,12 @@ const Vector3f &AP_GPS::get_antenna_offset(uint8_t instance) const
return _antenna_offset[0];
}
#if defined(GPS_BLENDED_INSTANCE)
if (instance == GPS_BLENDED_INSTANCE) {
// return an offset for the blended GPS solution
return _blended_antenna_offset;
}
#endif
return _antenna_offset[instance];
}
@ -1181,6 +1222,7 @@ uint16_t AP_GPS::get_rate_ms(uint8_t instance) const
return MIN(_rate_ms[instance], GPS_MAX_RATE_MS);
}
#if defined(GPS_BLENDED_INSTANCE)
/*
calculate the weightings used to blend GPSs location and velocity data
*/
@ -1550,6 +1592,7 @@ void AP_GPS::calc_blended_state(void)
timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = (uint32_t)temp_time_1;
timing[GPS_BLENDED_INSTANCE].last_message_time_ms = (uint32_t)temp_time_2;
}
#endif // GPS_BLENDED_INSTANCE
bool AP_GPS::is_healthy(uint8_t instance) const
{
@ -1561,9 +1604,11 @@ bool AP_GPS::is_healthy(uint8_t instance) const
bool last_msg_valid = last_message_delta_time_ms(instance) < gps_max_delta_ms;
#if defined(GPS_BLENDED_INSTANCE)
if (instance == GPS_BLENDED_INSTANCE) {
return last_msg_valid && blend_health_check();
}
#endif
return last_msg_valid &&
drivers[instance] != nullptr &&

6
libraries/AP_GPS/AP_GPS.h
View File

@ -26,9 +26,15 @@
maximum number of GPS instances available on this platform. If more
than 1 then redundant sensors may be available
*/
#ifndef GPS_MAX_RECEIVERS
#define GPS_MAX_RECEIVERS 2 // maximum number of physical GPS sensors allowed - does not include virtual GPS created by blending receiver data
#endif
#ifndef GPS_MAX_INSTANCES
#define GPS_MAX_INSTANCES (GPS_MAX_RECEIVERS + 1) // maximum number of GPS instances including the 'virtual' GPS created by blending receiver data
#endif
#if GPS_MAX_INSTANCES > GPS_MAX_RECEIVERS
#define GPS_BLENDED_INSTANCE GPS_MAX_RECEIVERS // the virtual blended GPS is always the highest instance (2)
#endif
#define GPS_UNKNOWN_DOP UINT16_MAX // set unknown DOP's to maximum value, which is also correct for MAVLink
// the number of GPS leap seconds

24
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -38,6 +38,12 @@
extern const AP_HAL::HAL& hal;
#ifdef HAL_NO_GCS
#define GCS_SEND_TEXT(severity, format, args...)
#else
#define GCS_SEND_TEXT(severity, format, args...) gcs().send_text(severity, format, ##args)
#endif
#if UBLOX_DEBUGGING
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
#else
@ -503,6 +509,7 @@ AP_GPS_UBLOX::read(void)
// Private Methods /////////////////////////////////////////////////////////////
void AP_GPS_UBLOX::log_mon_hw(void)
{
#ifndef HAL_NO_LOGGING
if (!should_log()) {
return;
}
@ -523,10 +530,12 @@ void AP_GPS_UBLOX::log_mon_hw(void)
pkt.agcCnt = _buffer.mon_hw_68.agcCnt;
}
AP::logger().WriteBlock(&pkt, sizeof(pkt));
#endif
}
void AP_GPS_UBLOX::log_mon_hw2(void)
{
#ifndef HAL_NO_LOGGING
if (!should_log()) {
return;
}
@ -541,11 +550,13 @@ void AP_GPS_UBLOX::log_mon_hw2(void)
magQ : _buffer.mon_hw2.magQ,
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
#endif
}
#if UBLOX_RXM_RAW_LOGGING
void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
{
#ifndef HAL_NO_LOGGING
if (!should_log()) {
return;
}
@ -568,10 +579,12 @@ void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
#endif
}
void AP_GPS_UBLOX::log_rxm_rawx(const struct ubx_rxm_rawx &raw)
{
#ifndef HAL_NO_LOGGING
if (!should_log()) {
return;
}
@ -608,6 +621,7 @@ void AP_GPS_UBLOX::log_rxm_rawx(const struct ubx_rxm_rawx &raw)
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
#endif
}
#endif // UBLOX_RXM_RAW_LOGGING
@ -872,7 +886,7 @@ AP_GPS_UBLOX::_parse_gps(void)
_have_version = true;
strncpy(_version.hwVersion, _buffer.mon_ver.hwVersion, sizeof(_version.hwVersion));
strncpy(_version.swVersion, _buffer.mon_ver.swVersion, sizeof(_version.swVersion));
gcs().send_text(MAV_SEVERITY_INFO,
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
"u-blox %d HW: %s SW: %s",
state.instance + 1,
_version.hwVersion,
@ -1044,7 +1058,7 @@ AP_GPS_UBLOX::_parse_gps(void)
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
break;
case 4:
gcs().send_text(MAV_SEVERITY_INFO,
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
"Unexpected state %d", _buffer.pvt.flags);
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
@ -1273,7 +1287,7 @@ AP_GPS_UBLOX::_save_cfg()
_send_message(CLASS_CFG, MSG_CFG_CFG, &save_cfg, sizeof(save_cfg));
_last_cfg_sent_time = AP_HAL::millis();
_num_cfg_save_tries++;
gcs().send_text(MAV_SEVERITY_INFO,
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
"GPS: u-blox %d saving config",
state.instance + 1);
}
@ -1380,7 +1394,7 @@ void
AP_GPS_UBLOX::broadcast_configuration_failure_reason(void) const {
for (uint8_t i = 0; i < ARRAY_SIZE(reasons); i++) {
if (_unconfigured_messages & (1 << i)) {
gcs().send_text(MAV_SEVERITY_INFO, "GPS %u: u-blox %s configuration 0x%02x",
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: u-blox %s configuration 0x%02x",
(unsigned int)(state.instance + 1), reasons[i], (unsigned int)_unconfigured_messages);
break;
}
@ -1420,6 +1434,7 @@ bool AP_GPS_UBLOX::get_lag(float &lag_sec) const
void AP_GPS_UBLOX::Write_AP_Logger_Log_Startup_messages() const
{
#ifndef HAL_NO_LOGGING
AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages();
if (_have_version) {
@ -1428,6 +1443,7 @@ void AP_GPS_UBLOX::Write_AP_Logger_Log_Startup_messages() const
_version.hwVersion,
_version.swVersion);
}
#endif
}
// uBlox specific check_new_itow(), handling message length

6
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -159,16 +159,20 @@ void AP_GPS_Backend::_detection_message(char *buffer, const uint8_t buflen) cons
void AP_GPS_Backend::broadcast_gps_type() const
{
#ifndef HAL_NO_GCS
char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
_detection_message(buffer, sizeof(buffer));
gcs().send_text(MAV_SEVERITY_INFO, "%s", buffer);
#endif
}
void AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages() const
{
#ifndef HAL_NO_LOGGING
char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
_detection_message(buffer, sizeof(buffer));
AP::logger().Write_Message(buffer);
#endif
}
bool AP_GPS_Backend::should_log() const
@ -179,6 +183,7 @@ bool AP_GPS_Backend::should_log() const
void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
{
#ifndef HAL_NO_GCS
const uint8_t instance = state.instance;
// send status
switch (instance) {
@ -215,6 +220,7 @@ void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
state.rtk_iar_num_hypotheses);
break;
}
#endif
}