SITL: fixed quadplane mass and rpm indexing

This commit is contained in:
Andrew Tridgell 2020-10-21 15:42:36 +11:00
parent a95c2e9db3
commit a0bc9d3148
7 changed files with 18 additions and 2 deletions

View File

@ -352,6 +352,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
fdm.battery_voltage = battery_voltage; fdm.battery_voltage = battery_voltage;
fdm.battery_current = battery_current; fdm.battery_current = battery_current;
fdm.num_motors = num_motors; fdm.num_motors = num_motors;
fdm.vtol_motor_start = vtol_motor_start;
memcpy(fdm.rpm, rpm, num_motors * sizeof(float)); memcpy(fdm.rpm, rpm, num_motors * sizeof(float));
fdm.rcin_chan_count = rcin_chan_count; fdm.rcin_chan_count = rcin_chan_count;
fdm.range = range; fdm.range = range;

View File

@ -171,6 +171,7 @@ protected:
float battery_voltage = -1.0f; float battery_voltage = -1.0f;
float battery_current; float battery_current;
uint8_t num_motors = 1; uint8_t num_motors = 1;
uint8_t vtol_motor_start;
float rpm[12]; float rpm[12];
uint8_t rcin_chan_count; uint8_t rcin_chan_count;
float rcin[12]; float rcin[12];

View File

@ -489,8 +489,7 @@ void Frame::calculate_forces(const Aircraft &aircraft,
thrust += mthrust; thrust += mthrust;
// simulate motor rpm // simulate motor rpm
if (!is_zero(AP::sitl()->vibe_motor)) { if (!is_zero(AP::sitl()->vibe_motor)) {
const float mot_thrust_max = thrust_max / num_motors; rpm[i] = motors[i].get_command() * AP::sitl()->vibe_motor * 60.0f;
rpm[i] = sqrtf(mthrust.length() / mot_thrust_max) * AP::sitl()->vibe_motor * 60.0f;
} }
} }

View File

@ -65,6 +65,11 @@ public:
return mass; return mass;
} }
// set mass in kg
void set_mass(float new_mass) {
mass = new_mass;
}
private: private:
/* /*
parameters that define the multicopter model. Can be loaded from parameters that define the multicopter model. Can be loaded from

View File

@ -96,6 +96,10 @@ public:
slew_max = _slew_max; slew_max = _slew_max;
} }
float get_command(void) const {
return last_command;
}
// calculate thrust of motor // calculate thrust of motor
float calc_thrust(float command, float air_density, float effective_prop_area, float velocity_in, float velocity_max) const; float calc_thrust(float command, float air_density, float effective_prop_area, float velocity_in, float velocity_max) const;

View File

@ -74,6 +74,7 @@ QuadPlane::QuadPlane(const char *frame_str) :
exit(1); exit(1);
} }
num_motors = 1 + frame->num_motors; num_motors = 1 + frame->num_motors;
vtol_motor_start = 1;
if (strstr(frame_str, "cl84")) { if (strstr(frame_str, "cl84")) {
// setup retract servos at front // setup retract servos at front
@ -89,6 +90,10 @@ QuadPlane::QuadPlane(const char *frame_str) :
// we use zero terminal velocity to let the plane model handle the drag // we use zero terminal velocity to let the plane model handle the drag
frame->init(frame_str); frame->init(frame_str);
// increase mass for plane components
mass = frame->get_mass() * 1.5;
frame->set_mass(mass);
ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT; ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
} }

View File

@ -56,6 +56,7 @@ struct sitl_fdm {
double battery_voltage; // Volts double battery_voltage; // Volts
double battery_current; // Amps double battery_current; // Amps
uint8_t num_motors; uint8_t num_motors;
uint8_t vtol_motor_start;
float rpm[12]; // RPM of all motors float rpm[12]; // RPM of all motors
uint8_t rcin_chan_count; uint8_t rcin_chan_count;
float rcin[12]; // RC input 0..1 float rcin[12]; // RC input 0..1