Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: integrate oadb ekf-offset change

Browse Source
This commit is contained in:
Randy Mackay 2019-12-04 16:17:23 +09:00
parent f474cfdf12
commit 69e85d2e08
4 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -248,14 +248,14 @@ bool AP_Proximity_Backend::ignore_reading(uint16_t angle_deg) const
}
// returns true if database is ready to be pushed to and all cached data is ready
bool AP_Proximity_Backend::database_prepare_for_push(Location &current_loc, float &current_heading)
bool AP_Proximity_Backend::database_prepare_for_push(Vector2f &current_pos, float &current_heading)
{
AP_OADatabase *oaDb = AP::oadatabase();
if (oaDb == nullptr || !oaDb->healthy()) {
return false;
}
if (!AP::ahrs().get_position(current_loc)) {
if (!AP::ahrs().get_relative_position_NE_origin(current_pos)) {
return false;
}
@ -266,22 +266,22 @@ bool AP_Proximity_Backend::database_prepare_for_push(Location &current_loc, floa
// update Object Avoidance database with Earth-frame point
void AP_Proximity_Backend::database_push(float angle, float distance)
{
Location current_loc;
Vector2f current_pos;
float current_heading;
if (database_prepare_for_push(current_loc, current_heading)) {
database_push(angle, distance, AP_HAL::millis(), current_loc, current_heading);
if (database_prepare_for_push(current_pos, current_heading)) {
database_push(angle, distance, AP_HAL::millis(), current_pos, current_heading);
}
}
// update Object Avoidance database with Earth-frame point
void AP_Proximity_Backend::database_push(float angle, float distance, uint32_t timestamp_ms, const Location &current_loc, float current_heading)
void AP_Proximity_Backend::database_push(float angle, float distance, uint32_t timestamp_ms, const Vector2f &current_pos, float current_heading)
{
AP_OADatabase *oaDb = AP::oadatabase();
if (oaDb == nullptr || !oaDb->healthy()) {
return;
}
Location temp_loc = current_loc;
temp_loc.offset_bearing(wrap_180(current_heading + angle), distance);
oaDb->queue_push(temp_loc, timestamp_ms, angle, distance);
Vector2f temp_pos = current_pos;
temp_pos.offset_bearing(wrap_180(current_heading + angle), distance);
oaDb->queue_push(temp_pos, timestamp_ms, distance);
}

4
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -84,9 +84,9 @@ protected:
bool ignore_reading(uint16_t angle_deg) const;
// database helpers. all angles are in degrees
bool database_prepare_for_push(Location &current_loc, float &current_heading);
bool database_prepare_for_push(Vector2f &current_pos, float &current_heading);
void database_push(float angle, float distance);
void database_push(float angle, float distance, uint32_t timestamp_ms, const Location &current_loc, float current_heading);
void database_push(float angle, float distance, uint32_t timestamp_ms, const Vector2f &current_pos, float current_heading);
AP_Proximity &frontend;
AP_Proximity::Proximity_State &state; // reference to this instances state

6
libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp
View File

@ -328,9 +328,9 @@ void AP_Proximity_LightWareSF40C::process_message()
}
// prepare to push to object database
Location current_loc;
Vector2f current_pos;
float current_heading;
const bool database_ready = database_prepare_for_push(current_loc, current_heading);
const bool database_ready = database_prepare_for_push(current_pos, current_heading);
// process each point
const float angle_inc_deg = (1.0f / point_total) * 360.0f;
@ -386,7 +386,7 @@ void AP_Proximity_LightWareSF40C::process_message()
// send combined distance to object database
if ((i+1 >= point_count) || (combined_count >= PROXIMITY_SF40C_COMBINE_READINGS)) {
if ((combined_dist_m < INT16_MAX) && database_ready) {
database_push(combined_angle_deg, combined_dist_m, _last_distance_received_ms, current_loc, current_heading);
database_push(combined_angle_deg, combined_dist_m, _last_distance_received_ms, current_pos, current_heading);
}
combined_count = 0;
combined_dist_m = INT16_MAX;

6
libraries/AP_Proximity/AP_Proximity_MAV.cpp
View File

@ -104,9 +104,9 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
increment *= -1;
}
Location current_loc;
Vector2f current_pos;
float current_heading;
const bool database_ready = database_prepare_for_push(current_loc, current_heading);
const bool database_ready = database_prepare_for_push(current_pos, current_heading);
// initialise updated array and proximity sector angles (to closest object) and distances
bool sector_updated[PROXIMITY_NUM_SECTORS];
@ -144,7 +144,7 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
// update Object Avoidance database with Earth-frame point
if (database_ready) {
database_push(mid_angle, packet_distance_m, _last_update_ms, current_loc, current_heading);
database_push(mid_angle, packet_distance_m, _last_update_ms, current_pos, current_heading);
}
}