diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp index 25cc7f22ec..2cacecd5e3 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Backend.cpp @@ -248,40 +248,53 @@ 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(Vector2f ¤t_pos, float ¤t_heading) +bool AP_Proximity_Backend::database_prepare_for_push(Vector3f ¤t_pos, Matrix3f &body_to_ned) { AP_OADatabase *oaDb = AP::oadatabase(); if (oaDb == nullptr || !oaDb->healthy()) { return false; } - if (!AP::ahrs().get_relative_position_NE_origin(current_pos)) { + if (!AP::ahrs().get_relative_position_NED_origin(current_pos)) { return false; } - current_heading = AP::ahrs().yaw_sensor * 0.01f; + body_to_ned = AP::ahrs().get_rotation_body_to_ned(); + return true; } // update Object Avoidance database with Earth-frame point void AP_Proximity_Backend::database_push(float angle, float distance) { - Vector2f current_pos; - float current_heading; - if (database_prepare_for_push(current_pos, current_heading)) { - database_push(angle, distance, AP_HAL::millis(), current_pos, current_heading); + Vector3f current_pos; + Matrix3f body_to_ned; + + if (database_prepare_for_push(current_pos, body_to_ned)) { + database_push(angle, distance, AP_HAL::millis(), current_pos, body_to_ned); } } // update Object Avoidance database with Earth-frame point -void AP_Proximity_Backend::database_push(float angle, float distance, uint32_t timestamp_ms, const Vector2f ¤t_pos, float current_heading) +void AP_Proximity_Backend::database_push(float angle, float distance, uint32_t timestamp_ms, const Vector3f ¤t_pos, const Matrix3f &body_to_ned) { AP_OADatabase *oaDb = AP::oadatabase(); if (oaDb == nullptr || !oaDb->healthy()) { return; } - - Vector2f temp_pos = current_pos; - temp_pos.offset_bearing(wrap_180(current_heading + angle), distance); + + //Assume object is angle bearing and distance meters away from the vehicle + Vector2f object_2D = {0.0f,0.0f}; + object_2D.offset_bearing(wrap_180(angle), distance); + + //rotate that vector to a 3D vector in NED frame + const Vector3f object_3D = {object_2D.x,object_2D.y,0.0f}; + const Vector3f rotated_object_3D = body_to_ned * object_3D; + + //Calculate the position vector from origin + Vector3f temp_pos = current_pos + rotated_object_3D; + //Convert the vector to a NEU frame from NED + temp_pos.z = temp_pos.z * -1.0f; + oaDb->queue_push(temp_pos, timestamp_ms, distance); -} +} \ No newline at end of file diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h index 949b7086ee..4692df395a 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.h +++ b/libraries/AP_Proximity/AP_Proximity_Backend.h @@ -84,10 +84,9 @@ protected: bool ignore_reading(uint16_t angle_deg) const; // database helpers. all angles are in degrees - bool database_prepare_for_push(Vector2f ¤t_pos, float ¤t_heading); + bool database_prepare_for_push(Vector3f ¤t_pos, Matrix3f &body_to_ned); void database_push(float angle, float distance); - void database_push(float angle, float distance, uint32_t timestamp_ms, const Vector2f ¤t_pos, float current_heading); - + void database_push(float angle, float distance, uint32_t timestamp_ms, const Vector3f ¤t_pos, const Matrix3f &body_to_ned); 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 f7ae89d595..8db6fdd1e4 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp @@ -303,9 +303,9 @@ void AP_Proximity_LightWareSF40C::process_message() } // prepare to push to object database - Vector2f current_pos; - float current_heading; - const bool database_ready = database_prepare_for_push(current_pos, current_heading); + Vector3f current_pos; + Matrix3f body_to_ned; + const bool database_ready = database_prepare_for_push(current_pos, body_to_ned); // process each point const float angle_inc_deg = (1.0f / point_total) * 360.0f; @@ -361,7 +361,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_pos, current_heading); + database_push(combined_angle_deg, combined_dist_m, _last_distance_received_ms, current_pos,body_to_ned); } combined_count = 0; combined_dist_m = INT16_MAX; diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp index 94f159d5eb..22351cd2c9 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.cpp +++ b/libraries/AP_Proximity/AP_Proximity_MAV.cpp @@ -103,9 +103,9 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg) increment *= -1; } - Vector2f current_pos; - float current_heading; - const bool database_ready = database_prepare_for_push(current_pos, current_heading); + Vector3f current_pos; + Matrix3f body_to_ned; + const bool database_ready = database_prepare_for_push(current_pos, body_to_ned); // initialise updated array and proximity sector angles (to closest object) and distances bool sector_updated[PROXIMITY_NUM_SECTORS]; @@ -143,7 +143,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_pos, current_heading); + database_push(mid_angle, packet_distance_m, _last_update_ms, current_pos, body_to_ned); } }