Browse Source

SITL: expose home yaw to FDMs

mission-4.1.18
Andrew Tridgell 9 years ago
parent
commit
d425965f6d
  1. 6
      libraries/SITL/SIM_Aircraft.cpp
  2. 1
      libraries/SITL/SIM_Aircraft.h

6
libraries/SITL/SIM_Aircraft.cpp

@ -55,13 +55,11 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) : @@ -55,13 +55,11 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) :
min_sleep_time(5000)
#endif
{
float yaw_degrees;
parse_home(home_str, home, yaw_degrees);
parse_home(home_str, home, home_yaw);
location = home;
ground_level = home.alt*0.01;
dcm.from_euler(0, 0, radians(yaw_degrees));
dcm.from_euler(0, 0, radians(home_yaw));
set_speedup(1);

1
libraries/SITL/SIM_Aircraft.h

@ -98,6 +98,7 @@ protected: @@ -98,6 +98,7 @@ protected:
Location location;
float ground_level;
float home_yaw;
float frame_height;
Matrix3f dcm; // rotation matrix, APM conventions, from body to earth
Vector3f gyro; // rad/s

Loading…
Cancel
Save