AP_GPS: save 800 bytes of code space on APM2

we can assume a single GPS
This commit is contained in:
Andrew Tridgell 2014-04-04 10:08:28 +11:00
parent 9f857529ca
commit d9d038345a
2 changed files with 27 additions and 12 deletions

View File

@ -30,11 +30,13 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL
AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], 1), AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], 1),
#if GPS_MAX_INSTANCES > 1
// @Param: TYPE2 // @Param: TYPE2
// @DisplayName: 2nd GPS type // @DisplayName: 2nd GPS type
// @Description: GPS type of 2nd GPS // @Description: GPS type of 2nd GPS
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL
AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0), AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0),
#endif
// @Param: NAVFILTER // @Param: NAVFILTER
// @DisplayName: Navigation filter setting // @DisplayName: Navigation filter setting
@ -50,9 +52,11 @@ void AP_GPS::init(DataFlash_Class *dataflash)
{ {
_DataFlash = dataflash; _DataFlash = dataflash;
hal.uartB->begin(38400UL, 256, 16); hal.uartB->begin(38400UL, 256, 16);
#if GPS_MAX_INSTANCES > 1
if (hal.uartE != NULL) { if (hal.uartE != NULL) {
hal.uartE->begin(38400UL, 256, 16); hal.uartE->begin(38400UL, 256, 16);
} }
#endif
} }
// baudrates to try to detect GPSes with // baudrates to try to detect GPSes with
@ -251,6 +255,7 @@ AP_GPS::update(void)
// update notify with gps status. We always base this on the first GPS // update notify with gps status. We always base this on the first GPS
AP_Notify::flags.gps_status = state[0].status; AP_Notify::flags.gps_status = state[0].status;
#if GPS_MAX_INSTANCES > 1
// work out which GPS is the primary, and how many sensors we have // work out which GPS is the primary, and how many sensors we have
for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) { for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
if (state[i].status != NO_GPS) { if (state[i].status != NO_GPS) {
@ -274,6 +279,7 @@ AP_GPS::update(void)
primary_instance = i; primary_instance = i;
} }
} }
#endif // GPS_MAX_INSTANCES
} }
/* /*

View File

@ -119,9 +119,18 @@ public:
return num_instances; return num_instances;
} }
// using these macros saves some code space on APM2
#if GPS_MAX_INSTANCES == 1
# define _GPS_STATE(instance) state[0]
# define _GPS_TIMING(instance) timing[0]
#else
# define _GPS_STATE(instance) state[instance]
# define _GPS_TIMING(instance) timing[instance]
#endif
/// Query GPS status /// Query GPS status
GPS_Status status(uint8_t instance) const { GPS_Status status(uint8_t instance) const {
return state[instance].status; return _GPS_STATE(instance).status;
} }
GPS_Status status(void) const { GPS_Status status(void) const {
return status(primary_instance); return status(primary_instance);
@ -129,7 +138,7 @@ public:
// location of last fix // location of last fix
const Location &location(uint8_t instance) const { const Location &location(uint8_t instance) const {
return state[instance].location; return _GPS_STATE(instance).location;
} }
const Location &location() const { const Location &location() const {
return location(primary_instance); return location(primary_instance);
@ -137,7 +146,7 @@ public:
// 3D velocity in NED format // 3D velocity in NED format
const Vector3f &velocity(uint8_t instance) const { const Vector3f &velocity(uint8_t instance) const {
return state[instance].velocity; return _GPS_STATE(instance).velocity;
} }
const Vector3f &velocity() const { const Vector3f &velocity() const {
return velocity(primary_instance); return velocity(primary_instance);
@ -145,7 +154,7 @@ public:
// ground speed in m/s // ground speed in m/s
float ground_speed(uint8_t instance) const { float ground_speed(uint8_t instance) const {
return state[instance].ground_speed; return _GPS_STATE(instance).ground_speed;
} }
float ground_speed() const { float ground_speed() const {
return ground_speed(primary_instance); return ground_speed(primary_instance);
@ -158,7 +167,7 @@ public:
// ground course in centidegrees // ground course in centidegrees
int32_t ground_course_cd(uint8_t instance) const { int32_t ground_course_cd(uint8_t instance) const {
return state[instance].ground_course_cd; return _GPS_STATE(instance).ground_course_cd;
} }
int32_t ground_course_cd() const { int32_t ground_course_cd() const {
return ground_course_cd(primary_instance); return ground_course_cd(primary_instance);
@ -166,7 +175,7 @@ public:
// number of locked satellites // number of locked satellites
uint8_t num_sats(uint8_t instance) const { uint8_t num_sats(uint8_t instance) const {
return state[instance].num_sats; return _GPS_STATE(instance).num_sats;
} }
uint8_t num_sats() const { uint8_t num_sats() const {
return num_sats(primary_instance); return num_sats(primary_instance);
@ -174,7 +183,7 @@ public:
// GPS time of week in milliseconds // GPS time of week in milliseconds
uint32_t time_week_ms(uint8_t instance) const { uint32_t time_week_ms(uint8_t instance) const {
return state[instance].time_week_ms; return _GPS_STATE(instance).time_week_ms;
} }
uint32_t time_week_ms() const { uint32_t time_week_ms() const {
return time_week_ms(primary_instance); return time_week_ms(primary_instance);
@ -182,7 +191,7 @@ public:
// GPS week // GPS week
uint16_t time_week(uint8_t instance) const { uint16_t time_week(uint8_t instance) const {
return state[instance].time_week; return _GPS_STATE(instance).time_week;
} }
uint16_t time_week() const { uint16_t time_week() const {
return time_week(primary_instance); return time_week(primary_instance);
@ -190,7 +199,7 @@ public:
// horizontal dilution of precision // horizontal dilution of precision
uint16_t get_hdop(uint8_t instance) const { uint16_t get_hdop(uint8_t instance) const {
return state[instance].hdop; return _GPS_STATE(instance).hdop;
} }
uint16_t get_hdop() const { uint16_t get_hdop() const {
return get_hdop(primary_instance); return get_hdop(primary_instance);
@ -199,7 +208,7 @@ public:
// the time we got our last fix in system milliseconds. This is // the time we got our last fix in system milliseconds. This is
// used when calculating how far we might have moved since that fix // used when calculating how far we might have moved since that fix
uint32_t last_fix_time_ms(uint8_t instance) const { uint32_t last_fix_time_ms(uint8_t instance) const {
return timing[instance].last_fix_time_ms; return _GPS_TIMING(instance).last_fix_time_ms;
} }
uint32_t last_fix_time_ms(void) const { uint32_t last_fix_time_ms(void) const {
return last_fix_time_ms(primary_instance); return last_fix_time_ms(primary_instance);
@ -208,7 +217,7 @@ public:
// the time we last processed a message in milliseconds. This is // the time we last processed a message in milliseconds. This is
// used to indicate that we have new GPS data to process // used to indicate that we have new GPS data to process
uint32_t last_message_time_ms(uint8_t instance) const { uint32_t last_message_time_ms(uint8_t instance) const {
return timing[instance].last_message_time_ms; return _GPS_TIMING(instance).last_message_time_ms;
} }
uint32_t last_message_time_ms(void) const { uint32_t last_message_time_ms(void) const {
return last_message_time_ms(primary_instance); return last_message_time_ms(primary_instance);
@ -222,7 +231,7 @@ public:
// return true if the GPS supports vertical velocity values // return true if the GPS supports vertical velocity values
bool have_vertical_velocity(uint8_t instance) const { bool have_vertical_velocity(uint8_t instance) const {
return state[instance].have_vertical_velocity; return _GPS_STATE(instance).have_vertical_velocity;
} }
bool have_vertical_velocity(void) const { bool have_vertical_velocity(void) const {
return have_vertical_velocity(primary_instance); return have_vertical_velocity(primary_instance);