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

AP_HAL_SITL: make throttle comply with quadplane dynamics

This commit is contained in:
Andy Piper 2020-01-16 19:00:58 +00:00 committed by Andrew Tridgell
parent ba67ad39a0
commit dcdd1707f7
1 changed files with 21 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -688,7 +688,26 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
} }
if (_vehicle == ArduPlane) { if (_vehicle == ArduPlane) {
throttle = constrain_float((input.servos[2] - 1000) / 1000.0f, 0.0f, 1.0f); float forward_throttle = constrain_float((input.servos[2] - 1000) / 1000.0f, 0.0f, 1.0f);
// do a little quadplane dance
float hover_throttle = 0.0f;
uint8_t running_motors = 0;
for (i=0; i < sitl_model->get_num_motors() - 1; i++) {
float motor_throttle = constrain_float((input.servos[sitl_model->get_motors_offset() + i] - 1000) / 1000.0f, 0.0f, 1.0f);
// update motor_on flag
if (!is_zero(motor_throttle)) {
hover_throttle += motor_throttle;
running_motors++;
}
}
if (running_motors > 0) {
hover_throttle /= running_motors;
}
if (!is_zero(forward_throttle)) {
throttle = forward_throttle;
} else {
throttle = hover_throttle;
}
} else if (_vehicle == APMrover2) { } else if (_vehicle == APMrover2) {
input.servos[2] = static_cast<uint16_t>(constrain_int16(input.servos[2], 1000, 2000)); 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)); input.servos[0] = static_cast<uint16_t>(constrain_int16(input.servos[0], 1000, 2000));
@ -696,7 +715,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
} else { } else {
// run checks on each motor // run checks on each motor
uint8_t running_motors = 0; uint8_t running_motors = 0;
for (i=0; i<SITL_NUM_CHANNELS; i++) { for (i=0; i < sitl_model->get_num_motors(); i++) {
float motor_throttle = constrain_float((input.servos[i] - 1000) / 1000.0f, 0.0f, 1.0f); float motor_throttle = constrain_float((input.servos[i] - 1000) / 1000.0f, 0.0f, 1.0f);
// update motor_on flag // update motor_on flag
if (!is_zero(motor_throttle)) { if (!is_zero(motor_throttle)) {