mirror of https://github.com/ArduPilot/ardupilot
109 lines
3.5 KiB
C++
109 lines
3.5 KiB
C++
/// -*- 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;
|
|
|
|
if (strstr(frame_str, "coax")) {
|
|
frame_type = FRAME_COAX;
|
|
} else {
|
|
frame_type = FRAME_SINGLE;
|
|
}
|
|
|
|
/*
|
|
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;
|
|
float yaw_thrust;
|
|
float roll_thrust;
|
|
float pitch_thrust;
|
|
|
|
switch (frame_type) {
|
|
case FRAME_SINGLE:
|
|
thrust = constrain_float((input.servos[4]-1000) / 1000.0f, 0, 1);
|
|
yaw_thrust = -(actuator[0] + actuator[1] + actuator[2] + actuator[3]) * 0.25f * thrust + thrust * rotor_rot_accel;
|
|
roll_thrust = (actuator[0] - actuator[2]) * 0.5f * thrust;
|
|
pitch_thrust = (actuator[1] - actuator[3]) * 0.5f * thrust;
|
|
break;
|
|
|
|
case FRAME_COAX:
|
|
default: {
|
|
float motor1 = constrain_float((input.servos[4]-1000) / 1000.0f, 0, 1);
|
|
float motor2 = constrain_float((input.servos[5]-1000) / 1000.0f, 0, 1);
|
|
thrust = 0.5f*(motor1 + motor2);
|
|
yaw_thrust = -(actuator[0] + actuator[1] + actuator[2] + actuator[3]) * 0.25f * thrust + (motor2 - motor1) * rotor_rot_accel;
|
|
roll_thrust = (actuator[0] - actuator[2]) * 0.5f * thrust;
|
|
pitch_thrust = (actuator[1] - actuator[3]) * 0.5f * thrust;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// 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);
|
|
|
|
// 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.transposed() * air_resistance;
|
|
|
|
update_dynamics(rot_accel);
|
|
|
|
// update lat/lon/altitude
|
|
update_position();
|
|
|
|
// update magnetic field
|
|
update_mag_field_bf();
|
|
}
|