aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: reformat SITL code with astyle

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-05 11:49:54 +10:00
parent c81ad1d622
commit 42c6801d61
10 changed files with 122 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/SITL/SIM_Aircraft.cpp
View File

@ -65,16 +65,16 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) :
set_speedup(1);
}
/*
return true if we are on the ground
/*
return true if we are on the ground
*/
bool Aircraft::on_ground(const Vector3f &pos) const
{
return (-pos.z) + home.alt*0.01f <= ground_level + frame_height;
}
/*
update location from position
/*
update location from position
*/
void Aircraft::update_position(void)
{
@ -90,8 +90,8 @@ void Aircraft::update_position(void)
sync_frame_time();
}
/*
rotate to the given yaw
/*
rotate to the given yaw
*/
void Aircraft::set_yaw_degrees(float yaw_degrees)
{
@ -101,7 +101,7 @@ void Aircraft::set_yaw_degrees(float yaw_degrees)
yaw = radians(yaw_degrees);
dcm.from_euler(roll, pitch, yaw);
}
/* advance time by deltat in seconds */
void Aircraft::time_advance(float deltat)
{
@ -202,8 +202,8 @@ double Aircraft::rand_normal(double mean, double stddev)
/*
fill a sitl_fdm structure from the simulator state
/*
fill a sitl_fdm structure from the simulator state
*/
void Aircraft::fill_fdm(struct sitl_fdm &fdm) const
{
@ -233,9 +233,9 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) const
uint64_t Aircraft::get_wall_time_us() const
{
struct timeval tp;
gettimeofday(&tp,NULL);
return tp.tv_sec*1.0e6 + tp.tv_usec;
struct timeval tp;
gettimeofday(&tp,NULL);
return tp.tv_sec*1.0e6 + tp.tv_usec;
}
/*

4
libraries/SITL/SIM_Aircraft.h
View File

@ -27,7 +27,7 @@
/*
parent class for all simulator types
*/
class Aircraft
class Aircraft
{
public:
Aircraft(const char *home_str, const char *frame_str);
@ -84,7 +84,7 @@ protected:
/* rotate to the given yaw */
void set_yaw_degrees(float yaw_degrees);
/* advance time by deltat in seconds */
void time_advance(float deltat);

4
libraries/SITL/SIM_Helicopter.cpp
View File

@ -26,7 +26,7 @@
constructor
*/
Helicopter::Helicopter(const char *home_str, const char *frame_str) :
Aircraft(home_str, frame_str),
Aircraft(home_str, frame_str),
terminal_rotation_rate(4*radians(360.0)),
hover_throttle(0.65f),
terminal_velocity(40.0f),
@ -40,7 +40,7 @@ Helicopter::Helicopter(const char *home_str, const char *frame_str) :
{
mass = 2.13f;
/*
/*
scaling from motor power to Newtons. Allows the copter
to hover against gravity when the motor is at hover_throttle
*/

4
libraries/SITL/SIM_Helicopter.h
View File

@ -34,7 +34,9 @@ public:
void update(const struct sitl_input &input);
/* static object creator */
static Aircraft *create(const char *home_str, const char *frame_str) { return new Helicopter(home_str, frame_str); }
static Aircraft *create(const char *home_str, const char *frame_str) {
return new Helicopter(home_str, frame_str);
}
private:
float terminal_rotation_rate;

22
libraries/SITL/SIM_Multicopter.cpp
View File

@ -24,23 +24,23 @@
Motor m(90, false, 1);
static const Motor quad_plus_motors[4] =
{
static const Motor quad_plus_motors[4] =
{
Motor(90, false, 1),
Motor(270, false, 2),
Motor(0, true, 3),
Motor(180, true, 4)
};
static const Motor quad_x_motors[4] =
{
static const Motor quad_x_motors[4] =
{
Motor(45, false, 1),
Motor(225, false, 2),
Motor(315, true, 3),
Motor(135, true, 4)
};
static const Motor hexa_motors[6] =
static const Motor hexa_motors[6] =
{
Motor(60, false, 1),
Motor(60, true, 7),
@ -50,7 +50,7 @@ static const Motor hexa_motors[6] =
Motor(-60, false, 3),
};
static const Motor hexax_motors[6] =
static const Motor hexax_motors[6] =
{
Motor(30, false, 7),
Motor(90, true, 1),
@ -60,7 +60,7 @@ static const Motor hexax_motors[6] =
Motor(330, true, 3)
};
static const Motor octa_motors[8] =
static const Motor octa_motors[8] =
{
Motor(0, true, 1),
Motor(180, true, 2),
@ -72,7 +72,7 @@ static const Motor octa_motors[8] =
Motor(90, true, 8)
};
static const Motor octa_quad_motors[8] =
static const Motor octa_quad_motors[8] =
{
Motor( 45, false, 1),
Motor( -45, true, 2),
@ -87,7 +87,7 @@ static const Motor octa_quad_motors[8] =
/*
table of supported frame types
*/
static const Frame supported_frames[] =
static const Frame supported_frames[] =
{
Frame("+", 4, quad_plus_motors),
Frame("x", 4, quad_x_motors),
@ -101,7 +101,7 @@ static const Frame supported_frames[] =
constructor
*/
MultiCopter::MultiCopter(const char *home_str, const char *frame_str) :
Aircraft(home_str, frame_str),
Aircraft(home_str, frame_str),
frame(NULL),
hover_throttle(0.51),
terminal_velocity(15.0),
@ -116,7 +116,7 @@ MultiCopter::MultiCopter(const char *home_str, const char *frame_str) :
printf("Frame '%s' not found", frame_str);
exit(1);
}
/*
/*
scaling from total motor power to Newtons. Allows the copter
to hover against gravity when each motor is at hover_throttle
*/

6
libraries/SITL/SIM_Multicopter.h
View File

@ -35,7 +35,7 @@ public:
angle(_angle), // angle in degrees from front
clockwise(_clockwise), // clockwise == true, anti-clockwise == false
servo(_servo) // what servo output drives this motor
{}
{}
};
/*
@ -67,7 +67,9 @@ public:
void update(const struct sitl_input &input);
/* static object creator */
static Aircraft *create(const char *home_str, const char *frame_str) { return new MultiCopter(home_str, frame_str); }
static Aircraft *create(const char *home_str, const char *frame_str) {
return new MultiCopter(home_str, frame_str);
}
private:
const Frame *frame;

4
libraries/SITL/SIM_Rover.cpp
View File

@ -27,7 +27,7 @@
constructor
*/
Rover::Rover(const char *home_str, const char *frame_str) :
Aircraft(home_str, frame_str),
Aircraft(home_str, frame_str),
max_speed(20),
max_accel(30),
wheelbase(0.335),
@ -60,7 +60,7 @@ float Rover::turn_circle(float steering)
return turning_circle * sinf(radians(35)) / sinf(radians(steering*35));
}
/*
/*
return yaw rate in degrees/second given steering_angle and speed
*/
float Rover::calc_yaw_rate(float steering, float speed)

4
libraries/SITL/SIM_Rover.h
View File

@ -34,7 +34,9 @@ public:
void update(const struct sitl_input &input);
/* static object creator */
static Aircraft *create(const char *home_str, const char *frame_str) { return new Rover(home_str, frame_str); }
static Aircraft *create(const char *home_str, const char *frame_str) {
return new Rover(home_str, frame_str);
}
private:
float max_speed;

75
libraries/SITL/SITL.cpp
View File

@ -15,8 +15,7 @@
*/
/*
SITL.cpp - software in the loop state
SITL.cpp - software in the loop state
*/
#include <AP_Common.h>
@ -77,20 +76,20 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = {
/* report SITL state via MAVLink */
void SITL::simstate_send(mavlink_channel_t chan)
{
double p, q, r;
float yaw;
double p, q, r;
float yaw;
// we want the gyro values to be directly comparable to the
// raw_imu message, which is in body frame
convert_body_frame(state.rollDeg, state.pitchDeg,
// we want the gyro values to be directly comparable to the
// raw_imu message, which is in body frame
convert_body_frame(state.rollDeg, state.pitchDeg,
state.rollRate, state.pitchRate, state.yawRate,
&p, &q, &r);
// convert to same conventions as DCM
yaw = state.yawDeg;
if (yaw > 180) {
yaw -= 360;
}
// convert to same conventions as DCM
yaw = state.yawDeg;
if (yaw > 180) {
yaw -= 360;
}
mavlink_msg_simstate_send(chan,
ToRad(state.rollDeg),
@ -107,30 +106,30 @@ void SITL::simstate_send(mavlink_channel_t chan)
/* report SITL state to DataFlash */
void SITL::Log_Write_SIMSTATE(DataFlash_Class &DataFlash)
{
double p, q, r;
float yaw;
double p, q, r;
float yaw;
// we want the gyro values to be directly comparable to the
// raw_imu message, which is in body frame
convert_body_frame(state.rollDeg, state.pitchDeg,
// we want the gyro values to be directly comparable to the
// raw_imu message, which is in body frame
convert_body_frame(state.rollDeg, state.pitchDeg,
state.rollRate, state.pitchRate, state.yawRate,
&p, &q, &r);
// convert to same conventions as DCM
yaw = state.yawDeg;
if (yaw > 180) {
yaw -= 360;
}
// convert to same conventions as DCM
yaw = state.yawDeg;
if (yaw > 180) {
yaw -= 360;
}
struct log_AHRS pkt = {
LOG_PACKET_HEADER_INIT(LOG_SIMSTATE_MSG),
time_ms : hal.scheduler->millis(),
roll : (int16_t)(state.rollDeg*100),
pitch : (int16_t)(state.pitchDeg*100),
yaw : (uint16_t)(wrap_360_cd(yaw*100)),
alt : (float)state.altitude,
lat : (int32_t)(state.latitude*1.0e7),
lng : (int32_t)(state.longitude*1.0e7)
roll : (int16_t)(state.rollDeg*100),
pitch : (int16_t)(state.pitchDeg*100),
yaw : (uint16_t)(wrap_360_cd(yaw*100)),
alt : (float)state.altitude,
lat : (int32_t)(state.latitude*1.0e7),
lng : (int32_t)(state.longitude*1.0e7)
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
@ -143,24 +142,24 @@ void SITL::convert_body_frame(double rollDeg, double pitchDeg,
double rollRate, double pitchRate, double yawRate,
double *p, double *q, double *r)
{
double phi, theta, phiDot, thetaDot, psiDot;
double phi, theta, phiDot, thetaDot, psiDot;
phi = ToRad(rollDeg);
theta = ToRad(pitchDeg);
phiDot = ToRad(rollRate);
thetaDot = ToRad(pitchRate);
psiDot = ToRad(yawRate);
phi = ToRad(rollDeg);
theta = ToRad(pitchDeg);
phiDot = ToRad(rollRate);
thetaDot = ToRad(pitchRate);
psiDot = ToRad(yawRate);
*p = phiDot - psiDot*sinf(theta);
*q = cosf(phi)*thetaDot + sinf(phi)*psiDot*cosf(theta);
*r = cosf(phi)*psiDot*cosf(theta) - sinf(phi)*thetaDot;
*p = phiDot - psiDot*sinf(theta);
*q = cosf(phi)*thetaDot + sinf(phi)*psiDot*cosf(theta);
*r = cosf(phi)*psiDot*cosf(theta) - sinf(phi)*thetaDot;
}
/*
convert angular velocities from body frame to
earth frame.
all inputs and outputs are in radians/s
*/
Vector3f SITL::convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro)

92
libraries/SITL/SITL.h
View File

@ -10,33 +10,33 @@
#include <DataFlash.h>
struct PACKED sitl_fdm {
// this is the packet sent by the simulator
// to the APM executable to update the simulator state
// All values are little-endian
// this is the packet sent by the simulator
// to the APM executable to update the simulator state
// All values are little-endian
uint64_t timestamp_us;
double latitude, longitude; // degrees
double altitude; // MSL
double heading; // degrees
double speedN, speedE, speedD; // m/s
double xAccel, yAccel, zAccel; // m/s/s in body frame
double rollRate, pitchRate, yawRate; // degrees/s/s in earth frame
double rollDeg, pitchDeg, yawDeg; // euler angles, degrees
double airspeed; // m/s
uint32_t magic; // 0x4c56414f
double latitude, longitude; // degrees
double altitude; // MSL
double heading; // degrees
double speedN, speedE, speedD; // m/s
double xAccel, yAccel, zAccel; // m/s/s in body frame
double rollRate, pitchRate, yawRate; // degrees/s/s in earth frame
double rollDeg, pitchDeg, yawDeg; // euler angles, degrees
double airspeed; // m/s
uint32_t magic; // 0x4c56414f
};
class SITL
{
public:
SITL() {
// set a default compass offset
mag_ofs.set(Vector3f(5, 13, -18));
AP_Param::setup_object_defaults(this, var_info);
AP_Param::setup_object_defaults(this, var_info);
}
enum GPSType {
enum GPSType {
GPS_TYPE_NONE = 0,
GPS_TYPE_UBLOX = 1,
GPS_TYPE_MTK = 2,
@ -46,34 +46,34 @@ public:
GPS_TYPE_SBP = 6,
};
struct sitl_fdm state;
struct sitl_fdm state;
static const struct AP_Param::GroupInfo var_info[];
static const struct AP_Param::GroupInfo var_info[];
// noise levels for simulated sensors
AP_Float baro_noise; // in metres
AP_Float baro_drift; // in metres per second
AP_Float baro_glitch; // glitch in meters
AP_Float gyro_noise; // in degrees/second
AP_Float accel_noise; // in m/s/s
AP_Vector3f accel_bias; // in m/s/s
AP_Float aspd_noise; // in m/s
AP_Float mag_noise; // in mag units (earth field is 818)
AP_Float mag_error; // in degrees
AP_Vector3f mag_mot; // in mag units per amp
AP_Vector3f mag_ofs; // in mag units
// noise levels for simulated sensors
AP_Float baro_noise; // in metres
AP_Float baro_drift; // in metres per second
AP_Float baro_glitch; // glitch in meters
AP_Float gyro_noise; // in degrees/second
AP_Float accel_noise; // in m/s/s
AP_Vector3f accel_bias; // in m/s/s
AP_Float aspd_noise; // in m/s
AP_Float mag_noise; // in mag units (earth field is 818)
AP_Float mag_error; // in degrees
AP_Vector3f mag_mot; // in mag units per amp
AP_Vector3f mag_ofs; // in mag units
AP_Float servo_rate; // servo speed in degrees/second
AP_Float sonar_glitch;// probablility between 0-1 that any given sonar sample will read as max distance
AP_Float sonar_noise; // in metres
AP_Float sonar_scale; // meters per volt
AP_Float drift_speed; // degrees/second/minute
AP_Float drift_time; // period in minutes
AP_Float drift_speed; // degrees/second/minute
AP_Float drift_time; // period in minutes
AP_Float engine_mul; // engine multiplier
AP_Int8 gps_disable; // disable simulated GPS
AP_Int8 gps2_enable; // enable 2nd simulated GPS
AP_Int8 gps_delay; // delay in samples
AP_Int8 gps_disable; // disable simulated GPS
AP_Int8 gps2_enable; // enable 2nd simulated GPS
AP_Int8 gps_delay; // delay in samples
AP_Int8 gps_type; // see enum GPSType
AP_Float gps_byteloss;// byte loss as a percent
AP_Int8 gps_numsats; // number of visible satellites
@ -81,13 +81,13 @@ public:
AP_Int8 gps_hertz; // GPS update rate in Hz
AP_Float batt_voltage; // battery voltage base
AP_Float accel_fail; // accelerometer failure value
AP_Int8 rc_fail; // fail RC input
AP_Int8 baro_disable; // disable simulated barometer
AP_Int8 rc_fail; // fail RC input
AP_Int8 baro_disable; // disable simulated barometer
AP_Int8 float_exception; // enable floating point exception checks
AP_Int8 flow_enable; // enable simulated optflow
AP_Int16 flow_rate; // optflow data rate (Hz)
AP_Int8 flow_delay; // optflow data delay
AP_Int8 terrain_enable; // enable using terrain for height
AP_Int8 flow_enable; // enable simulated optflow
AP_Int16 flow_rate; // optflow data rate (Hz)
AP_Int8 flow_delay; // optflow data delay
AP_Int8 terrain_enable; // enable using terrain for height
// wind control
AP_Float wind_speed;
@ -99,16 +99,16 @@ public:
AP_Int16 mag_delay; // magnetometer data delay in ms
AP_Int16 wind_delay; // windspeed data delay in ms
void simstate_send(mavlink_channel_t chan);
void simstate_send(mavlink_channel_t chan);
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,
double rollRate, double pitchRate, double yawRate,
double *p, double *q, double *r);
// convert a set of roll rates from earth frame to body frame
static void convert_body_frame(double rollDeg, double pitchDeg,
double rollRate, double pitchRate, double yawRate,
double *p, double *q, double *r);
// convert a set of roll rates from body frame to earth frame
// convert a set of roll rates from body frame to earth frame
static Vector3f convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro);
};