Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_AS5600_ENABLED
       4             : 
       5             : #include "SIM_AS5600.h"
       6             : 
       7             : #include <stdio.h>
       8             : 
       9        2094 : void SITL::AS5600::init()
      10             : {
      11        2094 :     add_register("ZMCO", AS5600DevReg::ZMCO, I2CRegisters::RegMode::RDONLY);
      12        2094 :     set_register(AS5600DevReg::ZMCO, (uint8_t)1U);
      13             : 
      14        2094 :     add_register("RAH", AS5600DevReg::RAW_ANGLE_HIGH, I2CRegisters::RegMode::RDONLY);
      15        2094 :     set_register(AS5600DevReg::RAW_ANGLE_HIGH, (uint8_t)0U);
      16        2094 :     add_register("RAL", AS5600DevReg::RAW_ANGLE_LOW, I2CRegisters::RegMode::RDONLY);
      17        2094 :     set_register(AS5600DevReg::RAW_ANGLE_LOW, (uint8_t)0U);
      18        2094 : }
      19             : 
      20   178759942 : void SITL::AS5600::update(const class Aircraft &aircraft)
      21             : {
      22             : 
      23             :     // 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);
      24   178759942 :     Quaternion attitude;
      25   178759942 :     aircraft.get_attitude(attitude);
      26   178759942 :     const float pitch = degrees(attitude.get_euler_pitch());
      27   178759942 :     uint16_t pitch_180 = pitch + 90;
      28             : 
      29   178759942 :     set_register(AS5600DevReg::RAW_ANGLE_HIGH, uint8_t(pitch_180 >> 8));
      30   178759942 :     set_register(AS5600DevReg::RAW_ANGLE_LOW, uint8_t(pitch_180 & 0xff));
      31   178759942 : }
      32             : 
      33             : #endif