diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index 8f922bafba..30dd310d76 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/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(),
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index 0e970db901..41baebb1ea 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/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
diff --git a/libraries/SITL/SIM_Helicopter.cpp b/libraries/SITL/SIM_Helicopter.cpp
new file mode 100644
index 0000000000..b43897991d
--- /dev/null
+++ b/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 .
+ */
+/*
+ helicopter simulator class
+*/
+
+#include "SIM_Helicopter.h"
+#include
+
+/*
+ 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();
+}
diff --git a/libraries/SITL/SIM_Helicopter.h b/libraries/SITL/SIM_Helicopter.h
new file mode 100644
index 0000000000..a09d653a8f
--- /dev/null
+++ b/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 .
+ */
+/*
+ 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
diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp
index 704670f579..f5dcf8b1f8 100644
--- a/libraries/SITL/SIM_Multicopter.cpp
+++ b/libraries/SITL/SIM_Multicopter.cpp
@@ -14,7 +14,7 @@
along with this program. If not, see .
*/
/*
- 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),
diff --git a/libraries/SITL/SIM_Multicopter.h b/libraries/SITL/SIM_Multicopter.h
index 367cf7a5d7..eb62fc949b 100644
--- a/libraries/SITL/SIM_Multicopter.h
+++ b/libraries/SITL/SIM_Multicopter.h
@@ -14,7 +14,7 @@
along with this program. If not, see .
*/
/*
- 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