SITL: break up multicopter into Motor/Frame/Multicopter classes
ready for more tiltrotors
This commit is contained in:
Andrew Tridgell
parent
a39a4a6243
commit
c262d6a1b4
171
libraries/SITL/SIM_Frame.cpp
Normal file
171
libraries/SITL/SIM_Frame.cpp
Normal file
@ -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;
|
||||
}
|
||||
|
||||
60
libraries/SITL/SIM_Frame.h
Normal file
60
libraries/SITL/SIM_Frame.h
Normal file
@ -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;
|
||||
};
|
||||
38
libraries/SITL/SIM_Motor.cpp
Normal file
38
libraries/SITL/SIM_Motor.cpp
Normal file
@ -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
|
||||
}
|
||||
|
||||
72
libraries/SITL/SIM_Motor.h
Normal file
72
libraries/SITL/SIM_Motor.h
Normal file
@ -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
|
||||
};
|
||||
@ -24,108 +24,6 @@
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
MultiCopter::MultiCopter(const char *home_str, const char *frame_str) :
|
||||
Aircraft(home_str, frame_str),
|
||||
frame(NULL)
|
||||
@ -139,51 +37,6 @@ MultiCopter::MultiCopter(const char *home_str, const char *frame_str) :
|
||||
frame_height = 0.1;
|
||||
}
|
||||
|
||||
// calculate rotational and linear accelerations
|
||||
void Frame::calculate_forces(const Aircraft &aircraft,
|
||||
const Aircraft::sitl_input &input,
|
||||
Vector3f &rot_accel,
|
||||
Vector3f &body_accel)
|
||||
{
|
||||
// rotational acceleration, in rad/s/s, in body frame
|
||||
float thrust = 0.0f; // newtons
|
||||
|
||||
for (uint8_t i=0; i<num_motors; i++) {
|
||||
float motor_speed = constrain_float((input.servos[motor_offset+motors[i].servo]-1100)/900.0, 0, 1);
|
||||
rot_accel.x += -radians(5000.0) * sinf(radians(motors[i].angle)) * motor_speed;
|
||||
rot_accel.y += radians(5000.0) * cosf(radians(motors[i].angle)) * motor_speed;
|
||||
rot_accel.z += motors[i].yaw_factor * motor_speed * radians(400.0);
|
||||
thrust += motor_speed * thrust_scale; // newtons
|
||||
}
|
||||
|
||||
body_accel = Vector3f(0, 0, -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 / (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;
|
||||
}
|
||||
|
||||
// calculate rotational and linear accelerations
|
||||
void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
|
||||
{
|
||||
|
||||
@ -20,61 +20,10 @@
|
||||
#pragma once
|
||||
|
||||
#include "SIM_Aircraft.h"
|
||||
#include "SIM_Motor.h"
|
||||
#include "SIM_Frame.h"
|
||||
|
||||
namespace SITL {
|
||||
|
||||
/*
|
||||
class to describe a motor position
|
||||
*/
|
||||
class Motor {
|
||||
public:
|
||||
float angle;
|
||||
float yaw_factor;
|
||||
uint8_t servo;
|
||||
uint8_t display_order;
|
||||
|
||||
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
|
||||
{}
|
||||
};
|
||||
|
||||
/*
|
||||
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;
|
||||
};
|
||||
using namespace SITL;
|
||||
|
||||
/*
|
||||
a multicopter simulator
|
||||
@ -97,4 +46,3 @@ protected:
|
||||
Frame *frame;
|
||||
};
|
||||
|
||||
} // namespace SITL
|
||||
|
||||
Loading…
Reference in New Issue
Block a user