Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 04:45:37 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-10-23 21:43:52 +00:00
parent 14b1a3064b
commit ec5d1fa975
17 changed files with 454 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz.xml
+6 -6
View File
@@ -1,10 +1,10 @@
<airframe name="BOOZ">
<servos min="0" neutral="0" max="0x3ff">
<servo name="MOTOR_FRONT" no="1" min="1000" neutral="1000" max="2000"/>
<servo name="MOTOR_BACK" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="MOTOR_LEFT" no="3" min="1000" neutral="1000" max="2000"/>
<servo name="MOTOR_RIGHT" no="2" min="1000" neutral="1000" max="2000"/>
<servo name="MOTOR_FRONT" no="1" min="1200" neutral="1200" max="1850"/>
<servo name="MOTOR_BACK" no="0" min="1200" neutral="1200" max="1850"/>
<servo name="MOTOR_LEFT" no="3" min="1200" neutral="1200" max="1850"/>
<servo name="MOTOR_RIGHT" no="2" min="1200" neutral="1200" max="1850"/>
</servos>
<commands>
@@ -21,8 +21,8 @@
<let var="r" value="trim_r + @R"/>
<set servo="MOTOR_FRONT" value="$throttle + $q - $r"/>
<set servo="MOTOR_BACK" value="$throttle - $q - $r"/>
<set servo="MOTOR_RIGHT" value="$throttle + $p + $r"/>
<set servo="MOTOR_LEFT" value="$throttle - $p + $r"/>
<set servo="MOTOR_RIGHT" value="$throttle - $p + $r"/>
<set servo="MOTOR_LEFT" value="$throttle + $p + $r"/>
</command_laws>
<rc_commands>
conf/messages.xml
+7
View File
@@ -437,6 +437,13 @@
<!-- <field name="vsupply" type="uint8" unit="decivolt"/> -->
</message>
<message name="BOOZ_DEBUG" ID="216">
<field name="sp_p" type="float"/>
<field name="sp_q" type="float"/>
<field name="sp_r" type="float"/>
<field name="sp_t" type="float"/>
</message>
<message name="ENOSE_STATUS" ID="218">
<field name="val1" type="uint16"/>
<field name="val2" type="uint16"/>
conf/settings/booz.xml
+4
View File
@@ -2,5 +2,9 @@
<settings>
<dl_settings>
<dl_setting var="booz_control_rate_pq_pgain" min="-2000" step="10" max="-100" module="booz_control" shortname="pq_p"/>
<dl_setting var="booz_control_rate_pq_dgain" min="0" step="0.1" max="10" module="booz_control" shortname="pq_d"/>
<dl_setting var="booz_control_rate_r_pgain" min="-500" step="10" max="-10" module="booz_control" shortname="r_p"/>
<dl_setting var="booz_control_rate_r_dgain" min="0" step="0.1" max="10" module="booz_control" shortname="r_d"/>
</dl_settings>
</settings>
conf/telemetry/booz.xml
+1
View File
@@ -6,6 +6,7 @@
<mode name="default">
<message name="BOOZ_STATUS" period="1."/>
<message name="BOOZ_FD" period="0.05"/>
<message name="BOOZ_DEBUG" period="0.25"/>
</mode>
<mode name="debug">
</mode>
sw/airborne/6dof.h
+5
View File
@@ -10,4 +10,9 @@
#define AXIS_Q 1
#define AXIS_R 2
#define EULER_PHI 0
#define EULER_THETA 1
#define EULER_PSI 2
#define EULER_NB 3
#endif /* _6DOF_H */
sw/airborne/booz_control.c
+1 -1
View File
@@ -52,7 +52,7 @@ void booz_control_rate_compute_setpoints(void) {
booz_control_p_sp = -rc_values[RADIO_ROLL] * RadOfDeg(BOOZ_CONTROL_RATE_PQ_MAX_SP)/MAX_PPRZ;
booz_control_q_sp = rc_values[RADIO_PITCH] * RadOfDeg(BOOZ_CONTROL_RATE_PQ_MAX_SP)/MAX_PPRZ;
booz_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
booz_control_power_sp = rc_values[RADIO_THROTTLE] / MAX_PPRZ;
booz_control_power_sp = rc_values[RADIO_THROTTLE] / (float)MAX_PPRZ;
}
sw/airborne/booz_telemetry.h
+3
View File
@@ -10,12 +10,15 @@
#include "link_imu.h"
#include "booz_estimator.h"
#include "booz_autopilot.h"
#include "booz_control.h"
#include "downlink.h"
#define PERIODIC_SEND_BOOZ_STATUS() DOWNLINK_SEND_BOOZ_STATUS(&link_imu_nb_err, &link_imu_status, &rc_status, &booz_autopilot_mode)
#define PERIODIC_SEND_BOOZ_FD() DOWNLINK_SEND_BOOZ_FD(&booz_estimator_p, &booz_estimator_q, &booz_estimator_r,\
&booz_estimator_phi, &booz_estimator_theta, &booz_estimator_psi);
#define PERIODIC_SEND_BOOZ_DEBUG() DOWNLINK_SEND_BOOZ_DEBUG(&booz_control_p_sp, &booz_control_q_sp, &booz_control_r_sp, &booz_control_power_sp);
extern uint8_t telemetry_mode_Main;
sw/airborne/main_booz.c
+6
View File
@@ -25,6 +25,10 @@ static inline void main_init( void );
static inline void main_periodic_task( void );
static inline void main_event_task( void );
int16_t trim_p = 0;
int16_t trim_q = 0;
int16_t trim_r = 0;
int main( void ) {
main_init();
while(1) {
@@ -64,6 +68,8 @@ static inline void main_periodic_task( void ) {
booz_autopilot_periodic_task();
SetActuatorsFromCommands(commands);
static uint8_t _50hz = 0;
_50hz++;
if (_50hz > 5) _50hz = 0;
sw/simulator/Makefile
+1 -1
View File
@@ -97,5 +97,5 @@ CC = gcc
CFLAGS=-g -O2 -Wall `pkg-config glib-2.0 --cflags` -I /usr/include/meschach -I../airborne -I../../var/BOOZ
LDFLAGS=`pkg-config glib-2.0 --libs` -lm -lmeschach
booz_sim: main_booz_sim.c booz_flight_model.c booz_flight_model_utils.c
booz_sim: main_booz_sim.c booz_flight_model.c booz_flight_model_utils.c booz_flightgear.c
$(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS)
sw/simulator/booz_flight_model.c
+208 -17
View File
@@ -1,5 +1,7 @@
#include "booz_flight_model.h"
#include <math.h>
#include "booz_flight_model_params.h"
#include "booz_flight_model_utils.h"
#include "airframe.h"
@@ -7,33 +9,222 @@
#include "6dof.h"
struct BoozState bs;
struct BoozFlightModel bfm;
static void motor_model_run( double dt );
static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot);
//static void motor_model_run( double dt );
//static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot);
static VEC* booz_get_forces_body_frame( VEC* X, VEC* M, MAT* dcm, VEC* omega_square);
static VEC* booz_get_moments_body_frame( VEC* X, VEC* M, VEC* omega_square );
static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot);
void booz_flight_model_init( void ) {
bs.position = v_get(AXIS_NB);
bs.speed = v_get(AXIS_NB);
bs.eulers = v_get(AXIS_NB);
bs.eulers_dot = v_get(AXIS_NB);
bs.body_rates = v_get(AXIS_NB);
bs.bat_voltage = BAT_VOLTAGE;
bs.mot_voltage = v_get(SERVOS_NB);
bs.mot_omega = v_get(SERVOS_NB);
bfm.bat_voltage = BAT_VOLTAGE;
bfm.mot_voltage = v_get(SERVOS_NB);
bfm.state = v_get(BFMS_SIZE);
/* constants */
bfm.g_earth = v_get(AXIS_NB);
bfm.g_earth->ve[AXIS_X] = 0.;
bfm.g_earth->ve[AXIS_Y] = 0.;
bfm.g_earth->ve[AXIS_Z] = G;
bfm.thrust_factor = 0.5 * RHO * PROP_AREA * C_t * PROP_RADIUS * PROP_RADIUS;
bfm.torque_factor = 0.5 * RHO * PROP_AREA * C_q * PROP_RADIUS * PROP_RADIUS;
bfm.props_moment_matrix = m_get(AXIS_NB, SERVOS_NB);
m_zero(bfm.props_moment_matrix);
bfm.props_moment_matrix->me[AXIS_X][SERVO_MOTOR_LEFT] = L * bfm.thrust_factor;
bfm.props_moment_matrix->me[AXIS_X][SERVO_MOTOR_RIGHT] = -L * bfm.thrust_factor;
bfm.props_moment_matrix->me[AXIS_Y][SERVO_MOTOR_BACK] = L * bfm.thrust_factor;
bfm.props_moment_matrix->me[AXIS_Y][SERVO_MOTOR_FRONT] = -L * bfm.thrust_factor;
bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_LEFT] = -bfm.torque_factor;
bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_RIGHT] = -bfm.torque_factor;
bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_BACK] = bfm.torque_factor;
bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_FRONT] = bfm.torque_factor;
bfm.Inert = m_get(AXIS_NB, AXIS_NB);
m_zero(bfm.Inert);
bfm.Inert->me[AXIS_X][AXIS_X] = Ix;
bfm.Inert->me[AXIS_Y][AXIS_Y] = Iy;
bfm.Inert->me[AXIS_Z][AXIS_Z] = Iz;
bfm.Inert_inv = m_get(AXIS_NB, AXIS_NB);
m_zero(bfm.Inert_inv);
bfm.Inert_inv->me[AXIS_X][AXIS_X] = 1./Ix;
bfm.Inert_inv->me[AXIS_Y][AXIS_Y] = 1./Iy;
bfm.Inert_inv->me[AXIS_Z][AXIS_Z] = 1./Iz;
}
void booz_flight_model_run( double dt, double* commands ) {
int i;
for (i=0; i<SERVOS_NB; i++)
bs.mot_voltage->ve[i] = bs.bat_voltage * commands[i];
motor_model_run(dt);
bfm.mot_voltage->ve[i] = bfm.bat_voltage * commands[i];
// motor_model_run(dt);
rk4(booz_flight_model_get_derivatives, bfm.state, bfm.mot_voltage, dt);
}
/*
compute the sum of external forces.
assumes that dcm and omega_square are already precomputed from X
*/
static VEC* booz_get_forces_body_frame( VEC* X, VEC* F , MAT* dcm, VEC* omega_square) {
// propeller thrust
F->ve[AXIS_X] = 0;
F->ve[AXIS_Y] = 0;
F->ve[AXIS_Z] = -v_sum(omega_square) * bfm.thrust_factor;
// gravity
static VEC *g_body = VNULL;
g_body = v_resize(g_body, AXIS_NB);
g_body = mv_mlt(dcm, bfm.g_earth, g_body);
F = v_mltadd(F, g_body, MASS, F);
// body drag
static VEC *v_body = VNULL;
v_body = v_resize(v_body, AXIS_NB);
v_body->ve[AXIS_X] = X->ve[BFMS_U];
v_body->ve[AXIS_Y] = X->ve[BFMS_V];
v_body->ve[AXIS_Z] = X->ve[BFMS_W];
double norm_speed = v_norm2(v_body);
F = v_mltadd(F, v_body, - norm_speed * C_d_body, F);
return F;
}
/*
compute the sum of external moments.
assumes that omega_square is already precomputed from X
*/
static VEC* booz_get_moments_body_frame( VEC* X, VEC* M, VEC* omega_square ) {
M = mv_mlt(bfm.props_moment_matrix, omega_square, M);
return M;
}
static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot) {
/* square of prop rotational speeds */
static VEC *omega_square = VNULL;
omega_square = v_resize(omega_square,SERVOS_NB);
omega_square->ve[SERVO_MOTOR_BACK] = X->ve[BFMS_OM_B];
omega_square->ve[SERVO_MOTOR_FRONT] = X->ve[BFMS_OM_F];
omega_square->ve[SERVO_MOTOR_RIGHT] = X->ve[BFMS_OM_R];
omega_square->ve[SERVO_MOTOR_LEFT] = X->ve[BFMS_OM_L];
omega_square = v_star(omega_square, omega_square, omega_square);
/* extract eulers angles from state */
static VEC *eulers = VNULL;
eulers = v_resize(eulers, AXIS_NB);
eulers->ve[EULER_PHI] = X->ve[BFMS_PHI];
eulers->ve[EULER_THETA] = X->ve[BFMS_THETA];
eulers->ve[EULER_PSI] = X->ve[BFMS_PSI];
/* direct cosine matrix ( inertial to body )*/
static MAT *dcm = MNULL;
dcm = m_resize(dcm,AXIS_NB, AXIS_NB);
dcm = dcm_of_eulers(eulers, dcm);
/* transpose of dcm ( body to inertial ) */
static MAT *dcm_t = MNULL;
dcm_t = m_resize(dcm_t,AXIS_NB, AXIS_NB);
dcm_t = m_transp(dcm, dcm_t);
/* compute external forces */
static VEC *f_body = VNULL;
f_body = v_resize(f_body, AXIS_NB);
f_body = booz_get_forces_body_frame( X, f_body , dcm, omega_square);
/* compute external moments */
static VEC *m_body = VNULL;
m_body = v_resize(m_body, AXIS_NB);
m_body = booz_get_moments_body_frame( X, m_body, omega_square);
/* derivatives of position */
static VEC *speed_body = VNULL;
speed_body = v_resize(speed_body, AXIS_NB);
speed_body->ve[AXIS_X] = X->ve[BFMS_U];
speed_body->ve[AXIS_Y] = X->ve[BFMS_V];
speed_body->ve[AXIS_Z] = X->ve[BFMS_W];
static VEC *speed_earth = VNULL;
speed_earth = v_resize(speed_earth, AXIS_NB);
speed_earth = mv_mlt(dcm_t, speed_body,speed_earth);
Xdot->ve[BFMS_X] = speed_earth->ve[AXIS_X];
Xdot->ve[BFMS_Y] = speed_earth->ve[AXIS_Y];
Xdot->ve[BFMS_Z] = speed_earth->ve[AXIS_Z];
/* derivatives of speed */
/* extracts body rates from state */
static VEC *rate_body = VNULL;
rate_body = v_resize(rate_body, AXIS_NB);
rate_body->ve[AXIS_P] = X->ve[BFMS_P];
rate_body->ve[AXIS_Q] = X->ve[BFMS_Q];
rate_body->ve[AXIS_R] = X->ve[BFMS_R];
/* Newton in body frame */
static VEC *fict_f = VNULL;
fict_f = v_resize(fict_f, AXIS_NB);
fict_f = out_prod(speed_body, rate_body, fict_f);
Xdot->ve[BFMS_U] = 1./MASS * f_body->ve[AXIS_X] + fict_f->ve[AXIS_X];
Xdot->ve[BFMS_V] = 1./MASS * f_body->ve[AXIS_Y] + fict_f->ve[AXIS_Y];
Xdot->ve[BFMS_W] = 1./MASS * f_body->ve[AXIS_Z] + fict_f->ve[AXIS_Z];
/* derivatives of eulers */
double sinPHI = sin(X->ve[BFMS_PHI]);
double cosPHI = cos(X->ve[BFMS_PHI]);
double cosTHETA = cos(X->ve[BFMS_THETA]);
double tanTHETA = tan(X->ve[BFMS_THETA]);
static MAT *euler_dot_of_pqr = MNULL;
euler_dot_of_pqr = m_resize(euler_dot_of_pqr,AXIS_NB, AXIS_NB);
euler_dot_of_pqr->me[EULER_PHI][AXIS_P] = 1.;
euler_dot_of_pqr->me[EULER_PHI][AXIS_Q] = sinPHI*tanTHETA;
euler_dot_of_pqr->me[EULER_PHI][AXIS_R] = cosPHI*tanTHETA;
euler_dot_of_pqr->me[EULER_THETA][AXIS_P] = 0.;
euler_dot_of_pqr->me[EULER_THETA][AXIS_Q] = cosPHI;
euler_dot_of_pqr->me[EULER_THETA][AXIS_R] = -sinPHI;
euler_dot_of_pqr->me[EULER_PSI][AXIS_P] = 0.;
euler_dot_of_pqr->me[EULER_PSI][AXIS_Q] = sinPHI/cosTHETA;
euler_dot_of_pqr->me[EULER_PSI][AXIS_R] = cosPHI/cosTHETA;
static VEC *euler_dot = VNULL;
euler_dot = v_resize(euler_dot, AXIS_NB);
euler_dot = mv_mlt(euler_dot_of_pqr, rate_body, euler_dot);
Xdot->ve[BFMS_PHI] = euler_dot->ve[EULER_PHI];
Xdot->ve[BFMS_THETA] = euler_dot->ve[EULER_THETA];
Xdot->ve[BFMS_PSI] = euler_dot->ve[EULER_PSI];
/* derivatives of rates */
/* Newton in body frame */
static VEC *i_omega = VNULL;
i_omega = v_resize(i_omega, AXIS_NB);
i_omega = mv_mlt(bfm.Inert, rate_body, i_omega);
static VEC *omega_i_omega = VNULL;
omega_i_omega = v_resize(omega_i_omega, AXIS_NB);
omega_i_omega = out_prod(rate_body, i_omega, omega_i_omega);
static VEC *m_tot = VNULL;
m_tot = v_resize(m_tot, AXIS_NB);
m_tot = v_sub(m_body, omega_i_omega, m_tot);
static VEC *I_inv_m_tot = VNULL;
I_inv_m_tot = v_resize(I_inv_m_tot, AXIS_NB);
I_inv_m_tot = mv_mlt(bfm.Inert_inv, m_tot, I_inv_m_tot);
Xdot->ve[BFMS_P] = I_inv_m_tot->ve[AXIS_P];
Xdot->ve[BFMS_Q] = I_inv_m_tot->ve[AXIS_Q];
Xdot->ve[BFMS_R] = I_inv_m_tot->ve[AXIS_R];
/* derivatives of motors rpm */
/* omega_dot = -1/THAU*omega - Kq*omega^2 + Kv/THAU * V */
Xdot->ve[BFMS_OM_B] = -1./THAU * X->ve[BFMS_OM_B] - Kq * omega_square->ve[SERVO_MOTOR_BACK] + Kv/THAU * u->ve[SERVO_MOTOR_BACK];
Xdot->ve[BFMS_OM_F] = -1./THAU * X->ve[BFMS_OM_F] - Kq * omega_square->ve[SERVO_MOTOR_FRONT] + Kv/THAU * u->ve[SERVO_MOTOR_FRONT];
Xdot->ve[BFMS_OM_R] = -1./THAU * X->ve[BFMS_OM_R] - Kq * omega_square->ve[SERVO_MOTOR_RIGHT] + Kv/THAU * u->ve[SERVO_MOTOR_RIGHT];
Xdot->ve[BFMS_OM_L] = -1./THAU * X->ve[BFMS_OM_L] - Kq * omega_square->ve[SERVO_MOTOR_LEFT] + Kv/THAU * u->ve[SERVO_MOTOR_LEFT];
}
#if 0
static void motor_model_run( double dt ) {
rk4(motor_model_derivative, bs.mot_omega, bs.mot_voltage, dt);
rk4(motor_model_derivative, bfm.mot_omega, bfm.mot_voltage, dt);
}
static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot) {
@@ -42,10 +233,10 @@ static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot) {
temp1 = v_resize(temp1,SERVOS_NB);
temp2 = v_resize(temp2,SERVOS_NB);
// omega_dot = -1/THAU*omega - Kq*omega*omega + Kv/THAU * v;
// omega_dot = -1/THAU*omega - Kq*omega^2 + Kv/THAU * V;
temp1 = sv_mlt(-1./THAU, x, temp1); /* temp1 = -1/THAU * x */
temp2 = v_star(x, x, temp2); /* temp2 = x^2 */
xdot = v_mltadd(temp1, temp2, -Kq, xdot); /* xdot = temp1 - Kq*temp2 */
xdot = v_mltadd(xdot, u, Kv/THAU, xdot); /* xdot = xdot + Kv/THAU * u */
}
#endif
sw/simulator/booz_flight_model.h
+40 -15
View File
@@ -3,29 +3,54 @@
#include <matrix.h>
struct BoozState {
/* position in earth frame */
VEC* position;
/* speed in earth frame */
VEC* speed;
/* euler angles in earth frame */
VEC* eulers;
/* euler angles derivatives in earth frame */
VEC* eulers_dot;
/* body rates */
VEC* body_rates;
/* battery volatge in V */
#define BFMS_X 0
#define BFMS_Y 1
#define BFMS_Z 2
#define BFMS_U 3
#define BFMS_V 4
#define BFMS_W 5
#define BFMS_PHI 6
#define BFMS_THETA 7
#define BFMS_PSI 8
#define BFMS_P 9
#define BFMS_Q 10
#define BFMS_R 11
#define BFMS_OM_B 12
#define BFMS_OM_F 13
#define BFMS_OM_R 14
#define BFMS_OM_L 15
#define BFMS_SIZE 16
struct BoozFlightModel {
/* battery voltage in V */
double bat_voltage;
/* motors supply voltage in V */
VEC* mot_voltage;
/* motors rotational speed in rad/s */
VEC* mot_omega;
/* state : see define above for fields */
VEC* state;
/* constants used in derivative computation */
/* gravitation in earth frame */
VEC* g_earth;
/* propeller thrust factor */
double thrust_factor;
/* propeller torque factor */
double torque_factor;
/* Matrix used to compute the moments produced by props */
MAT* props_moment_matrix;
/* inertia matrix */
MAT* Inert;
/* invert of inertia matrix */
MAT* Inert_inv;
};
extern struct BoozState bs;
extern struct BoozFlightModel bfm;
extern void booz_flight_model_init( void );
extern void booz_flight_model_run( double t, double* commands );
#endif /* BOOZ_FLIGHT_MODEL_H */
sw/simulator/booz_flight_model_params.h
+5 -5
View File
@@ -4,9 +4,9 @@
/* drag coefficient of the body */
#define C_d_body .2
/* thrust aerodynamic coefficient */
#define C_t 1.
#define C_t 0.132
/* moment aerodynamic coefficient */
#define C_q 0.1
#define C_q 0.01
/* propeller radius in m */
#define PROP_RADIUS 0.125
/* propeller area in m2 */
@@ -38,9 +38,9 @@
*/
#define BAT_VOLTAGE 11.
#define THAU 100.
#define Kq 0.01
#define Kv 10000.
#define THAU 25.
#define Kq 0.02
#define Kv 1500.
sw/simulator/booz_flight_model_utils.c
+39 -1
View File
@@ -1,5 +1,8 @@
#include "booz_flight_model_utils.h"
#include <math.h>
#include "6dof.h"
void rk4(ode_fun f, VEC* x, VEC* u, double dt) {
static VEC *v1=VNULL, *v2=VNULL, *v3=VNULL, *v4=VNULL;
@@ -32,6 +35,41 @@ void rk4(ode_fun f, VEC* x, VEC* u, double dt) {
v_add(temp,v4,temp); /* temp = v1+2*v2+2*v3+v4 */
/* adjust x */
v_mltadd(x,temp,dt/6.0,x); /* x = x+(h/6)*temp */
v_mltadd(x,temp,dt/6.0,x); /* x = x+(h/6) * temp */
}
MAT* dcm_of_eulers (VEC* eulers, MAT* dcm ) {
dcm = m_resize(dcm, 3,3);
double sinPHI = sin(eulers->ve[EULER_PHI]);
double cosPHI = cos(eulers->ve[EULER_PHI]);
double sinTHETA = sin(eulers->ve[EULER_THETA]);
double cosTHETA = cos(eulers->ve[EULER_THETA]);
double sinPSI = sin(eulers->ve[EULER_PSI]);
double cosPSI = cos(eulers->ve[EULER_PSI]);
dcm->me[0][0] = cosTHETA * cosPSI;
dcm->me[0][1] = cosTHETA * sinPSI;
dcm->me[0][2] = -sinTHETA;
dcm->me[1][0] = sinPHI * sinTHETA * cosPSI - cosPHI * sinPSI;
dcm->me[1][1] = sinPHI * sinTHETA * sinPSI + cosPHI * cosPSI;
dcm->me[1][2] = sinPHI * cosTHETA;
dcm->me[2][0] = cosPHI * sinTHETA * cosPSI + sinPHI * sinPSI;
dcm->me[2][1] = cosPHI * sinTHETA * sinPSI - sinPHI * cosPSI;
dcm->me[2][2] = cosPHI * cosTHETA;
return dcm;
}
VEC* out_prod( VEC* a, VEC* b, VEC* out) {
if ( a->dim != 3 || b->dim != 3 )
error(E_SIZES,"out_prod");
if ( out==(VEC *)NULL || out->dim != a->dim )
out = v_resize(out,a->dim);
out->ve[0] = a->ve[1]*b->ve[2] - a->ve[2]*b->ve[1];
out->ve[1] = a->ve[2]*b->ve[0] - a->ve[0]*b->ve[2];
out->ve[2] = a->ve[0]*b->ve[1] - a->ve[1]*b->ve[0];
return out;
}
sw/simulator/booz_flight_model_utils.h
+7
View File
@@ -7,4 +7,11 @@ typedef void (*ode_fun)(VEC* x, VEC* u, VEC* xdot);
extern void rk4(ode_fun f, VEC* x, VEC* u, double dt);
extern MAT* dcm_of_eulers (VEC* eulers, MAT* dcm );
extern VEC* out_prod( VEC* a, VEC* b, VEC* out);
#endif /* BOOZ_FM_UTILS_H */
sw/simulator/booz_flightgear.c
+84
View File
@@ -0,0 +1,84 @@
#include "booz_flightgear.h"
#include "flight_gear.h"
#include <sys/socket.h>
#include <netdb.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
static int fg_socket;
static struct sockaddr_in fg_addr;
void net_gui_init (struct FGNetGUI* gui);
void booz_flightgear_init(const char* host, unsigned int port) {
int so_reuseaddr = 1;
struct protoent * pte = getprotobyname("UDP");
fg_socket = socket( PF_INET, SOCK_DGRAM, pte->p_proto);
setsockopt(fg_socket, SOL_SOCKET, SO_REUSEADDR,
&so_reuseaddr, sizeof(so_reuseaddr));
fg_addr.sin_family = PF_INET;
fg_addr.sin_port = htons(port);
fg_addr.sin_addr.s_addr = inet_addr(host);
}
void booz_flightgear_send() {
const double earth_radius = 6372795.;
double lat = 0.656480 + asin(bfm.state->ve[BFMS_X]/earth_radius);
double lon = -2.135537 + asin(bfm.state->ve[BFMS_Y]/earth_radius);
struct FGNetGUI gui;
net_gui_init(&gui);
gui.latitude = lat;
gui.longitude = lon;
gui.altitude = 10 - bfm.state->ve[BFMS_Z];
gui.phi = bfm.state->ve[BFMS_PHI];
gui.theta = bfm.state->ve[BFMS_THETA];
gui.psi = bfm.state->ve[BFMS_PSI];
if (sendto(fg_socket, (char*)(&gui), sizeof(gui), 0,
(struct sockaddr*)&fg_addr, sizeof(fg_addr)) == -1)
printf("error sending\n");
}
void net_gui_init (struct FGNetGUI* gui) {
gui->version = FG_NET_GUI_VERSION;
gui->latitude = 0.656480;
gui->longitude = -2.135537;
gui->altitude = 0.807609;
gui->agl = 1.111652;
gui->phi = 0.;
gui->theta = 0.;
gui->psi = 5.20;
gui->vcas = 0.;
gui->climb_rate = 0.;
gui->num_tanks = 1;
gui->fuel_quantity[0] = 0.;
gui->cur_time = 3198060679ul;
gui->warp = 1122474394ul;
gui->ground_elev = 0.;
gui->tuned_freq = 125.65;
gui->nav_radial = 90.;
gui->in_range = 1;
gui->dist_nm = 10.;
gui->course_deviation_deg = 0.;
gui->gs_deviation_deg = 0.;
}
sw/simulator/booz_flightgear.h
+9
View File
@@ -0,0 +1,9 @@
#ifndef BOOZ_FLIGHTGEAR_H
#define BOOZ_FLIGHTGEAR_H
#include "booz_flight_model.h"
extern void booz_flightgear_init(const char* host, unsigned int port);
extern void booz_flightgear_send();
#endif /* BOOZ_FLIGHTGEAR_H */
sw/simulator/main_booz_sim.c
+28 -4
View File
@@ -1,30 +1,54 @@
#include <glib.h>
#include "booz_flight_model.h"
#include "booz_flightgear.h"
/* 250Hz <-> 4ms */
#define TIMEOUT_PERIOD 4
#define DT (TIMEOUT_PERIOD*1e-3)
double sim_time;
double commands[] = {0.7, 0.7, 0.7, 0.7};
#define DT_DISPLAY 0.04
double next_display;
gboolean timeout_callback(gpointer data) {
double commands[] = {0.651, 0.649, 0.65, 0.65};
static void output_task( void );
static gboolean timeout_callback(gpointer data);
static gboolean timeout_callback(gpointer data) {
booz_flight_model_run(DT, commands);
sim_time += DT;
printf("%f %f %f %f %f\n", sim_time, bs.mot_omega->ve[0], bs.mot_omega->ve[1],
bs.mot_omega->ve[2], bs.mot_omega->ve[3]);
if (sim_time >= next_display) {
next_display+= DT_DISPLAY;
// output_task();
booz_flightgear_send();
}
return TRUE;
}
static void output_task( void ) {
printf("%f %f %f %f %f %f %f\n", sim_time,
bfm.state->ve[BFMS_X], bfm.state->ve[BFMS_Y], bfm.state->ve[BFMS_Z],
bfm.state->ve[BFMS_PHI], bfm.state->ve[BFMS_THETA], bfm.state->ve[BFMS_PSI]);
}
int main ( int argc, char** argv) {
sim_time = 0.;
next_display = 0.;
booz_flight_model_init();
booz_flightgear_init("10.31.4.107", 5501);
GMainLoop *ml = g_main_loop_new(NULL, FALSE);