diff --git a/libraries/SITL/SIM_SingleCopter.cpp b/libraries/SITL/SIM_SingleCopter.cpp new file mode 100644 index 0000000000..a0fefd38c5 --- /dev/null +++ b/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 . + */ +/* + singlecopter simulator class +*/ + +#include "SIM_SingleCopter.h" + +#include + +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(); +} diff --git a/libraries/SITL/SIM_SingleCopter.h b/libraries/SITL/SIM_SingleCopter.h new file mode 100644 index 0000000000..6d6278c31d --- /dev/null +++ b/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 . + */ +/* + single copter simulator class +*/ + +#pragma once + +#include "SIM_Aircraft.h" +#include + +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