AP_Simulink: simulink support for ArduCopter-Renaming AC_Simulink to AP_Simulink

Author: amathamk

libraries/AP_Simulink/AC_Simulink_AHRSHandle.cpp

@@ -0,0 +1,47 @@
+// @file    AC_Simulink_AHRSHandle.cpp
+// @brief   This class provides AP_AHRS_View class instance and enables access to AP_AHRS_View methods from Simulink-generated code.
+#include "AC_Simulink_AHRSHandle.h"
+
+AC_Simulink_AHRS *AC_Simulink_AHRS::_singleton = nullptr;
+
+void AC_Simulink_AHRS::initializeInterfaces(AP_AHRS_View *&ahrs)
+{
+    if (_singleton == nullptr) {
+        _singleton = new AC_Simulink_AHRS(ahrs);
+    }
+}
+
+AP_AHRS_View *&AC_Simulink_AHRS::getHandle_AP_AHRS_View() const
+{
+    return _ahrs;
+}
+
+// get singleton instance
+AC_Simulink_AHRS *AC_Simulink_AHRS::get_singleton()
+{
+    return _singleton;
+}
+
+AC_Simulink_AHRS::AC_Simulink_AHRS(AP_AHRS_View *&ahrs) : _ahrs(ahrs)
+{
+}
+void AC_Simulink_AHRS::init(void)
+{
+}
+Vector3f AC_Simulink_AHRS::getAngularVelocity(void)
+{
+    return _ahrs->get_gyro_latest();
+}
+void AC_Simulink_AHRS::getAttitudeQuat(Quaternion &attQuat)
+{
+    _ahrs->get_quat_body_to_ned(attQuat);
+}
+bool AC_Simulink_AHRS::getPositionNED(Vector3f &posNED)
+{
+    return _ahrs->get_relative_position_NED_origin(posNED);
+}
+bool AC_Simulink_AHRS::getVelocityNED(Vector3f &velNED)
+{
+    return _ahrs->get_velocity_NED(velNED);
+}
+AC_Simulink_AHRS *sl_ahrs_handle = AC_Simulink_AHRS::get_singleton();

libraries/AP_Simulink/AC_Simulink_AHRSHandle.h

@@ -0,0 +1,40 @@
+#pragma once
+// @file    AC_Simulink_AHRSHandle.h
+// @brief   This class provides AP_AHRS_View class instance and enables access to AP_AHRS_View methods from Simulink-generated code.
+#include <AP_Common/AP_Common.h>
+#include <AP_AHRS/AP_AHRS_View.h>
+
+class AC_Simulink_AHRS
+{
+public:
+    static void initializeInterfaces(AP_AHRS_View *&ahrs);
+
+    AP_AHRS_View *&getHandle_AP_AHRS_View() const ;
+
+    // get singleton instance
+    static AC_Simulink_AHRS *get_singleton();
+
+    // Functions specific to Simulink blocks.
+    // The following functions are invoked by Simulink-generated code when the corresponding blocks are added to the model.
+
+    void init(void);
+    // get estimated angular velocity
+    Vector3f getAngularVelocity();
+    // get estimated attitude in Quaternion
+    void getAttitudeQuat(Quaternion &attQuat);
+    // get estimated position in NED frame
+    bool getPositionNED(Vector3f &posNED);
+    // get estimated velocity in NED frame
+    bool getVelocityNED(Vector3f &velNED);
+
+protected:
+    // References to external libraries
+    AP_AHRS_View *&_ahrs;
+
+private:
+    AC_Simulink_AHRS(AP_AHRS_View *&ahrs) ;
+
+    static AC_Simulink_AHRS *_singleton;
+};
+
+extern AC_Simulink_AHRS *sl_ahrs_handle;

libraries/AP_Simulink/AC_Simulink_AttControlHandle.cpp

@@ -0,0 +1,39 @@
+// @file    AC_Simulink_AttControlHandle.cpp
+// @brief   This class provides AC_PosControl class instance and enables access to AC_PosControl methods from Simulink-generated code.
+#include "AC_Simulink_AttControlHandle.h"
+
+AC_Simulink_AttControlHandle *AC_Simulink_AttControlHandle::_singleton = nullptr;
+
+void AC_Simulink_AttControlHandle::initializeInterfaces(AC_AttitudeControl *&att_control)
+{
+    if (_singleton == nullptr) {
+        _singleton = new AC_Simulink_AttControlHandle(att_control);
+    }
+}
+
+AC_AttitudeControl *&AC_Simulink_AttControlHandle::getHandle_AC_AttControl() const
+{
+    return _att_control;
+}
+
+// get singleton instance
+AC_Simulink_AttControlHandle *AC_Simulink_AttControlHandle::get_singleton()
+{
+    return _singleton;
+}
+
+AC_Simulink_AttControlHandle::AC_Simulink_AttControlHandle(AC_AttitudeControl *&att_control) : _att_control(att_control)
+{
+}
+void AC_Simulink_AttControlHandle::init(void)
+{
+}
+Quaternion AC_Simulink_AttControlHandle::getAttitudeSetpointQuat(void)
+{
+    return _att_control->get_attitude_target_quat();
+}
+Vector3f AC_Simulink_AttControlHandle::getAngularVelocitySetpoint(void)
+{
+    return _att_control->get_attitude_target_ang_vel();
+}
+AC_Simulink_AttControlHandle *sl_attControl_handle = AC_Simulink_AttControlHandle::get_singleton();

libraries/AP_Simulink/AC_Simulink_AttControlHandle.h

@@ -0,0 +1,35 @@
+#pragma once
+// @file    AC_Simulink_AttControlHandle.h
+// @brief   This class provides AC_AttitudeControl class instance and enables access to AC_AttitudeControl methods from Simulink-generated code.
+#include <AP_Common/AP_Common.h>
+#include <AC_AttitudeControl/AC_AttitudeControl.h>
+
+class AC_Simulink_AttControlHandle
+{
+public:
+    static void initializeInterfaces(AC_AttitudeControl *&att_control);
+
+    AC_AttitudeControl *&getHandle_AC_AttControl() const ;
+
+    // get singleton instance
+    static AC_Simulink_AttControlHandle *get_singleton();
+
+    // Functions specific to Simulink blocks.
+    // The following functions are invoked by Simulink-generated code when the corresponding blocks are added to the model.
+    void init(void);
+    // get attitude setpoint in quaternion
+    Quaternion getAttitudeSetpointQuat(void);
+    // get angular velocity setpoint in quaternion
+    Vector3f getAngularVelocitySetpoint(void);
+
+protected:
+    // References to external libraries
+    AC_AttitudeControl *&_att_control;
+
+private:
+    AC_Simulink_AttControlHandle(AC_AttitudeControl *&att_control) ;
+
+    static AC_Simulink_AttControlHandle *_singleton;
+};
+
+extern AC_Simulink_AttControlHandle *sl_attControl_handle;

libraries/AP_Simulink/AC_Simulink_ControllerInterfaces.cpp

@@ -0,0 +1,18 @@
+/// @file    AC_Simulink_ControllerInterfaces.cpp
+/// @brief This class is responsible for storing instances of the AHRS, Attitude & Position Controller,
+///        and Motor classes. These instances are provided by the copter class and stored for access by Simulink-generated code.
+#include <AP_HAL/AP_HAL.h>
+#include "AC_Simulink_ControllerInterfaces.h"
+#include "AC_Simulink_AHRSHandle.h"
+#include "AC_Simulink_AttControlHandle.h"
+#include "AC_Simulink_PosControlHandle.h"
+#include "AC_Simulink_MotorsHandle.h"
+
+AC_Simulink_ControllerInterfaces::AC_Simulink_ControllerInterfaces(AP_AHRS_View *&ahrs,
+        AC_AttitudeControl *&att_control, AC_PosControl *&pos_control, AP_MotorsMulticopter *&motors)
+{
+    AC_Simulink_AHRS::initializeInterfaces(ahrs);
+    AC_Simulink_AttControlHandle::initializeInterfaces(att_control);
+    AC_Simulink_PosControlHandle::initializeInterfaces(pos_control);
+    AC_Simulink_MotorsHandle::initializeInterfaces(motors);
+}

libraries/AP_Simulink/AC_Simulink_ControllerInterfaces.h

@@ -0,0 +1,19 @@
+#pragma once
+/// @file    AC_Simulink_ControllerInterfaces.h
+/// @brief This class is responsible for storing instances of the AHRS, Attitude & Position Controller,
+///        and Motor classes. These instances are provided by the copter class and stored for access by Simulink-generated code.
+#include <AP_Common/AP_Common.h>
+#include <AP_AHRS/AP_AHRS_View.h>
+#include <AC_AttitudeControl/AC_AttitudeControl.h>
+#include <AC_AttitudeControl/AC_PosControl.h>
+#include <AP_Motors/AP_MotorsMulticopter.h>
+
+class AC_Simulink_ControllerInterfaces
+{
+public:
+    AC_Simulink_ControllerInterfaces(AP_AHRS_View *&ahrs,
+                                     AC_AttitudeControl *&_att_control, AC_PosControl *&_pos_control,
+                                     AP_MotorsMulticopter *&motors);
+
+    CLASS_NO_COPY(AC_Simulink_ControllerInterfaces);
+};

libraries/AP_Simulink/AC_Simulink_MotorsHandle.cpp

@@ -0,0 +1,42 @@
+// @file    AC_Simulink_MotorsHandle.cpp
+// @brief   This class provides AC_PosControl class instance and enables access to AC_PosControl methods from Simulink-generated code.
+#include "AC_Simulink_MotorsHandle.h"
+
+AC_Simulink_MotorsHandle *AC_Simulink_MotorsHandle::_singleton = nullptr;
+
+void AC_Simulink_MotorsHandle::initializeInterfaces(AP_MotorsMulticopter *&motors)
+{
+    if (_singleton == nullptr) {
+        _singleton = new AC_Simulink_MotorsHandle(motors);
+    }
+}
+
+AP_MotorsMulticopter *&AC_Simulink_MotorsHandle::getHandle_AP_Motors() const
+{
+    return _motors;
+}
+
+// get singleton instance
+AC_Simulink_MotorsHandle *AC_Simulink_MotorsHandle::get_singleton()
+{
+    return _singleton;
+}
+
+AC_Simulink_MotorsHandle::AC_Simulink_MotorsHandle(AP_MotorsMulticopter *&motors) : _motors(motors)
+{
+}
+void AC_Simulink_MotorsHandle::init(void)
+{
+}
+void AC_Simulink_MotorsHandle::setTorque(float tauRoll,float tauPitch,float tauYaw)
+{
+    _motors->set_roll(tauRoll);
+    _motors->set_pitch(tauPitch);
+    _motors->set_yaw(tauYaw);
+}
+void AC_Simulink_MotorsHandle::setThrust(float throttle)
+{
+    _motors->set_throttle(throttle);
+}
+
+AC_Simulink_MotorsHandle *sl_motors_handle = AC_Simulink_MotorsHandle::get_singleton();

libraries/AP_Simulink/AC_Simulink_MotorsHandle.h

@@ -0,0 +1,35 @@
+#pragma once
+// @file    AC_Simulink_MotorsHandle.h
+// @brief   This class provides AP_MotorsMulticopter class instance and enables access to AP_MotorsMulticopter methods from Simulink-generated code.
+#include <AP_Common/AP_Common.h>
+#include <AP_Motors/AP_MotorsMulticopter.h>
+
+class AC_Simulink_MotorsHandle
+{
+public:
+    static void initializeInterfaces(AP_MotorsMulticopter *&motors);
+
+    AP_MotorsMulticopter *&getHandle_AP_Motors() const ;
+
+    // get singleton instance
+    static AC_Simulink_MotorsHandle *get_singleton();
+
+    // Functions specific to Simulink blocks.
+    // The following functions are invoked by Simulink-generated code when the corresponding blocks are added to the model.
+    void init(void);
+    // set roll, pitch and yaw torue setpoint
+    void setTorque(float tauRoll,float tauPitch,float tauYaw);
+    // set vertical thrust
+    void setThrust(float throttle);
+
+protected:
+    // References to external libraries
+    AP_MotorsMulticopter *&_motors;
+
+private:
+    AC_Simulink_MotorsHandle(AP_MotorsMulticopter *&motors) ;
+
+    static AC_Simulink_MotorsHandle *_singleton;
+};
+
+extern AC_Simulink_MotorsHandle *sl_motors_handle;

libraries/AP_Simulink/AC_Simulink_PosControlHandle.cpp

@@ -0,0 +1,40 @@
+// @file    AC_Simulink_PosControlHandle.h
+// @brief   This class provides AC_PosControl class instance and enables access to AC_PosControl methods from Simulink-generated code.
+#include "AC_Simulink_PosControlHandle.h"
+
+AC_Simulink_PosControlHandle *AC_Simulink_PosControlHandle::_singleton = nullptr;
+
+void AC_Simulink_PosControlHandle::initializeInterfaces(AC_PosControl *&pos_control)
+{
+    if (_singleton == nullptr) {
+        _singleton = new AC_Simulink_PosControlHandle(pos_control);
+    }
+}
+
+AC_PosControl *&AC_Simulink_PosControlHandle::getHandle_AC_PosControl() const
+{
+    return _pos_control;
+}
+
+// get singleton instance
+AC_Simulink_PosControlHandle *AC_Simulink_PosControlHandle::get_singleton()
+{
+    return _singleton;
+}
+
+AC_Simulink_PosControlHandle::AC_Simulink_PosControlHandle(AC_PosControl *&pos_control) : _pos_control(pos_control)
+{
+}
+void AC_Simulink_PosControlHandle::init(void)
+{
+}
+Vector3p AC_Simulink_PosControlHandle::getPositionSetPointNEU(void)
+{
+    return _pos_control->get_pos_target_cm();
+}
+Vector3f AC_Simulink_PosControlHandle::getVelocitySetPointNEU(void)
+{
+    return _pos_control->get_vel_desired_cms();
+}
+
+AC_Simulink_PosControlHandle *sl_posControl_handle = AC_Simulink_PosControlHandle::get_singleton();

libraries/AP_Simulink/AC_Simulink_PosControlHandle.h

@@ -0,0 +1,34 @@
+#pragma once
+// @file    AC_Simulink_PosControlHandle.h
+// @brief   This class provides AC_PosControl class instance and enables access to AC_PosControl methods from Simulink-generated code.
+#include <AP_Common/AP_Common.h>
+#include <AC_AttitudeControl/AC_PosControl.h>
+
+class AC_Simulink_PosControlHandle
+{
+public:
+    static void initializeInterfaces(AC_PosControl *&pos_control);
+
+    AC_PosControl *&getHandle_AC_PosControl() const ;
+
+    // get singleton instance
+    static AC_Simulink_PosControlHandle *get_singleton();
+
+    // Functions specific to Simulink blocks.
+    // The following functions are invoked by Simulink-generated code when the corresponding blocks are added to the model.
+    void init(void);
+    // get position setpoint in NEU frame
+    Vector3p getPositionSetPointNEU(void);
+    // get velocity setpoint in NEU frame
+    Vector3f getVelocitySetPointNEU(void);
+protected:
+    // References to external libraries
+    AC_PosControl *&_pos_control;
+
+private:
+    AC_Simulink_PosControlHandle(AC_PosControl *&pos_control) ;
+
+    static AC_Simulink_PosControlHandle *_singleton;
+};
+
+extern AC_Simulink_PosControlHandle *sl_posControl_handle;