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

SITL: factor out an I2C command/response class from simulated MaxSonar sensor

Browse Source
This commit is contained in:
Peter Barker 2020-12-04 14:14:59 +11:00 committed by Peter Barker
parent 1c19a228ac
commit 6bc8ff2ee8
4 changed files with 67 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/SITL/SIM_I2CDevice.cpp
View File

@ -125,3 +125,37 @@ int SITL::I2CRegisters_8Bit::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
return -1;
};
int SITL::I2CCommandResponseDevice::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 < command_processing_time_ms()) {
// 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 value = reading();
msg.buf[0] = value >> 8;
msg.buf[1] = value & 0xff;
return 0;
}
const uint8_t cmd = msg.buf[0];
if (cmd != command_take_reading()) {
AP_HAL::panic("Unknown command (%u)", cmd);
}
cmd_take_reading_received_ms = now;
return 0;
}

19
libraries/SITL/SIM_I2CDevice.h
View File

@ -50,6 +50,25 @@ protected:
uint8_t byte[256];
};
/*
for devices that take a command then will provide data to a read();
for example, MCU writes 0x07 to device then expects to be able to
read 2 bytes from it.
*/
class I2CCommandResponseDevice {
public:
int rdwr(I2C::i2c_rdwr_ioctl_data *&data);
protected:
// time taken for device to process command:
uint16_t command_processing_time_ms() const { return 20; }
virtual uint8_t command_take_reading() const = 0;
virtual uint16_t reading() const = 0;
uint32_t cmd_take_reading_received_ms;
};
class I2CDevice {
public:
virtual void init() {}

45
libraries/SITL/SIM_MaxSonarI2CXL.cpp
View File

@ -1,45 +0,0 @@
#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();
}

18
libraries/SITL/SIM_MaxSonarI2CXL.h
View File

@ -4,16 +4,26 @@
namespace SITL {
class MaxSonarI2CXL : public I2CDevice
class MaxSonarI2CXL : public I2CDevice, public I2CCommandResponseDevice
{
public:
void update(const class Aircraft &aircraft) override;
// methods to make I2CCommandResponseDevice happy:
uint8_t command_take_reading() const override { return 0x51; }
uint16_t reading() const override {
return rangefinder_range * 100;
};
void update(const class Aircraft &aircraft) override {
rangefinder_range = aircraft.rangefinder_range();
}
int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override {
return I2CCommandResponseDevice::rdwr(data);
}
int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
private:
uint32_t cmd_take_reading_received_ms;
float rangefinder_range;
};