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

SITL: add simulated AS5600 sensor

Browse Source
This commit is contained in:
Peter Barker 2021-01-21 16:15:18 +11:00
parent dd37065bb5
commit db6323f9b4
3 changed files with 97 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/SITL/SIM_AS5600.cpp Normal file
View File

@ -0,0 +1,31 @@
#include "SIM_AS5600.h"
#if AP_SIM_AS5600_ENABLED
#include <stdio.h>
void SITL::AS5600::init()
{
add_register("ZMCO", AS5600DevReg::ZMCO, I2CRegisters::RegMode::RDONLY);
set_register(AS5600DevReg::ZMCO, (uint8_t)1U);
add_register("RAH", AS5600DevReg::RAW_ANGLE_HIGH, I2CRegisters::RegMode::RDONLY);
set_register(AS5600DevReg::RAW_ANGLE_HIGH, (uint8_t)0U);
add_register("RAL", AS5600DevReg::RAW_ANGLE_LOW, I2CRegisters::RegMode::RDONLY);
set_register(AS5600DevReg::RAW_ANGLE_LOW, (uint8_t)0U);
}
void SITL::AS5600::update(const class Aircraft &aircraft)
{
// gcs().send_text(MAV_SEVERITY_INFO, "SIM_AS5600: PWM0=%u PWM1=%u PWM2=%u ENABLE=%u", last_print_pwm0, last_print_pwm1, last_print_pwm2, last_print_enable);
Quaternion attitude;
aircraft.get_attitude(attitude);
const float pitch = degrees(attitude.get_euler_pitch());
uint16_t pitch_180 = pitch + 90;
set_register(AS5600DevReg::RAW_ANGLE_HIGH, uint8_t(pitch_180 >> 8));
set_register(AS5600DevReg::RAW_ANGLE_LOW, uint8_t(pitch_180 & 0xff));
}
#endif

59
libraries/SITL/SIM_AS5600.h Normal file
View File

@ -0,0 +1,59 @@
#include "SIM_I2CDevice.h"
#ifndef AP_SIM_AS5600_ENABLED
#define AP_SIM_AS5600_ENABLED 1
#endif
#if AP_SIM_AS5600_ENABLED
namespace SITL {
// datasheet: https://ams.com/documents/20143/36005/AS5600_DS000365_5-00.pdf
class AS5600DevReg : public I2CRegEnum {
public:
// low
static const uint8_t ZMCO = 0x00;
static const uint8_t ZPOS_HIGH = 0x01;
static const uint8_t ZPOS_LOW = 0x02;
static const uint8_t MPOS_HIGH = 0x03;
static const uint8_t MPOS_LOW = 0x04;
static const uint8_t MANG_HIGH = 0x05;
static const uint8_t MANG_LOW = 0x06;
static const uint8_t CONF_HIGH = 0x07;
static const uint8_t CONF_LOW = 0x08;
// output registers
static const uint8_t RAW_ANGLE_HIGH = 0x0C;
static const uint8_t RAW_ANGLE_LOW = 0x0D;
static const uint8_t ANGLE_HIGH = 0x0E;
static const uint8_t ANGLE_LOW = 0x0F;
// status registers
static const uint8_t STATUS = 0x0B;
static const uint8_t AGC = 0x1A;
static const uint8_t MAGNITUDE_HIGH = 0x1B;
static const uint8_t MAGNITUDE_LOW = 0x1C;
// Burn commands
static const uint8_t BURN = 0xFF;
};
class AS5600 : public I2CDevice, protected I2CRegisters_8Bit
{
public:
void init() override;
void update(const class Aircraft &aircraft) override;
int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override {
return I2CRegisters_8Bit::rdwr(data);
}
private:
};
} // namespace SITL
#endif // AP_SIM_AS5600_ENABLED

7
libraries/SITL/SIM_I2C.cpp
View File

@ -38,6 +38,7 @@
#include "SIM_MS5611.h"
#include "SIM_QMC5883L.h"
#include "SIM_INA3221.h"
#include "SIM_AS5600.h"
#include <signal.h>
@ -89,6 +90,9 @@ static QMC5883L qmc5883l;
#if AP_SIM_INA3221_ENABLED
static INA3221 ina3221;
#endif
#if AP_SIM_AS5600_ENABLED
static AS5600 as5600; // AoA sensor
#endif
struct i2c_device_at_address {
uint8_t bus;
@ -104,6 +108,9 @@ struct i2c_device_at_address {
#endif
{ 1, 0x38, ignored }, // NCP5623
{ 1, 0x39, ignored }, // NCP5623C
#if AP_SIM_AS5600_ENABLED
{ 1, 0x36, as5600 },
#endif
{ 1, 0x40, ignored }, // KellerLD
{ 1, 0x76, ms5525 }, // MS5525: ARSPD_TYPE = 4
#if AP_SIM_INA3221_ENABLED