From fcfd11ef535bd51157a18eaa34542f89614305c0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Jan 2016 12:26:32 +1100 Subject: [PATCH] SITL: very simple fixed wing simulator useful for debugging --- libraries/SITL/SIM_Plane.cpp | 202 +++++++++++++++++++++++++++++++++++ libraries/SITL/SIM_Plane.h | 74 +++++++++++++ 2 files changed, 276 insertions(+) create mode 100644 libraries/SITL/SIM_Plane.cpp create mode 100644 libraries/SITL/SIM_Plane.h diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp new file mode 100644 index 0000000000..f28e99c561 --- /dev/null +++ b/libraries/SITL/SIM_Plane.cpp @@ -0,0 +1,202 @@ +/// -*- 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 . + */ +/* + very simple plane simulator class. Not aerodynamically accurate, + just enough to be able to debug control logic for new frame types +*/ + +#include "SIM_Plane.h" + +#include + +using namespace SITL; + +Plane::Plane(const char *home_str, const char *frame_str) : + Aircraft(home_str, frame_str) +{ + mass = 1.0f; + + /* + scaling from motor power to Newtons. Allows the plane to hold + vertically against gravity when the motor is at hover_throttle + */ + thrust_scale = (mass * GRAVITY_MSS) / hover_throttle; + frame_height = 0.1f; +} + +/* + calculate lift in neutons + */ +float Plane::calculate_lift(void) const +{ + // simple lift equation from http://wright.nasa.gov/airplane/lifteq.html + const float max_angle = radians(30); + const float max_angle_delta = radians(10); + const float clift_at_max = coefficient.lift * 2 * M_PI_F * max_angle; + float Cl = coefficient.lift * 2 * M_PI_F * angle_of_attack; + if (fabsf(angle_of_attack) > max_angle+max_angle_delta) { + return 0; + } + if (angle_of_attack > max_angle) { + Cl = clift_at_max * (1-(angle_of_attack - max_angle)/max_angle_delta); + } else if (angle_of_attack < -max_angle) { + Cl = -clift_at_max * (1+(angle_of_attack - max_angle)/max_angle_delta); + } + float lift = 0.5 * Cl * air_density * sq(airspeed) * wing_area; + return lift; +} + + +/* + calculate induced drag in neutons + */ +float Plane::calculate_drag_induced(void) const +{ + float lift = calculate_lift(); + + // simple induced drag from https://en.wikipedia.org/wiki/Lift-induced_drag + if (airspeed < 0.1) { + return 0; + } + float drag_i = sq(lift) / (0.25 * sq(air_density) * sq(airspeed) * wing_area * M_PI_F * wing_efficiency * aspect_ratio); + return drag_i; +} + +/* + calculate form drag in neutons + */ +float Plane::calculate_drag_form(void) const +{ + // simple form drag + float drag_f = 0.5 * air_density * sq(airspeed) * coefficient.drag; + return drag_f; +} + +/* + calculate lift+drag in neutons in body frame + */ +Vector3f Plane::calculate_lift_drag(void) const +{ + if (velocity_ef.is_zero()) { + return Vector3f(0,0,0); + } + float lift = calculate_lift(); + float drag = calculate_drag_induced() + calculate_drag_form(); + return velocity_bf.normalized()*(-drag) + Vector3f(0, 0, -lift); +} + +void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel) +{ + float aileron = (input.servos[0]-1500)/500.0f; + float elevator = (input.servos[1]-1500)/500.0f; + float rudder = (input.servos[3]-1500)/500.0f; + float throttle = constrain_float((input.servos[2]-1000)/1000.0f, 0, 1); + float speed_scaling = airspeed / cruise_airspeed; + + float thrust = throttle; + float roll_rate = aileron * speed_scaling; + float pitch_rate = elevator * speed_scaling; + float yaw_rate = rudder * speed_scaling; + + // rotational acceleration, in rad/s/s, in body frame + rot_accel.x = roll_rate * max_rates.x; + rot_accel.y = pitch_rate * max_rates.y; + rot_accel.z = yaw_rate * max_rates.z; + + // rotational air resistance + rot_accel.x -= gyro.x * radians(800.0) / terminal_rotation_rate.x; + rot_accel.y -= gyro.y * radians(800.0) / terminal_rotation_rate.y; + rot_accel.z -= gyro.z * radians(1200.0) / terminal_rotation_rate.z; + + // add torque of stabilisers + rot_accel.z += velocity_bf.y * speed_scaling * coefficient.vertical_stabiliser; + rot_accel.y -= velocity_bf.z * speed_scaling * coefficient.horizontal_stabiliser; + + // velocity in body frame + velocity_bf = dcm.transposed() * velocity_ef; + + // calculate angle of attack + angle_of_attack = atan2f(velocity_bf.z, velocity_bf.x); + + // get lift and drag in body frame, in neutons + Vector3f lift_drag = calculate_lift_drag(); + + // air resistance + Vector3f air_resistance = -velocity_ef * (GRAVITY_MSS/terminal_velocity); + + // scale thrust to newtons + thrust *= thrust_scale; + + accel_body = Vector3f(thrust/mass, 0, 0); + accel_body += lift_drag/mass; + accel_body += dcm.transposed() * air_resistance; + + // add some noise + add_noise(thrust / thrust_scale); +} + +/* + update the plane simulation by one time step + */ +void Plane::update(const struct sitl_input &input) +{ + float delta_time = frame_time_us * 1.0e-6f; + + Vector3f rot_accel; + + calculate_forces(input, rot_accel, accel_body); + + // update rotational rates in body frame + gyro += rot_accel * delta_time; + + // update attitude + dcm.rotate(gyro * delta_time); + dcm.normalize(); + + Vector3f accel_earth = dcm * accel_body; + accel_earth += Vector3f(0, 0, GRAVITY_MSS); + + // 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)); + + // new velocity vector + velocity_ef += accel_earth * delta_time; + + // new position vector + Vector3f old_position = position; + position += velocity_ef * delta_time; + + // assume zero wind for now + airspeed = velocity_ef.length(); + + // 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); + position.z = -(ground_level + frame_height - home.alt*0.01f); + } + } + + // update lat/lon/altitude + update_position(); +} diff --git a/libraries/SITL/SIM_Plane.h b/libraries/SITL/SIM_Plane.h new file mode 100644 index 0000000000..25494d2619 --- /dev/null +++ b/libraries/SITL/SIM_Plane.h @@ -0,0 +1,74 @@ +/// -*- 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 . + */ +/* + simple plane simulator class +*/ + +#pragma once + +#include "SIM_Aircraft.h" + +namespace SITL { + +/* + a very simple plane simulator + */ +class Plane : public Aircraft { +public: + Plane(const char *home_str, const char *frame_str); + + /* update model by one time step */ + virtual void update(const struct sitl_input &input); + + /* static object creator */ + static Aircraft *create(const char *home_str, const char *frame_str) { + return new Plane(home_str, frame_str); + } + +protected: + const float hover_throttle = 0.5f; + const float cruise_airspeed = 20; + const float cruise_pitch = radians(4); + const float terminal_velocity = 35; + const float wing_efficiency = 0.9; + const float wing_span = 2.0; + const float wing_chord = 0.15; + const float aspect_ratio = wing_span / wing_chord; + const float wing_area = wing_span * wing_chord; + const float air_density = 1.225; // kg/m^3 at sea level, ISA conditions + float angle_of_attack; + Vector3f velocity_bf; + + // manually tweaked coefficients. Not even close to reality + struct { + float drag = 0.01; + float lift = 3.0; + float vertical_stabiliser = 0.1; + float horizontal_stabiliser = 0.001; + } coefficient; + + float thrust_scale; + Vector3f terminal_rotation_rate{radians(360), radians(360), radians(180)}; + Vector3f max_rates{radians(350), radians(250), radians(100)}; + + float calculate_lift(void) const; + float calculate_drag_induced(void) const; + float calculate_drag_form(void) const; + Vector3f calculate_lift_drag(void) const; + void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel); +}; + +} // namespace SITL