SITL: added StratoBlimp simulator

libraries/SITL/SIM_Aircraft.cpp

@@ -628,6 +628,12 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
     gyro.y = constrain_float(gyro.y, -radians(2000.0f), radians(2000.0f));
     gyro.z = constrain_float(gyro.z, -radians(2000.0f), radians(2000.0f));
 
+    // limit body accel to 64G
+    const float accel_limit = 64*GRAVITY_MSS;
+    accel_body.x = constrain_float(accel_body.x, -accel_limit, accel_limit);
+    accel_body.y = constrain_float(accel_body.y, -accel_limit, accel_limit);
+    accel_body.z = constrain_float(accel_body.z, -accel_limit, accel_limit);
+
     // update attitude
     dcm.rotate(gyro * delta_time);
     dcm.normalize();
@@ -675,7 +681,7 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
         // get speed of ground movement (for ship takeoff/landing)
         float yaw_rate = 0;
 #if AP_SIM_SHIP_ENABLED
-        const Vector2f ship_movement = sitl->shipsim.get_ground_speed_adjustment(location, yaw_rate);
+        const Vector2f ship_movement = sitl->models.shipsim.get_ground_speed_adjustment(location, yaw_rate);
         const Vector3f gnd_movement(ship_movement.x, ship_movement.y, 0);
 #else
         const Vector3f gnd_movement;
@@ -1024,7 +1030,7 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
     }
 
 #if AP_SIM_SHIP_ENABLED
-    sitl->shipsim.update();
+    sitl->models.shipsim.update();
 #endif
 
     // update IntelligentEnergy 2.4kW generator
@@ -1167,3 +1173,14 @@ Vector3d Aircraft::get_position_relhome() const
     pos.xy() += home.get_distance_NE_double(origin);
     return pos;
 }
+
+// get air density in kg/m^3
+float Aircraft::get_air_density(float alt_amsl) const
+{
+    float sigma, delta, theta;
+
+    AP_Baro::SimpleAtmosphere(alt_amsl * 0.001f, sigma, delta, theta);
+
+    const float air_pressure = SSL_AIR_PRESSURE * delta;
+    return air_pressure / (ISA_GAS_CONSTANT * SSL_AIR_TEMPERATURE);
+}

libraries/SITL/SIM_Aircraft.h

@@ -128,6 +128,9 @@ public:
     // get position relative to home
     Vector3d get_position_relhome() const;
 
+    // get air density in kg/m^3
+    float get_air_density(float alt_amsl) const;
+
     // distance the rangefinder is perceiving
     float rangefinder_range() const;
 

libraries/SITL/SIM_Precland.cpp

@@ -150,7 +150,7 @@ void SIM_Precland::update(const Location &loc)
          */
         auto *sitl = AP::sitl();
         Location shiploc;
-        if (sitl != nullptr && sitl->shipsim.get_location(shiploc) && !shiploc.is_zero()) {
+        if (sitl != nullptr && sitl->models.shipsim.get_location(shiploc) && !shiploc.is_zero()) {
             shiploc.change_alt_frame(Location::AltFrame::ABOVE_ORIGIN);
             device_center = shiploc;
         }

libraries/SITL/SIM_StratoBlimp.cpp

@@ -0,0 +1,300 @@
+/*
+   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/>.
+ */
+/*
+  StratoBlimp simulator class
+*/
+
+#include "SIM_StratoBlimp.h"
+
+#if AP_SIM_STRATOBLIMP_ENABLED
+
+#include <AP_Logger/AP_Logger.h>
+#include <AP_Motors/AP_Motors.h>
+
+#include <stdio.h>
+
+using namespace SITL;
+
+extern const AP_HAL::HAL& hal;
+
+// SITL Ship parameters
+const AP_Param::GroupInfo StratoBlimp::var_info[] = {
+    // @Param: MASS
+    // @DisplayName: mass
+    // @Description: mass of blimp not including lifting gas
+    // @Units: kg
+    AP_GROUPINFO("MASS",     1, StratoBlimp,  mass, 80),
+
+    // @Param: HMASS
+    // @DisplayName: helium mass
+    // @Description: mass of lifting gas
+    // @Units: kg
+    AP_GROUPINFO("HMASS",    2, StratoBlimp,  helium_mass, 13.54),
+
+    // @Param: ARM_LEN
+    // @DisplayName: arm length
+    // @Description: distance from center of mass to one motor
+    // @Units: m
+    AP_GROUPINFO("ARM_LEN",  3, StratoBlimp,  arm_length, 3.6),
+
+    // @Param: MOT_THST
+    // @DisplayName: motor thrust
+    // @Description: thrust at max throttle for one motor
+    // @Units: N
+    AP_GROUPINFO("MOT_THST", 4, StratoBlimp,  motor_thrust, 145),
+
+    // @Param: DRAG_FWD
+    // @DisplayName: drag in forward direction
+    // @Description: drag on X axis
+    AP_GROUPINFO("DRAG_FWD", 5, StratoBlimp,  drag_fwd, 0.27),
+
+    // @Param: DRAG_FWD
+    // @DisplayName: drag in upward direction
+    // @Description: drag on Z axis
+    AP_GROUPINFO("DRAG_UP",  6, StratoBlimp,  drag_up, 0.1),
+
+    // @Param: MOI_YAW
+    // @DisplayName: moment of inertia in yaw
+    // @Description: moment of inertia in yaw
+    AP_GROUPINFO("MOI_YAW",  7, StratoBlimp,  moi_yaw, 2800),
+
+    // @Param: MOI_ROLL
+    // @DisplayName: moment of inertia in roll
+    // @Description: moment of inertia in roll
+    AP_GROUPINFO("MOI_ROLL", 8, StratoBlimp,  moi_roll, 1400),
+
+    // @Param: MOI_PITCH
+    // @DisplayName: moment of inertia in pitch
+    // @Description: moment of inertia in pitch
+    AP_GROUPINFO("MOI_PITCH", 9, StratoBlimp,  moi_pitch, 3050),
+    
+    // @Param: ALT_TARG
+    // @DisplayName: altitude target
+    // @Description: altitude target
+    // @Units: m
+    AP_GROUPINFO("ALT_TARG", 10, StratoBlimp,  altitude_target, 20000),
+
+    // @Param: CLMB_RT
+    // @DisplayName: climb rate
+    // @Description: climb rate
+    // @Units: m/s
+    AP_GROUPINFO("CLMB_RT",  11, StratoBlimp, climb_rate, 5),
+
+    // @Param: YAW_RT
+    // @DisplayName: yaw rate
+    // @Description: maximum yaw rate with full left throttle at target altitude
+    // @Units: deg/s
+    AP_GROUPINFO("YAW_RT",   12, StratoBlimp, yaw_rate_max, 60),
+
+    // @Param: MOT_ANG
+    // @DisplayName: motor angle
+    // @Description: maximum motor tilt angle
+    // @Units: deg
+    AP_GROUPINFO("MOT_ANG",  13, StratoBlimp,  motor_angle, 20),
+
+    // @Param: COL
+    // @DisplayName: center of lift
+    // @Description: center of lift position above CoG
+    // @Units: m
+    AP_GROUPINFO("COL",      14, StratoBlimp,  center_of_lift, 2.54),
+
+    // @Param: WVANE
+    // @DisplayName: weathervaning offset
+    // @Description: center of drag for weathervaning
+    // @Units: m
+    AP_GROUPINFO("WVANE",    15, StratoBlimp,  center_of_drag, 2),
+    
+    AP_GROUPEND
+};
+
+StratoBlimp::StratoBlimp(const char *frame_str) :
+    Aircraft(frame_str)
+{
+    AP::sitl()->models.stratoblimp_ptr = this;
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+/*
+  calculate coefficients to match parameters
+ */
+void StratoBlimp::calculate_coefficients(void)
+{
+    // calculate yaw drag based on turn rate at the given altitude
+    drag_yaw = 1.0;
+
+    // get full throttle rotational accel for one motor
+    Vector3f body_acc, rot_accel;
+    handle_motor(1, 0, body_acc, rot_accel, -arm_length);
+
+    // get rotational drag at target alt
+    Vector3f vel_bf, g, drag_linear, drag_rotaccel;
+    g.z = radians(yaw_rate_max);
+
+    get_drag(vel_bf, g,
+             altitude_target,
+             drag_linear, drag_rotaccel);
+
+    drag_yaw = rot_accel.z / -drag_rotaccel.z;
+}
+
+void StratoBlimp::handle_motor(float throttle, float tilt, Vector3f &body_acc, Vector3f &rot_accel, float lateral_position)
+{
+    const float angle_rad = radians(motor_angle) * tilt;
+    const float thrust_x = motor_thrust * throttle;
+    const float total_mass = mass + helium_mass;
+
+    const Vector3f thrust{cosf(angle_rad)*thrust_x, 0, -sinf(angle_rad)*thrust_x}; // assume constant with pressure alt and linear
+    Vector3f accel = thrust / total_mass;
+    Vector3f pos{0, lateral_position, 0};
+
+    Vector3f torque = (pos % thrust);
+
+    rot_accel.z += torque.z / moi_yaw;
+    body_acc += accel;
+}
+
+
+/*
+  get body frame linear and rotational drag for a given velocity and altitude
+ */
+void StratoBlimp::get_drag(const Vector3f &velocity_linear,
+                           const Vector3f &velocity_rot,
+                           float altitude,
+                           Vector3f &drag_linear, Vector3f &drag_rotaccel)
+{
+    Vector3f vel_air_bf = velocity_linear;
+    const float drag_x_sign = vel_air_bf.x>0? -1 : 1;
+    const float drag_y_sign = vel_air_bf.y>0? -1 : 1;
+    const float drag_z_sign = vel_air_bf.z>0? -1 : 1;
+    drag_linear.x = 0.5 * drag_x_sign * air_density * sq(vel_air_bf.x) * drag_fwd;
+    drag_linear.y = 0.5 * drag_y_sign * air_density * sq(vel_air_bf.y) * drag_fwd;
+    drag_linear.z = 0.5 * drag_z_sign * air_density * sq(vel_air_bf.z) * drag_up;
+
+    drag_rotaccel = -velocity_rot * drag_yaw;
+
+    /*
+      apply torque from drag
+    */
+    Vector3f drag_force = drag_linear * mass;
+    Vector3f drag_pos{-center_of_drag, 0, -center_of_lift};
+    Vector3f drag_torque = (drag_pos % drag_force);
+    drag_rotaccel += drag_torque / moi_pitch;
+}
+
+/*
+  get vertical thrust from lift in Newtons
+ */
+float StratoBlimp::get_lift(float altitude)
+{
+    // start with neutral buoyancy
+    float lift_accel = GRAVITY_MSS;
+
+    // assume lift will equal drag at the given climb rate
+    float climb_accel = 0.5 * air_density * sq(climb_rate) * drag_up;
+
+    // ramp down lift as we approach target alt
+    lift_accel += linear_interpolate(climb_accel, 0,
+                                     altitude,
+                                     altitude_target-1000, altitude_target);
+
+    return mass * lift_accel;
+}
+
+// calculate rotational and linear accelerations in body frame
+void StratoBlimp::calculate_forces(const struct sitl_input &input, Vector3f &body_acc, Vector3f &rot_accel)
+{
+    //float delta_time = frame_time_us * 1.0e-6f;
+
+    if (!hal.scheduler->is_system_initialized()) {
+        return;
+    }
+
+    const float left_tilt = filtered_servo_angle(input, 0);
+    const float right_tilt = filtered_servo_angle(input, 1);
+
+    const float left_throttle = filtered_servo_range(input, 2);
+    const float right_throttle = filtered_servo_range(input, 3);
+    const float ground_release = filtered_servo_range(input, 4);
+
+    body_acc.zero();
+    rot_accel.zero();
+
+    handle_motor(left_throttle, left_tilt, body_acc, rot_accel, -arm_length);
+    handle_motor(right_throttle, right_tilt, body_acc, rot_accel, arm_length);
+
+    Vector3f drag_linear, drag_rotaccel;
+    get_drag(velocity_air_bf, gyro,
+             location.alt*0.01,
+             drag_linear, drag_rotaccel);
+
+    body_acc += drag_linear;
+    rot_accel += drag_rotaccel;
+
+    if (ground_release > 0.9) {
+        released = true;
+    }
+    if (released) {
+        Vector3f lift_thrust_ef{0, 0, -get_lift(location.alt*0.01)};
+        Vector3f lift_thrust_bf = dcm.transposed() * lift_thrust_ef;
+
+        body_acc += lift_thrust_bf / mass;
+
+        /*
+          apply righting moment
+         */
+        Vector3f lift_pos{0, 0, -center_of_lift};
+        Vector3f lift_torque = (lift_pos % lift_thrust_bf);
+        rot_accel += lift_torque / moi_roll;
+    }
+}
+
+/*
+  update the airship simulation by one time step
+ */
+void StratoBlimp::update(const struct sitl_input &input)
+{
+    air_density = get_air_density(location.alt*0.01);
+    EAS2TAS = sqrtf(SSL_AIR_DENSITY / air_density);
+
+    calculate_coefficients();
+
+    float delta_time = frame_time_us * 1.0e-6f;
+
+    Vector3f rot_accel = Vector3f(0,0,0);
+    calculate_forces(input, accel_body, rot_accel);
+
+    // update rotational rates in body frame
+    gyro += rot_accel * delta_time;
+
+    gyro.x = constrain_float(gyro.x, -radians(2000.0f), radians(2000.0f));
+    gyro.y = constrain_float(gyro.y, -radians(2000.0f), radians(2000.0f));
+    gyro.z = constrain_float(gyro.z, -radians(2000.0f), radians(2000.0f));
+
+    dcm.rotate(gyro * delta_time);
+    dcm.normalize();
+
+    update_dynamics(rot_accel);
+    update_external_payload(input);
+
+    // update lat/lon/altitude
+    update_position();
+    update_wind(input);
+    time_advance();
+
+    // update magnetic field
+    update_mag_field_bf();
+}
+
+#endif // AP_SIM_STRATOBLIMP_ENABLED

libraries/SITL/SIM_StratoBlimp.h

@@ -0,0 +1,85 @@
+/*
+   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/>.
+ */
+/*
+  a stratospheric blimp simulator class
+*/
+
+#pragma once
+
+#include "SIM_config.h"
+
+#if AP_SIM_STRATOBLIMP_ENABLED
+
+#include "SIM_Aircraft.h"
+#include <AP_Param/AP_Param.h>
+
+namespace SITL {
+
+/*
+  a stratospheric blimp simulator
+ */
+
+class StratoBlimp : public Aircraft {
+public:
+    StratoBlimp(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 StratoBlimp(frame_str);
+    }
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+protected:
+    void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
+
+private:
+    void calculate_coefficients();
+    void handle_motor(float throttle, float tilt, Vector3f &body_acc, Vector3f &rot_accel, float lateral_position);
+    void get_drag(const Vector3f &velocity_linear,
+                  const Vector3f &velocity_rot,
+                  float altitude,
+                  Vector3f &drag_linear, Vector3f &drag_rotaccel);
+    float get_lift(float altitude);
+
+    float air_density;
+    float EAS2TAS;
+    float drag_yaw;
+    bool released;
+
+    AP_Float mass;
+    AP_Float helium_mass;
+    AP_Float arm_length;
+    AP_Float motor_thrust;
+    AP_Float drag_fwd;
+    AP_Float drag_up;
+    AP_Float altitude_target;
+    AP_Float climb_rate;
+    AP_Float turn_rate;
+    AP_Float motor_angle;
+    AP_Float yaw_rate_max;
+    AP_Float moi_roll;
+    AP_Float moi_yaw;
+    AP_Float moi_pitch;
+    AP_Float center_of_lift;
+    AP_Float center_of_drag;
+};
+
+}
+
+#endif // AP_SIM_STRATOBLIMP_ENABLED

libraries/SITL/SIM_config.h

@@ -93,3 +93,7 @@
 #ifndef AP_SIM_COMPASS_QMC5883L_ENABLED
 #define AP_SIM_COMPASS_QMC5883L_ENABLED AP_SIM_COMPASS_BACKEND_DEFAULT_ENABLED
 #endif
+
+#ifndef AP_SIM_STRATOBLIMP_ENABLED
+#define AP_SIM_STRATOBLIMP_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#endif

libraries/SITL/SITL.cpp

@@ -37,6 +37,8 @@
   #endif
 #endif // SFML_JOYSTICK
 
+#include "SIM_StratoBlimp.h"
+
 extern const AP_HAL::HAL& hal;
 
 #ifndef SIM_RATE_HZ_DEFAULT
@@ -175,9 +177,7 @@ const AP_Param::GroupInfo SIM::var_info[] = {
     // @Vector3Parameter: 1
     AP_GROUPINFO("FLOW_POS",      56, SIM,  optflow_pos_offset, 0),
     AP_GROUPINFO("ENGINE_FAIL",   58, SIM,  engine_fail,  0),
-#if AP_SIM_SHIP_ENABLED
-    AP_SUBGROUPINFO(shipsim, "SHIP_", 59, SIM, ShipSim),
-#endif
+    AP_SUBGROUPINFO(models, "",   59, SIM, SIM::ModelParm),
     AP_SUBGROUPEXTENSION("",      60, SIM,  var_mag),
 #if HAL_SIM_GPS_ENABLED
     AP_SUBGROUPEXTENSION("",      61, SIM,  var_gps),
@@ -537,7 +537,7 @@ const AP_Param::GroupInfo SIM::var_info3[] = {
     // @Range: 10 100
     AP_GROUPINFO("OSD_ROWS",     54, SIM,  osd_rows, 16),
 #endif
-    
+
 #ifdef SFML_JOYSTICK
     AP_SUBGROUPEXTENSION("",      63, SIM,  var_sfml_joystick),
 #endif // SFML_JOYSTICK
@@ -1185,6 +1185,23 @@ const AP_Param::GroupInfo SIM::var_ins[] = {
     AP_GROUPEND
 };
 
+// user settable parameters for the physics models
+const AP_Param::GroupInfo SIM::ModelParm::var_info[] = {
+
+#if AP_SIM_SHIP_ENABLED
+    // @Group: SHIP_
+    // @Path: ./SIM_Ship.cpp
+    AP_SUBGROUPINFO(shipsim, "SHIP_", 1, SIM::ModelParm, ShipSim),
+#endif
+#if AP_SIM_STRATOBLIMP_ENABLED
+    // @Group: SB_
+    // @Path: ./SIM_StratoBlimp.cpp
+    AP_SUBGROUPPTR(stratoblimp_ptr, "SB_",  2, SIM::ModelParm, StratoBlimp),
+#endif
+    
+    AP_GROUPEND
+};
+    
 const Location post_origin {
     518752066,
     146487830,

libraries/SITL/SITL.h

@@ -11,6 +11,7 @@
 #include <AP_Common/Location.h>
 #include <AP_Compass/AP_Compass.h>
 #include <AP_InertialSensor/AP_InertialSensor.h>
+
 #include "SIM_Buzzer.h"
 #include "SIM_Gripper_EPM.h"
 #include "SIM_Gripper_Servo.h"
@@ -46,7 +47,8 @@ struct float_array {
     uint16_t length;
     float *data;
 };
-    
+
+class StratoBlimp;
 
 struct sitl_fdm {
     // this is the structure passed between FDM models and the main SITL code
@@ -294,6 +296,19 @@ public:
         AP_Int8  signflip;
     };
     AirspeedParm airspeed[AIRSPEED_MAX_SENSORS];
+
+    // physics model parameters
+    class ModelParm {
+    public:
+        static const struct AP_Param::GroupInfo var_info[];
+#if AP_SIM_STRATOBLIMP_ENABLED
+        StratoBlimp *stratoblimp_ptr;
+#endif
+#if AP_SIM_SHIP_ENABLED
+        ShipSim shipsim;
+#endif
+    };
+    ModelParm models;
     
     // EFI type
     enum EFIType {
@@ -443,11 +458,6 @@ public:
 
     Sprayer sprayer_sim;
 
-    // simulated ship takeoffs
-#if AP_SIM_SHIP_ENABLED
-    ShipSim shipsim;
-#endif
-
     Gripper_Servo gripper_sim;
     Gripper_EPM gripper_epm_sim;