zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

SITL: initial conversion of multicopter sim to C++

mission-4.1.18
Andrew Tridgell 10 years ago
parent
2a6421d1bf
commit
914b91af9e
4 changed files with 592 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 193
      libraries/SITL/SIM_Aircraft.cpp
  2. 103
      libraries/SITL/SIM_Aircraft.h
  3. 221
      libraries/SITL/SIM_Multicopter.cpp
  4. 75
      libraries/SITL/SIM_Multicopter.h

193
libraries/SITL/SIM_Aircraft.cpp
Unescape View File

@ -0,0 +1,193 @@ @@ -0,0 +1,193 @@
/// -*- 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/>.
*/
/*
parent class for aircraft simulators
*/
#include <AP_Common.h>
#include "SIM_Aircraft.h"
#include <unistd.h>
/*
parent class for all simulator types
*/
/*
constructor
*/
Aircraft::Aircraft(const char *home_str) :
ground_level(0),
frame_height(0),
dcm(),
gyro(),
velocity_ef(),
velocity_body(),
mass(0),
update_frequency(50),
accel_body(0, 0, -GRAVITY_MSS),
time_now_us(0),
gyro_noise(radians(0.1f)),
accel_noise(0.3)
{
char *saveptr=NULL;
char *s = strdup(home_str);
char *lat_s = strtok_r(s, ",", &saveptr);
char *lon_s = strtok_r(NULL, ",", &saveptr);
char *alt_s = strtok_r(NULL, ",", &saveptr);
char *yaw_s = strtok_r(NULL, ",", &saveptr);
memset(&home, 0, sizeof(home));
home.lat = atof(lat_s) * 1.0e7;
home.lng = atof(lon_s) * 1.0e7;
home.alt = atof(alt_s) * 1.0e2;
location = home;
free(s);
dcm.from_euler(0, 0, atof(yaw_s));
}
/*
return true if we are on the ground
*/
bool Aircraft::on_ground(const Vector3f &pos) const
{
return (-pos.z) + home.alt*0.01f <= ground_level + frame_height;
}
/*
update location from position
*/
void Aircraft::update_position(void)
{
float bearing = degrees(atan2f(position.y, position.x));
float distance = sqrtf(sq(position.x) + sq(position.y));
location = home;
location_update(location, bearing, distance);
location.alt = home.alt - position.z*100.0f;
velocity_body = dcm.transposed() * velocity_ef;
}
/*
rotate to the given yaw
*/
void Aircraft::set_yaw_degrees(float yaw_degrees)
{
float roll, pitch, yaw;
dcm.to_euler(&roll, &pitch, &yaw);
yaw = radians(yaw_degrees);
dcm.from_euler(roll, pitch, yaw);
}
/* advance time by deltat in seconds */
void Aircraft::time_advance(float deltat)
{
time_now_us += deltat * 1.0e6f;
}
/* setup the frame step time */
void Aircraft::setup_frame_time(float new_rate, float new_speedup)
{
rate_hz = new_rate;
target_speedup = new_speedup;
frame_time_us = 1.0e6f/rate_hz;
scaled_frame_time_us = frame_time_us/target_speedup;
last_wall_time_us = get_wall_time_us();
achieved_rate_hz = rate_hz;
}
/* adjust frame_time calculation */
void Aircraft::adjust_frame_time(float new_rate)
{
rate_hz = new_rate;
frame_time_us = 1.0e6f/rate_hz;
scaled_frame_time_us = frame_time_us/target_speedup;
}
/* try to synchronise simulation time with wall clock time, taking
into account desired speedup */
void Aircraft::sync_frame_time(void)
{
uint64_t now = get_wall_time_us();
if (now < last_wall_time_us + scaled_frame_time_us) {
usleep(last_wall_time_us + scaled_frame_time_us - now);
now = get_wall_time_us();
if (now > last_wall_time_us && now - last_wall_time_us < 1.0e5) {
float rate = 1.0e6f/(now - last_wall_time_us);
achieved_rate_hz = (0.98f*achieved_rate_hz) + (0.02f*rate);
if (achieved_rate_hz < rate_hz * target_speedup) {
scaled_frame_time_us *= 0.999;
} else {
scaled_frame_time_us *= 1.001;
}
}
}
last_wall_time_us = now;
}
/* add noise based on throttle level (from 0..1) */
void Aircraft::add_noise(float throttle)
{
gyro += Vector3f(rand_normal(0, 1),
rand_normal(0, 1),
rand_normal(0, 1)) * gyro_noise;
accel_body += Vector3f(rand_normal(0, 1),
rand_normal(0, 1),
rand_normal(0, 1)) * accel_noise;
}
/*
normal distribution random numbers
See
http://en.literateprograms.org/index.php?title=Special:DownloadCode/Box-Muller_transform_%28C%29&oldid=7011
*/
double Aircraft::rand_normal(double mean, double stddev)
{
static double n2 = 0.0;
static int n2_cached = 0;
if (!n2_cached)
{
double x, y, r;
do
{
x = 2.0*rand()/RAND_MAX - 1;
y = 2.0*rand()/RAND_MAX - 1;
r = x*x + y*y;
}
while (r == 0.0 || r > 1.0);
{
double d = sqrt(-2.0*log(r)/r);
double n1 = x*d;
n2 = y*d;
double result = n1*stddev + mean;
n2_cached = 1;
return result;
}
}
else
{
n2_cached = 0;
return n2*stddev + mean;
}
}

103
libraries/SITL/SIM_Aircraft.h
Unescape View File

@ -0,0 +1,103 @@ @@ -0,0 +1,103 @@
/// -*- 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/>.
*/
/*
parent class for aircraft simulators
*/
#include "SITL.h"
#include <AP_Common.h>
#include <AP_Math.h>
/*
parent class for all simulator types
*/
class Aircraft
{
public:
Aircraft(const char *home_str);
/*
structure passed in giving servo positions as PWM values in
microseconds
*/
struct sitl_input {
uint16_t servos[16];
};
/*
step the FDM by one time step
*/
virtual void update(const struct sitl_input &input) = 0;
/* fill a sitl_fdm structure from the simulator state */
void fill_fdm(struct sitl_fdm &fdm) const;
protected:
Location home;
Location location;
float ground_level;
float frame_height;
Matrix3f dcm; // rotation matrix, APM conventions, from body to earth
Vector3f gyro; // rad/s
Vector3f velocity_ef; // m/s, earth frame
Vector3f velocity_body; // m/s, body frame
Vector3f position; // meters, NED from origin
float mass; // kg
float update_frequency;
Vector3f accel_body; // m/s/s NED, body frame
uint64_t time_now_us;
const float gyro_noise;
const float accel_noise;
float rate_hz;
float achieved_rate_hz;
float target_speedup;
float frame_time_us;
float scaled_frame_time_us;
uint64_t last_wall_time_us;
bool on_ground(const Vector3f &pos) const;
/* update location from position */
void update_position(void);
/* rotate to the given yaw */
void set_yaw_degrees(float yaw_degrees);
/* advance time by deltat in seconds */
void time_advance(float deltat);
/* setup the frame step time */
void setup_frame_time(float rate, float speedup);
/* adjust frame_time calculation */
void adjust_frame_time(float rate);
/* try to synchronise simulation time with wall clock time, taking
into account desired speedup */
void sync_frame_time(void);
/* add noise based on throttle level (from 0..1) */
void add_noise(float throttle);
/* return wall clock time in microseconds since 1970 */
uint64_t get_wall_time_us(void) const;
/* return normal distribution random numbers */
double rand_normal(double mean, double stddev);
};

221
libraries/SITL/SIM_Multicopter.cpp
Unescape View File

@ -0,0 +1,221 @@ @@ -0,0 +1,221 @@
/// -*- 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/>.
*/
/*
parent class for aircraft simulators
*/
#include "SIM_Multicopter.h"
#include <stdio.h>
Motor m(90, false, 1);
static const Motor quad_plus_motors[4] =
{
Motor(90, false, 1),
Motor(270, false, 2),
Motor(0, true, 3),
Motor(180, true, 4)
};
static const Motor quad_x_motors[4] =
{
Motor(45, false, 1),
Motor(225, false, 2),
Motor(315, true, 3),
Motor(135, true, 4)
};
static const Motor hexa_motors[6] =
{
Motor(60, false, 1),
Motor(60, true, 7),
Motor(180, true, 4),
Motor(180, false, 8),
Motor(-60, true, 2),
Motor(-60, false, 3),
};
static const Motor hexax_motors[6] =
{
Motor(30, false, 7),
Motor(90, true, 1),
Motor(150, false, 4),
Motor(210, true, 8),
Motor(270, false, 2),
Motor(330, true, 3)
};
static const Motor octa_motors[8] =
{
Motor(0, true, 1),
Motor(180, true, 2),
Motor(45, false, 3),
Motor(135, false, 4),
Motor(-45, false, 5),
Motor(-135, false, 6),
Motor(270, true, 7),
Motor(90, true, 8)
};
static const Motor octa_quad_motors[8] =
{
Motor( 45, false, 1),
Motor( -45, true, 2),
Motor(-135, false, 3),
Motor( 135, true, 4),
Motor( -45, false, 5),
Motor( 45, true, 6),
Motor( 135, false, 7),
Motor(-135, true, 8)
};
/*
table of supported frame types
*/
static const Frame supported_frames[] =
{
Frame("+", 4, quad_plus_motors),
Frame("x", 4, quad_x_motors),
Frame("hexa", 6, hexa_motors),
Frame("hexax", 6, hexax_motors),
Frame("octa", 8, octa_motors),
Frame("octa-quad", 8, octa_quad_motors)
};
/*
constructor
*/
MultiCopter::MultiCopter(const char *home_str, const char *frame_str) :
Aircraft(home_str),
frame(NULL),
hover_throttle(0.51),
terminal_velocity(15.0),
terminal_rotation_rate(4*radians(360.0))
{
for (uint8_t i=0; i<sizeof(supported_frames)/sizeof(supported_frames[0]); i++) {
if (strcasecmp(frame_str, supported_frames[i].name) == 0) {
frame = &supported_frames[i];
}
}
if (frame == NULL) {
printf("Frame '%s' not found", frame_str);
exit(1);
}
/*
scaling from total motor power to Newtons. Allows the copter
to hover against gravity when each motor is at hover_throttle
*/
thrust_scale = (mass * GRAVITY_MSS) / (frame->num_motors * hover_throttle);
frame_height = 0.1;
mass = 1.5;
}
/*
update the multicopter simulation by one time step
*/
void MultiCopter::update(const struct sitl_input &input)
{
float motor_speed[frame->num_motors];
for (uint8_t i=0; i<frame->num_motors; i++) {
uint16_t servo = input.servos[frame->motors[i].servo-1];
// assume 1000 to 2000 PWM range
if (servo <= 1000) {
motor_speed[i] = 0;
} else {
motor_speed[i] = (servo-1000) / 1000.0f;
}
}
// how much time has passed?
float delta_time = frame_time_us * 1.0e-6f;
// rotational acceleration, in rad/s/s, in body frame
Vector3f rot_accel;
float thrust = 0.0f; // newtons
for (uint8_t i=0; i<frame->num_motors; i++) {
rot_accel.x += -radians(5000.0) * sinf(radians(frame->motors[i].angle)) * motor_speed[i];
rot_accel.y += radians(5000.0) * cosf(radians(frame->motors[i].angle)) * motor_speed[i];
if (frame->motors[i].clockwise) {
rot_accel.z -= motor_speed[i] * radians(400.0);
} else {
rot_accel.z += motor_speed[i] * radians(400.0);
}
thrust += motor_speed[i] * thrust_scale; // newtons
}
// 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;
// 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);
accel_body = Vector3f(0, 0, -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 * frame->num_motors));
// 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();
}

75
libraries/SITL/SIM_Multicopter.h
Unescape View File

@ -0,0 +1,75 @@ @@ -0,0 +1,75 @@
/// -*- 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/>.
*/
/*
parent class for aircraft simulators
*/
#include "SIM_Aircraft.h"
/*
class to describe a motor position
*/
class Motor {
public:
float angle;
bool clockwise;
uint8_t servo;
Motor(float _angle, bool _clockwise, uint8_t _servo) :
angle(_angle), // angle in degrees from front
clockwise(_clockwise), // clockwise == true, anti-clockwise == false
servo(_servo) // what servo output drives this motor
{}
};
/*
class to describe a multicopter frame type
*/
class Frame {
public:
const char *name;
uint8_t num_motors;
const Motor *motors;
Frame(const char *_name,
uint8_t _num_motors,
const Motor *_motors) :
name(_name),
num_motors(_num_motors),
motors(_motors) {}
};
/*
a multicopter simulator
*/
class MultiCopter : public Aircraft
{
public:
MultiCopter(const char *home_str, const char *frame_str);
/* update model by one time step */
void update(const struct sitl_input &input);
private:
const Frame *frame;
float hover_throttle; // 0..1
float terminal_velocity; // m/s
const float terminal_rotation_rate;
float thrust_scale;
};
Write Preview
Loading…
Cancel
Save