Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: added simulation of pressure altitude for multicopter 

this gives us a ceiling of 5.4km above sea level for the default
copter
This commit is contained in:
Andrew Tridgell 2020-05-14 16:12:09 +10:00
parent a5e9da337a
commit 7c2d13c571
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/SITL/SIM_Frame.cpp
View File

@ -18,6 +18,7 @@
#include "SIM_Frame.h"
#include <AP_Motors/AP_Motors.h>
#include <AP_Baro/AP_Baro.h>
#include <stdio.h>
@ -314,14 +315,17 @@ void Frame::calculate_forces(const Aircraft &aircraft,
{
Vector3f thrust; // newtons
// scale thrust for altitude
float scaling = thrust_scale * AP::baro().get_air_density_ratio();
for (uint8_t i=0; i<num_motors; i++) {
Vector3f mraccel, mthrust;
motors[i].calculate_forces(input, thrust_scale, motor_offset, mraccel, mthrust);
motors[i].calculate_forces(input, scaling, 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;
rpm[i] = sqrtf(mthrust.length() / scaling) * AP::sitl()->vibe_motor * 60.0f;
}
}