SITL: break up multicopter into Motor/Frame/Multicopter classes

ready for more tiltrotors

libraries/SITL/SIM_Frame.cpp

@@ -0,0 +1,171 @@
+/// -*- 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/>.
+ */
+/*
+  multicopter frame simulator class
+*/
+
+#include "SIM_Frame.h"
+#include <AP_Motors/AP_Motors.h>
+
+#include <stdio.h>
+
+using namespace SITL;
+
+static const Motor quad_plus_motors[] =
+{
+    Motor(AP_MOTORS_MOT_1,  90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
+    Motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
+    Motor(AP_MOTORS_MOT_3,   0, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1),
+    Motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3),
+};
+
+static const Motor quad_x_motors[] =
+{
+    Motor(AP_MOTORS_MOT_1,   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
+    Motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
+    Motor(AP_MOTORS_MOT_3,  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4),
+    Motor(AP_MOTORS_MOT_4,  135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2),
+};
+
+static const Motor hexa_motors[] =
+{
+    Motor(AP_MOTORS_MOT_1,   0, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1),
+    Motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
+    Motor(AP_MOTORS_MOT_3,-120, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  5),
+    Motor(AP_MOTORS_MOT_4,  60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
+    Motor(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6),
+    Motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3)
+};
+
+static const Motor hexax_motors[] =
+{
+    Motor(AP_MOTORS_MOT_1,  90, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2),
+    Motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
+    Motor(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  6),
+    Motor(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
+    Motor(AP_MOTORS_MOT_5,  30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
+    Motor(AP_MOTORS_MOT_6,-150, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  4)
+};
+
+static const Motor octa_motors[] =
+{
+    Motor(AP_MOTORS_MOT_1,    0,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1),
+    Motor(AP_MOTORS_MOT_2,  180,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,  5),
+    Motor(AP_MOTORS_MOT_3,   45,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
+    Motor(AP_MOTORS_MOT_4,  135,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
+    Motor(AP_MOTORS_MOT_5,  -45,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8),
+    Motor(AP_MOTORS_MOT_6, -135,  AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6),
+    Motor(AP_MOTORS_MOT_7,  -90,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,  7),
+    Motor(AP_MOTORS_MOT_8,   90,  AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3)
+};
+
+static const Motor octa_quad_motors[] =
+{
+    Motor(AP_MOTORS_MOT_1,   45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
+    Motor(AP_MOTORS_MOT_2,  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  7),
+    Motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
+    Motor(AP_MOTORS_MOT_4,  135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3),
+    Motor(AP_MOTORS_MOT_5,  -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8),
+    Motor(AP_MOTORS_MOT_6,   45, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  2),
+    Motor(AP_MOTORS_MOT_7,  135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
+    Motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  6)
+};
+
+/*
+  table of supported frame types
+ */
+static Frame supported_frames[] =
+{
+    Frame("+",         4, quad_plus_motors),
+    Frame("quad",      4, quad_plus_motors),
+    Frame("copter",    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)
+};
+
+void Frame::init(float _mass, float hover_throttle, float _terminal_velocity, float _terminal_rotation_rate)
+{
+    mass = _mass;
+
+    /*
+       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) / (num_motors * hover_throttle);
+
+    terminal_velocity = _terminal_velocity;
+    terminal_rotation_rate = _terminal_rotation_rate;
+}
+
+/*
+  find a frame by name
+ */
+Frame *Frame::find_frame(const char *name)
+{
+    for (uint8_t i=0; i < ARRAY_SIZE(supported_frames); i++) {
+        if (strcasecmp(name, supported_frames[i].name) == 0) {
+            return &supported_frames[i];
+        }
+    }
+    return NULL;
+}
+
+// calculate rotational and linear accelerations
+void Frame::calculate_forces(const Aircraft &aircraft,
+                             const Aircraft::sitl_input &input,
+                             Vector3f &rot_accel,
+                             Vector3f &body_accel)
+{
+    Vector3f thrust; // newtons
+
+    for (uint8_t i=0; i<num_motors; i++) {
+        Vector3f mraccel, mthrust;
+        motors[i].calculate_forces(input, thrust_scale, motor_offset, mraccel, mthrust);
+        rot_accel += mraccel;
+        thrust += mthrust;
+    }
+
+    body_accel = -thrust/mass;
+
+    if (terminal_rotation_rate > 0) {
+        // rotational air resistance
+        const Vector3f &gyro = aircraft.get_gyro();
+        rot_accel.x -= gyro.x * radians(400.0) / terminal_rotation_rate;
+        rot_accel.y -= gyro.y * radians(400.0) / terminal_rotation_rate;
+        rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate;
+    }
+
+    if (terminal_velocity > 0) {
+        // air resistance
+        Vector3f air_resistance = -aircraft.get_velocity_air_ef() * (GRAVITY_MSS/terminal_velocity);
+        body_accel += aircraft.get_dcm().transposed() * air_resistance;
+    }
+
+    // add some noise
+    const float gyro_noise = radians(0.1);
+    const float accel_noise = 0.3;
+    const float noise_scale = thrust.length() / (thrust_scale * num_motors);
+    rot_accel += Vector3f(aircraft.rand_normal(0, 1),
+                          aircraft.rand_normal(0, 1),
+                          aircraft.rand_normal(0, 1)) * gyro_noise * noise_scale;
+    body_accel += Vector3f(aircraft.rand_normal(0, 1),
+                           aircraft.rand_normal(0, 1),
+                           aircraft.rand_normal(0, 1)) * accel_noise * noise_scale;
+}
+

libraries/SITL/SIM_Frame.h

@@ -0,0 +1,60 @@
+/// -*- 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/>.
+ */
+/*
+  multicopter simulator class
+*/
+
+#pragma once
+
+#include "SIM_Aircraft.h"
+#include "SIM_Motor.h"
+
+using namespace SITL;
+
+/*
+  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) {}
+
+
+    // find a frame by name
+    static Frame *find_frame(const char *name);
+
+    // initialise frame
+    void init(float mass, float hover_throttle, float terminal_velocity, float terminal_rotation_rate);
+
+    // calculate rotational and linear accelerations
+    void calculate_forces(const Aircraft &aircraft,
+                          const Aircraft::sitl_input &input,
+                          Vector3f &rot_accel, Vector3f &body_accel);
+
+    float terminal_velocity;
+    float terminal_rotation_rate;
+    float thrust_scale;
+    float mass;
+    uint8_t motor_offset;
+};

libraries/SITL/SIM_Motor.cpp

@@ -0,0 +1,38 @@
+/// -*- 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/>.
+ */
+/*
+  simple electric motor simulator class
+*/
+
+#include "SIM_Motor.h"
+#include <AP_Motors/AP_Motors.h>
+
+using namespace SITL;
+
+// calculate rotational accel and thrust for a motor
+void Motor::calculate_forces(const Aircraft::sitl_input &input,
+                             const float thrust_scale,
+                             uint8_t motor_offset,
+                             Vector3f &rot_accel,
+                             Vector3f &thrust) const
+{
+    float motor_speed = constrain_float((input.servos[motor_offset+servo]-1100)/900.0, 0, 1);
+    rot_accel.x = -radians(5000.0) * sinf(radians(angle)) * motor_speed;
+    rot_accel.y =  radians(5000.0) * cosf(radians(angle)) * motor_speed;
+    rot_accel.z = yaw_factor * motor_speed * radians(400.0);
+    thrust(0, 0, motor_speed * thrust_scale); // newtons
+}
+

libraries/SITL/SIM_Motor.h

@@ -0,0 +1,72 @@
+/// -*- 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/>.
+ */
+/*
+  simple electric motor simulation class
+*/
+
+#pragma once
+
+#include "SIM_Aircraft.h"
+
+using namespace SITL;
+
+/*
+  class to describe a motor position
+ */
+class Motor {
+public:
+    float angle;
+    float yaw_factor;
+    uint8_t servo;
+    uint8_t display_order;
+
+    // support for tilting motors
+    int8_t roll_servo = -1;
+    float roll_min, roll_max;
+    int8_t pitch_servo = -1;
+    float pitch_min, pitch_max;
+
+    Motor(uint8_t _servo, float _angle, float _yaw_factor, uint8_t _display_order) :
+        servo(_servo), // what servo output drives this motor
+        angle(_angle), // angle in degrees from front
+        yaw_factor(_yaw_factor), // positive is clockwise
+        display_order(_display_order) // order for clockwise display
+    {}
+
+    /*
+      alternative constructor for tiltable motors
+     */
+    Motor(uint8_t _servo, float _angle, float _yaw_factor, uint8_t _display_order,
+          int8_t _roll_servo, float _roll_min, float _roll_max,
+          int8_t _pitch_servo, float _pitch_min, float _pitch_max) :
+        servo(_servo), // what servo output drives this motor
+        angle(_angle), // angle in degrees from front
+        yaw_factor(_yaw_factor), // positive is clockwise
+        display_order(_display_order), // order for clockwise display
+        roll_servo(_roll_servo),
+        roll_min(_roll_min),
+        roll_max(_roll_max),
+        pitch_servo(_pitch_servo),
+        pitch_min(_pitch_min),
+        pitch_max(_pitch_max)
+    {}
+
+    void calculate_forces(const Aircraft::sitl_input &input,
+                          float thrust_scale,
+                          uint8_t motor_offset,
+                          Vector3f &rot_accel, // rad/sec
+                          Vector3f &body_thrust) const; // newtons
+};