diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 5a1339bb3d..d22f550a6c 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -72,6 +72,9 @@ public: /* fill a sitl_fdm structure from the simulator state */ void fill_fdm(struct sitl_fdm &fdm) const; + // get frame rate of model in Hz + float get_rate_hz(void) const { return rate_hz; } + protected: Location home; Location location; diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index ee20ca2771..b38ed9a5e1 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -23,6 +23,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -104,7 +105,7 @@ void SITL::simstate_send(mavlink_channel_t chan) } /* report SITL state to DataFlash */ -void SITL::Log_Write_SIMSTATE(DataFlash_Class &DataFlash) +void SITL::Log_Write_SIMSTATE(DataFlash_Class *DataFlash) { float yaw; @@ -124,7 +125,7 @@ void SITL::Log_Write_SIMSTATE(DataFlash_Class &DataFlash) lat : (int32_t)(state.latitude*1.0e7), lng : (int32_t)(state.longitude*1.0e7) }; - DataFlash.WriteBlock(&pkt, sizeof(pkt)); + DataFlash->WriteBlock(&pkt, sizeof(pkt)); } /* diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 85e0925176..4cf492740f 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -2,9 +2,10 @@ #pragma once -#include #include +class DataFlash_Class; + namespace SITL { struct PACKED sitl_fdm { @@ -26,7 +27,6 @@ struct PACKED sitl_fdm { // number of rc output channels #define SITL_NUM_CHANNELS 14 - class SITL { public: @@ -49,6 +49,12 @@ public: struct sitl_fdm state; + // loop update rate in Hz + uint16_t update_rate_hz; + + // true when motors are active + bool motors_on; + static const struct AP_Param::GroupInfo var_info[]; // noise levels for simulated sensors @@ -105,7 +111,7 @@ public: void simstate_send(mavlink_channel_t chan); - void Log_Write_SIMSTATE(DataFlash_Class &dataflash); + void Log_Write_SIMSTATE(DataFlash_Class *dataflash); // convert a set of roll rates from earth frame to body frame static void convert_body_frame(double rollDeg, double pitchDeg,