SITL: converted helicopter simulator from python

libraries/SITL/SIM_Aircraft.cpp

@@ -30,7 +30,7 @@
 /*
   constructor
  */
-Aircraft::Aircraft(const char *home_str) :
+Aircraft::Aircraft(const char *home_str, const char *frame_str) :
     ground_level(0),
     frame_height(0),
     dcm(),

libraries/SITL/SIM_Aircraft.h

@@ -30,7 +30,7 @@
 class Aircraft 
 {
 public:
-    Aircraft(const char *home_str);
+    Aircraft(const char *home_str, const char *frame_str);
 
     /*
       structure passed in giving servo positions as PWM values in

libraries/SITL/SIM_Helicopter.cpp

@@ -0,0 +1,150 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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/>.
+ */
+/*
+  helicopter simulator class
+*/
+
+#include "SIM_Helicopter.h"
+#include <stdio.h>
+
+/*
+  constructor
+ */
+Helicopter::Helicopter(const char *home_str, const char *frame_str) :
+    Aircraft(home_str, frame_str),    
+    terminal_rotation_rate(4*radians(360.0)),
+    hover_throttle(0.65f),
+    terminal_velocity(40.0f),
+    hover_lean(8.0f),
+    yaw_zero(0.1f),
+    rotor_rot_accel(radians(20)),
+    roll_rate_max(radians(1400)),
+    pitch_rate_max(radians(1400)),
+    yaw_rate_max(radians(1400)),
+    rsc_setpoint(0.8f)
+{
+    mass = 2.13f;
+
+    /* 
+       scaling from motor power to Newtons. Allows the copter
+       to hover against gravity when the motor is at hover_throttle
+    */
+    thrust_scale = (mass * GRAVITY_MSS) / hover_throttle;
+
+    // calculate lateral thrust ratio for tail rotor
+    tail_thrust_scale = sinf(radians(hover_lean)) * thrust_scale / yaw_zero;
+
+    frame_height = 0.1;
+}
+
+/*
+  update the helicopter simulation by one time step
+ */
+void Helicopter::update(const struct sitl_input &input)
+{
+    float swash1 = (input.servos[0]-1000) / 1000.0f;
+    float swash2 = (input.servos[1]-1000) / 1000.0f;
+    float swash3 = (input.servos[2]-1000) / 1000.0f;
+    float tail_rotor = (input.servos[3]-1000) / 1000.0f;
+    float rsc = (input.servos[7]-1000) / 1000.0f;
+
+    // how much time has passed?
+    float delta_time = frame_time_us * 1.0e-6f;
+
+    float thrust = (rsc/rsc_setpoint)*(swash1+swash2+swash3)/3.0f;
+
+    // very simplistic mapping to body euler rates
+    float roll_rate = swash1 - swash2;
+    float pitch_rate = (swash1 + swash2)/2.0f - swash3;
+    float yaw_rate = tail_rotor - 0.5f;
+
+    float rsc_scale = rsc/rsc_setpoint;
+
+    roll_rate *= rsc_scale;
+    pitch_rate *= rsc_scale;
+    yaw_rate *= rsc_scale;
+
+    // rotational acceleration, in rad/s/s, in body frame
+    Vector3f rot_accel;
+    rot_accel.x = roll_rate * roll_rate_max;
+    rot_accel.y = pitch_rate * pitch_rate_max;
+    rot_accel.z = yaw_rate * yaw_rate_max;
+
+    // rotational air resistance
+    rot_accel.x -= gyro.x * radians(5000.0) / terminal_rotation_rate;
+    rot_accel.y -= gyro.y * radians(5000.0) / terminal_rotation_rate;
+    rot_accel.z -= gyro.z * radians(400.0)  / terminal_rotation_rate;
+
+    // torque effect on tail
+    rot_accel.z += (rsc_scale+thrust) * rotor_rot_accel;
+
+    // update rotational rates in body frame
+    gyro += rot_accel * delta_time;
+
+    // update attitude
+    dcm.rotate(gyro * delta_time);
+    dcm.normalize();
+
+    // air resistance
+    Vector3f air_resistance = -velocity_ef * (GRAVITY_MSS/terminal_velocity);
+
+    // scale thrust to newtons
+    thrust *= thrust_scale;
+
+    accel_body = Vector3f(0, yaw_rate * rsc_scale * tail_thrust_scale, -thrust / mass);
+    Vector3f accel_earth = dcm * accel_body;
+    accel_earth += Vector3f(0, 0, GRAVITY_MSS);
+    accel_earth += air_resistance;
+
+    // if we're on the ground, then our vertical acceleration is limited
+    // to zero. This effectively adds the force of the ground on the aircraft
+    if (on_ground(position) && accel_earth.z > 0) {
+        accel_earth.z = 0;
+    }
+
+    // work out acceleration as seen by the accelerometers. It sees the kinematic
+    // acceleration (ie. real movement), plus gravity
+    accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS));
+
+    // add some noise
+    add_noise(thrust / thrust_scale);
+
+    // new velocity vector
+    velocity_ef += accel_earth * delta_time;
+
+    // new position vector
+    Vector3f old_position = position;
+    position += velocity_ef * delta_time;
+
+    // constrain height to the ground
+    if (on_ground(position)) {
+        if (!on_ground(old_position)) {
+            printf("Hit ground at %f m/s\n", velocity_ef.z);
+
+            velocity_ef.zero();
+
+            // zero roll/pitch, but keep yaw
+            float r, p, y;
+            dcm.to_euler(&r, &p, &y);
+            dcm.from_euler(0, 0, y);
+
+            position.z = -(ground_level + frame_height - home.alt*0.01f);
+        }
+    }
+
+    // update lat/lon/altitude
+    update_position();
+}

libraries/SITL/SIM_Helicopter.h

@@ -0,0 +1,55 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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/>.
+ */
+/*
+  helicopter simulator class
+*/
+
+#ifndef _SIM_HELICOPTER_H
+#define _SIM_HELICOPTER_H
+
+#include "SIM_Aircraft.h"
+
+/*
+  a helicopter simulator
+ */
+class Helicopter : public Aircraft
+{
+public:
+    Helicopter(const char *home_str, const char *frame_str);
+
+    /* update model by one time step */
+    void update(const struct sitl_input &input);
+
+    /* static object creator */
+    static Aircraft *create(const char *home_str, const char *frame_str) { return new Helicopter(home_str, frame_str); }
+
+private:
+    float terminal_rotation_rate;
+    float hover_throttle;
+    float terminal_velocity;
+    float hover_lean;
+    float yaw_zero;
+    float rotor_rot_accel;
+    float roll_rate_max;
+    float pitch_rate_max;
+    float yaw_rate_max;
+    float rsc_setpoint;
+    float thrust_scale;
+    float tail_thrust_scale;
+};
+
+
+#endif // _SIM_HELICOPTER_H

libraries/SITL/SIM_Multicopter.cpp

@@ -14,7 +14,7 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 /*
-  parent class for aircraft simulators
+  multicopter simulator class
 */
 
 #include "SIM_Multicopter.h"
@@ -99,7 +99,7 @@ static const Frame supported_frames[] =
   constructor
  */
 MultiCopter::MultiCopter(const char *home_str, const char *frame_str) :
-    Aircraft(home_str),    
+    Aircraft(home_str, frame_str),    
     frame(NULL),
     hover_throttle(0.51),
     terminal_velocity(15.0),

libraries/SITL/SIM_Multicopter.h

@@ -14,7 +14,7 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 /*
-  parent class for aircraft simulators
+  multicopter simulator class
 */
 
 #ifndef _SIM_MULTICOPTER_H
@@ -66,6 +66,9 @@ public:
     /* update model by one time step */
     void update(const struct sitl_input &input);
 
+    /* static object creator */
+    static Aircraft *create(const char *home_str, const char *frame_str) { return new MultiCopter(home_str, frame_str); }
+
 private:
     const Frame *frame;
     float hover_throttle; // 0..1