Andrew Tridgell
9 years ago
6 changed files with 344 additions and 202 deletions
@ -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 @@ |
|||||||
|
/// -*- 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 @@ |
|||||||
|
/// -*- 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 @@ |
|||||||
|
/// -*- 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