add AP_CustomRotations

libraries/AP_CustomRotations/AP_CustomRotations.cpp

@@ -0,0 +1,159 @@
+#include "AP_CustomRotations.h"
+
+#include <AP_InternalError/AP_InternalError.h>
+#include <AP_Vehicle/AP_Vehicle_Type.h>
+#include <AP_HAL/AP_HAL_Boards.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
+
+#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
+
+const AP_Param::GroupInfo AP_CustomRotations::var_info[] = {
+
+    // @Param: _ENABLE
+    // @DisplayName: Enable Custom rotations
+    // @Values: 0:Disable, 1:Enable
+    // @Description: This enables custom rotations
+    // @User: Standard
+    // @RebootRequired: True
+    AP_GROUPINFO_FLAGS("_ENABLE", 1, AP_CustomRotations, enable, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Group: 1_
+    // @Path: AP_CustomRotations_params.cpp
+    AP_SUBGROUPINFO(params[0], "1_", 2, AP_CustomRotations, AP_CustomRotation_params),
+
+    // @Group: 2_
+    // @Path: AP_CustomRotations_params.cpp
+    AP_SUBGROUPINFO(params[1], "2_", 3, AP_CustomRotations, AP_CustomRotation_params),
+
+    AP_GROUPEND
+};
+
+AP_CustomRotation::AP_CustomRotation(AP_CustomRotation_params _params):
+    params(_params)
+{
+    init();
+}
+
+void AP_CustomRotation::init()
+{
+    m.from_euler(radians(params.roll),radians(params.pitch),radians(params.yaw));
+    q.from_rotation_matrix(m);
+}
+
+AP_CustomRotations::AP_CustomRotations()
+{
+    AP_Param::setup_object_defaults(this, var_info);
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    if (singleton != nullptr) {
+        AP_HAL::panic("AP_CustomRotations must be singleton");
+    }
+#endif
+    singleton = this;
+}
+
+void AP_CustomRotations::init()
+{
+    if (enable == 0) {
+        return;
+    }
+
+    // make sure all custom rotations are allocated
+    for (uint8_t i = 0; i < NUM_CUST_ROT; i++) {
+        AP_CustomRotation* rot = get_rotation(Rotation(i + ROTATION_CUSTOM_1));
+        if (rot == nullptr) {
+            AP_BoardConfig::allocation_error("Custom Rotations");
+        }
+    }
+}
+
+void AP_CustomRotations::convert(Rotation r, float roll, float pitch, float yaw)
+{
+    AP_CustomRotation* rot = get_rotation(r);
+    if (rot == nullptr) {
+        return;
+    }
+    if (!rot->params.roll.configured() && !rot->params.pitch.configured() && !rot->params.yaw.configured()) {
+        rot->params.roll.set_and_save(roll);
+        rot->params.pitch.set_and_save(pitch);
+        rot->params.yaw.set_and_save(yaw);
+        rot->init();
+    }
+}
+
+void AP_CustomRotations::set(Rotation r, float roll, float pitch, float yaw)
+{
+    AP_CustomRotation* rot = get_rotation(r);
+    if (rot == nullptr) {
+        return;
+    }
+    rot->params.roll.set(roll);
+    rot->params.pitch.set(pitch);
+    rot->params.yaw.set(yaw);
+    rot->init();
+}
+
+void AP_CustomRotations::from_rotation(Rotation r, QuaternionD& q)
+{
+    AP_CustomRotation* rot = get_rotation(r);
+    if (rot == nullptr) {
+        return;
+    }
+    q = rot->q.todouble();
+}
+
+void AP_CustomRotations::from_rotation(Rotation r, Quaternion& q)
+{
+    AP_CustomRotation* rot = get_rotation(r);
+    if (rot == nullptr) {
+        return;
+    }
+    q = rot->q;
+}
+
+void AP_CustomRotations::rotate(Rotation r, Vector3d& v)
+{
+    AP_CustomRotation* rot = get_rotation(r);
+    if (rot == nullptr) {
+        return;
+    }
+    v = (rot->m * v.tofloat()).todouble();
+}
+
+void AP_CustomRotations::rotate(Rotation r, Vector3f& v)
+{
+    AP_CustomRotation* rot = get_rotation(r);
+    if (rot == nullptr) {
+        return;
+    }
+    v = rot->m * v;
+}
+
+AP_CustomRotation* AP_CustomRotations::get_rotation(Rotation r)
+{
+    if (r < ROTATION_CUSTOM_1 || r >= ROTATION_CUSTOM_END) {
+        INTERNAL_ERROR(AP_InternalError::error_t::bad_rotation);
+        return nullptr;
+    }
+    const uint8_t index = r - ROTATION_CUSTOM_1;
+    if (rotations[index] == nullptr) {
+        rotations[index] = new AP_CustomRotation(params[index]);
+         // make sure param is enabled if custom rotation is used
+        enable.set_and_save_ifchanged(1);
+    }
+    return rotations[index];
+}
+
+// singleton instance
+AP_CustomRotations *AP_CustomRotations::singleton;
+
+namespace AP {
+
+AP_CustomRotations &custom_rotations()
+{
+    return *AP_CustomRotations::get_singleton();
+}
+
+}
+
+#endif // !APM_BUILD_TYPE(APM_BUILD_AP_Periph)

libraries/AP_CustomRotations/AP_CustomRotations.h

@@ -0,0 +1,82 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <AP_Param/AP_Param.h>
+#include <AP_Math/AP_Math.h>
+
+#define NUM_CUST_ROT ROTATION_CUSTOM_END - ROTATION_CUSTOM_1
+
+struct AP_CustomRotation_params {
+public:
+    AP_CustomRotation_params();
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+    AP_Float roll;
+    AP_Float pitch;
+    AP_Float yaw;
+};
+
+class AP_CustomRotation {
+public:
+    AP_CustomRotation(AP_CustomRotation_params _params);
+
+    void init();
+
+    Quaternion q;
+    Matrix3f m;
+
+    AP_CustomRotation_params &params;
+};
+
+class AP_CustomRotations {
+public:
+    AP_CustomRotations();
+
+    CLASS_NO_COPY(AP_CustomRotations);
+
+    static AP_CustomRotations *get_singleton(void) { return singleton; }
+
+    void init();
+
+    void from_rotation(enum Rotation r, QuaternionD &q);
+    void from_rotation(enum Rotation r, Quaternion &q);
+
+    void rotate(enum Rotation r, Vector3d& v);
+    void rotate(enum Rotation r, Vector3f& v);
+
+    void convert(Rotation r, float roll, float pitch, float yaw);
+    void set(Rotation r, float roll, float pitch, float yaw);
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+    AP_Int8 enable;
+
+    AP_CustomRotation* get_rotation(Rotation r);
+
+    AP_CustomRotation* rotations[NUM_CUST_ROT];
+
+    AP_CustomRotation_params params[NUM_CUST_ROT];
+
+    static AP_CustomRotations *singleton;
+};
+
+namespace AP {
+    AP_CustomRotations &custom_rotations();
+};
+

libraries/AP_CustomRotations/AP_CustomRotations_params.cpp

@@ -0,0 +1,36 @@
+#include "AP_CustomRotations.h"
+#include <AP_Vehicle/AP_Vehicle_Type.h>
+
+#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
+
+const AP_Param::GroupInfo AP_CustomRotation_params::var_info[] = {
+
+    // @Param: ROLL
+    // @DisplayName: Custom roll
+    // @Description: Custom euler roll, euler 321 (yaw, pitch, roll) ordering
+    // @Units: deg
+    // @RebootRequired: True
+    AP_GROUPINFO("ROLL",  1, AP_CustomRotation_params, roll, 0),
+
+    // @Param: PITCH
+    // @DisplayName: Custom pitch
+    // @Description: Custom euler pitch, euler 321 (yaw, pitch, roll) ordering
+    // @Units: deg
+    // @RebootRequired: True
+    AP_GROUPINFO("PITCH",  2, AP_CustomRotation_params, pitch, 0),
+
+    // @Param: YAW
+    // @DisplayName: Custom yaw
+    // @Description: Custom euler yaw, euler 321 (yaw, pitch, roll) ordering
+    // @Units: deg
+    // @RebootRequired: True
+    AP_GROUPINFO("YAW",  3, AP_CustomRotation_params, yaw, 0),
+
+    AP_GROUPEND
+};
+
+AP_CustomRotation_params::AP_CustomRotation_params() {
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+#endif // !APM_BUILD_TYPE(APM_BUILD_AP_Periph)