Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
dd52ee29081eac905a62cc45fce1bcb8b42cf1a2
ardupilot/libraries/SITL/SIM_Frame.cpp
T
Codir de5add012e SITL: fix the ROMFS fallback when loading json models 
Both loaders build "@ROMFS/models/<name>" into fname when the model is not
found on the filesystem, then stat() the original path again -- the one
already known to be missing -- so the fallback can never succeed and a model
that exists only in ROMFS panics. fname was also never passed to load_json()
and never freed.

Stat and load the resolved path, guard it against a failed asprintf, and free
it.
2026-08-09 10:13:42 +10:00

951 lines
38 KiB
C++
Raw Blame History

/*
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 <AP_Baro/AP_Baro.h>
#include <AP_Filesystem/AP_Filesystem.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include "SIM_Aircraft.h"
#include "SIM_config.h"
#include <stdio.h>
#include <sys/stat.h>
using namespace SITL;
#if AP_SIM_ENABLED
// default to no bluff body or momentum drag on planes, where the
// plane model handles drag
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
#define SIM_FRAME_BBDRAG_DEFAULT 0.0
#define SIM_FRAME_MDRAG_DEFAULT 0.0
#else
#define SIM_FRAME_BBDRAG_DEFAULT 1.0
#define SIM_FRAME_MDRAG_DEFAULT 0.2
#endif
// user settable multicopter model parameters. Defaults can be
// overridden by a json model file
const AP_Param::GroupInfo Frame::var_info[] = {
// @Param: MASS
// @DisplayName: model mass
// @Description: mass of the multicopter model
// @Units: kg
AP_GROUPINFO("MASS", 1, Frame, model.mass, 3.0),
// @Param: DIAG_SZ
// @DisplayName: model diagonal size
// @Description: diagonal motor to motor distance of the model
// @Units: m
AP_GROUPINFO("DIAG_SZ", 2, Frame, model.diagonal_size, 0.35),
// @Param: REF_SPD
// @DisplayName: reference speed
// @Description: airspeed in the drag reference test, at a fixed lean angle
// @Units: m/s
AP_GROUPINFO("REF_SPD", 3, Frame, model.refSpd, 15.08),
// @Param: REF_ANG
// @DisplayName: reference angle
// @Description: lean angle in the drag reference test
// @Units: deg
AP_GROUPINFO("REF_ANG", 4, Frame, model.refAngle, 45),
// @Param: REF_VOLT
// @DisplayName: reference voltage
// @Description: battery voltage in the drag reference test
// @Units: V
AP_GROUPINFO("REF_VOLT", 5, Frame, model.refVoltage, 12.09),
// @Param: REF_AMP
// @DisplayName: reference current
// @Description: battery current in the drag reference test
// @Units: A
AP_GROUPINFO("REF_AMP", 6, Frame, model.refCurrent, 29.3),
// @Param: REF_ALT
// @DisplayName: reference altitude
// @Description: altitude AMSL of the drag reference test
// @Units: m
AP_GROUPINFO("REF_ALT", 7, Frame, model.refAlt, 593),
// @Param: BAT_RES
// @DisplayName: battery resistance
// @Description: battery resistance
// @Units: Ohm
AP_GROUPINFO("BAT_RES", 8, Frame, model.refBatRes, 0.01),
// @Param: BAT_VOLT
// @DisplayName: battery full voltage
// @Description: full pack battery voltage
// @Units: V
AP_GROUPINFO("BAT_VOLT", 9, Frame, model.maxVoltage, 12.6),
// @Param: BAT_CAP
// @DisplayName: battery capacity
// @Description: battery capacity, zero for unlimited
// @Units: Ah
AP_GROUPINFO("BAT_CAP", 10, Frame, model.battCapacityAh, 0),
// @Param: HOVR_THR
// @DisplayName: hover throttle
// @Description: throttle output (CTUN.ThO) at hover at the reference altitude
// @Range: 0.05 1
AP_GROUPINFO("HOVR_THR", 11, Frame, model.hoverThrOut, 0.39),
// @Param: EXPO
// @DisplayName: thrust expo
// @Description: motor thrust expo, matching MOT_THST_EXPO
AP_GROUPINFO("EXPO", 12, Frame, model.propExpo, 0.65),
// @Param: ROT_RATE
// @DisplayName: rotation rate
// @Description: terminal yaw rotation rate
// @Units: deg/s
AP_GROUPINFO("ROT_RATE", 13, Frame, model.refRotRate, 120),
// @Param: PWM_MIN
// @DisplayName: minimum PWM
// @Description: minimum motor PWM in the reference test, matching MOT_PWM_MIN
// @Units: PWM
AP_GROUPINFO("PWM_MIN", 14, Frame, model.pwmMin, 1000),
// @Param: PWM_MAX
// @DisplayName: maximum PWM
// @Description: maximum motor PWM in the reference test, matching MOT_PWM_MAX
// @Units: PWM
AP_GROUPINFO("PWM_MAX", 15, Frame, model.pwmMax, 2000),
// @Param: SPIN_MIN
// @DisplayName: motor spin minimum
// @Description: minimum motor spin fraction, matching MOT_SPIN_MIN
// @Range: 0 1
AP_GROUPINFO("SPIN_MIN", 16, Frame, model.spin_min, 0.15),
// @Param: SPIN_MAX
// @DisplayName: motor spin maximum
// @Description: maximum motor spin fraction, matching MOT_SPIN_MAX
// @Range: 0 1
AP_GROUPINFO("SPIN_MAX", 17, Frame, model.spin_max, 0.95),
// @Param: SLEW_MAX
// @DisplayName: motor slew rate
// @Description: maximum motor slew rate in throttle fraction per second
// @Units: 1/s
AP_GROUPINFO("SLEW_MAX", 18, Frame, model.slew_max, 150),
// @Param: DISCAREA
// @DisplayName: rotor disc area
// @Description: total rotor disc area. Coaxial rotors count as one rotor only
// @Units: m.m
AP_GROUPINFO("DISCAREA", 19, Frame, model.disc_area, 0.385),
// @Param: MDRAG
// @DisplayName: momentum drag coefficient
// @Description: momentum drag coefficient of the rotors. Defaults to zero on planes where the plane model handles drag
AP_GROUPINFO("MDRAG", 20, Frame, model.mdrag_coef, SIM_FRAME_MDRAG_DEFAULT),
// @Param: BBDRAG
// @DisplayName: bluff body drag coefficient
// @Description: scaling of the bluff body drag derived from the reference test, zero for no bluff body drag. Defaults to zero on planes where the plane model handles drag
AP_GROUPINFO("BBDRAG", 21, Frame, model.bbdrag_coef, SIM_FRAME_BBDRAG_DEFAULT),
AP_GROUPEND
};
#endif // AP_SIM_ENABLED
static 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 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),
};
// motor order to match betaflight conventions
// See: https://fpvfrenzy.com/betaflight-motor-order/
static Motor quad_bf_x_motors[] =
{
Motor(AP_MOTORS_MOT_1, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
Motor(AP_MOTORS_MOT_2, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,1),
Motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,3),
Motor(AP_MOTORS_MOT_4, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
};
// motor order to match betaflight conventions, reversed direction
static Motor quad_bf_x_rev_motors[] =
{
Motor(AP_MOTORS_MOT_1, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
Motor(AP_MOTORS_MOT_2, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1),
Motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3),
Motor(AP_MOTORS_MOT_4, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
};
// motor order to match DJI conventions
// See: https://forum44.djicdn.com/data/attachment/forum/201711/26/172348bppvtt1ot1nrtp5j.jpg
static Motor quad_dji_x_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, 4),
Motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
};
// motor order so that test order matches motor order ("clockwise X")
static Motor quad_cw_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_CW, 2),
Motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
Motor(AP_MOTORS_MOT_4, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
};
#if AP_SIM_FRAME_COPTER_DOTRIACONTA_OCTAQUAD_X_ENABLED
static Motor dotriaconta_octaquad_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, 17),
Motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 25),
Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 9),
Motor(AP_MOTORS_MOT_5, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
Motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 18),
Motor(AP_MOTORS_MOT_7, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 26),
Motor(AP_MOTORS_MOT_8, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 10),
Motor(AP_MOTORS_MOT_9, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
Motor(AP_MOTORS_MOT_10, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 19),
Motor(AP_MOTORS_MOT_11, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 27),
Motor(AP_MOTORS_MOT_12, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 11),
Motor(AP_MOTORS_MOT_13, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
Motor(AP_MOTORS_MOT_14, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 20),
Motor(AP_MOTORS_MOT_15, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 28),
Motor(AP_MOTORS_MOT_16, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 12),
Motor(AP_MOTORS_MOT_17, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
Motor(AP_MOTORS_MOT_18, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 21),
Motor(AP_MOTORS_MOT_19, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 29),
Motor(AP_MOTORS_MOT_20, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 13),
Motor(AP_MOTORS_MOT_21, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
Motor(AP_MOTORS_MOT_22, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 22),
Motor(AP_MOTORS_MOT_23, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 30),
Motor(AP_MOTORS_MOT_24, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 14),
Motor(AP_MOTORS_MOT_25, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
Motor(AP_MOTORS_MOT_26, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 23),
Motor(AP_MOTORS_MOT_27, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 31),
Motor(AP_MOTORS_MOT_28, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 15),
Motor(AP_MOTORS_MOT_29, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8),
Motor(AP_MOTORS_MOT_30, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 24),
Motor(AP_MOTORS_MOT_31, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 32),
Motor(AP_MOTORS_MOT_32, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 16),
};
#endif // AP_SIM_FRAME_COPTER_DOTRIACONTA_OCTAQUAD_X_ENABLED
static Motor tiltquad_h_vectored_motors[] =
{
Motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1, -1, 0, 0, 7, 10, -90),
Motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3, -1, 0, 0, 8, 10, -90),
Motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4, -1, 0, 0, 8, 10, -90),
Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2, -1, 0, 0, 7, 10, -90),
};
static Motor tiltquad[] =
{
Motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1, -1, 0, 0, 7, 10, -90),
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, -1, 0, 0, 8, 10, -90),
Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
};
// collective pitch quad helicopter, matching AP_MotorsHeli_Quad's
// layout, with rotor speed control on channel 8 to match
// AP_MotorsHeli_RSC's default channel
// Run in a shell: ./Tools/autotest/fg_heliquad_view.sh
// then Tools/autotest/sim_vehicle.py -v ArduCopter -f heli-quad --enable-fgview
static Motor heliquad_motors[] =
{
Motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1, 7),
Motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3, 7),
Motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4, 7),
Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2, 7),
};
static 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 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 Motor hexa_dji_x_motors[] =
{
Motor(AP_MOTORS_MOT_1, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
Motor(AP_MOTORS_MOT_2, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
Motor(AP_MOTORS_MOT_3, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
Motor(AP_MOTORS_MOT_4, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
Motor(AP_MOTORS_MOT_5, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
Motor(AP_MOTORS_MOT_6, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2)
};
static Motor hexa_cw_x_motors[] =
{
Motor(AP_MOTORS_MOT_1, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
Motor(AP_MOTORS_MOT_2, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
Motor(AP_MOTORS_MOT_3, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
Motor(AP_MOTORS_MOT_4, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
Motor(AP_MOTORS_MOT_5, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
Motor(AP_MOTORS_MOT_6, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6)
};
static 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 Motor octa_dji_x_motors[] =
{
Motor(AP_MOTORS_MOT_1, 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
Motor(AP_MOTORS_MOT_2, -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8),
Motor(AP_MOTORS_MOT_3, -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
Motor(AP_MOTORS_MOT_4, -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
Motor(AP_MOTORS_MOT_5, -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
Motor(AP_MOTORS_MOT_6, 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
Motor(AP_MOTORS_MOT_7, 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
Motor(AP_MOTORS_MOT_8, 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2)
};
static Motor octa_cw_x_motors[] =
{
Motor(AP_MOTORS_MOT_1, 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
Motor(AP_MOTORS_MOT_2, 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
Motor(AP_MOTORS_MOT_3, 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
Motor(AP_MOTORS_MOT_4, 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
Motor(AP_MOTORS_MOT_5, -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
Motor(AP_MOTORS_MOT_6, -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
Motor(AP_MOTORS_MOT_7, -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
Motor(AP_MOTORS_MOT_8, -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8)
};
static 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)
};
static Motor octa_quad_corotating_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_CW, 8),
Motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
Motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
Motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6)
};
static Motor octa_quad_cw_corotating_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_CCW, 2),
Motor(AP_MOTORS_MOT_3, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3),
Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
Motor(AP_MOTORS_MOT_5, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
Motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6),
Motor(AP_MOTORS_MOT_7, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7),
Motor(AP_MOTORS_MOT_8, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8),
};
static Motor octa_quad_cw_x_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, 2),
Motor(AP_MOTORS_MOT_3, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3),
Motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
Motor(AP_MOTORS_MOT_5, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
Motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
Motor(AP_MOTORS_MOT_7, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7),
Motor(AP_MOTORS_MOT_8, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8)
};
static Motor dodeca_hexa_motors[] =
{
Motor(AP_MOTORS_MOT_1, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
Motor(AP_MOTORS_MOT_2, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
Motor(AP_MOTORS_MOT_3, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3),
Motor(AP_MOTORS_MOT_4, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4),
Motor(AP_MOTORS_MOT_5, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
Motor(AP_MOTORS_MOT_6, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
Motor(AP_MOTORS_MOT_7, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7),
Motor(AP_MOTORS_MOT_8, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8),
Motor(AP_MOTORS_MOT_9, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 9),
Motor(AP_MOTORS_MOT_10, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 10),
Motor(AP_MOTORS_MOT_11, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 11),
Motor(AP_MOTORS_MOT_12, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 12)
};
static Motor hexadeca_octa_motors[] =
{
Motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1),
Motor(AP_MOTORS_MOT_2, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
Motor(AP_MOTORS_MOT_3, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
Motor(AP_MOTORS_MOT_4, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
Motor(AP_MOTORS_MOT_5, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5),
Motor(AP_MOTORS_MOT_6, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6),
Motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
Motor(AP_MOTORS_MOT_8, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8),
Motor(AP_MOTORS_MOT_9, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 9),
Motor(AP_MOTORS_MOT_10, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 10),
Motor(AP_MOTORS_MOT_11, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 11),
Motor(AP_MOTORS_MOT_12, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 12),
Motor(AP_MOTORS_MOT_13, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 13),
Motor(AP_MOTORS_MOT_14, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 14),
Motor(AP_MOTORS_MOT_15, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 15),
Motor(AP_MOTORS_MOT_16, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 16)
};
static Motor hexadeca_octa_cw_x_motors[] =
{
Motor(AP_MOTORS_MOT_1, 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1),
Motor(AP_MOTORS_MOT_2, 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
Motor(AP_MOTORS_MOT_3, 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
Motor(AP_MOTORS_MOT_4, 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
Motor(AP_MOTORS_MOT_5, 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5),
Motor(AP_MOTORS_MOT_6, 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6),
Motor(AP_MOTORS_MOT_7, 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
Motor(AP_MOTORS_MOT_8, 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8),
Motor(AP_MOTORS_MOT_9, -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 9),
Motor(AP_MOTORS_MOT_10, -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 10),
Motor(AP_MOTORS_MOT_11, -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 11),
Motor(AP_MOTORS_MOT_12, -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 12),
Motor(AP_MOTORS_MOT_13, -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 13),
Motor(AP_MOTORS_MOT_14, -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 14),
Motor(AP_MOTORS_MOT_15, -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 15),
Motor(AP_MOTORS_MOT_16, -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 16)
};
static Motor deca_motors[] =
{
Motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
Motor(AP_MOTORS_MOT_2, 36, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
Motor(AP_MOTORS_MOT_3, 72, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
Motor(AP_MOTORS_MOT_4, 108, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
Motor(AP_MOTORS_MOT_5, 144, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
Motor(AP_MOTORS_MOT_6, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
Motor(AP_MOTORS_MOT_7, -144, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
Motor(AP_MOTORS_MOT_8, -108, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8),
Motor(AP_MOTORS_MOT_9, -72, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 9),
Motor(AP_MOTORS_MOT_10, -36, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 10)
};
static Motor deca_cw_x_motors[] =
{
Motor(AP_MOTORS_MOT_1, 18, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
Motor(AP_MOTORS_MOT_2, 54, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2),
Motor(AP_MOTORS_MOT_3, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
Motor(AP_MOTORS_MOT_4, 126, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
Motor(AP_MOTORS_MOT_5, 162, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5),
Motor(AP_MOTORS_MOT_6, -162, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6),
Motor(AP_MOTORS_MOT_7, -126, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7),
Motor(AP_MOTORS_MOT_8, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8),
Motor(AP_MOTORS_MOT_9, -54, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 9),
Motor(AP_MOTORS_MOT_10, -18, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 10)
};
static Motor tri_motors[] =
{
Motor(AP_MOTORS_MOT_1, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1),
Motor(AP_MOTORS_MOT_2, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3),
Motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2, AP_MOTORS_MOT_7, 60, -60, -1, 0, 0),
};
static Motor tilttri_motors[] =
{
Motor(AP_MOTORS_MOT_1, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1, -1, 0, 0, AP_MOTORS_MOT_8, 0, -90),
Motor(AP_MOTORS_MOT_2, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3, -1, 0, 0, AP_MOTORS_MOT_8, 0, -90),
Motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2, AP_MOTORS_MOT_7, 60, -60, -1, 0, 0),
};
static Motor tilttri_vectored_motors[] =
{
Motor(AP_MOTORS_MOT_1, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1, -1, 0, 0, 7, 10, -90),
Motor(AP_MOTORS_MOT_2, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3, -1, 0, 0, 8, 10, -90),
Motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2)
};
static Motor y6_motors[] =
{
Motor(AP_MOTORS_MOT_1, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2),
Motor(AP_MOTORS_MOT_2, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5),
Motor(AP_MOTORS_MOT_3, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6),
Motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
Motor(AP_MOTORS_MOT_5, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1),
Motor(AP_MOTORS_MOT_6, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3)
};
/*
FireflyY6 is a Y6 with front motors tiltable using servo on channel 9 (output 8)
*/
static Motor firefly_motors[] =
{
Motor(AP_MOTORS_MOT_1, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3),
Motor(AP_MOTORS_MOT_2, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1, -1, 0, 0, 6, 0, -90),
Motor(AP_MOTORS_MOT_3, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5, -1, 0, 0, 6, 0, -90),
Motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4),
Motor(AP_MOTORS_MOT_5, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2, -1, 0, 0, 6, 0, -90),
Motor(AP_MOTORS_MOT_6, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6, -1, 0, 0, 6, 0, -90)
};
typedef struct {
const char *name;
uint8_t num_motors;
Motor *motors;
} FrameTemplate;
/*
table of supported frame types. String order is important for
partial name matching
*/
static const FrameTemplate supported_frame_templates[] =
{
{"+", 4, quad_plus_motors},
{"quad", 4, quad_plus_motors},
{"copter", 4, quad_plus_motors},
{"x", 4, quad_x_motors},
{"bfxrev", 4, quad_bf_x_rev_motors},
{"bfx", 4, quad_bf_x_motors},
#if AP_SIM_FRAME_COPTER_DOTRIACONTA_OCTAQUAD_X_ENABLED
{"dotriaconta", 32, dotriaconta_octaquad_x_motors},
#endif // AP_SIM_FRAME_COPTER_DOTRIACONTA_OCTAQUAD_X_ENABLED
{"djix", 4, quad_dji_x_motors},
{"cwx", 4, quad_cw_x_motors},
{"tilthvec", 4, tiltquad_h_vectored_motors},
{"heli-quad", 4, heliquad_motors},
{"hexadeca-octa", 16, hexadeca_octa_motors},
{"hexadeca-octa-cwx", 16, hexadeca_octa_cw_x_motors},
{"hexax", 6, hexax_motors},
{"hexa-cwx", 6, hexa_cw_x_motors},
{"hexa-dji", 6, hexa_dji_x_motors},
{"hexa", 6, hexa_motors},
{"octa-cwx", 8, octa_cw_x_motors},
{"octa-dji", 8, octa_dji_x_motors},
{"octa-quad-cwx",8, octa_quad_cw_x_motors},
{"octa-quad-cor", 8, octa_quad_corotating_motors},
{"octa-quad-cw-cor", 8, octa_quad_cw_corotating_motors},
{"octa-quad", 8, octa_quad_motors},
{"octa", 8, octa_motors},
{"deca", 10, deca_motors},
{"deca-cwx", 10, deca_cw_x_motors},
{"dodeca-hexa", 12, dodeca_hexa_motors},
{"tri", 3, tri_motors},
{"tilttrivec",3, tilttri_vectored_motors},
{"tilttri", 3, tilttri_motors},
{"y6", 6, y6_motors},
{"firefly", 6, firefly_motors},
{"tilt", 4, tiltquad},
};
// get air density in kg/m^3
float Frame::get_air_density(float alt_amsl) const
{
return AP_Baro::get_air_density_for_alt_amsl(alt_amsl);
}
/*
load frame specific parameters from a json file if available
*/
void Frame::load_frame_params(const char *model_json)
{
char *fname = nullptr;
struct stat st;
if (AP::FS().stat(model_json, &st) == 0) {
fname = strdup(model_json);
} else {
IGNORE_RETURN(asprintf(&fname, "@ROMFS/models/%s", model_json));
if (fname == nullptr || AP::FS().stat(fname, &st) != 0) {
AP_HAL::panic("%s failed to load", model_json);
}
}
if (fname == nullptr) {
AP_HAL::panic("%s failed to load", model_json);
}
AP_JSON::value *obj = AP_JSON::load_json(fname);
if (obj == nullptr) {
AP_HAL::panic("%s failed to load", fname);
}
enum class VarType {
FLOAT,
AP_FLOAT,
VECTOR3F,
};
struct json_search {
const char *label;
void *ptr;
VarType t;
};
json_search vars[] = {
#define FRAME_VAR(s) { #s, &model.s, VarType::AP_FLOAT }
FRAME_VAR(mass),
FRAME_VAR(diagonal_size),
FRAME_VAR(refSpd),
FRAME_VAR(refAngle),
FRAME_VAR(refVoltage),
FRAME_VAR(refCurrent),
FRAME_VAR(refAlt),
FRAME_VAR(maxVoltage),
FRAME_VAR(battCapacityAh),
FRAME_VAR(refBatRes),
FRAME_VAR(propExpo),
FRAME_VAR(refRotRate),
FRAME_VAR(hoverThrOut),
FRAME_VAR(pwmMin),
FRAME_VAR(pwmMax),
FRAME_VAR(spin_min),
FRAME_VAR(spin_max),
FRAME_VAR(slew_max),
FRAME_VAR(disc_area),
FRAME_VAR(mdrag_coef),
FRAME_VAR(bbdrag_coef),
{"moment_inertia", &model.moment_of_inertia, VarType::VECTOR3F},
{"refTempC", &model.refTempC, VarType::FLOAT},
{"num_motors", &model.num_motors, VarType::FLOAT},
};
for (uint8_t i=0; i<ARRAY_SIZE(vars); i++) {
auto v = obj->get(vars[i].label);
if (v.is<AP_JSON::null>()) {
// use default value
continue;
}
if (vars[i].t == VarType::FLOAT) {
parse_float(v, vars[i].label, *((float *)vars[i].ptr));
} else if (vars[i].t == VarType::AP_FLOAT) {
// json model values become the defaults for the SIM_FRM_ parameters
float value;
parse_float(v, vars[i].label, value);
((AP_Float *)vars[i].ptr)->set_default(value);
} else if (vars[i].t == VarType::VECTOR3F) {
parse_vector3(v, vars[i].label, *(Vector3f *)vars[i].ptr);
}
}
json_search per_motor_vars[] = {
{"position", &model.motor_pos, VarType::VECTOR3F},
{"vector", &model.motor_thrust_vec, VarType::VECTOR3F},
{"yaw", &model.yaw_factor, VarType::FLOAT},
};
char label_name[20];
for (uint8_t i=0; i<ARRAY_SIZE(per_motor_vars); i++) {
for (uint8_t j=0; j<SIM_FRAME_MAX_ACTUATORS; j++) {
snprintf(label_name, 20, "motor%i_%s", j+1, per_motor_vars[i].label);
auto v = obj->get(label_name);
if (v.is<AP_JSON::null>()) {
// use default value
continue;
}
if (per_motor_vars[i].t == VarType::FLOAT) {
parse_float(v, label_name, *(((float *)per_motor_vars[i].ptr) + j));
} else if (per_motor_vars[i].t == VarType::VECTOR3F) {
parse_vector3(v, label_name, *(((Vector3f *)per_motor_vars[i].ptr) + j));
}
}
}
delete obj;
::printf("Loaded model params from %s\n", fname);
free(fname);
}
void Frame::parse_float(AP_JSON::value val, const char* label, float &param) {
if (!val.is<double>()) {
AP_HAL::panic("Bad json type for %s: %s", label, val.to_str().c_str());
}
param = val.get<double>();
}
void Frame::parse_vector3(AP_JSON::value val, const char* label, Vector3f &param) {
if (!val.is<AP_JSON::value::array>() || !val.contains(2) || val.contains(3)) {
AP_HAL::panic("Bad json type for %s: %s", label, val.to_str().c_str());
}
for (uint8_t j=0; j<3; j++) {
parse_float(val.get(j), label, param[j]);
}
}
#if AP_SIM_ENABLED
/*
initialise the frame
*/
void Frame::init(const char *frame_str)
{
auto *sitl = AP::sitl();
if (sitl != nullptr) {
sitl->models.simframe_ptr = this;
}
AP_Param::setup_object_defaults(this, var_info);
const char *colon = strchr(frame_str, ':');
size_t slen = strlen(frame_str);
if (colon != nullptr && slen > 5 && strcmp(&frame_str[slen-5], ".json") == 0) {
load_frame_params(colon+1);
}
if (uint8_t(model.num_motors) != num_motors) {
::printf("Warning model expected %u motors and got %u\n", uint8_t(model.num_motors), num_motors);
}
update_parameters();
}
/*
apply settings from the model parameters. Called on each physics
step so changes to SIM_FRM_ parameters take effect. Note that
parameters loaded from eeprom are only applied after the frame is
created, so this cannot be done just once in init()
*/
void Frame::update_parameters(void)
{
mass = model.mass * mass_scale;
const float drag_force = model.mass * GRAVITY_MSS * tanf(radians(model.refAngle));
const float cos_tilt = cosf(radians(model.refAngle));
const float airspeed_bf = model.refSpd * cos_tilt;
const float ref_thrust = model.mass * GRAVITY_MSS / cos_tilt;
const float ref_air_density = get_air_density(model.refAlt);
mdrag_coef = model.mdrag_coef;
const float momentum_drag = cos_tilt * mdrag_coef * airspeed_bf * sqrtf(ref_thrust * ref_air_density * model.disc_area);
if (momentum_drag > drag_force) {
mdrag_coef *= drag_force / momentum_drag;
areaCd = 0.0;
} else {
areaCd = model.bbdrag_coef * (drag_force - momentum_drag) / (0.5f * ref_air_density * sq(model.refSpd));
}
const float drag_bcoef = is_positive(areaCd) ? model.mass / areaCd : 0.0;
const float drag_mcoef = (momentum_drag / (model.mass * airspeed_bf)) * sqrtf(1.225f / ref_air_density);
if ((!is_equal(drag_bcoef, last_drag_bcoef) || !is_equal(drag_mcoef, last_drag_mcoef)) &&
isfinite(drag_bcoef) && isfinite(drag_mcoef)) {
last_drag_bcoef = drag_bcoef;
last_drag_mcoef = drag_mcoef;
::printf("Suggested EK3_DRAG_BCOEF_* = %.3f, EK3_DRAG_MCOEF = %.3f\n", drag_bcoef, drag_mcoef);
}
terminal_rotation_rate = model.refRotRate;
const float hover_thrust = model.mass * GRAVITY_MSS;
const float hover_power = model.refCurrent * model.refVoltage;
const float hover_velocity_out = 2 * hover_power / hover_thrust;
const float effective_disc_area = hover_thrust / (0.5 * ref_air_density * sq(hover_velocity_out));
const float velocity_max = hover_velocity_out / sqrtf(MAX(model.hoverThrOut.get(), 0.01f));
const float effective_prop_area = effective_disc_area / num_motors;
const float true_prop_area = model.disc_area / num_motors;
// power_factor is ratio of power consumed per newton of thrust
const float power_factor = hover_power / hover_thrust;
for (uint8_t i=0; i<num_motors; i++) {
motors[i].setup_params(model.pwmMin.get(), model.pwmMax.get(), model.spin_min, model.spin_max, model.propExpo, model.slew_max,
model.diagonal_size, power_factor, model.maxVoltage, effective_prop_area, velocity_max,
model.motor_pos[i], model.motor_thrust_vec[i], model.yaw_factor[i], true_prop_area,
mdrag_coef);
}
if (is_zero(model.moment_of_inertia.x) || is_zero(model.moment_of_inertia.y) || is_zero(model.moment_of_inertia.z)) {
// if no inertia provided, assume 50% of mass on ring around center
moment_of_inertia.x = model.mass * 0.25 * sq(model.diagonal_size*0.5);
moment_of_inertia.y = moment_of_inertia.x;
moment_of_inertia.z = model.mass * 0.5 * sq(model.diagonal_size*0.5);
} else {
moment_of_inertia = model.moment_of_inertia;
}
if (!is_equal(last_batt_voltage, model.maxVoltage.get()) ||
!is_equal(last_batt_cap, model.battCapacityAh.get()) ||
!is_equal(last_batt_res, model.refBatRes.get())) {
const float prev_cap = last_batt_cap;
last_batt_voltage = model.maxVoltage;
last_batt_cap = model.battCapacityAh;
last_batt_res = model.refBatRes;
battery_dirty = true;
// setup reasonable defaults for battery
AP_Param::set_default_by_name("SIM_BATT_VOLTAGE", model.maxVoltage);
AP_Param::set_default_by_name("SIM_BATT_CAP_AH", model.battCapacityAh);
if (model.battCapacityAh > 0) {
AP_Param::set_default_by_name("BATT_CAPACITY", model.battCapacityAh*1000);
} else if (prev_cap > 0) {
// returning to unlimited capacity, mark vehicle capacity unknown
AP_Param::set_default_by_name("BATT_CAPACITY", 0);
}
}
}
// returns true once when the battery model values have changed, so
// the vehicle can re-setup the simulated battery
bool Frame::battery_changed(void)
{
bool ret = battery_dirty;
battery_dirty = false;
return ret;
}
/*
create a frame by name from its template
*/
Frame *Frame::create_frame(const char *name)
{
for (uint8_t i=0; i < ARRAY_SIZE(supported_frame_templates); i++) {
auto &tplate = supported_frame_templates[i]; // `template` is a reserved word
// do partial name matching to allow for frame variants
if (strncasecmp(name, tplate.name, strlen(tplate.name)) == 0) {
return NEW_NOTHROW Frame(tplate.name, tplate.num_motors, tplate.motors);
}
}
return nullptr;
}
// calculate rotational and linear accelerations
void Frame::calculate_forces(const Aircraft &aircraft,
const struct sitl_input &input,
Vector3f &rot_accel,
Vector3f &body_accel,
float* rpm)
{
Vector3f thrust; // newtons
Vector3f torque;
update_parameters();
const float air_density = get_air_density(aircraft.get_location().alt*0.01);
const Vector3f gyro = aircraft.get_gyro();
Vector3f vel_air_bf = aircraft.get_dcm().transposed() * aircraft.get_velocity_air_ef();
const auto *_sitl = AP::sitl();
for (uint8_t i=0; i<num_motors; i++) {
Vector3f mtorque, mthrust;
motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf,
gyro, air_density, aircraft.get_battery_voltage(), true);
torque += mtorque;
thrust += mthrust;
// simulate motor rpm
if (!is_zero(_sitl->vibe_motor)) {
rpm[motor_offset+i] = motors[i].get_command() * AP::sitl()->vibe_motor * 60.0f;
}
}
// calculate total rotational acceleration
rot_accel.x = torque.x / moment_of_inertia.x;
rot_accel.y = torque.y / moment_of_inertia.y;
rot_accel.z = torque.z / moment_of_inertia.z;
if (terminal_rotation_rate > 0) {
// rotational air resistance
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 (is_positive(areaCd)) {
// use the model params to calculate drag
Vector3f drag_bf;
drag_bf.x = areaCd * 0.5f * air_density * sq(vel_air_bf.x);
if (is_negative(vel_air_bf.x)) {
drag_bf.x = -drag_bf.x;
}
drag_bf.y = areaCd * 0.5f * air_density * sq(vel_air_bf.y);
if (is_negative(vel_air_bf.y)) {
drag_bf.y = -drag_bf.y;
}
drag_bf.z = areaCd * 0.5f * air_density * sq(vel_air_bf.z);
if (is_negative(vel_air_bf.z)) {
drag_bf.z = -drag_bf.z;
}
thrust -= drag_bf;
}
body_accel = thrust/aircraft.gross_mass();
}
// computes (total) instantaneous current
float Frame::get_current_amp(void)
{
float current = 0;
for (uint8_t i=0; i<num_motors; i++) {
current += motors[i].get_current();
}
return current;
}
#endif // AP_SIM_ENABLED
Reference in New Issue View Git Blame Copy Permalink