SITL: improved multicopter simulation
this takes account of motor expo, velocity of air over propellers, mass, size and other factors It also allows for frame parameters to be supplied as an external json file
This commit is contained in:
Andrew Tridgell
parent
c2661a0f09
commit
767773da5e
@ -21,6 +21,23 @@
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <sys/stat.h>
|
||||
|
||||
/*
|
||||
use picojson to load optional frame files
|
||||
*/
|
||||
#define PICOJSON_NOEXCEPT
|
||||
#ifndef PICOJSON_ASSERT
|
||||
#define PICOJSON_ASSERT(e) \
|
||||
do { \
|
||||
if (!(e)) \
|
||||
::printf(#e "\n"); \
|
||||
} while (0)
|
||||
#endif
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wfloat-equal"
|
||||
#include "picojson.h"
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
using namespace SITL;
|
||||
|
||||
@ -278,16 +295,160 @@ static Frame supported_frames[] =
|
||||
Frame("firefly", 6, firefly_motors)
|
||||
};
|
||||
|
||||
void Frame::init(float _mass, float _hover_throttle, float _terminal_velocity, float _terminal_rotation_rate)
|
||||
// get air density in kg/m^3
|
||||
float Frame::get_air_density(float alt_amsl) const
|
||||
{
|
||||
/*
|
||||
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);
|
||||
float sigma, delta, theta;
|
||||
|
||||
terminal_velocity = _terminal_velocity;
|
||||
terminal_rotation_rate = _terminal_rotation_rate;
|
||||
AP_Baro::SimpleAtmosphere(alt_amsl * 0.001f, sigma, delta, theta);
|
||||
|
||||
const float air_pressure = SSL_AIR_PRESSURE * delta;
|
||||
return air_pressure / (ISA_GAS_CONSTANT * (C_TO_KELVIN + model.refTempC));
|
||||
}
|
||||
|
||||
/*
|
||||
load frame specific parameters from a json file if available
|
||||
*/
|
||||
void Frame::load_frame_params(const char *model_json)
|
||||
{
|
||||
::printf("Loading model %s\n", model_json);
|
||||
int fd = open(model_json, O_RDONLY);
|
||||
if (fd == -1) {
|
||||
AP_HAL::panic("%s failed to load\n", model_json);
|
||||
}
|
||||
struct stat st;
|
||||
if (fstat(fd, &st) != 0) {
|
||||
AP_HAL::panic("%s failed to load\n", model_json);
|
||||
}
|
||||
char buf[st.st_size];
|
||||
if (read(fd, buf, st.st_size) != st.st_size) {
|
||||
AP_HAL::panic("%s failed to load\n", model_json);
|
||||
}
|
||||
close(fd);
|
||||
|
||||
char *start = strchr(buf, '{');
|
||||
if (!start) {
|
||||
AP_HAL::panic("Invalid json %s", model_json);
|
||||
}
|
||||
|
||||
/*
|
||||
remove comments, as not allowed by the parser
|
||||
*/
|
||||
for (char *p = strchr(start,'#'); p; p=strchr(p+1, '#')) {
|
||||
// clear to end of line
|
||||
do {
|
||||
*p++ = ' ';
|
||||
} while (*p != '\n' && *p != '\r' && *p);
|
||||
}
|
||||
|
||||
picojson::value obj;
|
||||
std::string err = picojson::parse(obj, start);
|
||||
if (!err.empty()) {
|
||||
AP_HAL::panic("Failed to load %s: %s", model_json, err.c_str());
|
||||
exit(1);
|
||||
}
|
||||
|
||||
struct {
|
||||
const char *label;
|
||||
float &v;
|
||||
} vars[] = {
|
||||
#define FRAME_VAR(s) { #s, model.s }
|
||||
FRAME_VAR(mass),
|
||||
FRAME_VAR(diagonal_size),
|
||||
FRAME_VAR(refSpd),
|
||||
FRAME_VAR(refAngle),
|
||||
FRAME_VAR(refVoltage),
|
||||
FRAME_VAR(refCurrent),
|
||||
FRAME_VAR(refAlt),
|
||||
FRAME_VAR(refTempC),
|
||||
FRAME_VAR(maxVoltage),
|
||||
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),
|
||||
};
|
||||
static_assert(sizeof(model) == sizeof(float)*ARRAY_SIZE(vars), "incorrect model vars");
|
||||
|
||||
for (uint8_t i=0; i<ARRAY_SIZE(vars); i++) {
|
||||
auto v = obj.get(vars[i].label);
|
||||
if (v.is<picojson::null>()) {
|
||||
// use default value
|
||||
continue;
|
||||
}
|
||||
if (!v.is<double>()) {
|
||||
AP_HAL::panic("Bad json type for %s: %s", vars[i].label, v.to_str().c_str());
|
||||
}
|
||||
vars[i].v = v.get<double>();
|
||||
}
|
||||
|
||||
::printf("Loaded model params from %s\n", model_json);
|
||||
}
|
||||
|
||||
/*
|
||||
initialise the frame
|
||||
*/
|
||||
void Frame::init(const char *frame_str)
|
||||
{
|
||||
model = default_model;
|
||||
|
||||
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);
|
||||
}
|
||||
mass = model.mass;
|
||||
|
||||
const float drag_force = model.mass * GRAVITY_MSS * tanf(radians(model.refAngle));
|
||||
|
||||
float ref_air_density = get_air_density(model.refAlt);
|
||||
|
||||
areaCd = drag_force / (0.5 * ref_air_density * sq(model.refSpd));
|
||||
|
||||
terminal_rotation_rate = model.refRotRate;
|
||||
|
||||
float hover_thrust = mass * GRAVITY_MSS;
|
||||
float hover_power = model.refCurrent * model.refVoltage;
|
||||
float hover_velocity_out = 2 * hover_power / hover_thrust;
|
||||
float effective_disc_area = hover_thrust / (0.5 * ref_air_density * sq(hover_velocity_out));
|
||||
velocity_max = hover_velocity_out / sqrtf(model.hoverThrOut);
|
||||
thrust_max = 0.5 * ref_air_density * effective_disc_area * sq(velocity_max);
|
||||
effective_prop_area = effective_disc_area / num_motors;
|
||||
|
||||
// power_factor is ratio of power consumed per newton of thrust
|
||||
float power_factor = hover_power / hover_thrust;
|
||||
|
||||
// init voltage
|
||||
voltage_filter.reset(AP::sitl()->batt_voltage);
|
||||
|
||||
for (uint8_t i=0; i<num_motors; i++) {
|
||||
motors[i].setup_params(model.pwmMin, model.pwmMax, model.spin_min, model.spin_max, model.propExpo, model.slew_max,
|
||||
model.mass, model.diagonal_size, power_factor, model.maxVoltage);
|
||||
}
|
||||
|
||||
|
||||
#if 0
|
||||
// useful debug code for thrust curve
|
||||
{
|
||||
motors[0].set_slew_max(0);
|
||||
struct sitl_input input {};
|
||||
for (uint16_t pwm = 1000; pwm < 2000; pwm += 50) {
|
||||
input.servos[0] = pwm;
|
||||
|
||||
Vector3f rot_accel {}, thrust {};
|
||||
Vector3f vel_air_bf {};
|
||||
motors[0].calculate_forces(input, motor_offset, rot_accel, thrust, vel_air_bf,
|
||||
ref_air_density, velocity_max, effective_prop_area, voltage_filter.get());
|
||||
::printf("pwm[%u] cmd=%.3f thrust=%.3f hovthst=%.3f\n",
|
||||
pwm, motors[0].pwm_to_command(pwm), -thrust.z*num_motors, hover_thrust);
|
||||
}
|
||||
motors[0].set_slew_max(model.slew_max);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
@ -300,8 +461,6 @@ Frame *Frame::find_frame(const char *name)
|
||||
if (strncasecmp(name, supported_frames[i].name, strlen(supported_frames[i].name)) == 0) {
|
||||
return &supported_frames[i];
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
@ -311,21 +470,27 @@ void Frame::calculate_forces(const Aircraft &aircraft,
|
||||
const struct sitl_input &input,
|
||||
Vector3f &rot_accel,
|
||||
Vector3f &body_accel,
|
||||
float* rpm)
|
||||
float* rpm,
|
||||
bool use_drag)
|
||||
{
|
||||
Vector3f thrust; // newtons
|
||||
|
||||
// scale thrust for altitude
|
||||
float scaling = thrust_scale * AP::baro().get_air_density_ratio();
|
||||
const float air_density = get_air_density(aircraft.get_location().alt*0.01);
|
||||
|
||||
Vector3f vel_air_bf = aircraft.get_dcm().transposed() * aircraft.get_velocity_air_ef();
|
||||
|
||||
float current = 0;
|
||||
for (uint8_t i=0; i<num_motors; i++) {
|
||||
Vector3f mraccel, mthrust;
|
||||
motors[i].calculate_forces(input, scaling, motor_offset, mraccel, mthrust);
|
||||
motors[i].calculate_forces(input, motor_offset, mraccel, mthrust, vel_air_bf, air_density, velocity_max,
|
||||
effective_prop_area, voltage_filter.get());
|
||||
current += motors[i].get_current();
|
||||
rot_accel += mraccel;
|
||||
thrust += mthrust;
|
||||
// simulate motor rpm
|
||||
if (!is_zero(AP::sitl()->vibe_motor)) {
|
||||
rpm[i] = sqrtf(mthrust.length() / scaling) * AP::sitl()->vibe_motor * 60.0f;
|
||||
const float mot_thrust_max = thrust_max / num_motors;
|
||||
rpm[i] = sqrtf(mthrust.length() / mot_thrust_max) * AP::sitl()->vibe_motor * 60.0f;
|
||||
}
|
||||
}
|
||||
|
||||
@ -339,16 +504,25 @@ void Frame::calculate_forces(const Aircraft &aircraft,
|
||||
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;
|
||||
if (use_drag) {
|
||||
// use the model params to calculate drag
|
||||
const Vector3f vel_air_ef = aircraft.get_velocity_air_ef();
|
||||
const float speed = vel_air_ef.length();
|
||||
float drag_force = areaCd * 0.5 * air_density * sq(speed);
|
||||
Vector3f drag_ef;
|
||||
if (is_positive(speed)) {
|
||||
drag_ef = -(vel_air_ef / speed) * drag_force;
|
||||
}
|
||||
body_accel += aircraft.get_dcm().transposed() * drag_ef / mass;
|
||||
}
|
||||
|
||||
float voltage_drop = current * model.refBatRes;
|
||||
voltage_filter.apply(AP::sitl()->batt_voltage - voltage_drop);
|
||||
|
||||
// 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);
|
||||
const float noise_scale = thrust.length() / thrust_max;
|
||||
rot_accel += Vector3f(aircraft.rand_normal(0, 1),
|
||||
aircraft.rand_normal(0, 1),
|
||||
aircraft.rand_normal(0, 1)) * gyro_noise * noise_scale;
|
||||
@ -359,16 +533,11 @@ void Frame::calculate_forces(const Aircraft &aircraft,
|
||||
|
||||
|
||||
// calculate current and voltage
|
||||
void Frame::current_and_voltage(const struct sitl_input &input, float &voltage, float ¤t)
|
||||
void Frame::current_and_voltage(float &voltage, float ¤t)
|
||||
{
|
||||
voltage = 0;
|
||||
voltage = voltage_filter.get();
|
||||
current = 0;
|
||||
for (uint8_t i=0; i<num_motors; i++) {
|
||||
float c, v;
|
||||
motors[i].current_and_voltage(input, v, c, motor_offset);
|
||||
current += c;
|
||||
voltage += v;
|
||||
current += motors[i].get_current();
|
||||
}
|
||||
// use average for voltage, total for current
|
||||
voltage /= num_motors;
|
||||
}
|
||||
|
||||
@ -20,6 +20,7 @@
|
||||
|
||||
#include "SIM_Aircraft.h"
|
||||
#include "SIM_Motor.h"
|
||||
#include <Filter/LowPassFilter.h>
|
||||
|
||||
namespace SITL {
|
||||
|
||||
@ -44,19 +45,92 @@ public:
|
||||
static Frame *find_frame(const char *name);
|
||||
|
||||
// initialise frame
|
||||
void init(float mass, float hover_throttle, float terminal_velocity, float terminal_rotation_rate);
|
||||
void init(const char *frame_str);
|
||||
|
||||
// calculate rotational and linear accelerations
|
||||
void calculate_forces(const Aircraft &aircraft,
|
||||
const struct sitl_input &input,
|
||||
Vector3f &rot_accel, Vector3f &body_accel, float* rpm);
|
||||
Vector3f &rot_accel, Vector3f &body_accel, float* rpm,
|
||||
bool use_drag=true);
|
||||
|
||||
float terminal_velocity;
|
||||
float terminal_rotation_rate;
|
||||
float thrust_scale;
|
||||
uint8_t motor_offset;
|
||||
|
||||
// calculate current and voltage
|
||||
void current_and_voltage(const struct sitl_input &input, float &voltage, float ¤t);
|
||||
void current_and_voltage(float &voltage, float ¤t);
|
||||
|
||||
// get mass in kg
|
||||
float get_mass(void) const {
|
||||
return mass;
|
||||
}
|
||||
|
||||
private:
|
||||
/*
|
||||
parameters that define the multicopter model. Can be loaded from
|
||||
a json file to give a custom model
|
||||
*/
|
||||
const struct Model {
|
||||
// model mass kg
|
||||
float mass = 3.0;
|
||||
|
||||
// diameter of model
|
||||
float diagonal_size = 0.35;
|
||||
|
||||
/*
|
||||
the ref values are for a test at fixed angle, used to estimate drag
|
||||
*/
|
||||
float refSpd = 15.08; // m/s
|
||||
float refAngle = 45; // degrees
|
||||
float refVoltage = 12.09; // Volts
|
||||
float refCurrent = 29.3; // Amps
|
||||
float refAlt = 593; // altitude AMSL
|
||||
float refTempC = 25; // temperature C
|
||||
float refBatRes = 0.01; // BAT.Res
|
||||
|
||||
// full pack voltage
|
||||
float maxVoltage = 4.2*3;
|
||||
|
||||
// CTUN.ThO at bover at refAlt
|
||||
float hoverThrOut = 0.39;
|
||||
|
||||
// MOT_THST_EXPO
|
||||
float propExpo = 0.65;
|
||||
|
||||
// scaling factor for yaw response, deg/sec
|
||||
float refRotRate = 120;
|
||||
|
||||
// MOT params are from the reference test
|
||||
// MOT_PWM_MIN
|
||||
float pwmMin = 1000;
|
||||
// MOT_PWM_MAX
|
||||
float pwmMax = 2000;
|
||||
// MOT_SPIN_MIN
|
||||
float spin_min = 0.15;
|
||||
// MOT_SPIN_MAX
|
||||
float spin_max = 0.95;
|
||||
|
||||
// maximum slew rate of motors
|
||||
float slew_max = 150;
|
||||
} default_model;
|
||||
|
||||
struct Model model;
|
||||
|
||||
// exposed area times coefficient of drag
|
||||
float areaCd;
|
||||
float mass;
|
||||
float velocity_max;
|
||||
float thrust_max;
|
||||
float effective_prop_area;
|
||||
|
||||
// 10Hz filter for battery voltage
|
||||
LowPassFilterFloat voltage_filter{10};
|
||||
|
||||
// get air density in kg/m^3
|
||||
float get_air_density(float alt_amsl) const;
|
||||
|
||||
// load frame parameters from a json model file
|
||||
void load_frame_params(const char *model_json);
|
||||
|
||||
};
|
||||
}
|
||||
|
||||
@ -23,26 +23,47 @@ using namespace SITL;
|
||||
|
||||
// calculate rotational accel and thrust for a motor
|
||||
void Motor::calculate_forces(const struct sitl_input &input,
|
||||
const float thrust_scale,
|
||||
uint8_t motor_offset,
|
||||
Vector3f &rot_accel,
|
||||
Vector3f &thrust)
|
||||
Vector3f &thrust,
|
||||
const Vector3f &velocity_air_bf,
|
||||
float air_density,
|
||||
float velocity_max,
|
||||
float effective_prop_area,
|
||||
float voltage)
|
||||
{
|
||||
// fudge factors
|
||||
const float arm_scale = radians(5000);
|
||||
const float yaw_scale = radians(400);
|
||||
|
||||
// get motor speed from 0 to 1
|
||||
float motor_speed = constrain_float((input.servos[motor_offset+servo]-1100)/900.0, 0, 1);
|
||||
const float pwm = input.servos[motor_offset+servo];
|
||||
float command = pwm_to_command(pwm);
|
||||
float voltage_scale = voltage / voltage_max;
|
||||
|
||||
// apply slew limiter to command
|
||||
uint64_t now_us = AP_HAL::micros64();
|
||||
if (last_calc_us != 0 && slew_max > 0) {
|
||||
float dt = (now_us - last_calc_us)*1.0e-6;
|
||||
float slew_max_change = slew_max * dt;
|
||||
command = constrain_float(command, last_command-slew_max_change, last_command+slew_max_change);
|
||||
}
|
||||
last_calc_us = now_us;
|
||||
last_command = command;
|
||||
|
||||
// the yaw torque of the motor
|
||||
Vector3f rotor_torque(0, 0, yaw_factor * motor_speed * yaw_scale);
|
||||
Vector3f rotor_torque(0, 0, yaw_factor * command * yaw_scale * voltage_scale);
|
||||
|
||||
// calculate velocity into prop, clipping at zero, assumes zero roll/pitch
|
||||
float velocity_in = MAX(0, -velocity_air_bf.z);
|
||||
|
||||
// get thrust for untilted motor
|
||||
thrust = {0, 0, -motor_speed};
|
||||
float motor_thrust = calc_thrust(command * voltage_scale, air_density, effective_prop_area, velocity_in, velocity_max);
|
||||
|
||||
// thrust in NED
|
||||
thrust = {0, 0, -motor_thrust};
|
||||
|
||||
// define the arm position relative to center of mass
|
||||
Vector3f arm(arm_scale * cosf(radians(angle)), arm_scale * sinf(radians(angle)), 0);
|
||||
Vector3f arm(cosf(radians(angle)), sinf(radians(angle)), 0);
|
||||
arm *= diagonal_size;
|
||||
|
||||
// work out roll and pitch of motor relative to it pointing straight up
|
||||
float roll = 0, pitch = 0;
|
||||
@ -76,11 +97,17 @@ void Motor::calculate_forces(const struct sitl_input &input,
|
||||
rotor_torque = rotation * rotor_torque;
|
||||
}
|
||||
|
||||
// calculate total rotational acceleration
|
||||
rot_accel = (arm % thrust) + rotor_torque;
|
||||
// calculate torque in newton-meters
|
||||
Vector3f torque = (arm % thrust) + rotor_torque;
|
||||
|
||||
// scale the thrust
|
||||
thrust = thrust * thrust_scale;
|
||||
// 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;
|
||||
|
||||
// calculate current
|
||||
float power = power_factor * fabsf(motor_thrust);
|
||||
current = power / MAX(voltage, 0.1);
|
||||
}
|
||||
|
||||
/*
|
||||
@ -111,17 +138,56 @@ uint16_t Motor::update_servo(uint16_t demand, uint64_t time_usec, float &last_va
|
||||
|
||||
|
||||
// calculate current and voltage
|
||||
void Motor::current_and_voltage(const struct sitl_input &input, float &voltage, float ¤t,
|
||||
uint8_t motor_offset)
|
||||
float Motor::get_current(void) const
|
||||
{
|
||||
// get motor speed from 0 to 1
|
||||
float motor_speed = constrain_float((input.servos[motor_offset+servo]-1100)/900.0, 0, 1);
|
||||
|
||||
// assume 10A per motor at full speed
|
||||
current = 10 * motor_speed;
|
||||
|
||||
// assume 3S, and full throttle drops voltage by 0.7V
|
||||
if (AP::sitl()) {
|
||||
voltage = AP::sitl()->batt_voltage - motor_speed * 0.7;
|
||||
}
|
||||
return current;
|
||||
}
|
||||
|
||||
// setup PWM ranges for this motor
|
||||
void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max,
|
||||
float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max)
|
||||
{
|
||||
mot_pwm_min = _pwm_min;
|
||||
mot_pwm_max = _pwm_max;
|
||||
mot_spin_min = _spin_min;
|
||||
mot_spin_max = _spin_max;
|
||||
mot_expo = _expo;
|
||||
slew_max = _slew_max;
|
||||
vehicle_mass = _vehicle_mass;
|
||||
diagonal_size = _diagonal_size;
|
||||
power_factor = _power_factor;
|
||||
voltage_max = _voltage_max;
|
||||
|
||||
// assume 50% of mass on ring around center
|
||||
moment_of_inertia.x = vehicle_mass * 0.25 * sq(diagonal_size*0.5);
|
||||
moment_of_inertia.y = moment_of_inertia.x;
|
||||
moment_of_inertia.z = vehicle_mass * 0.5 * sq(diagonal_size*0.5);
|
||||
}
|
||||
|
||||
/*
|
||||
convert a PWM value to a command value from 0 to 1
|
||||
*/
|
||||
float Motor::pwm_to_command(float pwm) const
|
||||
{
|
||||
const float pwm_thrust_max = mot_pwm_min + mot_spin_max * (mot_pwm_max - mot_pwm_min);
|
||||
const float pwm_thrust_min = mot_pwm_min + mot_spin_min * (mot_pwm_max - mot_pwm_min);
|
||||
const float pwm_thrust_range = pwm_thrust_max - pwm_thrust_min;
|
||||
return constrain_float((pwm-pwm_thrust_min)/pwm_thrust_range, 0, 1);
|
||||
}
|
||||
|
||||
/*
|
||||
calculate thrust given a command value
|
||||
*/
|
||||
float Motor::calc_thrust(float command, float air_density, float effective_prop_area, float velocity_in, float velocity_max) const
|
||||
{
|
||||
float velocity_out = velocity_max * sqrtf((1-mot_expo)*command + mot_expo*sq(command));
|
||||
float ret = 0.5 * air_density * effective_prop_area * (sq(velocity_out) - sq(velocity_in));
|
||||
#if 0
|
||||
if (command > 0) {
|
||||
::printf("air_density=%f effective_prop_area=%f velocity_in=%f velocity_max=%f\n",
|
||||
air_density, effective_prop_area, velocity_in, velocity_max);
|
||||
::printf("calc_thrust %.3f %.3f\n", command, ret);
|
||||
}
|
||||
#endif
|
||||
return ret;
|
||||
}
|
||||
|
||||
@ -70,15 +70,51 @@ public:
|
||||
{}
|
||||
|
||||
void calculate_forces(const struct sitl_input &input,
|
||||
float thrust_scale,
|
||||
uint8_t motor_offset,
|
||||
Vector3f &rot_accel, // rad/sec
|
||||
Vector3f &body_thrust); // Z is down
|
||||
Vector3f &body_thrust, // Z is down
|
||||
const Vector3f &velocity_air_bf,
|
||||
float air_density,
|
||||
float velocity_max,
|
||||
float effective_prop_area,
|
||||
float voltage);
|
||||
|
||||
uint16_t update_servo(uint16_t demand, uint64_t time_usec, float &last_value);
|
||||
|
||||
// calculate current and voltage
|
||||
void current_and_voltage(const struct sitl_input &input, float &voltage, float ¤t, uint8_t motor_offset);
|
||||
// get current
|
||||
float get_current(void) const;
|
||||
|
||||
// convert a PWM value to a thrust demand from 0 to 1
|
||||
float pwm_to_command(float pwm) const;
|
||||
|
||||
// setup motor key parameters
|
||||
void setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max,
|
||||
float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max);
|
||||
|
||||
// override slew limit
|
||||
void set_slew_max(float _slew_max) {
|
||||
slew_max = _slew_max;
|
||||
}
|
||||
|
||||
// calculate thrust of motor
|
||||
float calc_thrust(float command, float air_density, float effective_prop_area, float velocity_in, float velocity_max) const;
|
||||
|
||||
private:
|
||||
float mot_pwm_min;
|
||||
float mot_pwm_max;
|
||||
float mot_spin_min;
|
||||
float mot_spin_max;
|
||||
float mot_expo;
|
||||
float slew_max;
|
||||
float vehicle_mass;
|
||||
float diagonal_size;
|
||||
float current;
|
||||
float power_factor;
|
||||
float voltage_max;
|
||||
Vector3f moment_of_inertia;
|
||||
|
||||
float last_command;
|
||||
uint64_t last_calc_us;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -26,20 +26,15 @@ using namespace SITL;
|
||||
MultiCopter::MultiCopter(const char *frame_str) :
|
||||
Aircraft(frame_str)
|
||||
{
|
||||
mass = 1.5f;
|
||||
|
||||
frame = Frame::find_frame(frame_str);
|
||||
if (frame == nullptr) {
|
||||
printf("Frame '%s' not found", frame_str);
|
||||
exit(1);
|
||||
}
|
||||
// initial mass is passed through to Frame for it to calculate a
|
||||
// hover thrust requirement.
|
||||
if (strstr(frame_str, "-fast")) {
|
||||
frame->init(gross_mass(), 0.5, 85, 4*radians(360));
|
||||
} else {
|
||||
frame->init(gross_mass(), 0.51, 15, 4*radians(360));
|
||||
}
|
||||
|
||||
frame->init(frame_str);
|
||||
|
||||
mass = frame->get_mass();
|
||||
frame_height = 0.1;
|
||||
num_motors = frame->num_motors;
|
||||
ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
|
||||
@ -67,7 +62,7 @@ void MultiCopter::update(const struct sitl_input &input)
|
||||
calculate_forces(input, rot_accel, accel_body);
|
||||
|
||||
// estimate voltage and current
|
||||
frame->current_and_voltage(input, battery_voltage, battery_current);
|
||||
frame->current_and_voltage(battery_voltage, battery_current);
|
||||
|
||||
update_dynamics(rot_accel);
|
||||
update_external_payload(input);
|
||||
|
||||
@ -87,7 +87,7 @@ QuadPlane::QuadPlane(const char *frame_str) :
|
||||
frame->motor_offset = motor_offset;
|
||||
|
||||
// we use zero terminal velocity to let the plane model handle the drag
|
||||
frame->init(mass, 0.51, 0, 0);
|
||||
frame->init(frame_str);
|
||||
|
||||
ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
|
||||
}
|
||||
@ -108,10 +108,10 @@ void QuadPlane::update(const struct sitl_input &input)
|
||||
Vector3f quad_rot_accel;
|
||||
Vector3f quad_accel_body;
|
||||
|
||||
frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body, &rpm[1]);
|
||||
frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body, &rpm[1], false);
|
||||
|
||||
// estimate voltage and current
|
||||
frame->current_and_voltage(input, battery_voltage, battery_current);
|
||||
frame->current_and_voltage(battery_voltage, battery_current);
|
||||
|
||||
float throttle;
|
||||
if (reverse_thrust) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user