mirror of https://github.com/ArduPilot/ardupilot
AP_GPS: save some memory and code space on APM2 for ublox
don't include structures we don't need for low end CPUs
This commit is contained in:
parent
8aa7a3cffc
commit
b6beeb6f2f
|
@ -41,18 +41,6 @@ extern const AP_HAL::HAL& hal;
|
||||||
# define Debug(fmt, args ...)
|
# define Debug(fmt, args ...)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/*
|
|
||||||
only do detailed hardware logging on boards likely to have more log
|
|
||||||
storage space
|
|
||||||
*/
|
|
||||||
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
|
||||||
#define UBLOX_HW_LOGGING 1
|
|
||||||
#define UBLOX_RXM_RAW_LOGGING 1
|
|
||||||
#else
|
|
||||||
#define UBLOX_HW_LOGGING 0
|
|
||||||
#define UBLOX_RXM_RAW_LOGGING 0
|
|
||||||
#endif
|
|
||||||
|
|
||||||
AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
|
AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
|
||||||
AP_GPS_Backend(_gps, _state, _port),
|
AP_GPS_Backend(_gps, _state, _port),
|
||||||
_step(0),
|
_step(0),
|
||||||
|
@ -452,6 +440,7 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if UBLOX_GNSS_SETTINGS
|
||||||
if (_class == CLASS_CFG && _msg_id == MSG_CFG_GNSS && gps._gnss_mode != 0) {
|
if (_class == CLASS_CFG && _msg_id == MSG_CFG_GNSS && gps._gnss_mode != 0) {
|
||||||
uint8_t gnssCount = 0;
|
uint8_t gnssCount = 0;
|
||||||
Debug("Got GNSS Settings %u %u %u %u:\n",
|
Debug("Got GNSS Settings %u %u %u %u:\n",
|
||||||
|
@ -510,6 +499,7 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||||
sizeof(_buffer.sbas));
|
sizeof(_buffer.sbas));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
#if UBLOX_HW_LOGGING
|
#if UBLOX_HW_LOGGING
|
||||||
if (_class == CLASS_MON) {
|
if (_class == CLASS_MON) {
|
||||||
|
|
|
@ -45,8 +45,12 @@
|
||||||
#define UBLOX_RXM_RAW_LOGGING 1
|
#define UBLOX_RXM_RAW_LOGGING 1
|
||||||
#define UBLOX_MAX_RXM_RAW_SATS 22
|
#define UBLOX_MAX_RXM_RAW_SATS 22
|
||||||
#define UBLOX_MAX_RXM_RAWX_SATS 32
|
#define UBLOX_MAX_RXM_RAWX_SATS 32
|
||||||
|
#define UBLOX_GNSS_SETTINGS 1
|
||||||
|
#define UBLOX_HW_LOGGING 1
|
||||||
#else
|
#else
|
||||||
#define UBLOX_RXM_RAW_LOGGING 0
|
#define UBLOX_RXM_RAW_LOGGING 0
|
||||||
|
#define UBLOX_GNSS_SETTINGS 0
|
||||||
|
#define UBLOX_HW_LOGGING 0
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7
|
#define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7
|
||||||
|
@ -72,6 +76,7 @@ private:
|
||||||
uint8_t msg_id;
|
uint8_t msg_id;
|
||||||
uint16_t length;
|
uint16_t length;
|
||||||
};
|
};
|
||||||
|
#if UBLOX_GNSS_SETTINGS
|
||||||
struct PACKED ubx_cfg_gnss {
|
struct PACKED ubx_cfg_gnss {
|
||||||
uint8_t msgVer;
|
uint8_t msgVer;
|
||||||
uint8_t numTrkChHw;
|
uint8_t numTrkChHw;
|
||||||
|
@ -85,6 +90,7 @@ private:
|
||||||
uint32_t flags;
|
uint32_t flags;
|
||||||
} configBlock[UBLOX_MAX_GNSS_CONFIG_BLOCKS];
|
} configBlock[UBLOX_MAX_GNSS_CONFIG_BLOCKS];
|
||||||
};
|
};
|
||||||
|
#endif
|
||||||
struct PACKED ubx_cfg_nav_rate {
|
struct PACKED ubx_cfg_nav_rate {
|
||||||
uint16_t measure_rate_ms;
|
uint16_t measure_rate_ms;
|
||||||
uint16_t nav_rate;
|
uint16_t nav_rate;
|
||||||
|
@ -113,6 +119,7 @@ private:
|
||||||
uint32_t res3;
|
uint32_t res3;
|
||||||
uint32_t res4;
|
uint32_t res4;
|
||||||
};
|
};
|
||||||
|
#if UBLOX_GNSS_SETTINGS
|
||||||
struct PACKED ubx_cfg_sbas {
|
struct PACKED ubx_cfg_sbas {
|
||||||
uint8_t mode;
|
uint8_t mode;
|
||||||
uint8_t usage;
|
uint8_t usage;
|
||||||
|
@ -120,7 +127,7 @@ private:
|
||||||
uint8_t scanmode2;
|
uint8_t scanmode2;
|
||||||
uint32_t scanmode1;
|
uint32_t scanmode1;
|
||||||
};
|
};
|
||||||
|
#endif
|
||||||
struct PACKED ubx_nav_posllh {
|
struct PACKED ubx_nav_posllh {
|
||||||
uint32_t time; // GPS msToW
|
uint32_t time; // GPS msToW
|
||||||
int32_t longitude;
|
int32_t longitude;
|
||||||
|
@ -179,6 +186,8 @@ private:
|
||||||
uint32_t speed_accuracy;
|
uint32_t speed_accuracy;
|
||||||
uint32_t heading_accuracy;
|
uint32_t heading_accuracy;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#if UBLOX_HW_LOGGING
|
||||||
// Lea6 uses a 60 byte message
|
// Lea6 uses a 60 byte message
|
||||||
struct PACKED ubx_mon_hw_60 {
|
struct PACKED ubx_mon_hw_60 {
|
||||||
uint32_t pinSel;
|
uint32_t pinSel;
|
||||||
|
@ -231,6 +240,7 @@ private:
|
||||||
uint32_t postStatus;
|
uint32_t postStatus;
|
||||||
uint32_t reserved2;
|
uint32_t reserved2;
|
||||||
};
|
};
|
||||||
|
#endif
|
||||||
struct PACKED ubx_nav_svinfo_header {
|
struct PACKED ubx_nav_svinfo_header {
|
||||||
uint32_t itow;
|
uint32_t itow;
|
||||||
uint8_t numCh;
|
uint8_t numCh;
|
||||||
|
@ -286,11 +296,15 @@ private:
|
||||||
ubx_nav_solution solution;
|
ubx_nav_solution solution;
|
||||||
ubx_nav_velned velned;
|
ubx_nav_velned velned;
|
||||||
ubx_cfg_nav_settings nav_settings;
|
ubx_cfg_nav_settings nav_settings;
|
||||||
|
#if UBLOX_HW_LOGGING
|
||||||
ubx_mon_hw_60 mon_hw_60;
|
ubx_mon_hw_60 mon_hw_60;
|
||||||
ubx_mon_hw_68 mon_hw_68;
|
ubx_mon_hw_68 mon_hw_68;
|
||||||
ubx_mon_hw2 mon_hw2;
|
ubx_mon_hw2 mon_hw2;
|
||||||
|
#endif
|
||||||
|
#if UBLOX_GNSS_SETTINGS
|
||||||
ubx_cfg_gnss gnss;
|
ubx_cfg_gnss gnss;
|
||||||
ubx_cfg_sbas sbas;
|
ubx_cfg_sbas sbas;
|
||||||
|
#endif
|
||||||
ubx_nav_svinfo_header svinfo_header;
|
ubx_nav_svinfo_header svinfo_header;
|
||||||
#if UBLOX_RXM_RAW_LOGGING
|
#if UBLOX_RXM_RAW_LOGGING
|
||||||
ubx_rxm_raw rxm_raw;
|
ubx_rxm_raw rxm_raw;
|
||||||
|
|
Loading…
Reference in New Issue