SITL: added SingleCopter simulator

libraries/SITL/SIM_SingleCopter.cpp

@@ -0,0 +1,94 @@
+/// -*- 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/>.
+ */
+/*
+  singlecopter simulator class
+*/
+
+#include "SIM_SingleCopter.h"
+
+#include <stdio.h>
+
+using namespace SITL;
+
+SingleCopter::SingleCopter(const char *home_str, const char *frame_str) :
+    Aircraft(home_str, frame_str)
+{
+    mass = 2.0f;
+
+    /*
+       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;
+    frame_height = 0.1;
+}
+
+/*
+  update the copter simulation by one time step
+ */
+void SingleCopter::update(const struct sitl_input &input)
+{
+    // get wind vector setup
+    update_wind(input);
+
+    float actuator[4];
+    for (uint8_t i=0; i<4; i++) {
+        actuator[i] = constrain_float((input.servos[i]-1500) / 500.0f, -1, 1);
+    }
+    float thrust = constrain_float((input.servos[4]-1000) / 1000.0f, 0, 1);
+    float yaw_thrust   = (actuator[0] + actuator[1] + actuator[2] + actuator[3]) * 0.25f * thrust;
+    float roll_thrust  = (actuator[0] - actuator[2]) * 0.5f * thrust;
+    float pitch_thrust = (actuator[1] - actuator[3]) * 0.5f * thrust;
+
+    float torque_effect_accel = thrust * rotor_rot_accel;
+    
+    // rotational acceleration, in rad/s/s, in body frame
+    Vector3f rot_accel(roll_thrust * roll_rate_max,
+                       pitch_thrust * pitch_rate_max,
+                       yaw_thrust * yaw_rate_max + torque_effect_accel);
+
+    // 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;
+
+    // air resistance
+    Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity);
+
+    // scale thrust to newtons
+    thrust *= thrust_scale;
+
+    accel_body = Vector3f(0, 0, -thrust / mass);
+    accel_body += dcm * air_resistance;
+
+    bool was_on_ground = on_ground(position);
+    
+    update_dynamics(rot_accel);
+    
+    // constrain height to the ground
+    if (on_ground(position) && !was_on_ground) {
+        // 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);
+        velocity_ef.zero();
+    }
+
+    // update lat/lon/altitude
+    update_position();
+}

libraries/SITL/SIM_SingleCopter.h

@@ -0,0 +1,53 @@
+/// -*- 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/>.
+ */
+/*
+  single copter simulator class
+*/
+
+#pragma once
+
+#include "SIM_Aircraft.h"
+#include <AP_Motors/AP_Motors.h>
+
+namespace SITL {
+
+/*
+  a single copter simulator
+ */
+class SingleCopter : public Aircraft {
+public:
+    SingleCopter(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 SingleCopter(home_str, frame_str);
+    }
+
+private:
+    float terminal_rotation_rate = 4*radians(360.0f);
+    float hover_throttle = 0.65f;
+    float terminal_velocity = 40;
+    float rotor_rot_accel = radians(20) * AP_MOTORS_MATRIX_YAW_FACTOR_CW;
+    float roll_rate_max = radians(700);
+    float pitch_rate_max = radians(700);
+    float yaw_rate_max = radians(700);
+    float thrust_scale;
+};
+
+} // namespace SITL