SITL: Blimp SITL add initial dynamics

libraries/SITL/SIM_Blimp.cpp

@@ -17,30 +17,87 @@
 */
 
 #include "SIM_Blimp.h"
-#include <AP_Motors/AP_Motors.h>
 
 #include <stdio.h>
 
 using namespace SITL;
 
+extern const AP_HAL::HAL& hal;
+
 Blimp::Blimp(const char *frame_str) :
     Aircraft(frame_str)
 {
-    frame_height = 0.0;
-    ground_behavior = GROUND_BEHAVIOR_NONE;
+    mass = 0.07;
+    radius = 0.25;
+    moment_of_inertia = {0.004375, 0.004375, 0.004375}; //m*r^2 for hoop...
+    k_tan = 5.52e-4; //Tangential (thrust) multiplier
+    drag_constant = 0.05;
+    drag_gyr_constant = 0.08;
+
     lock_step_scheduled = true;
+    ::printf("Starting Blimp AirFish model...\n");
 }
 
 // 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);
+  if (!hal.scheduler->is_system_initialized()) {
+      return;
+  }
 
-    // ::printf("FINS (%.1f %.1f %.1f %.1f)\n",
-    //          fin_back, fin_front, fin_right, fin_left);
+  //all fin setup
+  for (uint8_t i=0; i<4; i++) {
+    fin[i].last_angle = fin[i].angle;
+    fin[i].angle = filtered_servo_angle(input, i)*radians(75.0f); //for servo range of -75 deg to +75 deg
+    
+    if (fin[i].angle < fin[i].last_angle) fin[i].dir = 0; //thus 0 = "angle is reducing"
+    else fin[i].dir = 1;
+    
+    fin[i].vel = (fin[i].angle - fin[i].last_angle)/delta_time; //rad/s
+    fin[i].vel = constrain_float(fin[i].vel, radians(-450), radians(450));
+    fin[i].T = pow(fin[i].vel,2) * k_tan;
+    
+    fin[i].Fx = 0;
+    fin[i].Fy = 0;
+    fin[i].Fz = 0;
+  }
+
+  //TODO: Add normal force calculations and include roll & pitch oscillation.
+  //Back fin
+  fin[0].Fx =  fin[0].T*cos(fin[0].angle); //causes forward movement
+  fin[0].Fz =  fin[0].T*sin(fin[0].angle); //causes height change
+
+  //Front fin
+  fin[1].Fx = -fin[1].T*cos(fin[1].angle); //causes backward movement
+  fin[1].Fz =  fin[1].T*sin(fin[1].angle); //causes height change
+
+  //Right fin
+  fin[2].Fy = -fin[2].T*cos(fin[2].angle); //causes left movement
+  fin[2].Fx =  fin[2].T*sin(fin[2].angle); //causes yaw
+
+  //Left fin
+  fin[3].Fy =  fin[3].T*cos(fin[3].angle); //causes right movement
+  fin[3].Fx = -fin[3].T*sin(fin[3].angle); //causes yaw
+
+  Vector3f force_bf{0,0,0}; 
+  for (uint8_t i=0; i<4; i++) {
+    force_bf.x = force_bf.x + fin[i].Fx;
+    force_bf.y = force_bf.y + fin[i].Fy;
+    force_bf.z = force_bf.z + fin[i].Fz;
+  }
+
+  //mass in kg, thus accel in m/s/s
+  body_accel.x = force_bf.x/mass;
+  body_accel.y = force_bf.y/mass;
+  body_accel.z = force_bf.z/mass;
+
+  Vector3f rot_T{0,0,0};
+  rot_T.z = fin[2].Fx * radius + fin[3].Fx * radius;//in N*m (Torque = force * lever arm)
+
+  //rot accel = torque / moment of inertia
+  rot_accel.x = 0;
+  rot_accel.y = 0;
+  rot_accel.z = rot_T.z / moment_of_inertia.z;
 }
 
 /*
@@ -48,20 +105,50 @@ void Blimp::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel
  */
 void Blimp::update(const struct sitl_input &input)
 {
-    // get wind vector setup
-    update_wind(input);
+  delta_time = frame_time_us * 1.0e-6f;
 
-    Vector3f rot_accel;
+  Vector3f rot_accel = Vector3f(0,0,0);
+  calculate_forces(input, rot_accel, accel_body);
 
-    calculate_forces(input, rot_accel, accel_body);
+  if (hal.scheduler->is_system_initialized()) {
+    float gyr_sq = gyro.length_squared();
+    if (is_positive(gyr_sq)) {
+        Vector3f force_gyr = (gyro.normalized() * drag_gyr_constant * gyr_sq);
+        Vector3f ef_drag_accel_gyr = -force_gyr / mass;
+        Vector3f bf_drag_accel_gyr = dcm.transposed() * ef_drag_accel_gyr;
+        rot_accel += bf_drag_accel_gyr;
+    }
+  }
 
-    update_dynamics(rot_accel);
-    update_external_payload(input);
+  // 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));
 
-    // update lat/lon/altitude
-    update_position();
-    time_advance();
+  // update attitude
+  dcm.rotate(gyro * delta_time);
+  dcm.normalize();
 
-    // update magnetic field
-    update_mag_field_bf();
+  if (hal.scheduler->is_system_initialized()) {
+    float speed_sq = velocity_ef.length_squared();
+    if (is_positive(speed_sq)) {
+        Vector3f force = (velocity_ef.normalized() * drag_constant * speed_sq);
+        Vector3f ef_drag_accel = -force / mass;
+        Vector3f bf_drag_accel = dcm.transposed() * ef_drag_accel;
+        accel_body += bf_drag_accel;
+    }
+
+    // add lifting force exactly equal to gravity, for neutral buoyancy
+    accel_body += dcm.transposed() * Vector3f(0,0,-GRAVITY_MSS);
+  }
+  
+  Vector3f accel_earth = dcm * accel_body;
+  accel_earth += Vector3f(0.0f, 0.0f, GRAVITY_MSS); //add gravity
+  velocity_ef += accel_earth * delta_time;
+  position += (velocity_ef * delta_time).todouble(); //update position vector
+
+  update_position(); //updates the position from the Vector3f position
+  time_advance();
+  update_mag_field_bf();
 }

libraries/SITL/SIM_Blimp.h

@@ -19,11 +19,21 @@
 #pragma once
 
 #include "SIM_Aircraft.h"
-#include "SIM_Motor.h"
-#include "SIM_Frame.h"
 
 namespace SITL {
 
+struct Fins
+{
+  float angle;
+  float last_angle;
+  bool dir;
+  float vel; // velocity, in rad/s
+  float T; //Tangential (thrust) force, in Neutons
+  float Fx; //Fx,y,z = Force in bodyframe orientation at servo position, in Newtons
+  float Fy;
+  float Fz;
+};
+
 /*
   a blimp simulator
  */
@@ -41,10 +51,18 @@ public:
     }
 
 protected:
-    const struct {
-        float mass = 0.02;
-    } model;
+    float mass; //kilograms
+    float radius; //metres
+    Vector3f moment_of_inertia;
+
+    //Airfish-specific variables
+    Fins fin[4];
+    float k_tan; //Tangential force multiplier
+    float drag_constant;
+    float drag_gyr_constant;
+    float delta_time;
 
     void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
 };
+
 }