From 437e1bdba0c0101b1cf82149198a57f92c57e35b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 2 May 2015 20:28:57 +1000 Subject: [PATCH] SITL: added fill_fdm() for FDM output from simulator --- libraries/SITL/SIM_Aircraft.cpp | 58 ++++++++++++++++++++++++++---- libraries/SITL/SIM_Aircraft.h | 6 ++++ libraries/SITL/SIM_Multicopter.cpp | 4 ++- libraries/SITL/SIM_Multicopter.h | 4 +++ 4 files changed, 64 insertions(+), 8 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index fe7f2800f0..a522f9a901 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -20,6 +20,8 @@ #include #include "SIM_Aircraft.h" #include +#include +#include /* parent class for all simulator types @@ -54,10 +56,10 @@ Aircraft::Aircraft(const char *home_str) : home.lng = atof(lon_s) * 1.0e7; home.alt = atof(alt_s) * 1.0e2; location = home; - - free(s); + ground_level = home.alt*0.01; dcm.from_euler(0, 0, atof(yaw_s)); + free(s); } /* @@ -81,6 +83,9 @@ void Aircraft::update_position(void) location.alt = home.alt - position.z*100.0f; velocity_body = dcm.transposed() * velocity_ef; + + time_now_us += frame_time_us; + sync_frame_time(); } /* @@ -126,9 +131,9 @@ void Aircraft::adjust_frame_time(float new_rate) void Aircraft::sync_frame_time(void) { uint64_t now = get_wall_time_us(); - - if (now < last_wall_time_us + scaled_frame_time_us) { - usleep(last_wall_time_us + scaled_frame_time_us - now); + uint64_t dt_us = now - last_wall_time_us; + if (dt_us < scaled_frame_time_us) { + usleep(scaled_frame_time_us - dt_us); now = get_wall_time_us(); if (now > last_wall_time_us && now - last_wall_time_us < 1.0e5) { @@ -150,10 +155,10 @@ void Aircraft::add_noise(float throttle) { gyro += Vector3f(rand_normal(0, 1), rand_normal(0, 1), - rand_normal(0, 1)) * gyro_noise; + rand_normal(0, 1)) * gyro_noise * throttle; accel_body += Vector3f(rand_normal(0, 1), rand_normal(0, 1), - rand_normal(0, 1)) * accel_noise; + rand_normal(0, 1)) * accel_noise * throttle; } /* @@ -191,3 +196,42 @@ double Aircraft::rand_normal(double mean, double stddev) return n2*stddev + mean; } } + + + + +/* + fill a sitl_fdm structure from the simulator state +*/ +void Aircraft::fill_fdm(struct sitl_fdm &fdm) const +{ + fdm.timestamp_us = time_now_us; + fdm.latitude = location.lat * 1.0e-7; + fdm.longitude = location.lng * 1.0e-7; + fdm.altitude = location.alt * 1.0e-2; + fdm.heading = degrees(atan2f(velocity_ef.y, velocity_ef.x)); + fdm.speedN = velocity_ef.x; + fdm.speedE = velocity_ef.y; + fdm.speedD = velocity_ef.z; + fdm.xAccel = accel_body.x; + fdm.yAccel = accel_body.y; + fdm.zAccel = accel_body.z; + Vector3f gyro_ef = SITL::convert_earth_frame(dcm, gyro); + fdm.rollRate = degrees(gyro_ef.x); + fdm.pitchRate = degrees(gyro_ef.y); + fdm.yawRate = degrees(gyro_ef.z); + float r, p, y; + dcm.to_euler(&r, &p, &y); + fdm.rollDeg = degrees(r); + fdm.pitchDeg = degrees(p); + fdm.yawDeg = degrees(y); + fdm.airspeed = velocity_ef.length(); + fdm.magic = 0x4c56414f; +} + +uint64_t Aircraft::get_wall_time_us() const +{ + 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 d1a62f51b3..61e4d22a29 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -17,6 +17,9 @@ parent class for aircraft simulators */ +#ifndef _SIM_AIRCRAFT_H +#define _SIM_AIRCRAFT_H + #include "SITL.h" #include #include @@ -101,3 +104,6 @@ protected: /* return normal distribution random numbers */ double rand_normal(double mean, double stddev); }; + +#endif // _SIM_AIRCRAFT_H + diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 93914b0825..1994957521 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -118,10 +118,12 @@ MultiCopter::MultiCopter(const char *home_str, const char *frame_str) : 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_height = 0.1; - mass = 1.5; + + setup_frame_time(400, 3); } /* diff --git a/libraries/SITL/SIM_Multicopter.h b/libraries/SITL/SIM_Multicopter.h index e4887c36d2..367cf7a5d7 100644 --- a/libraries/SITL/SIM_Multicopter.h +++ b/libraries/SITL/SIM_Multicopter.h @@ -17,6 +17,9 @@ parent class for aircraft simulators */ +#ifndef _SIM_MULTICOPTER_H +#define _SIM_MULTICOPTER_H + #include "SIM_Aircraft.h" /* @@ -73,3 +76,4 @@ private: }; +#endif // _SIM_MULTICOPTER_H