mirror of https://github.com/ArduPilot/ardupilot
46 lines
1.2 KiB
C++
46 lines
1.2 KiB
C++
#include "SIM_MaxSonarI2CXL.h"
|
|
|
|
#include <SITL/SITL.h>
|
|
#include <SITL/SIM_Aircraft.h>
|
|
|
|
#include <stdio.h>
|
|
|
|
int SITL::MaxSonarI2CXL::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
|
|
{
|
|
const uint32_t now = AP_HAL::millis();
|
|
|
|
struct I2C::i2c_msg &msg = data->msgs[0];
|
|
if (msg.flags == I2C_M_RD) {
|
|
// driver is attempting to receive reading...
|
|
if (now - cmd_take_reading_received_ms < 20) {
|
|
// not sure we ought to be returning -1 here - what does
|
|
// the real device do? return stale data? garbage data?
|
|
return -1;
|
|
}
|
|
if (msg.len != 2) {
|
|
AP_HAL::panic("Unxpected message length (%u)", msg.len);
|
|
}
|
|
|
|
const uint16_t range_cm = rangefinder_range * 100;
|
|
|
|
msg.buf[0] = range_cm >> 8;
|
|
msg.buf[1] = range_cm & 0xff;
|
|
|
|
return 0;
|
|
}
|
|
|
|
const uint8_t CMD_TAKE_READING = 0x51;
|
|
const uint8_t cmd = msg.buf[0];
|
|
if (cmd != CMD_TAKE_READING) {
|
|
AP_HAL::panic("Unknown command (%u)", cmd);
|
|
}
|
|
cmd_take_reading_received_ms = now;
|
|
|
|
return 0;
|
|
};
|
|
|
|
void SITL::MaxSonarI2CXL::update(const Aircraft &aircraft)
|
|
{
|
|
rangefinder_range = aircraft.rangefinder_range();
|
|
}
|