zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_GPS: fixups after peer review 

This includes these changes:
RATE_MS, RATE_MS2 parameter description Range minimum reduced to 50
_blend_health_counter is reset to 0 if blending is disabled
GPS_MAX_RECEIVERS is replaced with GPS_BLENDED_INSTANCE where appropriate
simplify all_consistent functions check of number of receivers
calc_blended_weights fix for initial check of how many receivers we have
remove unnecessary setting of GPS last time when blending fails
remove RebootRequired from AUTO_SWITCH param description
master
Randy Mackay 8 years ago
parent
2f21e3b40c
commit
ef1399a52f
2 changed files with 72 additions and 73 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 142
      libraries/AP_GPS/AP_GPS.cpp
  2. 3
      libraries/AP_GPS/AP_GPS.h

142
libraries/AP_GPS/AP_GPS.cpp
Unescape View File

@ -81,7 +81,6 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { @@ -81,7 +81,6 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @DisplayName: Automatic Switchover Setting
// @Description: Automatic switchover to GPS reporting best lock
// @Values: 0:Disabled,1:UseBest,2:Blend
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("AUTO_SWITCH", 3, AP_GPS, _auto_switch, 1),
@ -165,7 +164,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { @@ -165,7 +164,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz is not allowed
// @Units: milliseconds
// @Values: 100:10Hz,125:8Hz,200:5Hz
// @Range: 100 200
// @Range: 50 200
// @User: Advanced
AP_GROUPINFO("RATE_MS", 14, AP_GPS, _rate_ms[0], 200),
@ -174,7 +173,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { @@ -174,7 +173,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz is not allowed
// @Units: milliseconds
// @Values: 100:10Hz,125:8Hz,200:5Hz
// @Range: 100 200
// @Range: 50 200
// @User: Advanced
AP_GROUPINFO("RATE_MS2", 15, AP_GPS, _rate_ms[1], 200),
@ -641,19 +640,20 @@ AP_GPS::update(void) @@ -641,19 +640,20 @@ AP_GPS::update(void)
}
} else {
_output_is_blended = false;
_blend_health_counter = 0;
}
if (_output_is_blended) {
// Use the weighting to calculate blended GPS states
calc_blended_state();
// set primary to the virtual instance
primary_instance = GPS_MAX_RECEIVERS;
primary_instance = GPS_BLENDED_INSTANCE;
} else {
// use switch logic to find best GPS
uint32_t now = AP_HAL::millis();
if (_auto_switch >= 1) {
// handling switching away from blended GPS
if (primary_instance == GPS_MAX_RECEIVERS) {
if (primary_instance == GPS_BLENDED_INSTANCE) {
primary_instance = 0;
for (uint8_t i=1; i<GPS_MAX_RECEIVERS; i++) {
// choose GPS with highest state or higher number of satellites
@ -701,7 +701,7 @@ AP_GPS::update(void) @@ -701,7 +701,7 @@ AP_GPS::update(void)
}
// copy the primary instance to the blended instance in case it is enabled later
state[GPS_MAX_RECEIVERS] = state[primary_instance];
state[GPS_BLENDED_INSTANCE] = state[primary_instance];
_blended_antenna_offset = _antenna_offset[primary_instance];
}
@ -927,8 +927,7 @@ bool AP_GPS::all_consistent(float &distance) const @@ -927,8 +927,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[1] == nullptr || _type[1] == GPS_TYPE_NONE) {
drivers[0] == nullptr || _type[0] == GPS_TYPE_NONE) {
distance = 0;
return true;
}
@ -1104,7 +1103,7 @@ bool AP_GPS::calc_blend_weights(void) @@ -1104,7 +1103,7 @@ bool AP_GPS::calc_blend_weights(void)
memset(&_blend_weights, 0, sizeof(_blend_weights));
// exit immediately if not enough receivers to do blending
if (num_instances < 2) {
if (num_instances < 2 || drivers[1] == nullptr || _type[1] == GPS_TYPE_NONE) {
return false;
}
@ -1126,10 +1125,9 @@ bool AP_GPS::calc_blend_weights(void) @@ -1126,10 +1125,9 @@ bool AP_GPS::calc_blend_weights(void)
}
if ((int32_t)(max_ms - min_ms) < (int32_t)(2 * max_rate_ms)) {
// data is not too delayed so use the oldest time_stamp to give a chance for data from that receiver to be updated
state[GPS_MAX_RECEIVERS].last_gps_time_ms = min_ms;
state[GPS_BLENDED_INSTANCE].last_gps_time_ms = min_ms;
} else {
// receiver data has timed out so fail out of blending
state[GPS_MAX_RECEIVERS].last_gps_time_ms = max_ms;
return false;
}
@ -1276,73 +1274,73 @@ bool AP_GPS::calc_blend_weights(void) @@ -1276,73 +1274,73 @@ bool AP_GPS::calc_blend_weights(void)
void AP_GPS::calc_blended_state(void)
{
// initialise the blended states so we can accumulate the results using the weightings for each GPS receiver
state[GPS_MAX_RECEIVERS].instance = GPS_MAX_RECEIVERS;
state[GPS_MAX_RECEIVERS].status = NO_GPS;
state[GPS_MAX_RECEIVERS].time_week_ms = 0;
state[GPS_MAX_RECEIVERS].time_week = 0;
state[GPS_MAX_RECEIVERS].ground_speed = 0.0f;
state[GPS_MAX_RECEIVERS].ground_course = 0.0f;
state[GPS_MAX_RECEIVERS].hdop = 9999;
state[GPS_MAX_RECEIVERS].vdop = 9999;
state[GPS_MAX_RECEIVERS].num_sats = 0;
state[GPS_MAX_RECEIVERS].velocity.zero();
state[GPS_MAX_RECEIVERS].speed_accuracy = 1e6f;
state[GPS_MAX_RECEIVERS].horizontal_accuracy = 1e6f;
state[GPS_MAX_RECEIVERS].vertical_accuracy = 1e6f;
state[GPS_MAX_RECEIVERS].have_vertical_velocity = false;
state[GPS_MAX_RECEIVERS].have_speed_accuracy = false;
state[GPS_MAX_RECEIVERS].have_horizontal_accuracy = false;
state[GPS_MAX_RECEIVERS].have_vertical_accuracy = false;
state[GPS_MAX_RECEIVERS].last_gps_time_ms = 0;
memset(&state[GPS_MAX_RECEIVERS].location, 0, sizeof(state[GPS_MAX_RECEIVERS].location));
state[GPS_BLENDED_INSTANCE].instance = GPS_BLENDED_INSTANCE;
state[GPS_BLENDED_INSTANCE].status = NO_FIX;
state[GPS_BLENDED_INSTANCE].time_week_ms = 0;
state[GPS_BLENDED_INSTANCE].time_week = 0;
state[GPS_BLENDED_INSTANCE].ground_speed = 0.0f;
state[GPS_BLENDED_INSTANCE].ground_course = 0.0f;
state[GPS_BLENDED_INSTANCE].hdop = 9999;
state[GPS_BLENDED_INSTANCE].vdop = 9999;
state[GPS_BLENDED_INSTANCE].num_sats = 0;
state[GPS_BLENDED_INSTANCE].velocity.zero();
state[GPS_BLENDED_INSTANCE].speed_accuracy = 1e6f;
state[GPS_BLENDED_INSTANCE].horizontal_accuracy = 1e6f;
state[GPS_BLENDED_INSTANCE].vertical_accuracy = 1e6f;
state[GPS_BLENDED_INSTANCE].have_vertical_velocity = false;
state[GPS_BLENDED_INSTANCE].have_speed_accuracy = false;
state[GPS_BLENDED_INSTANCE].have_horizontal_accuracy = false;
state[GPS_BLENDED_INSTANCE].have_vertical_accuracy = false;
state[GPS_BLENDED_INSTANCE].last_gps_time_ms = 0;
memset(&state[GPS_BLENDED_INSTANCE].location, 0, sizeof(state[GPS_BLENDED_INSTANCE].location));
_blended_antenna_offset.zero();
_blended_lag_sec = 0;
timing[GPS_MAX_RECEIVERS].last_fix_time_ms = 0;
timing[GPS_MAX_RECEIVERS].last_message_time_ms = 0;
timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = 0;
timing[GPS_BLENDED_INSTANCE].last_message_time_ms = 0;
// combine the states into a blended solution
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
// use the highest status
if (state[i].status > state[GPS_MAX_RECEIVERS].status) {
state[GPS_MAX_RECEIVERS].status = state[i].status;
if (state[i].status > state[GPS_BLENDED_INSTANCE].status) {
state[GPS_BLENDED_INSTANCE].status = state[i].status;
}
// calculate a blended average velocity
state[GPS_MAX_RECEIVERS].velocity += state[i].velocity * _blend_weights[i];
state[GPS_BLENDED_INSTANCE].velocity += state[i].velocity * _blend_weights[i];
// report the best valid accuracies and DOP metrics
if (state[i].have_horizontal_accuracy && state[i].horizontal_accuracy > 0.0f && state[i].horizontal_accuracy < state[GPS_MAX_RECEIVERS].horizontal_accuracy) {
state[GPS_MAX_RECEIVERS].have_horizontal_accuracy = true;
state[GPS_MAX_RECEIVERS].horizontal_accuracy = state[i].horizontal_accuracy;
if (state[i].have_horizontal_accuracy && state[i].horizontal_accuracy > 0.0f && state[i].horizontal_accuracy < state[GPS_BLENDED_INSTANCE].horizontal_accuracy) {
state[GPS_BLENDED_INSTANCE].have_horizontal_accuracy = true;
state[GPS_BLENDED_INSTANCE].horizontal_accuracy = state[i].horizontal_accuracy;
}
if (state[i].have_vertical_accuracy && state[i].vertical_accuracy > 0.0f && state[i].vertical_accuracy < state[GPS_MAX_RECEIVERS].vertical_accuracy) {
state[GPS_MAX_RECEIVERS].have_vertical_accuracy = true;
state[GPS_MAX_RECEIVERS].vertical_accuracy = state[i].vertical_accuracy;
if (state[i].have_vertical_accuracy && state[i].vertical_accuracy > 0.0f && state[i].vertical_accuracy < state[GPS_BLENDED_INSTANCE].vertical_accuracy) {
state[GPS_BLENDED_INSTANCE].have_vertical_accuracy = true;
state[GPS_BLENDED_INSTANCE].vertical_accuracy = state[i].vertical_accuracy;
}
if (state[i].have_vertical_velocity ) {
state[GPS_MAX_RECEIVERS].have_vertical_velocity = true;
state[GPS_BLENDED_INSTANCE].have_vertical_velocity = true;
}
if (state[i].have_speed_accuracy && state[i].speed_accuracy > 0.0f && state[i].speed_accuracy < state[GPS_MAX_RECEIVERS].speed_accuracy) {
state[GPS_MAX_RECEIVERS].have_speed_accuracy = true;
state[GPS_MAX_RECEIVERS].speed_accuracy = state[i].speed_accuracy;
if (state[i].have_speed_accuracy && state[i].speed_accuracy > 0.0f && state[i].speed_accuracy < state[GPS_BLENDED_INSTANCE].speed_accuracy) {
state[GPS_BLENDED_INSTANCE].have_speed_accuracy = true;
state[GPS_BLENDED_INSTANCE].speed_accuracy = state[i].speed_accuracy;
}
if (state[i].hdop > 0 && state[i].hdop < state[GPS_MAX_RECEIVERS].hdop) {
state[GPS_MAX_RECEIVERS].hdop = state[i].hdop;
if (state[i].hdop > 0 && state[i].hdop < state[GPS_BLENDED_INSTANCE].hdop) {
state[GPS_BLENDED_INSTANCE].hdop = state[i].hdop;
}
if (state[i].vdop > 0 && state[i].vdop < state[GPS_MAX_RECEIVERS].vdop) {
state[GPS_MAX_RECEIVERS].vdop = state[i].vdop;
if (state[i].vdop > 0 && state[i].vdop < state[GPS_BLENDED_INSTANCE].vdop) {
state[GPS_BLENDED_INSTANCE].vdop = state[i].vdop;
}
if (state[i].num_sats > 0 && state[i].num_sats > state[GPS_MAX_RECEIVERS].num_sats) {
state[GPS_MAX_RECEIVERS].num_sats = state[i].num_sats;
if (state[i].num_sats > 0 && state[i].num_sats > state[GPS_BLENDED_INSTANCE].num_sats) {
state[GPS_BLENDED_INSTANCE].num_sats = state[i].num_sats;
}
// report a blended average GPS antenna position
@ -1351,11 +1349,11 @@ void AP_GPS::calc_blended_state(void) @@ -1351,11 +1349,11 @@ void AP_GPS::calc_blended_state(void)
_blended_antenna_offset += temp_antenna_offset;
// blend the timing data
if (timing[i].last_fix_time_ms > timing[GPS_MAX_RECEIVERS].last_fix_time_ms) {
timing[GPS_MAX_RECEIVERS].last_fix_time_ms = timing[i].last_fix_time_ms;
if (timing[i].last_fix_time_ms > timing[GPS_BLENDED_INSTANCE].last_fix_time_ms) {
timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = timing[i].last_fix_time_ms;
}
if (timing[i].last_message_time_ms > timing[GPS_MAX_RECEIVERS].last_message_time_ms) {
timing[GPS_MAX_RECEIVERS].last_message_time_ms = timing[i].last_message_time_ms;
if (timing[i].last_message_time_ms > timing[GPS_BLENDED_INSTANCE].last_message_time_ms) {
timing[GPS_BLENDED_INSTANCE].last_message_time_ms = timing[i].last_message_time_ms;
}
}
@ -1372,7 +1370,7 @@ void AP_GPS::calc_blended_state(void) @@ -1372,7 +1370,7 @@ void AP_GPS::calc_blended_state(void)
if (_blend_weights[i] > best_weight) {
best_weight = _blend_weights[i];
best_index = i;
state[GPS_MAX_RECEIVERS].location = state[i].location;
state[GPS_BLENDED_INSTANCE].location = state[i].location;
}
}
@ -1382,18 +1380,18 @@ void AP_GPS::calc_blended_state(void) @@ -1382,18 +1380,18 @@ void AP_GPS::calc_blended_state(void)
blended_NE_offset_m.zero();
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (_blend_weights[i] > 0.0f && i != best_index) {
blended_NE_offset_m += location_diff(state[GPS_MAX_RECEIVERS].location, state[i].location) * _blend_weights[i];
blended_alt_offset_cm += (float)(state[i].location.alt - state[GPS_MAX_RECEIVERS].location.alt) * _blend_weights[i];
blended_NE_offset_m += location_diff(state[GPS_BLENDED_INSTANCE].location, state[i].location) * _blend_weights[i];
blended_alt_offset_cm += (float)(state[i].location.alt - state[GPS_BLENDED_INSTANCE].location.alt) * _blend_weights[i];
}
}
// Add the sum of weighted offsets to the reference location to obtain the blended location
location_offset(state[GPS_MAX_RECEIVERS].location, blended_NE_offset_m.x, blended_NE_offset_m.y);
state[GPS_MAX_RECEIVERS].location.alt += (int)blended_alt_offset_cm;
location_offset(state[GPS_BLENDED_INSTANCE].location, blended_NE_offset_m.x, blended_NE_offset_m.y);
state[GPS_BLENDED_INSTANCE].location.alt += (int)blended_alt_offset_cm;
// Calculate ground speed and course from blended velocity vector
state[GPS_MAX_RECEIVERS].ground_speed = norm(state[GPS_MAX_RECEIVERS].velocity.x, state[GPS_MAX_RECEIVERS].velocity.y);
state[GPS_MAX_RECEIVERS].ground_course = wrap_360(degrees(atan2f(state[GPS_MAX_RECEIVERS].velocity.x, state[GPS_MAX_RECEIVERS].velocity.x)));
state[GPS_BLENDED_INSTANCE].ground_speed = norm(state[GPS_BLENDED_INSTANCE].velocity.x, state[GPS_BLENDED_INSTANCE].velocity.y);
state[GPS_BLENDED_INSTANCE].ground_course = wrap_360(degrees(atan2f(state[GPS_BLENDED_INSTANCE].velocity.x, state[GPS_BLENDED_INSTANCE].velocity.x)));
/*
* The blended location in _output_state.location is not stable enough to be used by the autopilot
@ -1419,8 +1417,8 @@ void AP_GPS::calc_blended_state(void) @@ -1419,8 +1417,8 @@ void AP_GPS::calc_blended_state(void)
// Calculate the offset from each GPS solution to the blended solution
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
_NE_pos_offset_m[i] = location_diff(state[i].location, state[GPS_MAX_RECEIVERS].location) * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]);
_hgt_offset_cm[i] = (float)(state[GPS_MAX_RECEIVERS].location.alt - state[i].location.alt) * alpha[i] + _hgt_offset_cm[i] * (1.0f - alpha[i]);
_NE_pos_offset_m[i] = location_diff(state[i].location, state[GPS_BLENDED_INSTANCE].location) * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]);
_hgt_offset_cm[i] = (float)(state[GPS_BLENDED_INSTANCE].location.alt - state[i].location.alt) * alpha[i] + _hgt_offset_cm[i] * (1.0f - alpha[i]);
}
// Calculate a corrected location for each GPS
@ -1436,8 +1434,8 @@ void AP_GPS::calc_blended_state(void) @@ -1436,8 +1434,8 @@ void AP_GPS::calc_blended_state(void)
blended_NE_offset_m.zero();
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (_blend_weights[i] > 0.0f) {
blended_NE_offset_m += location_diff(state[GPS_MAX_RECEIVERS].location, corrected_location[i]) * _blend_weights[i];
blended_alt_offset_cm += (float)(corrected_location[i].alt - state[GPS_MAX_RECEIVERS].location.alt) * _blend_weights[i];
blended_NE_offset_m += location_diff(state[GPS_BLENDED_INSTANCE].location, corrected_location[i]) * _blend_weights[i];
blended_alt_offset_cm += (float)(corrected_location[i].alt - state[GPS_BLENDED_INSTANCE].location.alt) * _blend_weights[i];
}
}
@ -1458,11 +1456,11 @@ void AP_GPS::calc_blended_state(void) @@ -1458,11 +1456,11 @@ void AP_GPS::calc_blended_state(void)
// calculate output
if (!weeks_consistent) {
// use data from highest weighted sensor
state[GPS_MAX_RECEIVERS].time_week = state[best_index].time_week;
state[GPS_MAX_RECEIVERS].time_week_ms = state[best_index].time_week_ms;
state[GPS_BLENDED_INSTANCE].time_week = state[best_index].time_week;
state[GPS_BLENDED_INSTANCE].time_week_ms = state[best_index].time_week_ms;
} else {
// use week number from highest weighting GPS (they should all have the same week number)
state[GPS_MAX_RECEIVERS].time_week = state[best_index].time_week;
state[GPS_BLENDED_INSTANCE].time_week = state[best_index].time_week;
// calculate a blended value for the number of ms lapsed in the week
double temp_time_0 = 0.0;
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
@ -1470,7 +1468,7 @@ void AP_GPS::calc_blended_state(void) @@ -1470,7 +1468,7 @@ void AP_GPS::calc_blended_state(void)
temp_time_0 += (double)state[i].time_week_ms * _blend_weights[i];
}
}
state[GPS_MAX_RECEIVERS].time_week_ms = (uint32_t)temp_time_0;
state[GPS_BLENDED_INSTANCE].time_week_ms = (uint32_t)temp_time_0;
}
// calculate a blended value for the timing data and lag
@ -1483,7 +1481,7 @@ void AP_GPS::calc_blended_state(void) @@ -1483,7 +1481,7 @@ void AP_GPS::calc_blended_state(void)
_blended_lag_sec += get_lag(i) * _blended_lag_sec;
}
}
timing[GPS_MAX_RECEIVERS].last_fix_time_ms = (uint32_t)temp_time_1;
timing[GPS_MAX_RECEIVERS].last_message_time_ms = (uint32_t)temp_time_2;
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;
}

3
libraries/AP_GPS/AP_GPS.h
Unescape View File

@ -30,6 +30,7 @@ @@ -30,6 +30,7 @@
*/
#define GPS_MAX_RECEIVERS 2 // maximum number of physical GPS sensors allowed - does not include virtual GPS created by blending receiver data
#define GPS_MAX_INSTANCES (GPS_MAX_RECEIVERS + 1) // maximumum number of GPs instances including the 'virtual' GPS created by blending receiver data
#define GPS_BLENDED_INSTANCE GPS_MAX_RECEIVERS // the virtual blended GPS is always the highest instance (2)
#define GPS_RTK_INJECT_TO_ALL 127
#define GPS_MAX_RATE_MS 200 // maximum value of rate_ms (i.e. slowest update rate) is 5hz or 200ms
@ -379,7 +380,7 @@ protected: @@ -379,7 +380,7 @@ protected:
AP_Int8 _min_elevation;
AP_Int8 _raw_data;
AP_Int8 _gnss_mode[GPS_MAX_RECEIVERS];
AP_Int16 _rate_ms[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];

Write Preview
Loading…
Cancel
Save