Line data    Source code

       1             : #include "AP_CustomRotations_config.h"
       2             : 
       3             : #if AP_CUSTOMROTATIONS_ENABLED
       4             : 
       5             : #include "AP_CustomRotations.h"
       6             : 
       7             : #include <AP_InternalError/AP_InternalError.h>
       8             : #include <AP_Vehicle/AP_Vehicle_Type.h>
       9             : #include <AP_HAL/AP_HAL_Boards.h>
      10             : #include <AP_BoardConfig/AP_BoardConfig.h>
      11             : 
      12             : const AP_Param::GroupInfo AP_CustomRotations::var_info[] = {
      13             : 
      14             :     // @Param: _ENABLE
      15             :     // @DisplayName: Enable Custom rotations
      16             :     // @Values: 0:Disable, 1:Enable
      17             :     // @Description: This enables custom rotations
      18             :     // @User: Standard
      19             :     // @RebootRequired: True
      20             :     AP_GROUPINFO_FLAGS("_ENABLE", 1, AP_CustomRotations, enable, 0, AP_PARAM_FLAG_ENABLE),
      21             : 
      22             :     // @Group: 1_
      23             :     // @Path: AP_CustomRotations_params.cpp
      24             :     AP_SUBGROUPINFO(params[0], "1_", 2, AP_CustomRotations, AP_CustomRotation_params),
      25             : 
      26             :     // @Group: 2_
      27             :     // @Path: AP_CustomRotations_params.cpp
      28             :     AP_SUBGROUPINFO(params[1], "2_", 3, AP_CustomRotations, AP_CustomRotation_params),
      29             : 
      30             :     AP_GROUPEND
      31             : };
      32             : 
      33           2 : AP_CustomRotation::AP_CustomRotation(AP_CustomRotation_params &_params):
      34           2 :     params(_params)
      35             : {
      36           2 :     init();
      37           2 : }
      38             : 
      39          86 : void AP_CustomRotation::init()
      40             : {
      41          86 :     m.from_euler(radians(params.roll),radians(params.pitch),radians(params.yaw));
      42          86 :     q.from_rotation_matrix(m);
      43          86 : }
      44             : 
      45        6300 : AP_CustomRotations::AP_CustomRotations()
      46             : {
      47        2100 :     AP_Param::setup_object_defaults(this, var_info);
      48             : 
      49             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
      50        2097 :     if (singleton != nullptr) {
      51           0 :         AP_HAL::panic("AP_CustomRotations must be singleton");
      52             :     }
      53             : #endif
      54        2100 :     singleton = this;
      55        2100 : }
      56             : 
      57        2086 : void AP_CustomRotations::init()
      58             : {
      59        2086 :     if (enable == 0) {
      60        2086 :         return;
      61             :     }
      62             : 
      63             :     // make sure all custom rotations are allocated
      64           0 :     for (uint8_t i = 0; i < NUM_CUST_ROT; i++) {
      65           0 :         AP_CustomRotation* rot = get_rotation(Rotation(i + ROTATION_CUSTOM_1));
      66           0 :         if (rot == nullptr) {
      67           0 :             AP_BoardConfig::allocation_error("Custom Rotations");
      68             :         }
      69             :     }
      70             : }
      71             : 
      72          85 : void AP_CustomRotations::set(Rotation r, float roll, float pitch, float yaw)
      73             : {
      74          85 :     AP_CustomRotation* rot = get_rotation(r);
      75          85 :     if (rot == nullptr) {
      76           1 :         return;
      77             :     }
      78          84 :     rot->params.roll.set(roll);
      79          84 :     rot->params.pitch.set(pitch);
      80          84 :     rot->params.yaw.set(yaw);
      81          84 :     rot->init();
      82             : }
      83             : 
      84           1 : void AP_CustomRotations::from_rotation(Rotation r, QuaternionD& q)
      85             : {
      86           1 :     AP_CustomRotation* rot = get_rotation(r);
      87           1 :     if (rot == nullptr) {
      88           1 :         return;
      89             :     }
      90           0 :     q = rot->q.todouble();
      91             : }
      92             : 
      93          85 : void AP_CustomRotations::from_rotation(Rotation r, Quaternion& q)
      94             : {
      95          85 :     AP_CustomRotation* rot = get_rotation(r);
      96          85 :     if (rot == nullptr) {
      97           1 :         return;
      98             :     }
      99          84 :     q = rot->q;
     100             : }
     101             : 
     102           1 : void AP_CustomRotations::rotate(Rotation r, Vector3d& v)
     103             : {
     104           1 :     AP_CustomRotation* rot = get_rotation(r);
     105           1 :     if (rot == nullptr) {
     106           1 :         return;
     107             :     }
     108           0 :     v = (rot->m * v.tofloat()).todouble();
     109             : }
     110             : 
     111          85 : void AP_CustomRotations::rotate(Rotation r, Vector3f& v)
     112             : {
     113          85 :     AP_CustomRotation* rot = get_rotation(r);
     114          85 :     if (rot == nullptr) {
     115           1 :         return;
     116             :     }
     117          84 :     v = rot->m * v;
     118             : }
     119             : 
     120         257 : AP_CustomRotation* AP_CustomRotations::get_rotation(Rotation r)
     121             : {
     122         257 :     if (r < ROTATION_CUSTOM_1 || r >= ROTATION_CUSTOM_END) {
     123           5 :         INTERNAL_ERROR(AP_InternalError::error_t::bad_rotation);
     124           5 :         return nullptr;
     125             :     }
     126         252 :     const uint8_t index = r - ROTATION_CUSTOM_1;
     127         252 :     if (rotations[index] == nullptr) {
     128           2 :         rotations[index] = NEW_NOTHROW AP_CustomRotation(params[index]);
     129             :          // make sure param is enabled if custom rotation is used
     130           2 :         enable.set_and_save_ifchanged(1);
     131             :     }
     132         252 :     return rotations[index];
     133             : }
     134             : 
     135             : // singleton instance
     136             : AP_CustomRotations *AP_CustomRotations::singleton;
     137             : 
     138             : namespace AP {
     139             : 
     140         257 : AP_CustomRotations &custom_rotations()
     141             : {
     142         257 :     return *AP_CustomRotations::get_singleton();
     143             : }
     144             : 
     145             : }
     146             : 
     147             : #endif  // AP_CUSTOMROTATIONS_ENABLED