From 7d42c3c834570e8f4c836b67ae29d3766f406ce9 Mon Sep 17 00:00:00 2001
From: Michelle Rossouw <64898873+MichelleRos@users.noreply.github.com>
Date: Thu, 18 Mar 2021 14:12:54 +1100
Subject: [PATCH] SITL: Added simple blimp simulator

---
 libraries/SITL/SIM_Blimp.cpp | 66 ++++++++++++++++++++++++++++++++++++
 libraries/SITL/SIM_Blimp.h   | 50 +++++++++++++++++++++++++++
 2 files changed, 116 insertions(+)
 create mode 100644 libraries/SITL/SIM_Blimp.cpp
 create mode 100644 libraries/SITL/SIM_Blimp.h

diff --git a/libraries/SITL/SIM_Blimp.cpp b/libraries/SITL/SIM_Blimp.cpp
new file mode 100644
index 0000000000..06e8d50dc2
--- /dev/null
+++ b/libraries/SITL/SIM_Blimp.cpp
@@ -0,0 +1,66 @@
+/*
+   This program 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 program 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/>.
+ */
+/*
+  Blimp simulator class
+*/
+
+#include "SIM_Blimp.h"
+#include <AP_Motors/AP_Motors.h>
+
+#include <stdio.h>
+
+using namespace SITL;
+
+Blimp::Blimp(const char *frame_str) :
+    Aircraft(frame_str)
+{
+    frame_height = 0.0;
+    ground_behavior = GROUND_BEHAVIOR_NONE;
+}
+
+// calculate rotational and linear accelerations
+void Blimp::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
+{
+    // float fin_back  = filtered_servo_angle(input, 0);
+    // float fin_front = filtered_servo_angle(input, 1);
+    // float fin_right = filtered_servo_angle(input, 2);
+    // float fin_left  = filtered_servo_angle(input, 3);
+
+    // ::printf("FINS (%.1f %.1f %.1f %.1f)\n",
+    //          fin_back, fin_front, fin_right, fin_left);
+}
+
+/*
+  update the blimp simulation by one time step
+ */
+void Blimp::update(const struct sitl_input &input)
+{
+    // get wind vector setup
+    update_wind(input);
+
+    Vector3f rot_accel;
+
+    calculate_forces(input, rot_accel, accel_body);
+
+    update_dynamics(rot_accel);
+    update_external_payload(input);
+
+    // update lat/lon/altitude
+    update_position();
+    time_advance();
+
+    // update magnetic field
+    update_mag_field_bf();
+}
diff --git a/libraries/SITL/SIM_Blimp.h b/libraries/SITL/SIM_Blimp.h
new file mode 100644
index 0000000000..eba91f2954
--- /dev/null
+++ b/libraries/SITL/SIM_Blimp.h
@@ -0,0 +1,50 @@
+/*
+   This program 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 program 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/>.
+ */
+/*
+  ROV/AUV/Blimp simulator class
+*/
+
+#pragma once
+
+#include "SIM_Aircraft.h"
+#include "SIM_Motor.h"
+#include "SIM_Frame.h"
+
+namespace SITL {
+
+/*
+  a blimp simulator
+ */
+
+class Blimp : public Aircraft {
+public:
+    Blimp(const char *frame_str);
+
+    /* update model by one time step */
+    void update(const struct sitl_input &input) override;
+
+    /* static object creator */
+    static Aircraft *create(const char *frame_str) {
+        return new Blimp(frame_str);
+    }
+
+protected:
+    const struct {
+        float mass = 0.02;
+    } model;
+
+    void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
+};
+}