Line data    Source code

       1             : #include <AP_HAL/AP_HAL.h>
       2             : #include "AP_InertialSensor_ExternalAHRS.h"
       3             : #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
       4             : #include <stdio.h>
       5             : 
       6             : #if AP_EXTERNAL_AHRS_ENABLED
       7             : 
       8             : const extern AP_HAL::HAL& hal;
       9             : 
      10           6 : AP_InertialSensor_ExternalAHRS::AP_InertialSensor_ExternalAHRS(AP_InertialSensor &imu, uint8_t _serial_port) :
      11             :     AP_InertialSensor_Backend(imu),
      12           6 :     serial_port(_serial_port)
      13             : {
      14           6 : }
      15             : 
      16       75229 : void AP_InertialSensor_ExternalAHRS::handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt)
      17             : {
      18       75229 :     if (!started) {
      19           0 :         return;
      20             :     }
      21       75229 :     Vector3f accel = pkt.accel;
      22       75229 :     Vector3f gyro = pkt.gyro;
      23             : 
      24       75229 :     _rotate_and_correct_accel(accel_instance, accel);
      25       75229 :     _notify_new_accel_raw_sample(accel_instance, accel, AP_HAL::micros64());
      26             : 
      27       75229 :     _publish_temperature(accel_instance, pkt.temperature);
      28             : 
      29       75229 :     _notify_new_gyro_sensor_rate_sample(gyro_instance, gyro);
      30       75229 :     _rotate_and_correct_gyro(gyro_instance, gyro);
      31       75229 :     _notify_new_gyro_raw_sample(gyro_instance, gyro, AP_HAL::micros64());
      32             : }
      33             : 
      34       38478 : bool AP_InertialSensor_ExternalAHRS::update(void)
      35             : {
      36       38478 :     if (started) {
      37       38478 :         update_accel(accel_instance);
      38       38478 :         update_gyro(gyro_instance);
      39             :     }
      40       38478 :     return started;
      41             : }
      42             : 
      43           6 : void AP_InertialSensor_ExternalAHRS::start()
      44             : {
      45           6 :     const float rate = AP::externalAHRS().get_IMU_rate();
      46           6 :     if (_imu.register_gyro(gyro_instance, rate,
      47          18 :                            AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL, serial_port, 1, DEVTYPE_SERIAL)) &&
      48           6 :         _imu.register_accel(accel_instance, rate,
      49           6 :                             AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL, serial_port, 2, DEVTYPE_SERIAL))) {
      50           6 :         started = true;
      51             :     }
      52           6 : }
      53             : 
      54      562833 : void AP_InertialSensor_ExternalAHRS::accumulate()
      55             : {
      56      562833 :     AP::externalAHRS().update();
      57      562833 : }
      58             : 
      59             : // get a startup banner to output to the GCS
      60           0 : bool AP_InertialSensor_ExternalAHRS::get_output_banner(char* banner, uint8_t banner_len)
      61             : {
      62           0 :     const char* name = AP::externalAHRS().get_name();
      63           0 :     snprintf(banner, banner_len, "IMU%u: External: %s %0.0fHz",
      64           0 :              gyro_instance,
      65             :              (name != nullptr) ? name : "",
      66           0 :               AP::externalAHRS().get_IMU_rate());
      67           0 :     return true;
      68             : }
      69             : 
      70             : #endif // AP_EXTERNAL_AHRS_ENABLED
      71             :