diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp index e3d273e3a4..00ae1729df 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Backend.cpp @@ -335,7 +335,7 @@ bool AP_Proximity_Backend::get_next_ignore_start_or_end(uint8_t start_or_end, in } // 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 ¤t_loc, float ¤t_vehicle_bearing) +bool AP_Proximity_Backend::database_prepare_for_push(Location ¤t_loc, float ¤t_heading) { AP_OADatabase *oaDb = AP::oadatabase(); if (oaDb == nullptr || !oaDb->healthy()) { @@ -346,22 +346,22 @@ bool AP_Proximity_Backend::database_prepare_for_push(Location ¤t_loc, floa return false; } - current_vehicle_bearing = AP::ahrs().yaw_sensor * 0.01f; + current_heading = AP::ahrs().yaw_sensor * 0.01f; return true; } // update Object Avoidance database with Earth-frame point -void AP_Proximity_Backend::database_push(const float angle, const float distance) +void AP_Proximity_Backend::database_push(float angle, float distance) { Location current_loc; - float current_vehicle_bearing; - if (database_prepare_for_push(current_loc, current_vehicle_bearing) == true) { - database_push(angle, distance, AP_HAL::millis(), current_loc, current_vehicle_bearing); + float current_heading; + if (database_prepare_for_push(current_loc, current_heading)) { + database_push(angle, distance, AP_HAL::millis(), current_loc, current_heading); } } // update Object Avoidance database with Earth-frame point -void AP_Proximity_Backend::database_push(const float angle, const float distance, const uint32_t timestamp_ms, const Location ¤t_loc, const float current_vehicle_bearing) +void AP_Proximity_Backend::database_push(float angle, float distance, uint32_t timestamp_ms, const Location ¤t_loc, float current_heading) { AP_OADatabase *oaDb = AP::oadatabase(); if (oaDb == nullptr || !oaDb->healthy()) { @@ -369,6 +369,6 @@ void AP_Proximity_Backend::database_push(const float angle, const float distance } Location temp_loc = current_loc; - temp_loc.offset_bearing(wrap_180(current_vehicle_bearing + angle), distance); + temp_loc.offset_bearing(wrap_180(current_heading + angle), distance); oaDb->queue_push(temp_loc, timestamp_ms, distance, angle); } diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h index 5741bd4750..c9784cd82e 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.h +++ b/libraries/AP_Proximity/AP_Proximity_Backend.h @@ -87,10 +87,10 @@ protected: bool get_ignore_area(uint8_t index, uint16_t &angle_deg, uint8_t &width_deg) const; bool get_next_ignore_start_or_end(uint8_t start_or_end, int16_t start_angle, int16_t &ignore_start) const; - // database helpers - bool database_prepare_for_push(Location ¤t_loc, float ¤t_vehicle_bearing); - void database_push(const float angle, const float distance); - void database_push(const float angle, const float distance, const uint32_t timestamp_ms, const Location ¤t_loc, const float current_vehicle_bearing); + // database helpers. all angles are in degrees + bool database_prepare_for_push(Location ¤t_loc, float ¤t_heading); + void database_push(float angle, float distance); + void database_push(float angle, float distance, uint32_t timestamp_ms, const Location ¤t_loc, float current_heading); AP_Proximity &frontend; AP_Proximity::Proximity_State &state; // reference to this instances state diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp index 81218e8cce..47acd4f6f3 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp @@ -391,8 +391,8 @@ void AP_Proximity_LightWareSF40C::process_message() // prepare to push to object database Location current_loc; - float current_vehicle_bearing; - const bool database_ready = database_prepare_for_push(current_loc, current_vehicle_bearing); + float current_heading; + const bool database_ready = database_prepare_for_push(current_loc, current_heading); // process each point const float angle_inc_deg = (1.0f / point_total) * 360.0f; @@ -426,7 +426,7 @@ void AP_Proximity_LightWareSF40C::process_message() } // send point to object avoidance database if (database_ready) { - database_push(angle_deg, dist_m, _last_distance_received_ms, current_loc, current_vehicle_bearing); + database_push(angle_deg, dist_m, _last_distance_received_ms, current_loc, current_heading); } } } diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp index e764e25ffb..eb9e92e4f6 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.cpp +++ b/libraries/AP_Proximity/AP_Proximity_MAV.cpp @@ -105,8 +105,8 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg) } Location current_loc; - float current_vehicle_bearing; - const bool database_ready = database_prepare_for_push(current_loc, current_vehicle_bearing); + float current_heading; + const bool database_ready = database_prepare_for_push(current_loc, current_heading); // initialise updated array and proximity sector angles (to closest object) and distances bool sector_updated[_num_sectors]; @@ -146,7 +146,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_vehicle_bearing); + database_push(mid_angle, packet_distance_m, _last_update_ms, current_loc, current_heading); } }