Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: added sitl_simstate_send()

Browse Source 
used to report simulator state in logs
This commit is contained in:
Andrew Tridgell 2012-03-01 15:22:11 +11:00
parent d106e3a970
commit ae6a94a933
5 changed files with 54 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/Desktop/support/desktop.h
View File

@ -30,4 +30,6 @@ void sitl_update_adc(float roll, float pitch, float yaw,
void sitl_setup_adc(void);
void sitl_update_barometer(float altitude);
void sitl_simstate_send(uint8_t chan);
#endif

23
libraries/Desktop/support/sitl.cpp
View File

@ -25,6 +25,7 @@
#include <wiring.h>
#include <AP_PeriodicProcess.h>
#include <AP_TimerProcess.h>
#include <GCS_MAVLink.h>
#include "sitl_adc.h"
#include "sitl_rc.h"
#include "desktop.h"
@ -302,3 +303,25 @@ void sitl_setup(void)
sitl_update_compass(0, 0, 0, 0);
sitl_update_gps(0, 0, 0, 0, 0, false);
}
/* report SITL state via MAVLink */
void sitl_simstate_send(uint8_t chan)
{
double p, q, r;
// we want the gyro values to be directly comparable to the
// raw_imu message, which is in body frame
convert_body_frame(sim_state.rollDeg, sim_state.pitchDeg,
sim_state.rollRate, sim_state.pitchRate, sim_state.yawRate,
&p, &q, &r);
mavlink_msg_simstate_send((mavlink_channel_t)chan,
ToRad(sim_state.rollDeg),
ToRad(sim_state.pitchDeg),
ToRad(sim_state.yawDeg),
sim_state.xAccel,
sim_state.yAccel,
sim_state.zAccel,
p, q, r);
}

17
libraries/Desktop/support/sitl_adc.cpp
View File

@ -68,27 +68,16 @@ void sitl_update_adc(float roll, float pitch, float yaw, // Relative to earth
static const float _sensor_signs[6] = { 1, -1, -1, 1, -1, -1 };
const float accel_offset = 2041;
const float gyro_offset = 1658;
#define ToRad(x) (x*0.01745329252) // *pi/180
const float _gyro_gain_x = ToRad(0.4);
const float _gyro_gain_y = ToRad(0.41);
const float _gyro_gain_z = ToRad(0.41);
const float _accel_scale = 9.80665 / 423.8;
double adc[7];
double phi, theta, phiDot, thetaDot, psiDot;
double p, q, r;
/* convert the angular velocities from earth frame to
body frame. Thanks to James Goppert for the formula
*/
phi = ToRad(roll);
theta = ToRad(pitch);
phiDot = ToRad(rollRate);
thetaDot = ToRad(pitchRate);
psiDot = ToRad(yawRate);
p = phiDot - psiDot*sin(theta);
q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta);
r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot;
convert_body_frame(roll, pitch,
rollRate, pitchRate, yawRate,
&p, &q, &r);
/* work out the ADC channel values */
adc[0] = (p / (_gyro_gain_x * _sensor_signs[0])) + gyro_offset;

21
libraries/Desktop/support/util.cpp
View File

@ -14,6 +14,7 @@
#include <stdint.h>
#include <fcntl.h>
#include "desktop.h"
#include "util.h"
void set_nonblocking(int fd)
@ -37,3 +38,23 @@ double normalise180(double v)
{
return normalise(v, -180, 180);
}
/* convert the angular velocities from earth frame to
body frame. Thanks to James Goppert for the formula
*/
void 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;
phi = ToRad(rollDeg);
theta = ToRad(pitchDeg);
phiDot = ToRad(rollRate);
thetaDot = ToRad(pitchRate);
psiDot = ToRad(yawRate);
*p = phiDot - psiDot*sin(theta);
*q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta);
*r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot;
}

5
libraries/Desktop/support/util.h
View File

@ -2,8 +2,13 @@
#define ft2m(x) ((x) * 0.3048)
#define kt2mps(x) ((x) * 0.514444444)
#define sqr(x) ((x)*(x))
#define ToRad(x) (x*0.01745329252) // *pi/180
void set_nonblocking(int fd);
double normalise(double v, double min, double max);
double normalise180(double v);
void runInterrupt(uint8_t inum);
void convert_body_frame(double rollDeg, double pitchDeg,
double rollRate, double pitchRate, double yawRate,
double *p, double *q, double *r);