aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Proximity: SITL driver filles in backend sector data

Browse Source
This commit is contained in:
Randy Mackay 2016-11-15 16:12:45 +09:00
parent f594a5a7d6
commit a56c9545bd
2 changed files with 55 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
libraries/AP_Proximity/AP_Proximity_SITL.cpp
View File

@ -40,8 +40,53 @@ AP_Proximity_SITL::AP_Proximity_SITL(AP_Proximity &_frontend,
} }
} }
// get distance in meters in a particular direction in degrees (0 is forward, angles increase in the clockwise direction) // update the state of the sensor
bool AP_Proximity_SITL::get_horizontal_distance(float angle_deg, float &distance) const void AP_Proximity_SITL::update(void)
{
load_fence();
current_loc.lat = sitl->state.latitude * 1.0e7;
current_loc.lng = sitl->state.longitude * 1.0e7;
current_loc.alt = sitl->state.altitude * 1.0e2;
if (fence && fence_loader.boundary_valid(fence_count->get(), fence, true)) {
// update distance in one sector
if (get_distance_to_fence(_sector_middle_deg[last_sector], _distance[last_sector])) {
set_status(AP_Proximity::Proximity_Good);
_distance_valid[last_sector] = true;
_angle[last_sector] = _sector_middle_deg[last_sector];
update_boundary_for_sector(last_sector);
} else {
_distance_valid[last_sector] = false;
}
last_sector++;
if (last_sector >= _num_sectors) {
last_sector = 0;
}
} else {
set_status(AP_Proximity::Proximity_NoData);
}
}
void AP_Proximity_SITL::load_fence(void)
{
uint32_t now = AP_HAL::millis();
if (now - last_load_ms < 1000) {
return;
}
last_load_ms = now;
if (fence == nullptr) {
fence = (Vector2l *)fence_loader.create_point_array(sizeof(Vector2l));
}
if (fence == nullptr) {
return;
}
for (uint8_t i=0; i<fence_count->get(); i++) {
fence_loader.load_point_from_eeprom(i, fence[i]);
}
}
// get distance in meters to fence in a particular direction in degrees (0 is forward, angles increase in the clockwise direction)
bool AP_Proximity_SITL::get_distance_to_fence(float angle_deg, float &distance) const
{ {
if (!fence_loader.boundary_valid(fence_count->get(), fence, true)) { if (!fence_loader.boundary_valid(fence_count->get(), fence, true)) {
return false; return false;
@ -70,36 +115,4 @@ bool AP_Proximity_SITL::get_horizontal_distance(float angle_deg, float &distance
return true; return true;
} }
// update the state of the sensor
void AP_Proximity_SITL::update(void)
{
load_fence();
current_loc.lat = sitl->state.latitude * 1.0e7;
current_loc.lng = sitl->state.longitude * 1.0e7;
current_loc.alt = sitl->state.altitude * 1.0e2;
if (fence && fence_loader.boundary_valid(fence_count->get(), fence, true)) {
set_status(AP_Proximity::Proximity_Good);
} else {
set_status(AP_Proximity::Proximity_NoData);
}
}
void AP_Proximity_SITL::load_fence(void)
{
uint32_t now = AP_HAL::millis();
if (now - last_load_ms < 1000) {
return;
}
last_load_ms = now;
if (fence == nullptr) {
fence = (Vector2l *)fence_loader.create_point_array(sizeof(Vector2l));
}
if (fence == nullptr) {
return;
}
for (uint8_t i=0; i<fence_count->get(); i++) {
fence_loader.load_point_from_eeprom(i, fence[i]);
}
}
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

13
libraries/AP_Proximity/AP_Proximity_SITL.h
View File

@ -13,10 +13,6 @@ public:
// constructor // constructor
AP_Proximity_SITL(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); AP_Proximity_SITL(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state);
// get distance in meters in a particular direction in degrees (0 is forward, clockwise)
// returns true on successful read and places distance in distance
bool get_horizontal_distance(float angle_deg, float &distance) const override;
// update state // update state
void update(void) override; void update(void) override;
@ -27,7 +23,14 @@ private:
uint32_t last_load_ms; uint32_t last_load_ms;
AC_PolyFence_loader fence_loader; AC_PolyFence_loader fence_loader;
Location current_loc; Location current_loc;
// latest sector updated
uint8_t last_sector;
void load_fence(void); void load_fence(void);
// get distance in meters to fence in a particular direction in degrees (0 is forward, angles increase in the clockwise direction)
bool get_distance_to_fence(float angle_deg, float &distance) const;
}; };
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD