SITL: initial conversion of multicopter sim to C++

libraries/SITL/SIM_Aircraft.cpp

@@ -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;
+    }
+}

libraries/SITL/SIM_Aircraft.h

@@ -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);
+};

libraries/SITL/SIM_Multicopter.cpp

@@ -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();
+}

libraries/SITL/SIM_Multicopter.h

@@ -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;
+};
+
+