aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: add rpm-based motor noise per-motor to gyros and accels

Browse Source 
convert multicopter thrust to rpm per motor
This commit is contained in:
Andy Piper 2019-09-27 20:56:15 +01:00 committed by Andrew Tridgell
parent d34ccec084
commit c807267bab
16 changed files with 46 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/SITL/SIM_Aircraft.cpp
View File

@ -55,6 +55,7 @@ Aircraft::Aircraft(const char *frame_str) :
rate_hz(1200.0f),
autotest_dir(nullptr),
frame(frame_str),
num_motors(1),
#if defined(__CYGWIN__) || defined(__CYGWIN64__)
min_sleep_time(20000)
#else
@ -341,8 +342,8 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
fdm.airspeed = airspeed_pitot;
fdm.battery_voltage = battery_voltage;
fdm.battery_current = battery_current;
fdm.rpm1 = rpm1;
fdm.rpm2 = rpm2;
fdm.num_motors = num_motors;
memcpy(fdm.rpm, rpm, num_motors * sizeof(float));
fdm.rcin_chan_count = rcin_chan_count;
fdm.range = range;
memcpy(fdm.rcin, rcin, rcin_chan_count * sizeof(float));

4
libraries/SITL/SIM_Aircraft.h
View File

@ -155,8 +155,8 @@ protected:
float airspeed_pitot; // m/s, apparent airspeed, as seen by fwd pitot tube
float battery_voltage = -1.0f;
float battery_current = 0.0f;
float rpm1 = 0;
float rpm2 = 0;
uint8_t num_motors = 1;
float rpm[12];
uint8_t rcin_chan_count = 0;
float rcin[8];
float range = -1.0f; // rangefinder detection in m

2
libraries/SITL/SIM_EFI_MegaSquirt.cpp
View File

@ -46,7 +46,7 @@ void EFI_MegaSquirt::update()
if (!connected) {
return;
}
float rpm = sitl->state.rpm1;
float rpm = sitl->state.rpm[0];
table7.rpm = rpm;
table7.fuelload = 20;

5
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -86,6 +86,7 @@ FlightAxis::FlightAxis(const char *frame_str) :
Aircraft(frame_str)
{
use_time_sync = false;
num_motors = 2;
rate_hz = 250 / target_speedup;
heli_demix = strstr(frame_str, "helidemix") != nullptr;
rev4_servos = strstr(frame_str, "rev4") != nullptr;
@ -473,8 +474,8 @@ void FlightAxis::update(const struct sitl_input &input)
battery_voltage = state.m_batteryVoltage_VOLTS;
battery_current = state.m_batteryCurrentDraw_AMPS;
rpm1 = state.m_heliMainRotorRPM;
rpm2 = state.m_propRPM;
rpm[0] = state.m_heliMainRotorRPM;
rpm[1] = state.m_propRPM;
/*
the interlink interface supports 8 input channels

13
libraries/SITL/SIM_Frame.cpp
View File

@ -206,13 +206,13 @@ static Frame supported_frames[] =
Frame("firefly", 6, firefly_motors)
};
void Frame::init(float _mass, float hover_throttle, float _terminal_velocity, float _terminal_rotation_rate)
void Frame::init(float _mass, float _hover_throttle, float _terminal_velocity, float _terminal_rotation_rate)
{
/*
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);
thrust_scale = (_mass * GRAVITY_MSS) / (num_motors * _hover_throttle);
terminal_velocity = _terminal_velocity;
terminal_rotation_rate = _terminal_rotation_rate;
@ -228,6 +228,8 @@ 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;
}
@ -236,7 +238,8 @@ Frame *Frame::find_frame(const char *name)
void Frame::calculate_forces(const Aircraft &aircraft,
const struct sitl_input &input,
Vector3f &rot_accel,
Vector3f &body_accel)
Vector3f &body_accel,
float* rpm)
{
Vector3f thrust; // newtons
@ -245,6 +248,10 @@ void Frame::calculate_forces(const Aircraft &aircraft,
motors[i].calculate_forces(input, thrust_scale, motor_offset, mraccel, mthrust);
rot_accel += mraccel;
thrust += mthrust;
// simulate motor rpm
if (!is_zero(AP::sitl()->vibe_motor)) {
rpm[i] = sqrtf(mthrust.length() / thrust_scale) * AP::sitl()->vibe_motor * 60.0f;
}
}
body_accel = thrust/aircraft.gross_mass();

2
libraries/SITL/SIM_Frame.h
View File

@ -49,7 +49,7 @@ public:
// calculate rotational and linear accelerations
void calculate_forces(const Aircraft &aircraft,
const struct sitl_input &input,
Vector3f &rot_accel, Vector3f &body_accel);
Vector3f &rot_accel, Vector3f &body_accel, float* rpm);
float terminal_velocity;
float terminal_rotation_rate;

2
libraries/SITL/SIM_Helicopter.cpp
View File

@ -152,7 +152,7 @@ void Helicopter::update(const struct sitl_input &input)
Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity);
// simulate rotor speed
rpm1 = thrust * 1300;
rpm[0] = thrust * 1300;
// scale thrust to newtons
thrust *= thrust_scale;

5
libraries/SITL/SIM_JSBSim.cpp
View File

@ -62,6 +62,7 @@ JSBSim::JSBSim(const char *frame_str) :
}
control_port = 5505 + instance*10;
fdm_port = 5504 + instance*10;
num_motors = 2;
printf("JSBSim backend started: control_port=%u fdm_port=%u\n",
control_port, fdm_port);
@ -443,8 +444,8 @@ void JSBSim::recv_fdm(const struct sitl_input &input)
// update magnetic field
update_mag_field_bf();
rpm1 = fdm.rpm[0];
rpm2 = fdm.rpm[1];
rpm[0] = fdm.rpm[0];
rpm[1] = fdm.rpm[1];
time_now_us = fdm.cur_time;
}

5
libraries/SITL/SIM_Multicopter.cpp
View File

@ -42,13 +42,15 @@ MultiCopter::MultiCopter(const char *frame_str) :
frame->init(gross_mass(), 0.51, 15, 4*radians(360));
}
frame_height = 0.1;
num_motors = frame->num_motors;
ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
}
// calculate rotational and linear accelerations
void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
{
frame->calculate_forces(*this, input, rot_accel, body_accel);
frame->calculate_forces(*this, input, rot_accel, body_accel, rpm);
add_shove_forces(rot_accel, body_accel);
add_twist_forces(rot_accel);
}
@ -78,3 +80,4 @@ void MultiCopter::update(const struct sitl_input &input)
// update magnetic field
update_mag_field_bf();
}

3
libraries/SITL/SIM_Plane.cpp
View File

@ -34,6 +34,7 @@ Plane::Plane(const char *frame_str) :
*/
thrust_scale = (mass * GRAVITY_MSS) / hover_throttle;
frame_height = 0.1f;
num_motors = 1;
ground_behavior = GROUND_BEHAVIOR_FWD_ONLY;
@ -342,7 +343,7 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel
}
// simulate engine RPM
rpm1 = thrust * 7000;
rpm[0] = thrust * 7000;
// scale thrust to newtons
thrust *= thrust_scale;

3
libraries/SITL/SIM_QuadPlane.cpp
View File

@ -73,6 +73,7 @@ QuadPlane::QuadPlane(const char *frame_str) :
printf("Failed to find frame '%s'\n", frame_type);
exit(1);
}
num_motors = 1 + frame->num_motors;
if (strstr(frame_str, "cl84")) {
// setup retract servos at front
@ -107,7 +108,7 @@ 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);
frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body, &rpm[1]);
// estimate voltage and current
frame->current_and_voltage(input, battery_voltage, battery_current);

2
libraries/SITL/SIM_Sailboat.cpp
View File

@ -182,7 +182,7 @@ void Sailboat::update(const struct sitl_input &input)
const float wind_apparent_dir_bf = wrap_180(wind_apparent_dir_ef - degrees(AP::ahrs().yaw));
// set RPM and airspeed from wind speed, allows to test RPM and Airspeed wind vane back end in SITL
rpm1 = wind_apparent_speed;
rpm[0] = wind_apparent_speed;
airspeed_pitot = wind_apparent_speed;
// calculate angle-of-attack from wind to mainsail

4
libraries/SITL/SIM_Scrimmage.cpp
View File

@ -190,8 +190,8 @@ void Scrimmage::recv_fdm(const struct sitl_input &input)
battery_voltage = 0;
battery_current = 0;
rpm1 = 0;
rpm2 = 0;
rpm[0] = 0;
rpm[1] = 0;
airspeed = pkt.airspeed;
airspeed_pitot = pkt.airspeed;

5
libraries/SITL/SIM_XPlane.cpp
View File

@ -42,6 +42,7 @@ XPlane::XPlane(const char *frame_str) :
}
heli_frame = (strstr(frame_str, "-heli") != nullptr);
num_motors = 2;
socket_in.bind("0.0.0.0", bind_port);
printf("Waiting for XPlane data on UDP port %u and sending to port %u\n",
@ -258,11 +259,11 @@ bool XPlane::receive_data(void)
}
case EngineRPM:
rpm1 = data[1];
rpm[0] = data[1];
break;
case PropRPM:
rpm2 = data[1];
rpm[1] = data[1];
break;
case Joystick2:

3
libraries/SITL/SITL.cpp
View File

@ -204,6 +204,9 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
AP_GROUPINFO("MAG_SCALING", 60, SITL, mag_scaling, 1),
// max motor vibration frequency
AP_GROUPINFO("VIB_MOT_MAX", 61, SITL, vibe_motor, 0.0f),
AP_GROUPEND
};

7
libraries/SITL/SITL.h
View File

@ -45,8 +45,8 @@ struct sitl_fdm {
double airspeed; // m/s
double battery_voltage; // Volts
double battery_current; // Amps
double rpm1; // main prop RPM
double rpm2; // secondary RPM
uint8_t num_motors;
float rpm[12]; // RPM of all motors
uint8_t rcin_chan_count;
float rcin[8]; // RC input 0..1
double range; // rangefinder value
@ -248,6 +248,9 @@ public:
// vibration frequencies in Hz on each axis
AP_Vector3f vibe_freq;
// hover frequency to use as baseline for adding motor noise for the gyros and accels
AP_Float vibe_motor;
// gyro and accel fail masks
AP_Int8 gyro_fail_mask;
AP_Int8 accel_fail_mask;