mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
34 lines
1.1 KiB
C++
34 lines
1.1 KiB
C++
#include "SIM_config.h"
|
|
|
|
#if AP_SIM_AS5600_ENABLED
|
|
|
|
#include "SIM_AS5600.h"
|
|
|
|
#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
|