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

HAL_SITL: allow visualisation of quad motor speeds in flightgear

This commit is contained in:
Andrew Tridgell 2016-01-04 11:09:27 +11:00
parent 568b1da797
commit 2dd8a0af74
1 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -231,6 +231,19 @@ void SITL_State::_output_to_flightgear(void)
fdm.phi = radians(sfdm.rollDeg); fdm.phi = radians(sfdm.rollDeg);
fdm.theta = radians(sfdm.pitchDeg); fdm.theta = radians(sfdm.pitchDeg);
fdm.psi = radians(sfdm.yawDeg); fdm.psi = radians(sfdm.yawDeg);
if (_vehicle == ArduCopter) {
fdm.num_engines = 4;
for (uint8_t i=0; i<4; i++) {
fdm.rpm[i] = constrain_float((pwm_output[i]-1000), 0, 1000);
}
} else {
fdm.num_engines = 4;
fdm.rpm[0] = constrain_float((pwm_output[2]-1000)*3, 0, 3000);
// for quadplane
fdm.rpm[1] = constrain_float((pwm_output[5]-1000)*12, 0, 12000);
fdm.rpm[2] = constrain_float((pwm_output[6]-1000)*12, 0, 12000);
fdm.rpm[3] = constrain_float((pwm_output[7]-1000)*12, 0, 12000);
}
fdm.ByteSwap(); fdm.ByteSwap();
fg_socket.send(&fdm, sizeof(fdm)); fg_socket.send(&fdm, sizeof(fdm));