Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Beacon_Marvelmind: Remove unnecessary structures.

Browse Source 
This incorporates the distance into the stationary beacon structure.
This commit is contained in:
Karthik Desai 2018-05-18 15:20:15 +02:00 committed by Francisco Ferreira
parent ad05a5c694
commit b0e4a57a93
2 changed files with 19 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp
View File

@ -220,17 +220,27 @@ void AP_Beacon_Marvelmind::process_beacons_positions_highres_datagram()
void AP_Beacon_Marvelmind::process_beacons_distances_datagram() void AP_Beacon_Marvelmind::process_beacons_distances_datagram()
{ {
if (32 != input_buffer[4]) { if (32 != input_buffer[4]) {
Debug(1, "beacon dist pkt size %d != 32", input_buffer[4]);
return; // incorrect size return; // incorrect size
} }
raw_beacon_distances.address = input_buffer[5]; // Address of hedgehog bool set = false;
for (uint8_t i = 0; i < 4; i++) { for (uint8_t i = 0; i < hedge.positions_beacons.num_beacons; i++) {
const uint8_t ofs = 6 + i * 6; const uint8_t ofs = 6 + i * 6;
raw_beacon_distances.beacon[i].address = input_buffer[ofs]; const uint8_t address = input_buffer[ofs];
raw_beacon_distances.beacon[i].distance = input_buffer[ofs + 1] const int8_t instance = find_beacon_instance(address);
| (((uint32_t) input_buffer[ofs + 2]) << 8) if (instance != -1) {
| (((uint32_t) input_buffer[ofs + 3]) << 16) const uint32_t distance = input_buffer[ofs + 1]
| (((uint32_t) input_buffer[ofs + 4]) << 24); | (((uint32_t) input_buffer[ofs + 2]) << 8)
set_beacon_distance(i, raw_beacon_distances.beacon[i].distance * 0.001f); // millimeters -> meters | (((uint32_t) input_buffer[ofs + 3]) << 16)
| (((uint32_t) input_buffer[ofs + 4]) << 24);
hedge.positions_beacons.beacons[instance].distance__m = distance * 0.001f; // millimeters -> meters
set_beacon_distance(instance, hedge.positions_beacons.beacons[instance].distance__m);
set = true;
Debug(2, "Beacon %d is %.2fm", instance, hedge.positions_beacons.beacons[instance].distance__m);
}
}
if (set) {
last_update_ms = AP_HAL::millis();
} }
} }

15
libraries/AP_Beacon/AP_Beacon_Marvelmind.h
View File

@ -54,6 +54,7 @@ private:
uint8_t address; uint8_t address;
int32_t x, y, z;// coordinates in millimeters int32_t x, y, z;// coordinates in millimeters
bool high_resolution; bool high_resolution;
float distance__m; // Distance between beacon and hedge
}; };
struct StationaryBeaconsPositions struct StationaryBeaconsPositions
@ -63,19 +64,6 @@ private:
bool updated; bool updated;
}; };
struct DistanceToBeacon
{
uint8_t address; ///< Address of beacon (0 if item not filled)
uint32_t distance; ///< Distance to the beacon in millimeters
uint8_t reserved; ///< reserved (0)
};
struct RawBeaconDistances
{
uint8_t address; ///< Address of hedgehog
DistanceToBeacon beacon[4]; ///< Distance to beacon
uint8_t reserved[7]; ///< reserved
};
struct MarvelmindHedge struct MarvelmindHedge
{ {
@ -98,7 +86,6 @@ private:
} parse_state; // current state of receive data } parse_state; // current state of receive data
MarvelmindHedge *hedge; MarvelmindHedge *hedge;
RawBeaconDistances raw_beacon_distances;
PositionValue cur_position; PositionValue cur_position;
uint8_t input_buffer[AP_BEACON_MARVELMIND_BUF_SIZE]; uint8_t input_buffer[AP_BEACON_MARVELMIND_BUF_SIZE];
uint16_t num_bytes_in_block_received; uint16_t num_bytes_in_block_received;