Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_OADatabase: replace Location with offset from origin

This commit is contained in:
Randy Mackay 2019-12-04 16:16:00 +09:00
parent 449b8d124b
commit fe5ba3ccdb
2 changed files with 15 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_Avoidance/AP_OADatabase.cpp
View File

@ -133,7 +133,7 @@ void AP_OADatabase::update()
}
// push a location into the database
void AP_OADatabase::queue_push(const Location &loc, uint32_t timestamp_ms, float angle, float distance)
void AP_OADatabase::queue_push(const Vector2f &pos, uint32_t timestamp_ms, float distance)
{
if (!healthy()) {
return;
@ -144,7 +144,7 @@ void AP_OADatabase::queue_push(const Location &loc, uint32_t timestamp_ms, float
return;
}
const OA_DbItem item = {loc, timestamp_ms, MAX(_radius_min, distance * dist_to_radius_scalar), 0, AP_OADatabase::OA_DbItemImportance::Normal};
const OA_DbItem item = {pos, timestamp_ms, MAX(_radius_min, distance * dist_to_radius_scalar), 0, AP_OADatabase::OA_DbItemImportance::Normal};
{
WITH_SEMAPHORE(_queue.sem);
_queue.items->push(item);
@ -328,7 +328,8 @@ bool AP_OADatabase::is_close_to_item_in_database(const uint16_t index, const OA_
return false;
}
return (_database.items[index].loc.get_distance(item.loc) < item.radius);
const float distance_sq = (_database.items[index].pos - item.pos).length_squared();
return ((distance_sq < sq(item.radius)) || (distance_sq < sq(_database.items[index].radius)));
}
// send ADSB_VEHICLE mavlink messages
@ -374,12 +375,15 @@ void AP_OADatabase::send_adsb_vehicle(mavlink_channel_t chan, uint16_t interval_
continue;
}
// convert object's position as an offset from EKF origin to Location
const Location item_loc(Vector3f(_database.items[idx].pos.x * 100.0f, _database.items[idx].pos.y * 100.0f, 0));
mavlink_msg_adsb_vehicle_send(chan,
idx,
_database.items[idx].loc.lat,
_database.items[idx].loc.lng,
item_loc.lat,
item_loc.lng,
0, // altitude_type
_database.items[idx].loc.alt,
0, // altitude is always zero
0, // heading
0, // hor_velocity
0, // ver_velocity

10
libraries/AC_Avoidance/AP_OADatabase.h
View File

@ -1,7 +1,7 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/Location.h>
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#if !HAL_MINIMIZE_FEATURES
@ -26,7 +26,7 @@ public:
};
struct OA_DbItem {
Location loc; // location of object. TODO: turn this into Vector2Int to save memory
Vector2f pos; // position of the object as an offset in meters from the EKF origin
uint32_t timestamp_ms; // system time that object was last updated
float radius; // objects radius in meters
uint8_t send_to_gcs; // bitmask of mavlink comports to which details of this object should be sent
@ -36,8 +36,8 @@ public:
void init();
void update();
// push a location into the database
void queue_push(const Location &loc, uint32_t timestamp_ms, float angle, float distance);
// push an object into the database. Pos is the offset in meters from the EKF origin, angle is in degrees, distance in meters
void queue_push(const Vector2f &pos, uint32_t timestamp_ms, float distance);
// returns true if database is healthy
bool healthy() const { return (_queue.items != nullptr) && (_database.items != nullptr); }
@ -116,7 +116,7 @@ class AP_OADatabase {
public:
static AP_OADatabase *get_singleton() { return nullptr; }
void init() {};
void queue_push(const Location &loc, uint32_t timestamp_ms, float angle, float distance) {};
void queue_push(const Vector2f &pos, uint32_t timestamp_ms, float distance) {};
bool healthy() const { return false; }
void send_adsb_vehicle(mavlink_channel_t chan, uint16_t interval_ms) {};
};