Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: send state to flightgear viewer

Browse Source
This commit is contained in:
Andrew Tridgell 2016-01-01 17:09:07 +11:00
parent 55e8e0742a
commit 83c8505b3c
2 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -18,6 +18,7 @@
#include <sys/select.h> #include <sys/select.h>
#include <AP_Param/AP_Param.h> #include <AP_Param/AP_Param.h>
#include <SITL/SIM_JSBSim.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -96,6 +97,8 @@ void SITL_State::_sitl_setup(const char *home_str)
if (enable_ADSB) { if (enable_ADSB) {
adsb = new ADSB(_sitl->state, home_str); adsb = new ADSB(_sitl->state, home_str);
} }
fg_socket.connect("127.0.0.1", 5503);
} }
if (_synthetic_clock_mode) { if (_synthetic_clock_mode) {
@ -231,6 +234,27 @@ void SITL_State::_fdm_input(void)
} }
} }
/*
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));
}
/* /*
get FDM input from a local model get FDM input from a local model
@ -259,6 +283,8 @@ void SITL_State::_fdm_input_local(void)
adsb->update(); adsb->update();
} }
_output_to_flightgear();
// update simulation time // update simulation time
hal.scheduler->stop_clock(_sitl->state.timestamp_us); hal.scheduler->stop_clock(_sitl->state.timestamp_us);

5
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -24,6 +24,7 @@
#include <SITL/SITL.h> #include <SITL/SITL.h>
#include <SITL/SIM_Gimbal.h> #include <SITL/SIM_Gimbal.h>
#include <SITL/SIM_ADSB.h> #include <SITL/SIM_ADSB.h>
#include <AP_HAL/utility/Socket.h>
class HAL_SITL; class HAL_SITL;
@ -119,6 +120,7 @@ private:
float airspeed, float altitude); float airspeed, float altitude);
void _fdm_input(void); void _fdm_input(void);
void _fdm_input_local(void); void _fdm_input_local(void);
void _output_to_flightgear(void);
void _simulator_servos(SITL::Aircraft::sitl_input &input); void _simulator_servos(SITL::Aircraft::sitl_input &input);
void _simulator_output(bool synthetic_clock_mode); void _simulator_output(bool synthetic_clock_mode);
void _apply_servo_filter(float deltat); void _apply_servo_filter(float deltat);
@ -205,6 +207,9 @@ private:
bool enable_ADSB; bool enable_ADSB;
SITL::ADSB *adsb; SITL::ADSB *adsb;
// output socket for flightgear viewing
SocketAPM fg_socket{true};
// TCP address to connect uartC to // TCP address to connect uartC to
const char *_client_address; const char *_client_address;
}; };