Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include "AP_Compass_DroneCAN.h"
      17             : 
      18             : #if AP_COMPASS_DRONECAN_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : 
      22             : #include <AP_CANManager/AP_CANManager.h>
      23             : #include <AP_DroneCAN/AP_DroneCAN.h>
      24             : #include <AP_BoardConfig/AP_BoardConfig.h>
      25             : #include <AP_Logger/AP_Logger.h>
      26             : #include <SITL/SITL.h>
      27             : 
      28             : AP_Compass_DroneCAN::DetectedModules AP_Compass_DroneCAN::_detected_modules[];
      29             : HAL_Semaphore AP_Compass_DroneCAN::_sem_registry;
      30             : 
      31           0 : AP_Compass_DroneCAN::AP_Compass_DroneCAN(AP_DroneCAN* ap_dronecan, uint32_t devid) :
      32           0 :     _devid(devid)
      33             : {
      34           0 : }
      35             : 
      36           5 : bool AP_Compass_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
      37             : {
      38           5 :     const auto driver_index = ap_dronecan->get_driver_index();
      39             : 
      40           5 :     return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field, driver_index) != nullptr)
      41           5 :         && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_2, driver_index) != nullptr)
      42             : #if AP_COMPASS_DRONECAN_HIRES_ENABLED
      43             :         && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_hires, driver_index) != nullptr)
      44             : #endif
      45             :     ;
      46             : }
      47             : 
      48      167383 : AP_Compass_Backend* AP_Compass_DroneCAN::probe(uint8_t index)
      49             : {
      50      167383 :     AP_Compass_DroneCAN* driver = nullptr;
      51      167383 :     if (!_detected_modules[index].driver && _detected_modules[index].ap_dronecan) {
      52           0 :         WITH_SEMAPHORE(_sem_registry);
      53             :         // Register new Compass mode to a backend
      54           0 :         driver = NEW_NOTHROW AP_Compass_DroneCAN(_detected_modules[index].ap_dronecan, _detected_modules[index].devid);
      55           0 :         if (driver) {
      56           0 :             if (!driver->init()) {
      57           0 :                 delete driver;
      58           0 :                 return nullptr;
      59             :             }
      60           0 :             _detected_modules[index].driver = driver;
      61             : #if AP_TEST_DRONECAN_DRIVERS
      62             :             // Scroll through the registered compasses, and set the offsets
      63             :             if (driver->_compass.get_offsets(index).is_zero()) {
      64             :                 driver->_compass.set_offsets(index, AP::sitl()->mag_ofs[index]);
      65             :             }
      66             : 
      67             :             // we want to simulate a calibrated compass by default, so set
      68             :             // scale to 1
      69             :             AP_Param::set_default_by_name("COMPASS_SCALE", 1);
      70             :             AP_Param::set_default_by_name("COMPASS_SCALE2", 1);
      71             :             AP_Param::set_default_by_name("COMPASS_SCALE3", 1);
      72             :             driver->save_dev_id(index);
      73             :             driver->set_rotation(index, ROTATION_NONE);
      74             : 
      75             :             // make first compass external
      76             :             driver->set_external(index, true);
      77             : #endif
      78             :         }
      79           0 :     }
      80      167383 :     return driver;
      81             : }
      82             : 
      83           0 : bool AP_Compass_DroneCAN::init()
      84             : {
      85             :     // Adding 1 is necessary to allow backward compatibility, where this field was set as 1 by default
      86           0 :     if (!register_compass(_devid)) {
      87           0 :         return false;
      88             :     }
      89           0 :     set_external(true);
      90             : 
      91           0 :     return true;
      92             : }
      93             : 
      94           0 : AP_Compass_DroneCAN* AP_Compass_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id)
      95             : {
      96           0 :     if (ap_dronecan == nullptr) {
      97           0 :         return nullptr;
      98             :     }
      99           0 :     for (uint8_t i=0; i<ARRAY_SIZE(_detected_modules); i++) {
     100           0 :         if (_detected_modules[i].driver &&
     101           0 :             _detected_modules[i].ap_dronecan == ap_dronecan &&
     102           0 :             _detected_modules[i].node_id == node_id &&
     103           0 :             _detected_modules[i].sensor_id == sensor_id) {
     104           0 :             return _detected_modules[i].driver;
     105             :         }
     106             :     }
     107             : 
     108           0 :     bool already_detected = false;
     109             :     // Check if there's an empty spot for possible registration
     110           0 :     for (uint8_t i = 0; i < ARRAY_SIZE(_detected_modules); i++) {
     111           0 :         if (_detected_modules[i].ap_dronecan == ap_dronecan && 
     112           0 :             _detected_modules[i].node_id == node_id &&
     113           0 :             _detected_modules[i].sensor_id == sensor_id) {
     114             :             // Already Detected
     115           0 :             already_detected = true;
     116           0 :             break;
     117             :         }
     118             :     }
     119           0 :     if (!already_detected) {
     120           0 :         for (uint8_t i = 0; i < ARRAY_SIZE(_detected_modules); i++) {
     121           0 :             if (nullptr == _detected_modules[i].ap_dronecan) {
     122           0 :                 _detected_modules[i].ap_dronecan = ap_dronecan;
     123           0 :                 _detected_modules[i].node_id = node_id;
     124           0 :                 _detected_modules[i].sensor_id = sensor_id;
     125           0 :                 _detected_modules[i].devid = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN,
     126           0 :                                                  ap_dronecan->get_driver_index(),
     127             :                                                  node_id,
     128           0 :                                                  sensor_id + 1); // we use sensor_id as devtype
     129           0 :                 break;
     130             :             }
     131             :         }
     132             :     }
     133             : 
     134             :     struct DetectedModules tempslot;
     135             :     // Sort based on the node_id, larger values first
     136             :     // we do this, so that we have repeatable compass
     137             :     // registration, especially in cases of extraneous
     138             :     // CAN compass is connected.
     139           0 :     for (uint8_t i = 1; i < ARRAY_SIZE(_detected_modules); i++) {
     140           0 :         for (uint8_t j = i; j > 0; j--) {
     141           0 :             if (_detected_modules[j].node_id > _detected_modules[j-1].node_id) {
     142           0 :                 tempslot = _detected_modules[j];
     143           0 :                 _detected_modules[j] = _detected_modules[j-1];
     144           0 :                 _detected_modules[j-1] = tempslot;
     145             :             }
     146             :         }
     147             :     }
     148             : 
     149           0 :     return nullptr;
     150             : }
     151             : 
     152           0 : void AP_Compass_DroneCAN::handle_mag_msg(const Vector3f &mag)
     153             : {
     154           0 :     Vector3f raw_field = mag * 1000.0;
     155             : 
     156           0 :     accumulate_sample(raw_field, _instance);
     157           0 : }
     158             : 
     159           0 : void AP_Compass_DroneCAN::handle_magnetic_field(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength& msg)
     160             : {
     161           0 :     WITH_SEMAPHORE(_sem_registry);
     162             : 
     163           0 :     Vector3f mag_vector;
     164           0 :     AP_Compass_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, 0);
     165           0 :     if (driver != nullptr) {
     166           0 :         mag_vector[0] = msg.magnetic_field_ga[0];
     167           0 :         mag_vector[1] = msg.magnetic_field_ga[1];
     168           0 :         mag_vector[2] = msg.magnetic_field_ga[2];
     169           0 :         driver->handle_mag_msg(mag_vector);
     170             :     }
     171           0 : }
     172             : 
     173           0 : void AP_Compass_DroneCAN::handle_magnetic_field_2(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength2 &msg)
     174             : {
     175           0 :     WITH_SEMAPHORE(_sem_registry);
     176             : 
     177           0 :     Vector3f mag_vector;
     178           0 :     uint8_t sensor_id = msg.sensor_id;
     179           0 :     AP_Compass_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, sensor_id);
     180           0 :     if (driver != nullptr) {
     181           0 :         mag_vector[0] = msg.magnetic_field_ga[0];
     182           0 :         mag_vector[1] = msg.magnetic_field_ga[1];
     183           0 :         mag_vector[2] = msg.magnetic_field_ga[2];
     184           0 :         driver->handle_mag_msg(mag_vector);
     185             :     }
     186           0 : }
     187             : 
     188             : #if AP_COMPASS_DRONECAN_HIRES_ENABLED
     189             : /*
     190             :   just log hires magnetic field data for magnetic surveying
     191             :  */
     192             : void AP_Compass_DroneCAN::handle_magnetic_field_hires(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer,
     193             :                                                       const dronecan_sensors_magnetometer_MagneticFieldStrengthHiRes &msg)
     194             : {
     195             : // @LoggerMessage: MAGH
     196             : // @Description: Magnetometer high resolution data
     197             : // @Field: TimeUS: Time since system startup
     198             : // @Field: Node: CAN node
     199             : // @Field: Sensor: sensor ID on node
     200             : // @Field: Bus: CAN bus
     201             : // @Field: Mx: X axis field
     202             : // @Field: My: y axis field
     203             : // @Field: Mz: z axis field
     204             : 
     205             : #if HAL_LOGGING_ENABLED
     206             :     // just log it for now
     207             :     AP::logger().WriteStreaming("MAGH", "TimeUS,Node,Sensor,Bus,Mx,My,Mz", "s#-----", "F------", "QBBBfff",
     208             :                                 transfer.timestamp_usec,
     209             :                                 transfer.source_node_id,
     210             :                                 ap_dronecan->get_driver_index(),
     211             :                                 msg.sensor_id,
     212             :                                 msg.magnetic_field_ga[0]*1000,
     213             :                                 msg.magnetic_field_ga[1]*1000,
     214             :                                 msg.magnetic_field_ga[2]*1000);
     215             : #endif  // HAL_LOGGING_ENABLED
     216             : }
     217             : #endif  // AP_COMPASS_DRONECAN_HIRES_ENABLED
     218             : 
     219             : #endif  // AP_COMPASS_DRONECAN_ENABLED