Andrew Tridgell
9 years ago
6 changed files with 344 additions and 202 deletions
@ -0,0 +1,171 @@
@@ -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; |
||||
} |
||||
|
||||
@ -0,0 +1,60 @@
@@ -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; |
||||
}; |
||||
@ -0,0 +1,38 @@
@@ -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
|
||||
} |
||||
|
||||
@ -0,0 +1,72 @@
@@ -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
|
||||
}; |
||||
Loading…
Reference in new issue