/* output current state to flightgear */ void SITL_State::_output_to_flightgear(void) { SITL::FGNetFDM fdm {}; const SITL::sitl_fdm &sfdm = _sitl->state; fdm.version = 0x18; fdm.padding = 0; fdm.longitude = radians(sfdm.longitude); fdm.latitude = radians(sfdm.latitude); fdm.altitude = sfdm.altitude; fdm.agl = sfdm.altitude; fdm.phi = radians(sfdm.rollDeg); fdm.theta = radians(sfdm.pitchDeg); 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(); fg_socket.send(&fdm, sizeof(fdm)); }
/* output current state to flightgear */ void SITL_State::_output_to_flightgear(void) { SITL::FGNetFDM fdm {}; const SITL::sitl_fdm &sfdm = _sitl->state; fdm.version = 0x18; fdm.padding = 0; fdm.longitude = radians(sfdm.longitude); fdm.latitude = radians(sfdm.latitude); fdm.altitude = sfdm.altitude; fdm.agl = sfdm.altitude; fdm.phi = radians(sfdm.rollDeg); fdm.theta = radians(sfdm.pitchDeg); fdm.psi = radians(sfdm.yawDeg); fdm.ByteSwap(); fg_socket.send(&fdm, sizeof(fdm)); }