SITL: use a SITL namespace

This commit is contained in:
Caio Marcelo de Oliveira Filho 2015-10-22 11:04:42 -02:00
parent be8070e335
commit 49a42dc985
37 changed files with 124 additions and 23 deletions

View File

@ -485,7 +485,7 @@ const AP_Param::Info Rover::var_info[] PROGMEM = {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// @Group: SIM_ // @Group: SIM_
// @Path: ../libraries/SITL/SITL.cpp // @Path: ../libraries/SITL/SITL.cpp
GOBJECT(sitl, "SIM_", SITL), GOBJECT(sitl, "SIM_", SITL::SITL),
#endif #endif
// @Group: AHRS_ // @Group: AHRS_

View File

@ -169,7 +169,7 @@ private:
AP_RSSI rssi; AP_RSSI rssi;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL sitl; SITL::SITL sitl;
#endif #endif
// GCS handling // GCS handling

View File

@ -241,7 +241,7 @@ const AP_Param::Info Tracker::var_info[] PROGMEM = {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// @Group: SIM_ // @Group: SIM_
// @Path: ../libraries/SITL/SITL.cpp // @Path: ../libraries/SITL/SITL.cpp
GOBJECT(sitl, "SIM_", SITL), GOBJECT(sitl, "SIM_", SITL::SITL),
#endif #endif
// @Group: BRD_ // @Group: BRD_

View File

@ -122,7 +122,7 @@ private:
#endif #endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL sitl; SITL::SITL sitl;
#endif #endif
/** /**

View File

@ -186,7 +186,7 @@ private:
AP_AHRS_NavEKF ahrs{ins, barometer, gps, sonar, EKF, EKF2, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF}; AP_AHRS_NavEKF ahrs{ins, barometer, gps, sonar, EKF, EKF2, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL sitl; SITL::SITL sitl;
#endif #endif
// Mission library // Mission library

View File

@ -974,7 +974,7 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
#endif #endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
GOBJECT(sitl, "SIM_", SITL), GOBJECT(sitl, "SIM_", SITL::SITL),
#endif #endif
// @Group: GND_ // @Group: GND_

View File

@ -1161,7 +1161,7 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// @Group: SIM_ // @Group: SIM_
// @Path: ../libraries/SITL/SITL.cpp // @Path: ../libraries/SITL/SITL.cpp
GOBJECT(sitl, "SIM_", SITL), GOBJECT(sitl, "SIM_", SITL::SITL),
#endif #endif
#if OBC_FAILSAFE == ENABLED #if OBC_FAILSAFE == ENABLED

View File

@ -224,7 +224,7 @@ private:
AP_SteerController steerController {ahrs}; AP_SteerController steerController {ahrs};
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL sitl; SITL::SITL sitl;
#endif #endif
// Training mode // Training mode

View File

@ -230,7 +230,7 @@ private:
ReplayVehicle &_vehicle; ReplayVehicle &_vehicle;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL sitl; SITL::SITL sitl;
#endif #endif
LogReader logreader{_vehicle.ahrs, _vehicle.ins, _vehicle.barometer, _vehicle.compass, _vehicle.gps, _vehicle.airspeed, _vehicle.dataflash, log_structure, ARRAY_SIZE(log_structure), nottypes}; LogReader logreader{_vehicle.ahrs, _vehicle.ins, _vehicle.barometer, _vehicle.compass, _vehicle.gps, _vehicle.airspeed, _vehicle.dataflash, log_structure, ARRAY_SIZE(log_structure), nottypes};

View File

@ -22,6 +22,7 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
using namespace HALSITL; using namespace HALSITL;
using namespace SITL;
void SITL_State::_set_param_default(const char *parm) void SITL_State::_set_param_default(const char *parm)
{ {
@ -74,7 +75,7 @@ void SITL_State::_sitl_setup(void)
fprintf(stdout, "Starting SITL input\n"); fprintf(stdout, "Starting SITL input\n");
// find the barometer object if it exists // find the barometer object if it exists
_sitl = (SITL *)AP_Param::find_object("SIM_"); _sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
_barometer = (AP_Baro *)AP_Param::find_object("GND_"); _barometer = (AP_Baro *)AP_Param::find_object("GND_");
_ins = (AP_InertialSensor *)AP_Param::find_object("INS_"); _ins = (AP_InertialSensor *)AP_Param::find_object("INS_");
_compass = (Compass *)AP_Param::find_object("COMPASS_"); _compass = (Compass *)AP_Param::find_object("COMPASS_");

View File

@ -109,7 +109,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 _simulator_servos(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);
uint16_t _airspeed_sensor(float airspeed); uint16_t _airspeed_sensor(float airspeed);
@ -142,7 +142,7 @@ private:
AP_Terrain *_terrain; AP_Terrain *_terrain;
int _sitl_fd; int _sitl_fd;
SITL *_sitl; SITL::SITL *_sitl;
uint16_t _rcout_port; uint16_t _rcout_port;
uint16_t _simin_port; uint16_t _simin_port;
float _current; float _current;
@ -190,11 +190,11 @@ private:
uint32_t delayed_time_baro; uint32_t delayed_time_baro;
// internal SITL model // internal SITL model
Aircraft *sitl_model; SITL::Aircraft *sitl_model;
// simulated gimbal // simulated gimbal
bool enable_gimbal; bool enable_gimbal;
Gimbal *gimbal; SITL::Gimbal *gimbal;
// TCP address to connect uartC to // TCP address to connect uartC to
const char *_client_address; const char *_client_address;

View File

@ -26,6 +26,7 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
using namespace HALSITL; using namespace HALSITL;
using namespace SITL;
// catch floating point exceptions // catch floating point exceptions
static void _sig_fpe(int signum) static void _sig_fpe(int signum)

View File

@ -801,36 +801,36 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
return; return;
} }
switch ((SITL::GPSType)_sitl->gps_type.get()) { switch ((SITL::SITL::GPSType)_sitl->gps_type.get()) {
case SITL::GPS_TYPE_NONE: case SITL::SITL::GPS_TYPE_NONE:
// no GPS attached // no GPS attached
break; break;
case SITL::GPS_TYPE_UBLOX: case SITL::SITL::GPS_TYPE_UBLOX:
_update_gps_ubx(&d); _update_gps_ubx(&d);
break; break;
case SITL::GPS_TYPE_MTK: case SITL::SITL::GPS_TYPE_MTK:
_update_gps_mtk(&d); _update_gps_mtk(&d);
break; break;
case SITL::GPS_TYPE_MTK16: case SITL::SITL::GPS_TYPE_MTK16:
_update_gps_mtk16(&d); _update_gps_mtk16(&d);
break; break;
case SITL::GPS_TYPE_MTK19: case SITL::SITL::GPS_TYPE_MTK19:
_update_gps_mtk19(&d); _update_gps_mtk19(&d);
break; break;
case SITL::GPS_TYPE_NMEA: case SITL::SITL::GPS_TYPE_NMEA:
_update_gps_nmea(&d); _update_gps_nmea(&d);
break; break;
case SITL::GPS_TYPE_SBP: case SITL::SITL::GPS_TYPE_SBP:
_update_gps_sbp(&d); _update_gps_sbp(&d);
break; break;
case SITL::GPS_TYPE_FILE: case SITL::SITL::GPS_TYPE_FILE:
_update_gps_file(&d); _update_gps_file(&d);
break; break;

View File

@ -30,6 +30,8 @@
#include <Mmsystem.h> #include <Mmsystem.h>
#endif #endif
namespace SITL {
/* /*
parent class for all simulator types parent class for all simulator types
*/ */
@ -290,4 +292,7 @@ void Aircraft::set_speedup(float speedup)
{ {
setup_frame_time(rate_hz, speedup); setup_frame_time(rate_hz, speedup);
} }
} // namespace SITL
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

View File

@ -24,6 +24,8 @@
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
namespace SITL {
/* /*
parent class for all simulator types parent class for all simulator types
*/ */
@ -136,5 +138,7 @@ private:
const uint32_t min_sleep_time; const uint32_t min_sleep_time;
}; };
} // namespace SITL
#endif // _SIM_AIRCRAFT_H #endif // _SIM_AIRCRAFT_H

View File

@ -22,6 +22,8 @@
#include "SIM_Balloon.h" #include "SIM_Balloon.h"
#include <stdio.h> #include <stdio.h>
namespace SITL {
/* /*
constructor constructor
*/ */
@ -119,4 +121,7 @@ void Balloon::update(const struct sitl_input &input)
// update lat/lon/altitude // update lat/lon/altitude
update_position(); update_position();
} }
} // namespace SITL
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

View File

@ -22,6 +22,8 @@
#include "SIM_Aircraft.h" #include "SIM_Aircraft.h"
namespace SITL {
/* /*
a balloon simulator a balloon simulator
*/ */
@ -47,5 +49,6 @@ private:
bool released = false; bool released = false;
}; };
} // namespace SITL
#endif // _SIM_BALLOON_H #endif // _SIM_BALLOON_H

View File

@ -24,6 +24,8 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
namespace SITL {
/* /*
constructor constructor
*/ */
@ -152,4 +154,7 @@ void CRRCSim::update(const struct sitl_input &input)
recv_fdm(input); recv_fdm(input);
update_position(); update_position();
} }
} // namespace SITL
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

View File

@ -23,6 +23,8 @@
#include "SIM_Aircraft.h" #include "SIM_Aircraft.h"
#include <AP_HAL/utility/Socket.h> #include <AP_HAL/utility/Socket.h>
namespace SITL {
/* /*
a CRRCSim simulator a CRRCSim simulator
*/ */
@ -76,5 +78,6 @@ private:
SocketAPM sock; SocketAPM sock;
}; };
} // namespace SITL
#endif // _SIM_CRRCSIM_H #endif // _SIM_CRRCSIM_H

View File

@ -24,6 +24,8 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
namespace SITL {
/* /*
constructor constructor
*/ */
@ -127,4 +129,7 @@ void Gazebo::update(const struct sitl_input &input)
recv_fdm(input); recv_fdm(input);
update_position(); update_position();
} }
} // namespace SITL
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

View File

@ -23,6 +23,8 @@
#include "SIM_Aircraft.h" #include "SIM_Aircraft.h"
#include <AP_HAL/utility/Socket.h> #include <AP_HAL/utility/Socket.h>
namespace SITL {
/* /*
Gazebo simulator Gazebo simulator
*/ */
@ -68,5 +70,6 @@ private:
SocketAPM sock; SocketAPM sock;
}; };
} // namespace SITL
#endif // _SIM_GAZEBO_H #endif // _SIM_GAZEBO_H

View File

@ -24,6 +24,8 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
namespace SITL {
Gimbal::Gimbal(const struct sitl_fdm &_fdm) : Gimbal::Gimbal(const struct sitl_fdm &_fdm) :
fdm(_fdm), fdm(_fdm),
target_address("127.0.0.1"), target_address("127.0.0.1"),
@ -291,4 +293,7 @@ void Gimbal::send_report(void)
delta_angle.zero(); delta_angle.zero();
} }
} }
} // namespace SITL
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

View File

@ -24,6 +24,8 @@
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <AP_HAL/utility/Socket.h> #include <AP_HAL/utility/Socket.h>
namespace SITL {
class Gimbal class Gimbal
{ {
public: public:
@ -105,6 +107,9 @@ private:
void send_report(void); void send_report(void);
}; };
} // namespace SITL
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD
#endif // _SIM_GIMBAL_H #endif // _SIM_GIMBAL_H

View File

@ -22,6 +22,8 @@
#include "SIM_Helicopter.h" #include "SIM_Helicopter.h"
#include <stdio.h> #include <stdio.h>
namespace SITL {
/* /*
constructor constructor
*/ */
@ -209,4 +211,7 @@ void Helicopter::update(const struct sitl_input &input)
// update lat/lon/altitude // update lat/lon/altitude
update_position(); update_position();
} }
} // namespace SITL
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

View File

@ -22,6 +22,8 @@
#include "SIM_Aircraft.h" #include "SIM_Aircraft.h"
namespace SITL {
/* /*
a helicopter simulator a helicopter simulator
*/ */
@ -59,5 +61,6 @@ private:
bool gas_heli = false; bool gas_heli = false;
}; };
} // namespace SITL
#endif // _SIM_HELICOPTER_H #endif // _SIM_HELICOPTER_H

View File

@ -30,6 +30,8 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
namespace SITL {
// the asprintf() calls are not worth checking for SITL // the asprintf() calls are not worth checking for SITL
#pragma GCC diagnostic ignored "-Wunused-result" #pragma GCC diagnostic ignored "-Wunused-result"
@ -456,4 +458,7 @@ void JSBSim::update(const struct sitl_input &input)
sync_frame_time(); sync_frame_time();
drain_control_socket(); drain_control_socket();
} }
} // namespace SITL
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

View File

@ -23,6 +23,8 @@
#include "SIM_Aircraft.h" #include "SIM_Aircraft.h"
#include <AP_HAL/utility/Socket.h> #include <AP_HAL/utility/Socket.h>
namespace SITL {
/* /*
a Jsbsim simulator a Jsbsim simulator
*/ */
@ -175,4 +177,6 @@ public:
void ByteSwap(void); void ByteSwap(void);
}; };
} // namespace SITL
#endif // _SIM_JSBSIM_H #endif // _SIM_JSBSIM_H

View File

@ -22,6 +22,8 @@
#include "SIM_Multicopter.h" #include "SIM_Multicopter.h"
#include <stdio.h> #include <stdio.h>
namespace SITL {
Motor m(90, false, 1); Motor m(90, false, 1);
static const Motor quad_plus_motors[4] = static const Motor quad_plus_motors[4] =
@ -226,4 +228,7 @@ void MultiCopter::update(const struct sitl_input &input)
// update lat/lon/altitude // update lat/lon/altitude
update_position(); update_position();
} }
} // namespace SITL
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

View File

@ -22,6 +22,8 @@
#include "SIM_Aircraft.h" #include "SIM_Aircraft.h"
namespace SITL {
/* /*
class to describe a motor position class to describe a motor position
*/ */
@ -80,5 +82,6 @@ private:
float thrust_scale; float thrust_scale;
}; };
} // namespace SITL
#endif // _SIM_MULTICOPTER_H #endif // _SIM_MULTICOPTER_H

View File

@ -23,6 +23,8 @@
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
namespace SITL {
/* /*
constructor constructor
*/ */
@ -157,4 +159,7 @@ void SimRover::update(const struct sitl_input &input)
// update lat/lon/altitude // update lat/lon/altitude
update_position(); update_position();
} }
} // namespace SITL
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

View File

@ -22,6 +22,8 @@
#include "SIM_Aircraft.h" #include "SIM_Aircraft.h"
namespace SITL {
/* /*
a rover simulator a rover simulator
*/ */
@ -53,5 +55,6 @@ private:
float calc_lat_accel(float steering_angle, float speed); float calc_lat_accel(float steering_angle, float speed);
}; };
} // namespace SITL
#endif // _SIM_ROVER_H #endif // _SIM_ROVER_H

View File

@ -21,6 +21,8 @@
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <stdio.h> #include <stdio.h>
namespace SITL {
Tracker::Tracker(const char *home_str, const char *frame_str) : Tracker::Tracker(const char *home_str, const char *frame_str) :
Aircraft(home_str, frame_str) Aircraft(home_str, frame_str)
{} {}
@ -135,4 +137,6 @@ void Tracker::update(const struct sitl_input &input)
update_position(); update_position();
} }
} // namespace SITL
#endif #endif

View File

@ -22,6 +22,8 @@
#include "SIM_Aircraft.h" #include "SIM_Aircraft.h"
namespace SITL {
/* /*
a antenna tracker simulator a antenna tracker simulator
*/ */
@ -57,4 +59,6 @@ private:
void update_onoff_servos(float &yaw_rate, float &pitch_rate); void update_onoff_servos(float &yaw_rate, float &pitch_rate);
}; };
} // namespace SITL
#endif #endif

View File

@ -27,6 +27,8 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
namespace SITL {
/* /*
constructor constructor
*/ */
@ -143,4 +145,7 @@ void last_letter::update(const struct sitl_input &input)
recv_fdm(input); recv_fdm(input);
sync_frame_time(); sync_frame_time();
} }
} // namespace SITL
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

View File

@ -23,6 +23,8 @@
#include "SIM_Aircraft.h" #include "SIM_Aircraft.h"
#include <AP_HAL/utility/Socket.h> #include <AP_HAL/utility/Socket.h>
namespace SITL {
/* /*
a last_letter simulator a last_letter simulator
*/ */
@ -74,5 +76,6 @@ private:
const char *frame_str; const char *frame_str;
}; };
} // namespace SITL
#endif // _SIM_LAST_LETTER_H #endif // _SIM_LAST_LETTER_H

View File

@ -25,6 +25,8 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
namespace SITL {
// table of user settable parameters // table of user settable parameters
const AP_Param::GroupInfo SITL::var_info[] PROGMEM = { const AP_Param::GroupInfo SITL::var_info[] PROGMEM = {
AP_GROUPINFO("BARO_RND", 0, SITL, baro_noise, 0.2f), AP_GROUPINFO("BARO_RND", 0, SITL, baro_noise, 0.2f),
@ -170,3 +172,4 @@ Vector3f SITL::convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro)
return Vector3f(phiDot, thetaDot, psiDot); return Vector3f(phiDot, thetaDot, psiDot);
} }
} // namespace SITL

View File

@ -9,6 +9,8 @@
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
#include <DataFlash/DataFlash.h> #include <DataFlash/DataFlash.h>
namespace SITL {
struct PACKED sitl_fdm { struct PACKED sitl_fdm {
// this is the packet sent by the simulator // this is the packet sent by the simulator
// to the APM executable to update the simulator state // to the APM executable to update the simulator state
@ -119,4 +121,6 @@ public:
static Vector3f convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro); static Vector3f convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro);
}; };
} // namespace SITL
#endif // __SITL_H__ #endif // __SITL_H__