CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AP_CustomRotations/AP_CustomRotations.h
Views: 1798
1
/*

2
 * This file is free software: you can redistribute it and/or modify it

3
 * under the terms of the GNU General Public License as published by the

4
 * Free Software Foundation, either version 3 of the License, or

5
 * (at your option) any later version.

6
 *

7
 * This file is distributed in the hope that it will be useful, but

8
 * WITHOUT ANY WARRANTY; without even the implied warranty of

9
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.

10
 * See the GNU General Public License for more details.

11
 *

12
 * You should have received a copy of the GNU General Public License along

13
 * with this program.  If not, see <http://www.gnu.org/licenses/>.

14
 */

15
#pragma once

16


17
#include "AP_CustomRotations_config.h"

18


19
#if AP_CUSTOMROTATIONS_ENABLED

20


21
#include <AP_Param/AP_Param.h>

22
#include <AP_Math/AP_Math.h>

23


24
#define NUM_CUST_ROT ROTATION_CUSTOM_END - ROTATION_CUSTOM_1

25


26
struct AP_CustomRotation_params {

27
public:

28
    AP_CustomRotation_params();

29


30
    static const struct AP_Param::GroupInfo var_info[];

31


32
    AP_Float roll;

33
    AP_Float pitch;

34
    AP_Float yaw;

35
};

36


37
class AP_CustomRotation {

38
public:

39
    AP_CustomRotation(AP_CustomRotation_params &_params);

40


41
    void init();

42


43
    Quaternion q;

44
    Matrix3f m;

45


46
    AP_CustomRotation_params &params;

47
};

48


49
class AP_CustomRotations {

50
public:

51
    AP_CustomRotations();

52


53
    CLASS_NO_COPY(AP_CustomRotations);

54


55
    static AP_CustomRotations *get_singleton(void) { return singleton; }

56


57
    void init();

58


59
    void from_rotation(enum Rotation r, QuaternionD &q);

60
    void from_rotation(enum Rotation r, Quaternion &q);

61


62
    void rotate(enum Rotation r, Vector3d& v);

63
    void rotate(enum Rotation r, Vector3f& v);

64


65
    void convert(Rotation r, float roll, float pitch, float yaw);

66
    void set(Rotation r, float roll, float pitch, float yaw);

67


68
    static const struct AP_Param::GroupInfo var_info[];

69


70
private:

71


72
    AP_Int8 enable;

73


74
    AP_CustomRotation* get_rotation(Rotation r);

75


76
    AP_CustomRotation* rotations[NUM_CUST_ROT];

77


78
    AP_CustomRotation_params params[NUM_CUST_ROT];

79


80
    static AP_CustomRotations *singleton;

81
};

82


83
namespace AP {

84
    AP_CustomRotations &custom_rotations();

85
};

86


87


88
#endif  // AP_CUSTOMROTATIONS_ENABLED

89


90