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

AP_HAL_SITL: record throttle value instead of motors on/off

Browse Source
This commit is contained in:
Andy Piper 2019-12-24 21:19:05 +00:00 committed by Andrew Tridgell
parent a6a020b3c7
commit 8b0fc1207d
1 changed files with 16 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -640,7 +640,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
float engine_mul = _sitl?_sitl->engine_mul.get():1;
uint8_t engine_fail = _sitl?_sitl->engine_fail.get():0;
bool motors_on = false;
float throttle = 0.0f;
if (engine_fail >= ARRAY_SIZE(input.servos)) {
engine_fail = 0;
@ -651,28 +651,30 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
} else {
input.servos[engine_fail] = static_cast<uint16_t>(((input.servos[engine_fail] - 1500) * engine_mul) + 1500);
}
if (_vehicle == ArduPlane) {
motors_on = ((input.servos[2] - 1000) / 1000.0f) > 0;
throttle = constrain_float((input.servos[2] - 1000) / 1000.0f, 0.0f, 1.0f);
} else if (_vehicle == APMrover2) {
input.servos[2] = static_cast<uint16_t>(constrain_int16(input.servos[2], 1000, 2000));
input.servos[0] = static_cast<uint16_t>(constrain_int16(input.servos[0], 1000, 2000));
motors_on = !is_zero(((input.servos[2] - 1500) / 500.0f));
throttle = fabsf((input.servos[2] - 1500) / 500.0f);
} else {
motors_on = false;
// run checks on each motor
for (i=0; i<4; i++) {
// check motors do not exceed their limits
if (input.servos[i] > 2000) input.servos[i] = 2000;
if (input.servos[i] < 1000) input.servos[i] = 1000;
uint8_t running_motors = 0;
for (i=0; i<SITL_NUM_CHANNELS; i++) {
float motor_throttle = constrain_float((input.servos[i] - 1000) / 1000.0f, 0.0f, 1.0f);
// update motor_on flag
if ((input.servos[i]-1000)/1000.0f > 0) {
motors_on = true;
if (!is_zero(motor_throttle)) {
throttle += motor_throttle;
running_motors++;
}
}
if (running_motors > 0) {
throttle /= running_motors;
}
}
if (_sitl) {
_sitl->motors_on = motors_on;
_sitl->throttle = throttle;
}
float voltage = 0;
@ -694,17 +696,11 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
}
} else {
// simulate simple battery setup
float throttle;
if (_vehicle == APMrover2) {
throttle = motors_on ? (input.servos[2] - 1500) / 500.0f : 0;
} else {
throttle = motors_on ? (input.servos[2] - 1000) / 1000.0f : 0;
}
// lose 0.7V at full throttle
voltage = _sitl->batt_voltage - 0.7f*fabsf(throttle);
voltage = _sitl->batt_voltage - 0.7f * throttle;
// assume 50A at full throttle
_current = 50.0f * fabsf(throttle);
_current = 50.0f * throttle;
}
} else {
// FDM provides voltage and current