diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index bd34e8d99a..101616fcbd 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -80,6 +80,18 @@ public: // get frame rate of model in Hz float get_rate_hz(void) const { return rate_hz; } + + const Vector3f &get_gyro(void) const { + return gyro; + } + + const Vector3f &get_velocity_ef(void) const { + return velocity_ef; + } + + const Matrix3f &get_dcm(void) const { + return dcm; + } protected: Location home; diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index c316eebc23..5d1a2fae73 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -21,9 +21,7 @@ #include -namespace SITL { - -Motor m(90, false, 1); +using namespace SITL; static const Motor quad_plus_motors[4] = { @@ -88,7 +86,7 @@ static const Motor octa_quad_motors[8] = /* table of supported frame types */ -static const Frame supported_frames[] = +static Frame supported_frames[] = { Frame("+", 4, quad_plus_motors), Frame("quad", 4, quad_plus_motors), @@ -100,12 +98,23 @@ static const Frame supported_frames[] = Frame("octa-quad", 8, octa_quad_motors) }; +void Frame::init(float _mass, float hover_throttle, float _terminal_velocity, float _terminal_rotation_rate) +{ + mass = _mass; + + /* + scaling from total motor power to Newtons. Allows the copter + to hover against gravity when each motor is at hover_throttle + */ + thrust_scale = (mass * GRAVITY_MSS) / (num_motors * hover_throttle); + + terminal_velocity = _terminal_velocity; + terminal_rotation_rate = _terminal_rotation_rate; +} + MultiCopter::MultiCopter(const char *home_str, const char *frame_str) : Aircraft(home_str, frame_str), - frame(NULL), - hover_throttle(0.51), - terminal_velocity(15.0), - terminal_rotation_rate(4*radians(360.0)) + frame(NULL) { for (uint8_t i=0; i < ARRAY_SIZE(supported_frames); i++) { if (strcasecmp(frame_str, supported_frames[i].name) == 0) { @@ -116,25 +125,20 @@ 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 - */ - mass = 1.5; - thrust_scale = (mass * GRAVITY_MSS) / (frame->num_motors * hover_throttle); - + frame->init(1.5, 0.51, 15, 4*radians(360)); frame_height = 0.1; } -/* - update the multicopter simulation by one time step - */ -void MultiCopter::update(const struct sitl_input &input) +// calculate rotational and linear accelerations +void Frame::calculate_forces(const Aircraft &aircraft, + const Aircraft::sitl_input &input, + Vector3f &rot_accel, + Vector3f &body_accel) { - float motor_speed[frame->num_motors]; + float motor_speed[num_motors]; - for (uint8_t i=0; inum_motors; i++) { - uint16_t servo = input.servos[frame->motors[i].servo-1]; + for (uint8_t i=0; inum_motors; i++) { - rot_accel.x += -radians(5000.0) * sinf(radians(frame->motors[i].angle)) * motor_speed[i]; - rot_accel.y += radians(5000.0) * cosf(radians(frame->motors[i].angle)) * motor_speed[i]; - if (frame->motors[i].clockwise) { + for (uint8_t i=0; icalculate_forces(*this, input, rot_accel, body_accel); +} + +/* + update the multicopter simulation by one time step + */ +void MultiCopter::update(const struct sitl_input &input) +{ + // how much time has passed? + float delta_time = frame_time_us * 1.0e-6f; + Vector3f rot_accel; + + calculate_forces(input, rot_accel, accel_body); // update rotational rates in body frame gyro += rot_accel * delta_time; @@ -173,14 +209,9 @@ void MultiCopter::update(const struct sitl_input &input) dcm.rotate(gyro * delta_time); dcm.normalize(); - // air resistance - Vector3f air_resistance = -velocity_ef * (GRAVITY_MSS/terminal_velocity); - - accel_body = Vector3f(0, 0, -thrust / mass); Vector3f accel_earth = dcm * accel_body; accel_earth += Vector3f(0, 0, GRAVITY_MSS); - accel_earth += air_resistance; // if we're on the ground, then our vertical acceleration is limited // to zero. This effectively adds the force of the ground on the aircraft @@ -192,9 +223,6 @@ void MultiCopter::update(const struct sitl_input &input) // acceleration (ie. real movement), plus gravity accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS)); - // add some noise - add_noise(thrust / (thrust_scale * frame->num_motors)); - // new velocity vector velocity_ef += accel_earth * delta_time; @@ -225,4 +253,4 @@ void MultiCopter::update(const struct sitl_input &input) update_position(); } -} // namespace SITL + diff --git a/libraries/SITL/SIM_Multicopter.h b/libraries/SITL/SIM_Multicopter.h index 682a8c9872..397894a67c 100644 --- a/libraries/SITL/SIM_Multicopter.h +++ b/libraries/SITL/SIM_Multicopter.h @@ -54,6 +54,19 @@ public: name(_name), num_motors(_num_motors), motors(_motors) {} + + // initialise frame + void init(float mass, float hover_throttle, float terminal_velocity, float terminal_rotation_rate); + + // calculate rotational and linear accelerations + void calculate_forces(const Aircraft &aircraft, + const Aircraft::sitl_input &input, + Vector3f &rot_accel, Vector3f &body_accel); + + float terminal_velocity; + float terminal_rotation_rate; + float thrust_scale; + float mass; }; /* @@ -71,13 +84,10 @@ public: return new MultiCopter(home_str, frame_str); } -private: - const Frame *frame; - float hover_throttle; // 0..1 - float terminal_velocity; // m/s - - const float terminal_rotation_rate; - float thrust_scale; +protected: + // calculate rotational and linear accelerations + void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel); + Frame *frame; }; } // namespace SITL