diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index fb499bcee8..55793f73c4 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -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; } /* diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 02707155d7..42d6bf4806 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -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); diff --git a/libraries/SITL/SIM_Helicopter.cpp b/libraries/SITL/SIM_Helicopter.cpp index 6b99024246..0a311a37e3 100644 --- a/libraries/SITL/SIM_Helicopter.cpp +++ b/libraries/SITL/SIM_Helicopter.cpp @@ -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 */ diff --git a/libraries/SITL/SIM_Helicopter.h b/libraries/SITL/SIM_Helicopter.h index a09d653a8f..d436052b8a 100644 --- a/libraries/SITL/SIM_Helicopter.h +++ b/libraries/SITL/SIM_Helicopter.h @@ -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; diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 4bf6d390b4..089e1894b6 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -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 */ diff --git a/libraries/SITL/SIM_Multicopter.h b/libraries/SITL/SIM_Multicopter.h index eb62fc949b..8c85b39482 100644 --- a/libraries/SITL/SIM_Multicopter.h +++ b/libraries/SITL/SIM_Multicopter.h @@ -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; diff --git a/libraries/SITL/SIM_Rover.cpp b/libraries/SITL/SIM_Rover.cpp index 8386bff735..2941321788 100644 --- a/libraries/SITL/SIM_Rover.cpp +++ b/libraries/SITL/SIM_Rover.cpp @@ -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) diff --git a/libraries/SITL/SIM_Rover.h b/libraries/SITL/SIM_Rover.h index c1026b806b..2234736823 100644 --- a/libraries/SITL/SIM_Rover.h +++ b/libraries/SITL/SIM_Rover.h @@ -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; diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index a8c6cc03f0..844fa75c95 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -15,8 +15,7 @@ */ /* - SITL.cpp - software in the loop state - + SITL.cpp - software in the loop state */ #include @@ -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) diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 6892a8b7f5..74b0d925a9 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -10,33 +10,33 @@ #include 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); };