mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_Beacon_Marvelmind: Remove unnecessary structures.
This incorporates the distance into the stationary beacon structure.
This commit is contained in:
parent
ad05a5c694
commit
b0e4a57a93
@ -220,17 +220,27 @@ void AP_Beacon_Marvelmind::process_beacons_positions_highres_datagram()
|
||||
void AP_Beacon_Marvelmind::process_beacons_distances_datagram()
|
||||
{
|
||||
if (32 != input_buffer[4]) {
|
||||
Debug(1, "beacon dist pkt size %d != 32", input_buffer[4]);
|
||||
return; // incorrect size
|
||||
}
|
||||
raw_beacon_distances.address = input_buffer[5]; // Address of hedgehog
|
||||
for (uint8_t i = 0; i < 4; i++) {
|
||||
bool set = false;
|
||||
for (uint8_t i = 0; i < hedge.positions_beacons.num_beacons; i++) {
|
||||
const uint8_t ofs = 6 + i * 6;
|
||||
raw_beacon_distances.beacon[i].address = input_buffer[ofs];
|
||||
raw_beacon_distances.beacon[i].distance = input_buffer[ofs + 1]
|
||||
| (((uint32_t) input_buffer[ofs + 2]) << 8)
|
||||
| (((uint32_t) input_buffer[ofs + 3]) << 16)
|
||||
| (((uint32_t) input_buffer[ofs + 4]) << 24);
|
||||
set_beacon_distance(i, raw_beacon_distances.beacon[i].distance * 0.001f); // millimeters -> meters
|
||||
const uint8_t address = input_buffer[ofs];
|
||||
const int8_t instance = find_beacon_instance(address);
|
||||
if (instance != -1) {
|
||||
const uint32_t distance = input_buffer[ofs + 1]
|
||||
| (((uint32_t) input_buffer[ofs + 2]) << 8)
|
||||
| (((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();
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -54,6 +54,7 @@ private:
|
||||
uint8_t address;
|
||||
int32_t x, y, z;// coordinates in millimeters
|
||||
bool high_resolution;
|
||||
float distance__m; // Distance between beacon and hedge
|
||||
};
|
||||
|
||||
struct StationaryBeaconsPositions
|
||||
@ -63,19 +64,6 @@ private:
|
||||
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
|
||||
{
|
||||
@ -98,7 +86,6 @@ private:
|
||||
} parse_state; // current state of receive data
|
||||
|
||||
MarvelmindHedge *hedge;
|
||||
RawBeaconDistances raw_beacon_distances;
|
||||
PositionValue cur_position;
|
||||
uint8_t input_buffer[AP_BEACON_MARVELMIND_BUF_SIZE];
|
||||
uint16_t num_bytes_in_block_received;
|
||||
|
Loading…
Reference in New Issue
Block a user