ardupilot/libraries/AP_CustomRotations/AP_CustomRotations.cpp

160 lines
4.0 KiB
C++
Raw Normal View History

2021-11-12 17:33:18 -04:00
#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):
2021-11-12 17:33:18 -04:00
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)