mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 05:42:49 +08:00
[simulator] cleanup: remove old_booz sim
This commit is contained in:
Felix Ruess
@@ -1,449 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2008 Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <glib.h>
|
||||
#include <getopt.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
#include "booz_flight_model.h"
|
||||
#include "booz_sensors_model.h"
|
||||
#include "booz_wind_model.h"
|
||||
#include "booz_rc_sim.h"
|
||||
#include "firmwares/rotorcraft/battery.h"
|
||||
|
||||
#include "main.h"
|
||||
|
||||
|
||||
char* fg_host = "10.31.4.107";
|
||||
unsigned int fg_port = 5501;
|
||||
char* joystick_dev = "/dev/input/js0";
|
||||
|
||||
/* rate of the host mainloop */
|
||||
#define HOST_TIMEOUT_PERIOD 4
|
||||
struct timeval host_time_start;
|
||||
double host_time_elapsed;
|
||||
double host_time_factor = 1.;
|
||||
|
||||
/* 250Hz <-> 4ms */
|
||||
#define SIM_DT (1./512.)
|
||||
double sim_time;
|
||||
|
||||
#define DT_DISPLAY 0.04
|
||||
double disp_time;
|
||||
|
||||
double booz_sim_actuators_values[] = {0., 0., 0., 0.};
|
||||
|
||||
static void sim_run_one_step(void);
|
||||
#ifdef BYPASS_AHRS
|
||||
static void sim_overwrite_ahrs(void);
|
||||
#endif
|
||||
#ifdef BYPASS_INS
|
||||
static void sim_overwrite_ins(void);
|
||||
#endif
|
||||
static void sim_gps_feed_data(void);
|
||||
static void sim_mag_feed_data(void);
|
||||
|
||||
static void booz2_sim_parse_options(int argc, char** argv);
|
||||
static void booz2_sim_init(void);
|
||||
static gboolean booz2_sim_periodic(gpointer data);
|
||||
static void booz2_sim_display(void);
|
||||
static void booz_sim_read_actuators(void);
|
||||
|
||||
static void ivy_transport_init(void);
|
||||
static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
|
||||
void *user_data __attribute__ ((unused)),
|
||||
int argc __attribute__ ((unused)), char *argv[]);
|
||||
static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
|
||||
void *user_data __attribute__ ((unused)),
|
||||
int argc __attribute__ ((unused)), char *argv[]);
|
||||
static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
|
||||
void *user_data __attribute__ ((unused)),
|
||||
int argc __attribute__ ((unused)), char *argv[]);
|
||||
|
||||
|
||||
static void booz2_sim_init(void) {
|
||||
|
||||
gettimeofday (&host_time_start, NULL);
|
||||
sim_time = 0.;
|
||||
disp_time = 0.;
|
||||
|
||||
booz_flight_model_init();
|
||||
|
||||
booz_sensors_model_init(sim_time);
|
||||
|
||||
booz_wind_model_init();
|
||||
|
||||
ivy_transport_init();
|
||||
|
||||
main_init();
|
||||
|
||||
}
|
||||
|
||||
#include "booz2_analog_baro.h"
|
||||
#include "imu.h"
|
||||
|
||||
static gboolean booz2_sim_periodic(gpointer data __attribute__ ((unused))) {
|
||||
|
||||
struct timeval host_time_now;
|
||||
gettimeofday (&host_time_now, NULL);
|
||||
host_time_elapsed = host_time_factor *
|
||||
((host_time_now.tv_sec - host_time_start.tv_sec) +
|
||||
(host_time_now.tv_usec - host_time_start.tv_usec)*1e-6);
|
||||
|
||||
while (sim_time <= host_time_elapsed) {
|
||||
sim_run_one_step();
|
||||
sim_time += SIM_DT;
|
||||
}
|
||||
|
||||
|
||||
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
|
||||
#include "subsystems/ahrs.h"
|
||||
|
||||
static void sim_run_one_step(void) {
|
||||
|
||||
/* read actuators positions */
|
||||
booz_sim_read_actuators();
|
||||
|
||||
/* run our models */
|
||||
if (sim_time > 13.)
|
||||
bfm.on_ground = FALSE;
|
||||
|
||||
booz_wind_model_run(SIM_DT);
|
||||
|
||||
booz_flight_model_run(SIM_DT, booz_sim_actuators_values);
|
||||
|
||||
booz_sensors_model_run(sim_time);
|
||||
battery_voltage = bfm.bat_voltage * 10;
|
||||
|
||||
/* outputs models state */
|
||||
booz2_sim_display();
|
||||
|
||||
/* run the airborne code */
|
||||
|
||||
// feed a rc frame and signal event
|
||||
BoozRcSimFeed(sim_time);
|
||||
// process it
|
||||
main_event();
|
||||
|
||||
if (booz_sensors_model_baro_available()) {
|
||||
Booz2BaroISRHandler(bsm.baro);
|
||||
main_event();
|
||||
#ifdef BYPASS_INS
|
||||
sim_overwrite_ins();
|
||||
#endif /* BYPASS_INS */
|
||||
}
|
||||
if (booz_sensors_model_gyro_available()) {
|
||||
imu_feed_data();
|
||||
main_event();
|
||||
#ifdef BYPASS_AHRS
|
||||
sim_overwrite_ahrs();
|
||||
#endif /* BYPASS_AHRS */
|
||||
#ifdef BYPASS_INS
|
||||
sim_overwrite_ins();
|
||||
#endif /* BYPASS_INS */
|
||||
}
|
||||
|
||||
if (booz_sensors_model_gps_available()) {
|
||||
sim_gps_feed_data();
|
||||
main_event();
|
||||
}
|
||||
|
||||
if (booz_sensors_model_mag_available()) {
|
||||
sim_mag_feed_data();
|
||||
main_event();
|
||||
#ifdef BYPASS_AHRS
|
||||
sim_overwrite_ahrs();
|
||||
#endif /* BYPASS_AHRS */
|
||||
}
|
||||
|
||||
main_periodic();
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef BYPASS_AHRS
|
||||
#include "booz_geometry_mixed.h"
|
||||
#include "subsystems/ahrs.h"
|
||||
static void sim_overwrite_ahrs(void) {
|
||||
ahrs.ltp_to_body_euler.phi = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_X]);
|
||||
ahrs.ltp_to_body_euler.theta = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Y]);
|
||||
ahrs.ltp_to_body_euler.psi = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Z]);
|
||||
|
||||
ahrs.ltp_to_body_quat.qi = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QI], IQUAT_RES);
|
||||
ahrs.ltp_to_body_quat.qx = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QX], IQUAT_RES);
|
||||
ahrs.ltp_to_body_quat.qy = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QY], IQUAT_RES);
|
||||
ahrs.ltp_to_body_quat.qz = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QZ], IQUAT_RES);
|
||||
|
||||
ahrs.body_rate.p = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_X]);
|
||||
ahrs.body_rate.q = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Y]);
|
||||
ahrs.body_rate.r = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Z]);
|
||||
|
||||
}
|
||||
#endif /* BYPASS_AHRS */
|
||||
|
||||
|
||||
#ifdef BYPASS_INS
|
||||
#include "subsystems/ins.h"
|
||||
static void sim_overwrite_ins(void) {
|
||||
ins_position.z = BOOZ_POS_I_OF_F(bfm.pos_ltp->ve[AXIS_Z]);
|
||||
ins_speed_earth.z = BOOZ_SPEED_I_OF_F(bfm.speed_ltp->ve[AXIS_Z]);
|
||||
ins_accel_earth.z = BOOZ_ACCEL_I_OF_F(bfm.accel_ltp->ve[AXIS_Z]);
|
||||
}
|
||||
#endif /* BYPASS_INS */
|
||||
|
||||
|
||||
|
||||
|
||||
#include "booz2_gps.h"
|
||||
static void sim_gps_feed_data(void) {
|
||||
// booz2_gps_lat = bsm.gps_pos_lla.lat;
|
||||
// booz2_gps_lon = bsm.gps_pos_lla.lon;
|
||||
// speed?
|
||||
// booz2_gps_vel_n = rint(bsm.gps_speed->ve[AXIS_X] * 100.);
|
||||
// booz2_gps_vel_e = rint(bsm.gps_speed->ve[AXIS_Y] * 100.);
|
||||
|
||||
|
||||
booz_gps_state.ecef_pos.x = rint(bsm.gps_pos_ecef.x);
|
||||
booz_gps_state.ecef_pos.y = rint(bsm.gps_pos_ecef.y);
|
||||
booz_gps_state.ecef_pos.z = rint(bsm.gps_pos_ecef.z);
|
||||
// VECT3_COPY(booz_gps_state.ecef_pos, bsm.gps_pos_ecef);
|
||||
VECT3_COPY(booz_gps_state.ecef_speed, bsm.gps_speed_ecef);
|
||||
booz_gps_state.fix = BOOZ2_GPS_FIX_3D;
|
||||
|
||||
|
||||
}
|
||||
|
||||
#include "AMI601.h"
|
||||
static void sim_mag_feed_data(void) {
|
||||
ami601_val[IMU_MAG_X_CHAN] = bsm.mag->ve[AXIS_X];
|
||||
ami601_val[IMU_MAG_Y_CHAN] = bsm.mag->ve[AXIS_Y];
|
||||
ami601_val[IMU_MAG_Z_CHAN] = bsm.mag->ve[AXIS_Z];
|
||||
ami601_status = AMI601_DATA_AVAILABLE;
|
||||
}
|
||||
|
||||
|
||||
|
||||
#define RPM_OF_RAD_S(a) ((a)*60./M_PI)
|
||||
static void booz2_sim_display(void) {
|
||||
if (sim_time >= disp_time) {
|
||||
disp_time+= DT_DISPLAY;
|
||||
// booz_flightgear_send();
|
||||
IvySendMsg("%d NPS_RPMS %f %f %f %f",
|
||||
AC_ID,
|
||||
RPM_OF_RAD_S(bfm.omega->ve[SERVO_FRONT]),
|
||||
RPM_OF_RAD_S(bfm.omega->ve[SERVO_BACK]),
|
||||
RPM_OF_RAD_S(bfm.omega->ve[SERVO_RIGHT]),
|
||||
RPM_OF_RAD_S(bfm.omega->ve[SERVO_LEFT]) );
|
||||
IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f",
|
||||
AC_ID,
|
||||
DegOfRad(bfm.ang_rate_body->ve[AXIS_X]),
|
||||
DegOfRad(bfm.ang_rate_body->ve[AXIS_Y]),
|
||||
DegOfRad(bfm.ang_rate_body->ve[AXIS_Z]),
|
||||
DegOfRad(bfm.eulers->ve[AXIS_X]),
|
||||
DegOfRad(bfm.eulers->ve[AXIS_Y]),
|
||||
DegOfRad(bfm.eulers->ve[AXIS_Z]));
|
||||
IvySendMsg("%d NPS_SPEED_POS %f %f %f %f %f %f",
|
||||
AC_ID,
|
||||
(bfm.speed_ltp->ve[AXIS_X]),
|
||||
(bfm.speed_ltp->ve[AXIS_Y]),
|
||||
(bfm.speed_ltp->ve[AXIS_Z]),
|
||||
(bfm.pos_ltp->ve[AXIS_X]),
|
||||
(bfm.pos_ltp->ve[AXIS_Y]),
|
||||
(bfm.pos_ltp->ve[AXIS_Z]));
|
||||
#if 0
|
||||
IvySendMsg("%d NPS_WIND %f %f %f",
|
||||
AC_ID,
|
||||
bwm.velocity->ve[AXIS_X],
|
||||
bwm.velocity->ve[AXIS_Y],
|
||||
bwm.velocity->ve[AXIS_Z]);
|
||||
#endif
|
||||
IvySendMsg("%d NPS_ACCEL_LTP %f %f %f",
|
||||
AC_ID,
|
||||
bfm.accel_ltp->ve[AXIS_X],
|
||||
bfm.accel_ltp->ve[AXIS_Y],
|
||||
bfm.accel_ltp->ve[AXIS_Z]);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
int main ( int argc, char** argv) {
|
||||
|
||||
booz2_sim_parse_options(argc, argv);
|
||||
|
||||
booz2_sim_init();
|
||||
|
||||
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
|
||||
|
||||
g_timeout_add(HOST_TIMEOUT_PERIOD, booz2_sim_periodic, NULL);
|
||||
|
||||
g_main_loop_run(ml);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static void ivy_transport_init(void) {
|
||||
IvyInit ("BoozSim", "BoozSim READY", NULL, NULL, NULL, NULL);
|
||||
IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)");
|
||||
IvyBindMsg(on_DL_BLOCK, NULL, "^(\\S*) BLOCK (\\S*) (\\S*)");
|
||||
IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
|
||||
IvyStart("127.255.255.255");
|
||||
}
|
||||
|
||||
#include "std.h"
|
||||
#include "generated/settings.h"
|
||||
#include "dl_protocol.h"
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
|
||||
void *user_data __attribute__ ((unused)),
|
||||
int argc __attribute__ ((unused)), char *argv[]){
|
||||
uint8_t index = atoi(argv[2]);
|
||||
float value = atof(argv[3]);
|
||||
DlSetting(index, value);
|
||||
DOWNLINK_SEND_DL_VALUE(&index, &value);
|
||||
// printf("setting %d %f\n", index, value);
|
||||
}
|
||||
|
||||
static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
|
||||
void *user_data __attribute__ ((unused)),
|
||||
int argc __attribute__ ((unused)), char *argv[]){
|
||||
int block = atoi(argv[1]);
|
||||
nav_goto_block(block);
|
||||
}
|
||||
|
||||
#include "pprz_geodetic_int.h"
|
||||
#include "stdio.h"
|
||||
static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
|
||||
void *user_data __attribute__ ((unused)),
|
||||
int argc __attribute__ ((unused)), char *argv[]){
|
||||
int wp_id = atoi(argv[1]);
|
||||
//int ac_id = atoi(argv[1]);
|
||||
struct LlaCoor_i lla;
|
||||
struct EnuCoor_i enu;
|
||||
//printf("move deg %d %d %d\n",atoi(argv[3]),atoi(argv[4]),atoi(argv[5]));
|
||||
int lat = atoi(argv[3]);
|
||||
int lon = atoi(argv[4]);
|
||||
int alt = atoi(argv[5]);
|
||||
lla.lat = INT32_RAD_OF_DEG(lat);
|
||||
lla.lon = INT32_RAD_OF_DEG(lon);
|
||||
lla.alt = alt+ins_ltp_def.lla.alt;
|
||||
//printf("move rad %d %d %d (%d)\n",lla.lat,lla.lon,lla.alt,ins_ltp_def.lla.alt);
|
||||
enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
|
||||
enu.x = POS_BFP_OF_REAL(enu.x)/100;
|
||||
enu.y = POS_BFP_OF_REAL(enu.y)/100;
|
||||
enu.z = POS_BFP_OF_REAL(enu.z)/100;
|
||||
//printf("enu_of_lla %d %d %d -> %d %d %d (%f %f %f)\n",waypoints[wp_id].x,waypoints[wp_id].y,waypoints[wp_id].z,enu.x,enu.y,enu.z,
|
||||
// POS_FLOAT_OF_BFP(enu.x),POS_FLOAT_OF_BFP(enu.y),POS_FLOAT_OF_BFP(enu.z));
|
||||
VECT3_ASSIGN(waypoints[wp_id],enu.x,enu.y,enu.z);
|
||||
DOWNLINK_SEND_WP_MOVED_LTP(&wp_id, &enu.x, &enu.y, &enu.z);
|
||||
//DOWNLINK_SEND_WP_MOVED_LLA(&wp_id, &lat, &lon, &alt);
|
||||
//printf("WP_MOVED\n");
|
||||
//fflush(stdout);
|
||||
}
|
||||
|
||||
#include "actuators.h"
|
||||
static void booz_sim_read_actuators(void) {
|
||||
|
||||
|
||||
// printf("actatuors %d %d %d %d\n",
|
||||
// Actuator(SERVO_FRONT), Actuator(SERVO_BACK), Actuator(SERVO_RIGHT), Actuator(SERVO_LEFT));
|
||||
int32_t ut_front = Actuator(SERVO_FRONT) - TRIM_FRONT;
|
||||
int32_t ut_back = Actuator(SERVO_BACK) - TRIM_BACK;
|
||||
int32_t ut_right = Actuator(SERVO_RIGHT) - TRIM_RIGHT;
|
||||
int32_t ut_left = Actuator(SERVO_LEFT) - TRIM_LEFT;
|
||||
#if 1
|
||||
booz_sim_actuators_values[0] = (double)ut_front / SUPERVISION_MAX_MOTOR;
|
||||
booz_sim_actuators_values[1] = (double)ut_back / SUPERVISION_MAX_MOTOR;
|
||||
booz_sim_actuators_values[2] = (double)ut_right / SUPERVISION_MAX_MOTOR;
|
||||
booz_sim_actuators_values[3] = (double)ut_left / SUPERVISION_MAX_MOTOR;
|
||||
#else
|
||||
// double foo = 0.33;
|
||||
double foo = 0.35;
|
||||
booz_sim_actuators_values[0] = foo;
|
||||
booz_sim_actuators_values[1] = foo;
|
||||
booz_sim_actuators_values[2] = foo;
|
||||
booz_sim_actuators_values[3] = foo;
|
||||
#endif
|
||||
|
||||
|
||||
// printf("%f %f %f %f\n", booz_sim_actuators_values[0], booz_sim_actuators_values[1], booz_sim_actuators_values[2], booz_sim_actuators_values[3]);
|
||||
|
||||
}
|
||||
|
||||
static void booz2_sim_parse_options(int argc, char** argv) {
|
||||
|
||||
static const char* usage =
|
||||
"Usage: %s [options]\n"
|
||||
" Options :\n"
|
||||
" -j --js_dev joystick device\n"
|
||||
" --fg_host flight gear host\n"
|
||||
" --fg_port flight gear port\n";
|
||||
|
||||
|
||||
while (1) {
|
||||
|
||||
static struct option long_options[] = {
|
||||
{"fg_host", 1, NULL, 0},
|
||||
{"fg_port", 1, NULL, 0},
|
||||
{"js_dev", 1, NULL, 0},
|
||||
{0, 0, 0, 0}
|
||||
};
|
||||
int option_index = 0;
|
||||
int c = getopt_long(argc, argv, "j:",
|
||||
long_options, &option_index);
|
||||
if (c == -1)
|
||||
break;
|
||||
|
||||
switch (c) {
|
||||
case 0:
|
||||
switch (option_index) {
|
||||
case 0:
|
||||
fg_host = strdup(optarg); break;
|
||||
case 1:
|
||||
fg_port = atoi(optarg); break;
|
||||
case 2:
|
||||
joystick_dev = strdup(optarg); break;
|
||||
}
|
||||
break;
|
||||
|
||||
case 'j':
|
||||
joystick_dev = strdup(optarg);
|
||||
break;
|
||||
|
||||
default: /* $B!G(B?$B!G(B */
|
||||
printf("?? getopt returned character code 0%o ??\n", c);
|
||||
fprintf(stderr, usage, argv[0]);
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,362 +0,0 @@
|
||||
#include "booz_flight_model.h"
|
||||
|
||||
#define BFMS_X 0
|
||||
#define BFMS_Y 1
|
||||
#define BFMS_Z 2
|
||||
#define BFMS_XD 3
|
||||
#define BFMS_YD 4
|
||||
#define BFMS_ZD 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
|
||||
|
||||
#define BoozFlighModelGetPos(_dest) { \
|
||||
_dest->ve[AXIS_X] = bfm.state->ve[BFMS_X]; \
|
||||
_dest->ve[AXIS_Y] = bfm.state->ve[BFMS_Y]; \
|
||||
_dest->ve[AXIS_Z] = bfm.state->ve[BFMS_Z]; \
|
||||
}
|
||||
|
||||
#define BoozFlighModelGetSpeedLtp(_dest) { \
|
||||
_dest->ve[AXIS_X] = bfm.state->ve[BFMS_XD]; \
|
||||
_dest->ve[AXIS_Y] = bfm.state->ve[BFMS_YD]; \
|
||||
_dest->ve[AXIS_Z] = bfm.state->ve[BFMS_ZD]; \
|
||||
}
|
||||
|
||||
#define BoozFlighModelGetAngles(_dest) { \
|
||||
_dest->ve[EULER_PHI] = bfm.state->ve[BFMS_PHI]; \
|
||||
_dest->ve[EULER_THETA] = bfm.state->ve[BFMS_THETA]; \
|
||||
_dest->ve[EULER_PSI] = bfm.state->ve[BFMS_PSI]; \
|
||||
}
|
||||
|
||||
#define BoozFlighModelGetRate(_dest) { \
|
||||
_dest->ve[AXIS_P] = bfm.state->ve[BFMS_P]; \
|
||||
_dest->ve[AXIS_Q] = bfm.state->ve[BFMS_Q]; \
|
||||
_dest->ve[AXIS_R] = bfm.state->ve[BFMS_R]; \
|
||||
}
|
||||
|
||||
#define BoozFlighModelGetRPMS(_dest) { \
|
||||
_dest->ve[SERVO_BACK] = bfm.state->ve[BFMS_OM_B]; \
|
||||
_dest->ve[SERVO_FRONT] = bfm.state->ve[BFMS_OM_F]; \
|
||||
_dest->ve[SERVO_RIGHT] = bfm.state->ve[BFMS_OM_R]; \
|
||||
_dest->ve[SERVO_LEFT] = bfm.state->ve[BFMS_OM_L]; \
|
||||
}
|
||||
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#include "booz_flight_model_params.h"
|
||||
#include "booz_flight_model_utils.h"
|
||||
#include "booz_wind_model.h"
|
||||
|
||||
#include "6dof.h"
|
||||
|
||||
|
||||
struct BoozFlightModel bfm;
|
||||
|
||||
//static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot);
|
||||
|
||||
static void booz_flight_model_update_byproducts(void);
|
||||
static VEC* booz_get_forces_ltp(VEC* F , VEC* speed_ltp, MAT* dcm_t, VEC* omega_square);
|
||||
static VEC* booz_get_moments_body_frame(VEC* M, VEC* omega_square );
|
||||
static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot);
|
||||
|
||||
void booz_flight_model_init( void ) {
|
||||
bfm.on_ground = TRUE;
|
||||
|
||||
bfm.time = 0.;
|
||||
bfm.bat_voltage = BAT_VOLTAGE;
|
||||
bfm.mot_voltage = v_get(SERVOS_NB);
|
||||
|
||||
bfm.state = v_get(BFMS_SIZE);
|
||||
v_zero(bfm.state);
|
||||
|
||||
bfm.pos_ltp = v_get(AXIS_NB);
|
||||
bfm.speed_ltp = v_get(AXIS_NB);
|
||||
bfm.accel_ltp = v_get(AXIS_NB);
|
||||
|
||||
bfm.speed_body = v_get(AXIS_NB);
|
||||
bfm.accel_body = v_get(AXIS_NB);
|
||||
|
||||
bfm.eulers = v_get(AXIS_NB);
|
||||
bfm.ang_rate_body = v_get(AXIS_NB);
|
||||
bfm.ang_accel_body = v_get(AXIS_NB);
|
||||
|
||||
bfm.dcm = m_get(AXIS_NB, AXIS_NB);
|
||||
bfm.dcm_t = m_get(AXIS_NB, AXIS_NB);
|
||||
bfm.quat = v_get(AXIS_NB);
|
||||
|
||||
bfm.omega = v_get(SERVOS_NB);
|
||||
bfm.omega_square = v_get(SERVOS_NB);
|
||||
|
||||
|
||||
/* constants */
|
||||
bfm.g_ltp = v_get(AXIS_NB);
|
||||
bfm.g_ltp->ve[AXIS_X] = 0.;
|
||||
bfm.g_ltp->ve[AXIS_Y] = 0.;
|
||||
bfm.g_ltp->ve[AXIS_Z] = G;
|
||||
|
||||
/* FIXME */
|
||||
bfm.h_ltp = v_get(AXIS_NB);
|
||||
bfm.h_ltp->ve[AXIS_X] = 1.;
|
||||
bfm.h_ltp->ve[AXIS_Y] = 0.;
|
||||
bfm.h_ltp->ve[AXIS_Z] = 1.;
|
||||
|
||||
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_LEFT] = L * bfm.thrust_factor;
|
||||
bfm.props_moment_matrix->me[AXIS_X][SERVO_RIGHT] = -L * bfm.thrust_factor;
|
||||
bfm.props_moment_matrix->me[AXIS_Y][SERVO_BACK] = -L * bfm.thrust_factor;
|
||||
bfm.props_moment_matrix->me[AXIS_Y][SERVO_FRONT] = L * bfm.thrust_factor;
|
||||
bfm.props_moment_matrix->me[AXIS_Z][SERVO_LEFT] = bfm.torque_factor;
|
||||
bfm.props_moment_matrix->me[AXIS_Z][SERVO_RIGHT] = bfm.torque_factor;
|
||||
bfm.props_moment_matrix->me[AXIS_Z][SERVO_BACK] = -bfm.torque_factor;
|
||||
bfm.props_moment_matrix->me[AXIS_Z][SERVO_FRONT] = -bfm.torque_factor;
|
||||
|
||||
bfm.mass = MASS;
|
||||
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
|
||||
#define WRAP(x,a) { while (x > a) x -= 2 * a; while (x <= -a) x += 2 * a;}
|
||||
|
||||
void booz_flight_model_run( double dt, double* commands ) {
|
||||
|
||||
|
||||
int i;
|
||||
for (i=0; i<SERVOS_NB; i++)
|
||||
bfm.mot_voltage->ve[i] = bfm.bat_voltage * commands[i];
|
||||
// rk4(motor_model_derivative, bfm.mot_omega, bfm.mot_voltage, dt);
|
||||
rk4(booz_flight_model_get_derivatives, bfm.state, bfm.mot_voltage, dt);
|
||||
/* wrap euler angles */
|
||||
WRAP( bfm.state->ve[BFMS_PHI], M_PI);
|
||||
WRAP( bfm.state->ve[BFMS_THETA], M_PI_2);
|
||||
WRAP( bfm.state->ve[BFMS_PSI], M_PI);
|
||||
booz_flight_model_update_byproducts();
|
||||
bfm.time += dt;
|
||||
}
|
||||
|
||||
|
||||
static void booz_flight_model_update_byproducts(void) {
|
||||
|
||||
/* extract eulers angles from state */
|
||||
BoozFlighModelGetAngles( bfm.eulers);
|
||||
/* extract body rotational rates from state */
|
||||
BoozFlighModelGetRate( bfm.ang_rate_body);
|
||||
|
||||
/* direct cosine matrix ( inertial to body )*/
|
||||
dcm_of_eulers(bfm.eulers, bfm.dcm);
|
||||
/* transpose of dcm ( body to inertial ) */
|
||||
m_transp(bfm.dcm, bfm.dcm_t);
|
||||
/* quaternion */
|
||||
quat_of_eulers(bfm.quat, bfm.eulers);
|
||||
|
||||
BoozFlighModelGetPos(bfm.pos_ltp);
|
||||
/* extract speed in ltp frame from state */
|
||||
BoozFlighModelGetSpeedLtp(bfm.speed_ltp);
|
||||
/* extract prop rotational speeds from state */
|
||||
BoozFlighModelGetRPMS(bfm.omega);
|
||||
/* compute square */
|
||||
v_star(bfm.omega, bfm.omega, bfm.omega_square);
|
||||
/* compute ltp accelerations */
|
||||
static VEC *f_ltp = VNULL;
|
||||
f_ltp = v_resize(f_ltp, AXIS_NB);
|
||||
f_ltp = booz_get_forces_ltp(f_ltp , bfm.speed_ltp, bfm.dcm_t, bfm.omega_square);
|
||||
sv_mlt( 1./bfm.mass, f_ltp, bfm.accel_ltp);
|
||||
/* rotate speed and accel to body frame */
|
||||
mv_mlt(bfm.dcm, bfm.speed_ltp, bfm.speed_body);
|
||||
mv_mlt(bfm.dcm, bfm.accel_ltp, bfm.accel_body);
|
||||
|
||||
|
||||
/* rotational accelerations */
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
compute the sum of external forces.
|
||||
assumes that dcm and omega_square are already precomputed from X
|
||||
*/
|
||||
static VEC* booz_get_forces_ltp(VEC* F , VEC* speed_ltp, MAT* dcm_t, VEC* omega_square) {
|
||||
|
||||
// FIXME : nimporte koi !
|
||||
F = v_zero(F);
|
||||
if (!bfm.on_ground) {
|
||||
|
||||
// propeller thrust
|
||||
static VEC *prop_thrust_body = VNULL;
|
||||
prop_thrust_body = v_resize(prop_thrust_body, AXIS_NB);
|
||||
prop_thrust_body->ve[AXIS_X] = 0;
|
||||
prop_thrust_body->ve[AXIS_Y] = 0;
|
||||
prop_thrust_body->ve[AXIS_Z] = -v_sum(omega_square) * bfm.thrust_factor;
|
||||
static VEC *prop_thrust_ltp = VNULL;
|
||||
prop_thrust_ltp = v_resize(prop_thrust_ltp, AXIS_NB);
|
||||
prop_thrust_ltp = mv_mlt(dcm_t, prop_thrust_body, prop_thrust_ltp);
|
||||
F = v_add(F, prop_thrust_ltp, F);
|
||||
|
||||
// gravity
|
||||
F = v_mltadd(F, bfm.g_ltp, bfm.mass, F);
|
||||
|
||||
// drag
|
||||
static VEC *airspeed_ltp = VNULL;
|
||||
airspeed_ltp = v_resize(airspeed_ltp, AXIS_NB);
|
||||
airspeed_ltp = v_sub(speed_ltp, bwm.velocity, airspeed_ltp);
|
||||
double norm_speed = v_norm2(airspeed_ltp);
|
||||
F = v_mltadd(F, airspeed_ltp, -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* M, VEC* omega_square ) {
|
||||
if (bfm.on_ground) {
|
||||
M = v_zero(M);
|
||||
}
|
||||
else {
|
||||
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);
|
||||
BoozFlighModelGetRPMS(omega_square);
|
||||
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);
|
||||
BoozFlighModelGetAngles(eulers);
|
||||
/* 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);
|
||||
/* extract ltp_speeds_from state */
|
||||
static VEC *speed_ltp = VNULL;
|
||||
speed_ltp = v_resize(speed_ltp, AXIS_NB);
|
||||
BoozFlighModelGetSpeedLtp(speed_ltp);
|
||||
/* extracts body rates from state */
|
||||
static VEC *rate_body = VNULL;
|
||||
rate_body = v_resize(rate_body, AXIS_NB);
|
||||
BoozFlighModelGetRate(rate_body);
|
||||
|
||||
/* derivatives of position */
|
||||
Xdot->ve[BFMS_X] = speed_ltp->ve[AXIS_X];
|
||||
Xdot->ve[BFMS_Y] = speed_ltp->ve[AXIS_Y];
|
||||
Xdot->ve[BFMS_Z] = speed_ltp->ve[AXIS_Z];
|
||||
|
||||
/* derivatives of speed */
|
||||
static VEC *f_ltp = VNULL;
|
||||
f_ltp = v_resize(f_ltp, AXIS_NB);
|
||||
f_ltp = booz_get_forces_ltp(f_ltp , speed_ltp, dcm_t, omega_square);
|
||||
Xdot->ve[BFMS_XD] = 1./bfm.mass * f_ltp->ve[AXIS_X];
|
||||
Xdot->ve[BFMS_YD] = 1./bfm.mass * f_ltp->ve[AXIS_Y];
|
||||
Xdot->ve[BFMS_ZD] = 1./bfm.mass * f_ltp->ve[AXIS_Z];
|
||||
|
||||
/* derivatives of eulers */
|
||||
double sinPHI = sin(eulers->ve[EULER_PHI]);
|
||||
double cosPHI = cos(eulers->ve[EULER_PHI]);
|
||||
double cosTHETA = cos(eulers->ve[EULER_THETA]);
|
||||
double tanTHETA = tan(eulers->ve[EULER_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 */
|
||||
/* compute external moments */
|
||||
static VEC *m_body = VNULL;
|
||||
m_body = v_resize(m_body, AXIS_NB);
|
||||
m_body = booz_get_moments_body_frame(m_body, omega_square);
|
||||
/* 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_BACK] + Kv/THAU * u->ve[SERVO_BACK];
|
||||
Xdot->ve[BFMS_OM_F] = -1./THAU * X->ve[BFMS_OM_F] - Kq * omega_square->ve[SERVO_FRONT] + Kv/THAU * u->ve[SERVO_FRONT];
|
||||
Xdot->ve[BFMS_OM_R] = -1./THAU * X->ve[BFMS_OM_R] - Kq * omega_square->ve[SERVO_RIGHT] + Kv/THAU * u->ve[SERVO_RIGHT];
|
||||
Xdot->ve[BFMS_OM_L] = -1./THAU * X->ve[BFMS_OM_L] - Kq * omega_square->ve[SERVO_LEFT] + Kv/THAU * u->ve[SERVO_LEFT];
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#if 0
|
||||
static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot) {
|
||||
static VEC *temp1 = VNULL;
|
||||
static VEC *temp2 = VNULL;
|
||||
temp1 = v_resize(temp1,SERVOS_NB);
|
||||
temp2 = v_resize(temp2,SERVOS_NB);
|
||||
|
||||
// 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
|
||||
@@ -1,65 +0,0 @@
|
||||
#ifndef BOOZ_FLIGHT_MODEL_H
|
||||
#define BOOZ_FLIGHT_MODEL_H
|
||||
|
||||
#include <matrix.h>
|
||||
#include "generated/airframe.h"
|
||||
|
||||
|
||||
|
||||
struct BoozFlightModel {
|
||||
/* are we flying ? */
|
||||
int on_ground;
|
||||
|
||||
double time;
|
||||
/* battery voltage in V */
|
||||
double bat_voltage;
|
||||
/* motors supply voltage in V */
|
||||
VEC* mot_voltage;
|
||||
|
||||
/* private state : see defines in .c for fields */
|
||||
VEC* state;
|
||||
|
||||
/* user products */
|
||||
VEC* pos_ecef;
|
||||
VEC* pos_ltp;
|
||||
VEC* speed_ltp;
|
||||
VEC* accel_ltp;
|
||||
VEC* speed_body;
|
||||
VEC* accel_body;
|
||||
|
||||
VEC* eulers;
|
||||
VEC* ang_rate_body;
|
||||
VEC* ang_accel_body;
|
||||
MAT* dcm;
|
||||
MAT* dcm_t;
|
||||
VEC* quat;
|
||||
|
||||
VEC* omega;
|
||||
VEC* omega_square;
|
||||
|
||||
/* constants used in derivative computation */
|
||||
/* magnetic field in earth frame */
|
||||
VEC* h_ltp;
|
||||
/* gravitation in earth frame */
|
||||
VEC* g_ltp;
|
||||
/* 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;
|
||||
/* */
|
||||
double mass;
|
||||
/* inertia matrix */
|
||||
MAT* Inert;
|
||||
/* invert of inertia matrix */
|
||||
MAT* Inert_inv;
|
||||
};
|
||||
|
||||
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 */
|
||||
@@ -1,44 +0,0 @@
|
||||
#ifndef BOOZ_FLIGHT_MODEL_PARAMS_H
|
||||
#define BOOZ_FLIGHT_MODEL_PARAMS_H
|
||||
|
||||
/* body drag coefficient */
|
||||
#define C_d_body .0075
|
||||
/* propeller thrust aerodynamic coefficient */
|
||||
#define C_t 0.297
|
||||
/* propeller moment aerodynamic coefficient */
|
||||
#define C_q 0.0276
|
||||
/* propeller radius in m */
|
||||
#define PROP_RADIUS 0.125
|
||||
/* propeller area in m2 */
|
||||
#define PROP_AREA 0.005
|
||||
/* air density in kg/m3 */
|
||||
#define RHO 1.225
|
||||
/* gravity in m/s2 */
|
||||
#define G 9.81
|
||||
/* mass in kg */
|
||||
#define MASS 0.724
|
||||
/* inertia on x axis in kg * m2 */
|
||||
#define Ix .007
|
||||
/* inertia on y axis in kg * m2 */
|
||||
#define Iy .0073
|
||||
/* inertia on z axis in kg * m2 */
|
||||
#define Iz .0137
|
||||
/* lenght between centers of vehicle and prop in m */
|
||||
#define L 0.25
|
||||
|
||||
/* motors parameters
|
||||
|
||||
from http://cherokee.stanford.edu/~starmac/docs/DynamicsSummary
|
||||
|
||||
omega_dot = -1/thau * omega - Kq * omega^2 + Kv/thau * V
|
||||
|
||||
*/
|
||||
#define BAT_VOLTAGE 11.
|
||||
|
||||
#define THAU 0.05
|
||||
#define Kq 0.12
|
||||
#define Kv 304.
|
||||
|
||||
|
||||
|
||||
#endif /* BOOZ_FLIGHT_MODEL_PARAMS_H */
|
||||
@@ -1,100 +0,0 @@
|
||||
#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;
|
||||
static VEC *temp=VNULL;
|
||||
|
||||
v1 = v_resize(v1,x->dim);
|
||||
v2 = v_resize(v2,x->dim);
|
||||
v3 = v_resize(v3,x->dim);
|
||||
v4 = v_resize(v4,x->dim);
|
||||
temp = v_resize(temp,x->dim);
|
||||
|
||||
// MEM_STAT_REG(v1,TYPE_VEC);
|
||||
// MEM_STAT_REG(v2,TYPE_VEC);
|
||||
// MEM_STAT_REG(v3,TYPE_VEC);
|
||||
// MEM_STAT_REG(v4,TYPE_VEC);
|
||||
// MEM_STAT_REG(temp,TYPE_VEC);
|
||||
|
||||
f(x, u, v1);
|
||||
v_mltadd(x,v1,0.5*dt,temp); /* temp = x + .5*dt*v1 */
|
||||
f(temp, u, v2);
|
||||
v_mltadd(x,v2,0.5*dt,temp); /* temp = x + .5*dt*v2 */
|
||||
f(temp,u, v3);
|
||||
v_mltadd(x,v3,dt,temp); /* temp = x + dt*v3 */
|
||||
f(temp, u, v4);
|
||||
|
||||
/* now add: v1+2*v2+2*v3+v4 */
|
||||
v_copy(v1,temp); /* temp = v1 */
|
||||
v_mltadd(temp,v2,2.0,temp); /* temp = v1+2*v2 */
|
||||
v_mltadd(temp,v3,2.0,temp); /* temp = v1+2*v2+2*v3 */
|
||||
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 */
|
||||
|
||||
}
|
||||
|
||||
|
||||
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* quat_of_eulers(VEC* quat, VEC* eulers) {
|
||||
|
||||
double phi2 = eulers->ve[EULER_PHI] / 2.0;
|
||||
double theta2 = eulers->ve[EULER_THETA] / 2.0;
|
||||
double psi2 = eulers->ve[EULER_PSI] / 2.0;
|
||||
|
||||
double sinphi2 = sin( phi2 );
|
||||
double cosphi2 = cos( phi2 );
|
||||
double sintheta2 = sin( theta2 );
|
||||
double costheta2 = cos( theta2 );
|
||||
double sinpsi2 = sin( psi2 );
|
||||
double cospsi2 = cos( psi2 );
|
||||
|
||||
quat->ve[QUAT_QI] = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2;
|
||||
quat->ve[QUAT_QX] = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2;
|
||||
quat->ve[QUAT_QY] = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2;
|
||||
quat->ve[QUAT_QZ] = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2;
|
||||
|
||||
return quat;
|
||||
}
|
||||
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -1,18 +0,0 @@
|
||||
#ifndef BOOZ_FM_UTILS_H
|
||||
#define BOOZ_FM_UTILS_H
|
||||
|
||||
#include <matrix.h>
|
||||
|
||||
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* quat_of_eulers(VEC* quat, VEC* eulers);
|
||||
|
||||
extern VEC* out_prod( VEC* a, VEC* b, VEC* out);
|
||||
|
||||
|
||||
|
||||
|
||||
#endif /* BOOZ_FM_UTILS_H */
|
||||
@@ -1,88 +0,0 @@
|
||||
#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) {
|
||||
|
||||
printf("connecting to %s on port %d\n", host, 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] - 90)/earth_radius);
|
||||
double lon = -2.135537 + asin((bfm.state->ve[BFMS_Y] - 45)/earth_radius);
|
||||
|
||||
struct FGNetGUI gui;
|
||||
net_gui_init(&gui);
|
||||
|
||||
gui.latitude = lat;
|
||||
gui.longitude = lon;
|
||||
gui.altitude = 1.1 - 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];
|
||||
|
||||
gui.cur_time += (unsigned long)bfm.time;
|
||||
|
||||
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.;
|
||||
}
|
||||
@@ -1,9 +0,0 @@
|
||||
#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 */
|
||||
@@ -1,84 +0,0 @@
|
||||
#include "booz_joystick.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <sys/types.h>
|
||||
#include <glib.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/joystick.h>
|
||||
|
||||
double booz_joystick_value[JS_NB_AXIS];
|
||||
//const double booz_joystick_neutral[JS_NB_AXIS] = { 112., 113., 112., 112., 112., 112., 30.};
|
||||
//const double booz_joystick_max[JS_NB_AXIS] = { 1., 224., 224., 224., 224., 224., 223.};
|
||||
//const double booz_joystick_min[JS_NB_AXIS] = { 224., 1., 1., 1., 1., 1., 30.};
|
||||
|
||||
const double booz_joystick_neutral[JS_NB_AXIS] = { 113., 113., 112., 112., 112., 112., 30.};
|
||||
const double booz_joystick_max[JS_NB_AXIS] = { 46., 178., 224., 224., 224., 179., 223.};
|
||||
const double booz_joystick_min[JS_NB_AXIS] = { 179., 46., 1., 1., 1., 46., 30.};
|
||||
|
||||
|
||||
|
||||
static gboolean on_data_received(GIOChannel *source, GIOCondition condition, gpointer data);
|
||||
|
||||
void booz_joystick_init(const char* device) {
|
||||
int i;
|
||||
for (i=0; i<JS_NB_AXIS; i++)
|
||||
booz_joystick_value[i] = 0.;
|
||||
booz_joystick_value[JS_MODE] = -0.7;
|
||||
|
||||
int fd = open(device, O_RDONLY | O_NONBLOCK);
|
||||
if (fd == -1) {
|
||||
printf("opening joystick serial device %s : %s\n", device, strerror(errno));
|
||||
return;
|
||||
}
|
||||
GIOChannel* channel = g_io_channel_unix_new(fd);
|
||||
g_io_channel_set_encoding(channel, NULL, NULL);
|
||||
g_io_add_watch (channel, G_IO_IN , on_data_received, NULL);
|
||||
|
||||
}
|
||||
|
||||
|
||||
static gboolean on_data_received(GIOChannel *source,
|
||||
GIOCondition condition __attribute__ ((unused)),
|
||||
gpointer data __attribute__ ((unused))) {
|
||||
|
||||
struct js_event js;
|
||||
gsize len;
|
||||
GError *err = NULL;
|
||||
g_io_channel_read_chars(source, (void*)(&js), sizeof(struct js_event), &len, &err);
|
||||
|
||||
if (js.type == JS_EVENT_AXIS) {
|
||||
if (js.number < JS_NB_AXIS) {
|
||||
// if (js.number == JS_THROTTLE) printf("joystick value %d\n",js.value);
|
||||
booz_joystick_value[js.number] = (double)(js.value - booz_joystick_neutral[js.number]) /
|
||||
(booz_joystick_max[js.number] - booz_joystick_min[js.number]);
|
||||
}
|
||||
}
|
||||
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
|
||||
#if 0
|
||||
switch (js.number) {
|
||||
case JS_THROTTLE:
|
||||
ppm_pulses[RADIO_THROTTLE] = 1223 + (js.value - 30) * (float)(2050-1223) / (float)(223 - 30);
|
||||
break;
|
||||
case JS_PITCH:
|
||||
ppm_pulses[RADIO_PITCH] = 1498 + (js.value - 113) * (float)(2050-950) / (float)(224 - 1);
|
||||
break;
|
||||
case JS_ROLL:
|
||||
ppm_pulses[RADIO_ROLL] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(1 - 224);
|
||||
break;
|
||||
case JS_YAW:
|
||||
ppm_pulses[RADIO_YAW] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(224 - 1);
|
||||
break;
|
||||
case JS_MODE:
|
||||
ppm_pulses[RADIO_MODE] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(224 - 1);
|
||||
rc_values_contains_avg_channels = TRUE;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
@@ -1,19 +0,0 @@
|
||||
#ifndef BOOZ_JOYSTICK_H
|
||||
#define BOOZ_JOYSTICK_H
|
||||
|
||||
#define JS_ROLL 0
|
||||
#define JS_PITCH 1
|
||||
#define JS_MODE 2
|
||||
#define JS_YAW 5
|
||||
#define JS_THROTTLE 6
|
||||
#define JS_NB_AXIS 7
|
||||
|
||||
extern void booz_joystick_init(const char* device);
|
||||
|
||||
extern double booz_joystick_value[JS_NB_AXIS];
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif /* BOOZ_JOYSTICK_H */
|
||||
@@ -1,153 +0,0 @@
|
||||
#ifndef BOOZ_JOYSTICK_FAKE_H
|
||||
#define BOOZ_JOYSTICK_FAKE_H
|
||||
|
||||
#define BREAK_MTT() { \
|
||||
int foo = sim_time/10; \
|
||||
switch(foo%4) { \
|
||||
case 0: \
|
||||
ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950); \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
|
||||
break; \
|
||||
case 1: \
|
||||
ppm_pulses[RADIO_PITCH] = 1500 + 0.0 * (2050-950); \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + 0.4 * (2050-950); \
|
||||
break; \
|
||||
case 2: \
|
||||
ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950); \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
|
||||
break; \
|
||||
case 3: \
|
||||
ppm_pulses[RADIO_PITCH] = 1500 - 0.0 * (2050-950); \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + 0.4 * (2050-950); \
|
||||
break; \
|
||||
} \
|
||||
}
|
||||
|
||||
#define WALK_OVAL() { \
|
||||
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
|
||||
int foo = sim_time/10; \
|
||||
switch(foo%4) { \
|
||||
case 0: \
|
||||
ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
|
||||
break; \
|
||||
case 1: \
|
||||
ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950); \
|
||||
break; \
|
||||
case 2: \
|
||||
ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \
|
||||
break; \
|
||||
case 3: \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + 0.2 * (2050-950); \
|
||||
break; \
|
||||
} \
|
||||
}
|
||||
|
||||
#define HOVER() { \
|
||||
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.6 * (2050-1223); \
|
||||
ppm_pulses[RADIO_MODE] = 1500 - 0.5 * (2050-950); \
|
||||
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
|
||||
ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
|
||||
}
|
||||
|
||||
|
||||
#define TOUPIE() { \
|
||||
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
|
||||
ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); \
|
||||
int foo = sim_time/20; \
|
||||
switch(foo%4) { \
|
||||
case 0: \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
|
||||
break; \
|
||||
case 1: \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + 0.2 * (2050-950); \
|
||||
break; \
|
||||
case 2: \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
|
||||
break; \
|
||||
case 3: \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + -0.2 * (2050-950); \
|
||||
break; \
|
||||
} \
|
||||
}
|
||||
|
||||
|
||||
#define CIRCLE() { \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
|
||||
double alpha = 0.01 * sim_time; \
|
||||
ppm_pulses[RADIO_PITCH] = 1498 + cos(alpha) * 250.; \
|
||||
ppm_pulses[RADIO_ROLL] = 1500 + sin(alpha) * 250.; \
|
||||
}
|
||||
|
||||
#define ATTITUDE_ROLL_STEPS() { \
|
||||
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.66 * (2050-1223); \
|
||||
ppm_pulses[RADIO_MODE] = 1500 + 0. * (2050-950); \
|
||||
ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
|
||||
int foo = sim_time/10; \
|
||||
switch(foo%2) { \
|
||||
case 0: \
|
||||
ppm_pulses[RADIO_ROLL] = 1500 + 0.2 * (2050-950); \
|
||||
break; \
|
||||
case 1: \
|
||||
ppm_pulses[RADIO_ROLL] = 1500 - 0.2 * (2050-950); \
|
||||
break; \
|
||||
} \
|
||||
}
|
||||
|
||||
#define ATTITUDE_PITCH_STEPS() { \
|
||||
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.66 * (2050-1223); \
|
||||
ppm_pulses[RADIO_MODE] = 1500 + 0. * (2050-950); \
|
||||
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
|
||||
int foo = sim_time/10; \
|
||||
switch(foo%2) { \
|
||||
case 0: \
|
||||
ppm_pulses[RADIO_PITCH] = 1498 + 0.2 * (2050-950); \
|
||||
break; \
|
||||
case 1: \
|
||||
ppm_pulses[RADIO_PITCH] = 1498 - 0.2 * (2050-950); \
|
||||
break; \
|
||||
} \
|
||||
}
|
||||
|
||||
#define ATTITUDE_YAW_STEPS() { \
|
||||
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.66 * (2050-1223); \
|
||||
ppm_pulses[RADIO_MODE] = 1500 + 0. * (2050-950); \
|
||||
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
|
||||
ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \
|
||||
int foo = sim_time/10; \
|
||||
switch(foo%2) { \
|
||||
case 0: \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + 0.2 * (2050-950); \
|
||||
break; \
|
||||
case 1: \
|
||||
ppm_pulses[RADIO_YAW] = 1493 - 0.2 * (2050-950); \
|
||||
break; \
|
||||
} \
|
||||
}
|
||||
|
||||
#define TWO_POINTS() { \
|
||||
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
|
||||
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
|
||||
int foo = sim_time/10; \
|
||||
switch(foo%4) { \
|
||||
case 0: \
|
||||
ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); \
|
||||
break; \
|
||||
case 1: \
|
||||
ppm_pulses[RADIO_PITCH] = 1498 + 0.3 * (2050-950); \
|
||||
break; \
|
||||
case 2: \
|
||||
ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); \
|
||||
break; \
|
||||
case 3: \
|
||||
ppm_pulses[RADIO_PITCH] = 1498 - 0.3 * (2050-950); \
|
||||
break; \
|
||||
} \
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif /* BOOZ_JOYSTICK_FAKE_H */
|
||||
@@ -1,245 +0,0 @@
|
||||
/* r250.c the r250 uniform random number algorithm
|
||||
|
||||
Kirkpatrick, S., and E. Stoll, 1981; "A Very Fast
|
||||
Shift-Register Sequence Random Number Generator",
|
||||
Journal of Computational Physics, V.40
|
||||
|
||||
also:
|
||||
|
||||
see W.L. Maier, DDJ May 1991
|
||||
|
||||
|
||||
|
||||
*/
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
#include "booz_r250.h"
|
||||
|
||||
/* set the following if you trust rand(), otherwise the minimal standard
|
||||
generator is used
|
||||
*/
|
||||
/* #define TRUST_RAND */
|
||||
|
||||
|
||||
#ifndef TRUST_RAND
|
||||
#include "booz_randlcg.h"
|
||||
#endif
|
||||
|
||||
/* defines to allow for 16 or 32 bit integers */
|
||||
#define BITS 31
|
||||
|
||||
|
||||
#if WORD_BIT == 32
|
||||
#ifndef BITS
|
||||
#define BITS 32
|
||||
#endif
|
||||
#else
|
||||
#ifndef BITS
|
||||
#define BITS 16
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if BITS == 31
|
||||
#define MSB 0x40000000L
|
||||
#define ALL_BITS 0x7fffffffL
|
||||
#define HALF_RANGE 0x20000000L
|
||||
#define STEP 7
|
||||
#endif
|
||||
|
||||
#if BITS == 32
|
||||
#define MSB 0x80000000L
|
||||
#define ALL_BITS 0xffffffffL
|
||||
#define HALF_RANGE 0x40000000L
|
||||
#define STEP 7
|
||||
#endif
|
||||
|
||||
#if BITS == 16
|
||||
#define MSB 0x8000
|
||||
#define ALL_BITS 0xffff
|
||||
#define HALF_RANGE 0x4000
|
||||
#define STEP 11
|
||||
#endif
|
||||
|
||||
static unsigned int r250_buffer[ 250 ];
|
||||
static int r250_index;
|
||||
|
||||
#ifdef NO_PROTO
|
||||
void r250_init(sd)
|
||||
int seed;
|
||||
#else
|
||||
void r250_init(int sd)
|
||||
#endif
|
||||
{
|
||||
int j, k;
|
||||
unsigned int mask, msb;
|
||||
|
||||
#ifdef TRUST_RAND
|
||||
|
||||
#if BITS == 32 || BITS == 31
|
||||
srand48( sd );
|
||||
#else
|
||||
srand( sd );
|
||||
#endif
|
||||
|
||||
|
||||
#else
|
||||
set_seed( sd );
|
||||
#endif
|
||||
|
||||
r250_index = 0;
|
||||
for (j = 0; j < 250; j++) /* fill r250 buffer with BITS-1 bit values */
|
||||
#ifdef TRUST_RAND
|
||||
#if BITS == 32 || BITS == 31
|
||||
r250_buffer[j] = (unsigned int)lrand48();
|
||||
#else
|
||||
r250_buffer[j] = rand();
|
||||
#endif
|
||||
#else
|
||||
r250_buffer[j] = randlcg();
|
||||
#endif
|
||||
|
||||
|
||||
for (j = 0; j < 250; j++) /* set some MSBs to 1 */
|
||||
#ifdef TRUST_RAND
|
||||
if ( rand() > HALF_RANGE )
|
||||
r250_buffer[j] |= MSB;
|
||||
#else
|
||||
if ( randlcg() > HALF_RANGE )
|
||||
r250_buffer[j] |= MSB;
|
||||
#endif
|
||||
|
||||
|
||||
msb = MSB; /* turn on diagonal bit */
|
||||
mask = ALL_BITS; /* turn off the leftmost bits */
|
||||
|
||||
for (j = 0; j < BITS; j++)
|
||||
{
|
||||
k = STEP * j + 3; /* select a word to operate on */
|
||||
r250_buffer[k] &= mask; /* turn off bits left of the diagonal */
|
||||
r250_buffer[k] |= msb; /* turn on the diagonal bit */
|
||||
mask >>= 1;
|
||||
msb >>= 1;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
unsigned int r250() /* returns a random unsigned integer */
|
||||
{
|
||||
register int j;
|
||||
register unsigned int new_rand;
|
||||
|
||||
if ( r250_index >= 147 )
|
||||
j = r250_index - 147; /* wrap pointer around */
|
||||
else
|
||||
j = r250_index + 103;
|
||||
|
||||
new_rand = r250_buffer[ r250_index ] ^ r250_buffer[ j ];
|
||||
r250_buffer[ r250_index ] = new_rand;
|
||||
|
||||
if ( r250_index >= 249 ) /* increment pointer for next time */
|
||||
r250_index = 0;
|
||||
else
|
||||
r250_index++;
|
||||
|
||||
return new_rand;
|
||||
|
||||
}
|
||||
|
||||
|
||||
double dr250() /* returns a random double in range 0..1 */
|
||||
{
|
||||
register int j;
|
||||
register unsigned int new_rand;
|
||||
|
||||
if ( r250_index >= 147 )
|
||||
j = r250_index - 147; /* wrap pointer around */
|
||||
else
|
||||
j = r250_index + 103;
|
||||
|
||||
new_rand = r250_buffer[ r250_index ] ^ r250_buffer[ j ];
|
||||
r250_buffer[ r250_index ] = new_rand;
|
||||
|
||||
if ( r250_index >= 249 ) /* increment pointer for next time */
|
||||
r250_index = 0;
|
||||
else
|
||||
r250_index++;
|
||||
|
||||
return (double)new_rand / ALL_BITS;
|
||||
|
||||
}
|
||||
|
||||
#ifdef MAIN
|
||||
|
||||
/* test driver prints out either NMR_RAND values or a histogram */
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#define NMR_RAND 5000
|
||||
#define MAX_BINS 500
|
||||
|
||||
#ifdef NO_PROTO
|
||||
void main(argc, argv)
|
||||
int argc;
|
||||
char **argv;
|
||||
#else
|
||||
void main(int argc, char **argv)
|
||||
#endif
|
||||
{
|
||||
int j,k,nmr_bins,seed;
|
||||
int bins[MAX_BINS];
|
||||
double randm, bin_inc;
|
||||
double bin_limit[MAX_BINS];
|
||||
|
||||
if ( argc != 3 )
|
||||
{
|
||||
printf("Usage -- %s nmr_bins seed\n", argv[0]);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
nmr_bins = atoi( argv[1] );
|
||||
if ( nmr_bins > MAX_BINS )
|
||||
{
|
||||
printf("ERROR -- maximum number of bins is %d\n", MAX_BINS);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
seed = atoi( argv[2] );
|
||||
|
||||
r250_init( seed );
|
||||
|
||||
if ( nmr_bins < 1 ) /* just print out the numbers */
|
||||
{
|
||||
for (j = 0; j < NMR_RAND; j++)
|
||||
printf("%f\n", dr250() );
|
||||
exit(0);
|
||||
}
|
||||
|
||||
bin_inc = 1.0 / nmr_bins;
|
||||
for (j = 0; j < nmr_bins; j++) /* initialize bins to zero */
|
||||
{
|
||||
bins[j] = 0;
|
||||
bin_limit[j] = (j + 1) * bin_inc;
|
||||
}
|
||||
|
||||
bin_limit[nmr_bins-1] = 1.0e7; /* make sure all others are in last bin */
|
||||
|
||||
for (j = 0; j < NMR_RAND; j++)
|
||||
{
|
||||
randm = r250() / (double)ALL_BITS;
|
||||
for (k = 0; k < nmr_bins; k++)
|
||||
if ( randm < bin_limit[k] )
|
||||
{
|
||||
bins[k]++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
for (j = 0; j < nmr_bins; j++)
|
||||
printf("%d\n", bins[j]);
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
@@ -1,21 +0,0 @@
|
||||
/* r250.h prototypes for r250 random number generator,
|
||||
|
||||
Kirkpatrick, S., and E. Stoll, 1981; "A Very Fast
|
||||
Shift-Register Sequence Random Number Generator",
|
||||
Journal of Computational Physics, V.40
|
||||
|
||||
also:
|
||||
|
||||
see W.L. Maier, DDJ May 1991
|
||||
|
||||
|
||||
*/
|
||||
|
||||
#ifndef _R250_H_
|
||||
#define _R250_H_ 1.2
|
||||
|
||||
void r250_init(int seed);
|
||||
unsigned int r250( void );
|
||||
double dr250( void );
|
||||
|
||||
#endif
|
||||
@@ -1,51 +0,0 @@
|
||||
/* rndlcg Linear Congruential Method, the "minimal standard generator"
|
||||
Park & Miller, 1988, Comm of the ACM, 31(10), pp. 1192-1201
|
||||
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <limits.h>
|
||||
|
||||
#define ALL_BITS 0xffffffff
|
||||
|
||||
static long int quotient = LONG_MAX / 16807L;
|
||||
static long int my_remainder = LONG_MAX % 16807L;
|
||||
|
||||
static long int seed_val = 1L;
|
||||
|
||||
long set_seed(long int sd)
|
||||
{
|
||||
return seed_val = sd;
|
||||
}
|
||||
|
||||
long get_seed()
|
||||
{
|
||||
return seed_val;
|
||||
}
|
||||
|
||||
|
||||
unsigned long int randlcg() /* returns a random unsigned integer */
|
||||
{
|
||||
if ( seed_val <= quotient )
|
||||
seed_val = (seed_val * 16807L) % LONG_MAX;
|
||||
else
|
||||
{
|
||||
long int high_part = seed_val / quotient;
|
||||
long int low_part = seed_val % quotient;
|
||||
|
||||
long int test = 16807L * low_part - my_remainder * high_part;
|
||||
|
||||
if ( test > 0 )
|
||||
seed_val = test;
|
||||
else
|
||||
seed_val = test + LONG_MAX;
|
||||
|
||||
}
|
||||
|
||||
return seed_val;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,19 +0,0 @@
|
||||
|
||||
/* randlcg.h prototypes for the minimal standard random number generator,
|
||||
|
||||
Linear Congruential Method, the "minimal standard generator"
|
||||
Park & Miller, 1988, Comm of the ACM, 31(10), pp. 1192-1201
|
||||
|
||||
|
||||
rcsid: @(#)randlcg.h 1.1 15:48:09 11/21/94 EFC
|
||||
|
||||
*/
|
||||
|
||||
#ifndef _RANDLCG_H_
|
||||
#define _RANDLCG_H_ 1.1
|
||||
|
||||
long set_seed(long);
|
||||
long get_seed(long);
|
||||
unsigned long int randlcg();
|
||||
|
||||
#endif
|
||||
@@ -1,33 +0,0 @@
|
||||
#ifndef BOOZ_RC_SIM_H
|
||||
#define BOOZ_RC_SIM_H
|
||||
|
||||
#include "subsystems/radio_control.h"
|
||||
|
||||
#define MODE_SWITCH_MANUAL 1900
|
||||
#define MODE_SWITCH_AUTO1 1500
|
||||
#define MODE_SWITCH_AUTO2 1200
|
||||
|
||||
#define BoozRcSimFeed(_time) { \
|
||||
/* starts motors */ \
|
||||
if (_time < 0.93) { \
|
||||
ppm_pulses[RADIO_YAW] = 1500 - 0.5 * (2050-950); \
|
||||
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.0 * (2050-1223); \
|
||||
ppm_pulses[RADIO_MODE] = MODE_SWITCH_MANUAL; \
|
||||
} \
|
||||
else if (_time < 3.5) { \
|
||||
ppm_pulses[RADIO_YAW] = 1500 + 0. * (2050-950); \
|
||||
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.4 * (2050-1223); \
|
||||
ppm_pulses[RADIO_MODE] = MODE_SWITCH_MANUAL; \
|
||||
} \
|
||||
else { \
|
||||
ppm_pulses[RADIO_YAW] = 1500 + 0. * (2050-950); \
|
||||
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.99 * (2050-1223); \
|
||||
ppm_pulses[RADIO_MODE] = MODE_SWITCH_AUTO2; \
|
||||
} \
|
||||
ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \
|
||||
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
|
||||
ppm_valid = TRUE; \
|
||||
}
|
||||
|
||||
|
||||
#endif /* BOOZ_RC_SIM_H */
|
||||
@@ -1,64 +0,0 @@
|
||||
#include "booz_sensors_model.h"
|
||||
#include BSM_PARAMS
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#include "std.h"
|
||||
#include "booz_flight_model.h"
|
||||
#include "booz_flight_model_utils.h"
|
||||
|
||||
struct BoozSensorsModel bsm;
|
||||
|
||||
extern void booz_sensors_model_accel_init(double time);
|
||||
extern void booz_sensors_model_accel_run(double time);
|
||||
|
||||
extern void booz_sensors_model_gyro_init(double time);
|
||||
extern void booz_sensors_model_gyro_run(double time);
|
||||
|
||||
extern void booz_sensors_model_mag_init(double time);
|
||||
extern void booz_sensors_model_mag_run(double time);
|
||||
|
||||
extern void booz_sensors_model_rangemeter_init(double time);
|
||||
extern void booz_sensors_model_rangemeter_run(double time);
|
||||
|
||||
extern void booz_sensors_model_baro_init(double time);
|
||||
extern void booz_sensors_model_baro_run(double time);
|
||||
|
||||
extern void booz_sensors_model_gps_init(double time);
|
||||
extern void booz_sensors_model_gps_run(double time);
|
||||
|
||||
|
||||
void booz_sensors_model_init(double time) {
|
||||
|
||||
VEC* tmp_eulers = v_get(AXIS_NB);
|
||||
tmp_eulers->ve[EULER_PHI] = BSM_BODY_TO_IMU_PHI;
|
||||
tmp_eulers->ve[EULER_THETA] = BSM_BODY_TO_IMU_THETA;
|
||||
tmp_eulers->ve[EULER_PSI] = BSM_BODY_TO_IMU_PSI;
|
||||
bsm.body_to_imu = m_get(AXIS_NB, AXIS_NB);
|
||||
dcm_of_eulers (tmp_eulers, bsm.body_to_imu );
|
||||
|
||||
booz_sensors_model_accel_init(time);
|
||||
booz_sensors_model_gyro_init(time);
|
||||
booz_sensors_model_mag_init(time);
|
||||
booz_sensors_model_rangemeter_init(time);
|
||||
booz_sensors_model_baro_init(time);
|
||||
booz_sensors_model_gps_init(time);
|
||||
|
||||
}
|
||||
|
||||
void booz_sensors_model_run(double time) {
|
||||
|
||||
booz_sensors_model_accel_run(time);
|
||||
booz_sensors_model_gyro_run(time);
|
||||
booz_sensors_model_mag_run(time);
|
||||
booz_sensors_model_rangemeter_run(time);
|
||||
booz_sensors_model_baro_run(time);
|
||||
booz_sensors_model_gps_run(time);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,126 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2008 Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef BOOZ_SENSORS_MODEL_H
|
||||
#define BOOZ_SENSORS_MODEL_H
|
||||
|
||||
#include <matrix.h>
|
||||
#include <glib.h>
|
||||
|
||||
#include "pprz_algebra_float.h"
|
||||
#include "pprz_geodetic_double.h"
|
||||
#include "booz_geometry_int.h"
|
||||
#include "6dof.h"
|
||||
#include "std.h"
|
||||
|
||||
extern void booz_sensors_model_init(double time);
|
||||
extern void booz_sensors_model_run( double time);
|
||||
extern bool_t booz_sensors_model_accel_available();
|
||||
extern bool_t booz_sensors_model_gyro_available();
|
||||
extern bool_t booz_sensors_model_mag_available();
|
||||
extern bool_t booz_sensors_model_baro_available();
|
||||
extern bool_t booz_sensors_model_gps_available();
|
||||
|
||||
|
||||
struct BoozSensorsModel {
|
||||
|
||||
/* our imu alignement */
|
||||
MAT* body_to_imu;
|
||||
|
||||
/* Accelerometer */
|
||||
VEC* accel;
|
||||
unsigned int accel_resolution;
|
||||
MAT* accel_sensitivity;
|
||||
VEC* accel_neutral;
|
||||
VEC* accel_noise_std_dev;
|
||||
VEC* accel_bias;
|
||||
double accel_next_update;
|
||||
int accel_available;
|
||||
|
||||
|
||||
/* Gyrometer */
|
||||
VEC* gyro;
|
||||
unsigned int gyro_resolution;
|
||||
MAT* gyro_sensitivity;
|
||||
VEC* gyro_neutral;
|
||||
VEC* gyro_noise_std_dev;
|
||||
VEC* gyro_bias_initial;
|
||||
VEC* gyro_bias_random_walk_std_dev;
|
||||
VEC* gyro_bias_random_walk_value;
|
||||
double gyro_next_update;
|
||||
int gyro_available;
|
||||
|
||||
|
||||
/* Magnetometer */
|
||||
VEC* mag;
|
||||
MAT* mag_imu_to_sensor;
|
||||
MAT* mag_sensitivity;
|
||||
VEC* mag_neutral;
|
||||
VEC* mag_noise_std_dev;
|
||||
double mag_next_update;
|
||||
int mag_available;
|
||||
|
||||
|
||||
/* Rangemeter */
|
||||
double rangemeter;
|
||||
double rangemeter_next_update;
|
||||
int rangemeter_available;
|
||||
|
||||
|
||||
/* Barometer */
|
||||
double baro;
|
||||
unsigned int baro_resolution;
|
||||
double baro_next_update;
|
||||
int baro_available;
|
||||
|
||||
|
||||
/* GPS */
|
||||
/* state */
|
||||
VEC* gps_speed;
|
||||
VEC* gps_speed_noise_std_dev;
|
||||
GSList* gps_speed_history;
|
||||
VEC* gps_pos;
|
||||
VEC* gps_pos_noise_std_dev;
|
||||
VEC* gps_pos_bias_initial;
|
||||
VEC* gps_pos_bias_random_walk_std_dev;
|
||||
VEC* gps_pos_bias_random_walk_value;
|
||||
GSList* gps_pos_history;
|
||||
/* outputs */
|
||||
struct LtpDef_d gps_ltp_def;
|
||||
struct EcefCoor_d gps_speed_ecef;
|
||||
struct EcefCoor_d gps_pos_ecef;
|
||||
double gps_pos_utm_north;
|
||||
double gps_pos_utm_east;
|
||||
double gps_pos_utm_alt;
|
||||
// struct Pprz_double_lla gps_pos_lla;
|
||||
double gps_speed_course;
|
||||
double gps_speed_gspeed;
|
||||
double gps_speed_climb;
|
||||
double gps_next_update;
|
||||
int gps_available;
|
||||
|
||||
};
|
||||
|
||||
extern struct BoozSensorsModel bsm;
|
||||
|
||||
|
||||
#endif /* BOOZ_SENSORS_MODEL_H */
|
||||
@@ -1,100 +0,0 @@
|
||||
#include "booz_sensors_model.h"
|
||||
|
||||
#include BSM_PARAMS
|
||||
#include "booz_sensors_model_utils.h"
|
||||
#include "booz_flight_model.h"
|
||||
#include "booz_flight_model_utils.h"
|
||||
|
||||
|
||||
bool_t booz_sensors_model_accel_available() {
|
||||
if (bsm.accel_available) {
|
||||
bsm.accel_available = FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
|
||||
void booz_sensors_model_accel_init(double time) {
|
||||
|
||||
bsm.accel = v_get(AXIS_NB);
|
||||
bsm.accel->ve[AXIS_X] = 0.;
|
||||
bsm.accel->ve[AXIS_Y] = 0.;
|
||||
bsm.accel->ve[AXIS_Z] = 0.;
|
||||
bsm.accel_resolution = BSM_ACCEL_RESOLUTION;
|
||||
|
||||
bsm.accel_sensitivity = m_get(AXIS_NB, AXIS_NB);
|
||||
m_zero(bsm.accel_sensitivity);
|
||||
bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = BSM_ACCEL_SENSITIVITY_XX;
|
||||
bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_ACCEL_SENSITIVITY_YY;
|
||||
bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_ACCEL_SENSITIVITY_ZZ;
|
||||
|
||||
bsm.accel_neutral = v_get(AXIS_NB);
|
||||
bsm.accel_neutral->ve[AXIS_X] = BSM_ACCEL_NEUTRAL_X;
|
||||
bsm.accel_neutral->ve[AXIS_Y] = BSM_ACCEL_NEUTRAL_Y;
|
||||
bsm.accel_neutral->ve[AXIS_Z] = BSM_ACCEL_NEUTRAL_Z;
|
||||
|
||||
bsm.accel_noise_std_dev = v_get(AXIS_NB);
|
||||
bsm.accel_noise_std_dev->ve[AXIS_X] = BSM_ACCEL_NOISE_STD_DEV_X;
|
||||
bsm.accel_noise_std_dev->ve[AXIS_Y] = BSM_ACCEL_NOISE_STD_DEV_Y;
|
||||
bsm.accel_noise_std_dev->ve[AXIS_Z] = BSM_ACCEL_NOISE_STD_DEV_Z;
|
||||
|
||||
bsm.accel_bias = v_get(AXIS_NB);
|
||||
bsm.accel_bias->ve[AXIS_P] = BSM_ACCEL_BIAS_X;
|
||||
bsm.accel_bias->ve[AXIS_Q] = BSM_ACCEL_BIAS_Y;
|
||||
bsm.accel_bias->ve[AXIS_R] = BSM_ACCEL_BIAS_Z;
|
||||
|
||||
bsm.accel_next_update = time;
|
||||
bsm.accel_available = FALSE;
|
||||
|
||||
}
|
||||
|
||||
void booz_sensors_model_accel_run( double time ) {
|
||||
if (time < bsm.accel_next_update)
|
||||
return;
|
||||
|
||||
static VEC* accel_ltp = VNULL;
|
||||
accel_ltp = v_resize(accel_ltp, AXIS_NB);
|
||||
/* substract gravity to acceleration in ltp frame */
|
||||
accel_ltp = v_sub(bfm.accel_ltp, bfm.g_ltp, accel_ltp);
|
||||
/* convert to body frame */
|
||||
static VEC* accel_body = VNULL;
|
||||
accel_body = v_resize(accel_body, AXIS_NB);
|
||||
mv_mlt(bfm.dcm, accel_ltp, accel_body);
|
||||
/* convert to imu frame */
|
||||
static VEC* accel_imu = VNULL;
|
||||
accel_imu = v_resize(accel_imu, AXIS_NB);
|
||||
mv_mlt(bsm.body_to_imu, accel_body, accel_imu);
|
||||
|
||||
|
||||
|
||||
// printf(" accel_body ~ %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);
|
||||
|
||||
/* compute accel reading */
|
||||
mv_mlt(bsm.accel_sensitivity, accel_imu, bsm.accel);
|
||||
v_add(bsm.accel, bsm.accel_neutral, bsm.accel);
|
||||
|
||||
/* compute accel error readings */
|
||||
static VEC *accel_error = VNULL;
|
||||
accel_error = v_resize(accel_error, AXIS_NB);
|
||||
accel_error = v_zero(accel_error);
|
||||
/* add a gaussian noise */
|
||||
accel_error = v_add_gaussian_noise(accel_error, bsm.accel_noise_std_dev, accel_error);
|
||||
/* constant bias */
|
||||
accel_error = v_add(accel_error, bsm.accel_bias, accel_error);
|
||||
/* scale to adc units FIXME : should use full adc gain ? sum ? */
|
||||
accel_error->ve[AXIS_X] = accel_error->ve[AXIS_X] * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];
|
||||
accel_error->ve[AXIS_Y] = accel_error->ve[AXIS_Y] * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];
|
||||
accel_error->ve[AXIS_Z] = accel_error->ve[AXIS_Z] * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];
|
||||
/* add per accel error reading */
|
||||
bsm.accel = v_add(bsm.accel, accel_error, bsm.accel);
|
||||
/* round signal to account for adc discretisation */
|
||||
RoundSensor(bsm.accel);
|
||||
/* saturation */
|
||||
BoundSensor(bsm.accel, 0, bsm.accel_resolution);
|
||||
|
||||
// printf("sim adc %f %f %f\n",bsm.accel->ve[AXIS_X] ,bsm.accel->ve[AXIS_Y] ,bsm.accel->ve[AXIS_Z]);
|
||||
bsm.accel_next_update += BSM_ACCEL_DT;
|
||||
bsm.accel_available = TRUE;
|
||||
}
|
||||
|
||||
@@ -1,42 +0,0 @@
|
||||
#include "booz_sensors_model.h"
|
||||
|
||||
#include BSM_PARAMS
|
||||
#include "booz_sensors_model_utils.h"
|
||||
#include "booz_flight_model.h"
|
||||
|
||||
bool_t booz_sensors_model_baro_available() {
|
||||
if (bsm.baro_available) {
|
||||
bsm.baro_available = FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
|
||||
void booz_sensors_model_baro_init( double time ) {
|
||||
bsm.baro = 0.;
|
||||
|
||||
bsm.baro_next_update = time;
|
||||
bsm.baro_available = FALSE;
|
||||
|
||||
}
|
||||
|
||||
void booz_sensors_model_baro_run( double time ) {
|
||||
if (time < bsm.baro_next_update)
|
||||
return;
|
||||
|
||||
if (time < 12.5)
|
||||
bsm.baro = 840;
|
||||
else {
|
||||
double z = bfm.pos_ltp->ve[AXIS_Z] + get_gaussian_noise()*BSM_BARO_NOISE_STD_DEV;
|
||||
// double p = ( z / 0.084 ) + BSM_BARO_QNH;
|
||||
// double baro_reading = p * BSM_BARO_SENSITIVITY;
|
||||
double baro_reading = BSM_BARO_QNH + z * BSM_BARO_SENSITIVITY;
|
||||
|
||||
/* FIXME : add noise and random walk */
|
||||
baro_reading = rint(baro_reading);
|
||||
bsm.baro = baro_reading;
|
||||
}
|
||||
bsm.baro_next_update += BSM_BARO_DT;
|
||||
bsm.baro_available = TRUE;
|
||||
}
|
||||
@@ -1,151 +0,0 @@
|
||||
#include "booz_sensors_model.h"
|
||||
|
||||
#include BSM_PARAMS
|
||||
#include "booz_sensors_model_utils.h"
|
||||
#include "booz_flight_model.h"
|
||||
#include "pprz_geodetic_double.h"
|
||||
|
||||
bool_t booz_sensors_model_gps_available() {
|
||||
if (bsm.gps_available) {
|
||||
bsm.gps_available = FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void booz_sensors_model_gps_init( double time ) {
|
||||
|
||||
bsm.gps_speed = v_get(AXIS_NB);
|
||||
v_zero(bsm.gps_speed);
|
||||
|
||||
bsm.gps_speed_noise_std_dev = v_get(AXIS_NB);
|
||||
bsm.gps_speed_noise_std_dev->ve[AXIS_X] = BSM_GPS_SPEED_NOISE_STD_DEV;
|
||||
bsm.gps_speed_noise_std_dev->ve[AXIS_Y] = BSM_GPS_SPEED_NOISE_STD_DEV;
|
||||
bsm.gps_speed_noise_std_dev->ve[AXIS_Z] = BSM_GPS_SPEED_NOISE_STD_DEV;
|
||||
|
||||
bsm.gps_speed_history = NULL;
|
||||
|
||||
bsm.gps_pos = v_get(AXIS_NB);
|
||||
v_zero(bsm.gps_pos);
|
||||
|
||||
bsm.gps_pos_noise_std_dev = v_get(AXIS_NB);
|
||||
bsm.gps_pos_noise_std_dev->ve[AXIS_X] = BSM_GPS_POS_NOISE_STD_DEV;
|
||||
bsm.gps_pos_noise_std_dev->ve[AXIS_Y] = BSM_GPS_POS_NOISE_STD_DEV;
|
||||
bsm.gps_pos_noise_std_dev->ve[AXIS_Z] = BSM_GPS_POS_NOISE_STD_DEV;
|
||||
|
||||
bsm.gps_pos_bias_initial = v_get(AXIS_NB);
|
||||
bsm.gps_pos_bias_initial->ve[AXIS_X] = BSM_GPS_POS_BIAS_INITIAL_X;
|
||||
bsm.gps_pos_bias_initial->ve[AXIS_Y] = BSM_GPS_POS_BIAS_INITIAL_Y;
|
||||
bsm.gps_pos_bias_initial->ve[AXIS_Z] = BSM_GPS_POS_BIAS_INITIAL_Z;
|
||||
|
||||
bsm.gps_pos_bias_random_walk_std_dev = v_get(AXIS_NB);
|
||||
bsm.gps_pos_bias_random_walk_std_dev->ve[AXIS_X] = BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X;
|
||||
bsm.gps_pos_bias_random_walk_std_dev->ve[AXIS_Y] = BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y;
|
||||
bsm.gps_pos_bias_random_walk_std_dev->ve[AXIS_Z] = BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z;
|
||||
|
||||
bsm.gps_pos_bias_random_walk_value = v_get(AXIS_NB);
|
||||
bsm.gps_pos_bias_random_walk_value->ve[AXIS_X] = bsm.gps_pos_bias_initial->ve[AXIS_X];
|
||||
bsm.gps_pos_bias_random_walk_value->ve[AXIS_Y] = bsm.gps_pos_bias_initial->ve[AXIS_Y];
|
||||
bsm.gps_pos_bias_random_walk_value->ve[AXIS_Z] = bsm.gps_pos_bias_initial->ve[AXIS_Z];
|
||||
|
||||
bsm.gps_pos_history = NULL;
|
||||
|
||||
/* Toulouse */
|
||||
struct EcefCoor_d ref_coor = { 4624497.0 , 116475.0, 4376563.0};
|
||||
// struct EcefCoor_d ref_coor = { 6368189. , 0., 0.};
|
||||
|
||||
ltp_def_from_ecef_d(&bsm.gps_ltp_def, &ref_coor);
|
||||
|
||||
bsm.gps_next_update = time;
|
||||
bsm.gps_available = FALSE;
|
||||
|
||||
}
|
||||
|
||||
/* We store our positions like the fdm and convert to UTM and funcky course, gspeed, climb */
|
||||
void booz_sensors_model_gps_run( double time) {
|
||||
if (time < bsm.gps_next_update)
|
||||
return;
|
||||
|
||||
/*
|
||||
* simulate speed sensor
|
||||
*/
|
||||
static VEC *cur_speed_reading = VNULL;
|
||||
cur_speed_reading = v_resize(cur_speed_reading, AXIS_NB);
|
||||
v_copy(bfm.speed_ltp, cur_speed_reading);;
|
||||
/* add a gaussian noise */
|
||||
cur_speed_reading = v_add_gaussian_noise(cur_speed_reading, bsm.gps_speed_noise_std_dev,
|
||||
cur_speed_reading);
|
||||
UpdateSensorLatency(time, cur_speed_reading, bsm.gps_speed_history, BSM_GPS_SPEED_LATENCY, bsm.gps_speed);
|
||||
|
||||
/* course, gspeed, climb convertion */
|
||||
bsm.gps_speed_course = DegOfRad(atan2(bsm.gps_speed->ve[AXIS_Y], bsm.gps_speed->ve[AXIS_X]))*10.;
|
||||
bsm.gps_speed_course = rint(bsm.gps_speed_course);
|
||||
bsm.gps_speed_gspeed = sqrt(bsm.gps_speed->ve[AXIS_X] * bsm.gps_speed->ve[AXIS_X] +
|
||||
bsm.gps_speed->ve[AXIS_Y] * bsm.gps_speed->ve[AXIS_Y]) * 100.;
|
||||
bsm.gps_speed_gspeed = rint(bsm.gps_speed_gspeed);
|
||||
bsm.gps_speed_climb = rint(-bsm.gps_speed->ve[AXIS_Z] * 100);
|
||||
|
||||
|
||||
/*
|
||||
* simulate position sensor
|
||||
*/
|
||||
|
||||
/* compute position error */
|
||||
static VEC *pos_error = VNULL;
|
||||
pos_error = v_resize(pos_error, AXIS_NB);
|
||||
pos_error = v_zero(pos_error);
|
||||
/* add a gaussian noise */
|
||||
pos_error = v_add_gaussian_noise(pos_error, bsm.gps_pos_noise_std_dev, pos_error);
|
||||
/* update random walk bias */
|
||||
bsm.gps_pos_bias_random_walk_value =
|
||||
v_update_random_walk(bsm.gps_pos_bias_random_walk_value,
|
||||
bsm.gps_pos_bias_random_walk_std_dev, BSM_GPS_DT,
|
||||
bsm.gps_pos_bias_random_walk_value);
|
||||
/* add it */
|
||||
pos_error = v_add(pos_error, bsm.gps_pos_bias_random_walk_value, pos_error);
|
||||
/* sum true pos and error reading */
|
||||
static VEC *cur_pos_reading = VNULL;
|
||||
cur_pos_reading = v_resize(cur_pos_reading, AXIS_NB);
|
||||
v_add(bfm.pos_ltp, pos_error, cur_pos_reading);
|
||||
/* store that for later and retrieve a previously stored data */
|
||||
UpdateSensorLatency(time, cur_pos_reading, &bsm.gps_pos_history, BSM_GPS_POS_LATENCY, bsm.gps_pos);
|
||||
|
||||
/* UTM conversion */
|
||||
bsm.gps_pos_utm_north = bsm.gps_pos->ve[AXIS_X] * 100. + BSM_GPS_POS_INITIAL_UTM_EAST;
|
||||
bsm.gps_pos_utm_north = rint(bsm.gps_pos_utm_north);
|
||||
bsm.gps_pos_utm_east = bsm.gps_pos->ve[AXIS_Y] * 100. + BSM_GPS_POS_INITIAL_UTM_NORTH;
|
||||
bsm.gps_pos_utm_east = rint(bsm.gps_pos_utm_east);
|
||||
bsm.gps_pos_utm_alt = bsm.gps_pos->ve[AXIS_Z] * 100. + BSM_GPS_POS_INITIAL_UTM_ALT;
|
||||
bsm.gps_pos_utm_alt = rint(bsm.gps_pos_utm_alt);
|
||||
|
||||
/* LLA conversion */
|
||||
|
||||
#if 0
|
||||
#define LAT0 40.
|
||||
#define LON0 1.
|
||||
#define GROUND_ALT 180.
|
||||
|
||||
bsm.gps_pos_lla.lat = (bsm.gps_pos->ve[AXIS_X] * 9e-6 + LAT0) * 1e7;
|
||||
bsm.gps_pos_lla.lat = rint(bsm.gps_pos_lla.lat);
|
||||
bsm.gps_pos_lla.lon = (bsm.gps_pos->ve[AXIS_Y] * 9e-6 + LON0) * 1e7;
|
||||
bsm.gps_pos_lla.lon = rint(bsm.gps_pos_lla.lon);
|
||||
bsm.gps_pos_lla.alt = (bsm.gps_pos->ve[AXIS_Z] + GROUND_ALT)* 100.;
|
||||
bsm.gps_pos_lla.alt = rint(bsm.gps_pos_lla.alt);
|
||||
#endif
|
||||
|
||||
/* ECEF Conversion */
|
||||
struct NedCoor_d pos_ned = {bsm.gps_pos->ve[AXIS_X], bsm.gps_pos->ve[AXIS_Y], bsm.gps_pos->ve[AXIS_Z]};
|
||||
ecef_of_ned_point_d(&bsm.gps_pos_ecef, &bsm.gps_ltp_def, &pos_ned);
|
||||
VECT3_SMUL(bsm.gps_pos_ecef, bsm.gps_pos_ecef, (double)1e2);
|
||||
struct NedCoor_d speed_ned = {bsm.gps_speed->ve[AXIS_X], bsm.gps_speed->ve[AXIS_Y], bsm.gps_speed->ve[AXIS_Z]};
|
||||
ecef_of_ned_vect_d(&bsm.gps_speed_ecef, &bsm.gps_ltp_def , &speed_ned);
|
||||
VECT3_SMUL(bsm.gps_speed_ecef, bsm.gps_speed_ecef, (double)1e2);
|
||||
|
||||
bsm.gps_next_update += BSM_GPS_DT;
|
||||
bsm.gps_available = TRUE;
|
||||
|
||||
}
|
||||
|
||||
@@ -1,102 +0,0 @@
|
||||
#include "booz_sensors_model.h"
|
||||
|
||||
#include BSM_PARAMS
|
||||
#include "booz_sensors_model_utils.h"
|
||||
#include "booz_flight_model.h"
|
||||
|
||||
bool_t booz_sensors_model_gyro_available() {
|
||||
if (bsm.gyro_available) {
|
||||
bsm.gyro_available = FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
void booz_sensors_model_gyro_init(double time) {
|
||||
|
||||
bsm.gyro = v_get(AXIS_NB);
|
||||
bsm.gyro->ve[AXIS_P] = 0.;
|
||||
bsm.gyro->ve[AXIS_Q] = 0.;
|
||||
bsm.gyro->ve[AXIS_R] = 0.;
|
||||
bsm.gyro_resolution = BSM_GYRO_RESOLUTION;
|
||||
|
||||
bsm.gyro_sensitivity = m_get(AXIS_NB, AXIS_NB);
|
||||
m_zero(bsm.gyro_sensitivity);
|
||||
bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = BSM_GYRO_SENSITIVITY_PP;
|
||||
bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = BSM_GYRO_SENSITIVITY_QQ;
|
||||
bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = BSM_GYRO_SENSITIVITY_RR;
|
||||
|
||||
bsm.gyro_neutral = v_get(AXIS_NB);
|
||||
bsm.gyro_neutral->ve[AXIS_P] = BSM_GYRO_NEUTRAL_P;
|
||||
bsm.gyro_neutral->ve[AXIS_Q] = BSM_GYRO_NEUTRAL_Q;
|
||||
bsm.gyro_neutral->ve[AXIS_R] = BSM_GYRO_NEUTRAL_R;
|
||||
|
||||
bsm.gyro_noise_std_dev = v_get(AXIS_NB);
|
||||
bsm.gyro_noise_std_dev->ve[AXIS_P] = BSM_GYRO_NOISE_STD_DEV_P;
|
||||
bsm.gyro_noise_std_dev->ve[AXIS_Q] = BSM_GYRO_NOISE_STD_DEV_Q;
|
||||
bsm.gyro_noise_std_dev->ve[AXIS_R] = BSM_GYRO_NOISE_STD_DEV_R;
|
||||
|
||||
bsm.gyro_bias_initial = v_get(AXIS_NB);
|
||||
bsm.gyro_bias_initial->ve[AXIS_P] = BSM_GYRO_BIAS_INITIAL_P;
|
||||
bsm.gyro_bias_initial->ve[AXIS_Q] = BSM_GYRO_BIAS_INITIAL_Q;
|
||||
bsm.gyro_bias_initial->ve[AXIS_R] = BSM_GYRO_BIAS_INITIAL_R;
|
||||
|
||||
bsm.gyro_bias_random_walk_std_dev = v_get(AXIS_NB);
|
||||
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_P] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P;
|
||||
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_Q] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q;
|
||||
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_R] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R;
|
||||
|
||||
bsm.gyro_bias_random_walk_value = v_get(AXIS_NB);
|
||||
bsm.gyro_bias_random_walk_value->ve[AXIS_P] = bsm.gyro_bias_initial->ve[AXIS_P];
|
||||
bsm.gyro_bias_random_walk_value->ve[AXIS_Q] = bsm.gyro_bias_initial->ve[AXIS_Q];
|
||||
bsm.gyro_bias_random_walk_value->ve[AXIS_R] = bsm.gyro_bias_initial->ve[AXIS_R];
|
||||
|
||||
bsm.gyro_next_update = time;
|
||||
bsm.gyro_available = FALSE;
|
||||
|
||||
}
|
||||
|
||||
void booz_sensors_model_gyro_run( double time ) {
|
||||
if (time < bsm.gyro_next_update)
|
||||
return;
|
||||
|
||||
/* rotate to IMU frame */
|
||||
/* convert to imu frame */
|
||||
static VEC* rate_imu = VNULL;
|
||||
rate_imu = v_resize(rate_imu, AXIS_NB);
|
||||
mv_mlt(bsm.body_to_imu, bfm.ang_rate_body, rate_imu);
|
||||
/* compute gyros readings */
|
||||
bsm.gyro = mv_mlt(bsm.gyro_sensitivity, rate_imu, bsm.gyro);
|
||||
bsm.gyro = v_add(bsm.gyro, bsm.gyro_neutral, bsm.gyro);
|
||||
|
||||
/* compute gyro error readings */
|
||||
static VEC *gyro_error = VNULL;
|
||||
gyro_error = v_resize(gyro_error, AXIS_NB);
|
||||
gyro_error = v_zero(gyro_error);
|
||||
/* add a gaussian noise */
|
||||
gyro_error = v_add_gaussian_noise(gyro_error, bsm.gyro_noise_std_dev, gyro_error);
|
||||
/* update random walk bias */
|
||||
bsm.gyro_bias_random_walk_value =
|
||||
v_update_random_walk(bsm.gyro_bias_random_walk_value,
|
||||
bsm.gyro_bias_random_walk_std_dev, BSM_GYRO_DT,
|
||||
bsm.gyro_bias_random_walk_value);
|
||||
gyro_error = v_add(gyro_error, bsm.gyro_bias_random_walk_value, gyro_error);
|
||||
/* scale to adc units FIXME : should use full adc gain ? sum ? */
|
||||
gyro_error->ve[AXIS_P] =
|
||||
gyro_error->ve[AXIS_P] * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
|
||||
gyro_error->ve[AXIS_Q] =
|
||||
gyro_error->ve[AXIS_Q] * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
|
||||
gyro_error->ve[AXIS_R] =
|
||||
gyro_error->ve[AXIS_R] * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
|
||||
/* add per gyro error reading */
|
||||
bsm.gyro = v_add(bsm.gyro, gyro_error, bsm.gyro);
|
||||
/* round signal to account for adc discretisation */
|
||||
RoundSensor(bsm.gyro);
|
||||
/* saturation */
|
||||
BoundSensor(bsm.gyro, 0, bsm.gyro_resolution);
|
||||
|
||||
bsm.gyro_next_update += BSM_GYRO_DT;
|
||||
bsm.gyro_available = TRUE;
|
||||
}
|
||||
|
||||
|
||||
@@ -1,96 +0,0 @@
|
||||
#include "booz_sensors_model.h"
|
||||
|
||||
#include BSM_PARAMS
|
||||
#include "booz_sensors_model_utils.h"
|
||||
#include "booz_flight_model.h"
|
||||
#include "booz_flight_model_utils.h"
|
||||
|
||||
bool_t booz_sensors_model_mag_available() {
|
||||
if (bsm.mag_available) {
|
||||
bsm.mag_available = FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
|
||||
void booz_sensors_model_mag_init( double time ) {
|
||||
|
||||
bsm.mag = v_get(AXIS_NB);
|
||||
bsm.mag->ve[AXIS_X] = 0.;
|
||||
bsm.mag->ve[AXIS_Y] = 0.;
|
||||
bsm.mag->ve[AXIS_Z] = 0.;
|
||||
// bsm.mag_resolution = BSM_MAG_RESOLUTION;
|
||||
|
||||
bsm.mag_imu_to_sensor = m_get(AXIS_NB, AXIS_NB);
|
||||
VEC* tmp_eulers = v_get(AXIS_NB);
|
||||
tmp_eulers->ve[EULER_PHI] = BSM_MAG_IMU_TO_SENSOR_PHI;
|
||||
tmp_eulers->ve[EULER_THETA] = BSM_MAG_IMU_TO_SENSOR_THETA;
|
||||
tmp_eulers->ve[EULER_PSI] = BSM_MAG_IMU_TO_SENSOR_PSI;
|
||||
dcm_of_eulers (tmp_eulers, bsm.mag_imu_to_sensor );
|
||||
|
||||
bsm.mag_sensitivity = m_get(AXIS_NB, AXIS_NB);
|
||||
m_zero(bsm.mag_sensitivity);
|
||||
bsm.mag_sensitivity->me[AXIS_X][AXIS_X] = BSM_MAG_SENSITIVITY_XX;
|
||||
bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_MAG_SENSITIVITY_YY;
|
||||
bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_MAG_SENSITIVITY_ZZ;
|
||||
|
||||
bsm.mag_neutral = v_get(AXIS_NB);
|
||||
bsm.mag_neutral->ve[AXIS_X] = BSM_MAG_NEUTRAL_X;
|
||||
bsm.mag_neutral->ve[AXIS_Y] = BSM_MAG_NEUTRAL_Y;
|
||||
bsm.mag_neutral->ve[AXIS_Z] = BSM_MAG_NEUTRAL_Z;
|
||||
|
||||
bsm.mag_noise_std_dev = v_get(AXIS_NB);
|
||||
bsm.mag_noise_std_dev->ve[AXIS_X] = BSM_MAG_NOISE_STD_DEV_X;
|
||||
bsm.mag_noise_std_dev->ve[AXIS_Y] = BSM_MAG_NOISE_STD_DEV_Y;
|
||||
bsm.mag_noise_std_dev->ve[AXIS_Z] = BSM_MAG_NOISE_STD_DEV_Z;
|
||||
|
||||
bsm.mag_next_update = time;
|
||||
bsm.mag_available = FALSE;
|
||||
|
||||
}
|
||||
|
||||
void booz_sensors_model_mag_run( double time ) {
|
||||
if (time < bsm.mag_next_update)
|
||||
return;
|
||||
|
||||
/* rotate h to body frame */
|
||||
static VEC *h_body = VNULL;
|
||||
h_body = v_resize(h_body, AXIS_NB);
|
||||
mv_mlt(bfm.dcm, bfm.h_ltp, h_body);
|
||||
/* rotate to imu frame */
|
||||
static VEC *h_imu = VNULL;
|
||||
h_imu = v_resize(h_imu, AXIS_NB);
|
||||
mv_mlt(bsm.body_to_imu, h_body, h_imu);
|
||||
/* rotate to sensor frame */
|
||||
static VEC *h_sensor = VNULL;
|
||||
h_sensor = v_resize(h_sensor, AXIS_NB);
|
||||
mv_mlt(bsm.mag_imu_to_sensor, h_imu, h_sensor);
|
||||
|
||||
mv_mlt(bsm.mag_sensitivity, h_sensor, bsm.mag);
|
||||
v_add(bsm.mag, bsm.mag_neutral, bsm.mag);
|
||||
|
||||
/* compute mag error readings */
|
||||
static VEC *mag_error = VNULL;
|
||||
mag_error = v_resize(mag_error, AXIS_NB);
|
||||
/* add hard iron now ? */
|
||||
mag_error = v_zero(mag_error);
|
||||
/* add a gaussian noise */
|
||||
mag_error = v_add_gaussian_noise(mag_error, bsm.mag_noise_std_dev, mag_error);
|
||||
|
||||
mag_error->ve[AXIS_X] = mag_error->ve[AXIS_X] * bsm.mag_sensitivity->me[AXIS_X][AXIS_X];
|
||||
mag_error->ve[AXIS_Y] = mag_error->ve[AXIS_Y] * bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y];
|
||||
mag_error->ve[AXIS_Z] = mag_error->ve[AXIS_Z] * bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z];
|
||||
|
||||
/* add error */
|
||||
v_add(bsm.mag, mag_error, bsm.mag);
|
||||
|
||||
// printf("h body %f %f %f\n", h_body->ve[AXIS_X], h_body->ve[AXIS_Y], h_body->ve[AXIS_Z]);
|
||||
// printf("mag %f %f %f\n", bsm.mag->ve[AXIS_X], bsm.mag->ve[AXIS_Y], bsm.mag->ve[AXIS_Z]);
|
||||
/* round signal to account for adc discretisation */
|
||||
RoundSensor(bsm.mag);
|
||||
|
||||
bsm.mag_next_update += BSM_MAG_DT;
|
||||
bsm.mag_available = TRUE;
|
||||
}
|
||||
|
||||
@@ -1,145 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2008 Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef BOOZ_SENSORS_MODEL_PARAMS_H
|
||||
#define BOOZ_SENSORS_MODEL_PARAMS_H
|
||||
|
||||
/*
|
||||
* Accelerometer
|
||||
*/
|
||||
#define BSM_ACCEL_RESOLUTION (65536)
|
||||
/* ms-2 */
|
||||
#define BSM_ACCEL_SENSITIVITY_XX 1250.
|
||||
#define BSM_ACCEL_SENSITIVITY_YY 1250.
|
||||
#define BSM_ACCEL_SENSITIVITY_ZZ 1250.
|
||||
#define BSM_ACCEL_NEUTRAL_X 32768
|
||||
#define BSM_ACCEL_NEUTRAL_Y 32768
|
||||
#define BSM_ACCEL_NEUTRAL_Z 32768
|
||||
/* m2s-4 */
|
||||
//#define BSM_ACCEL_NOISE_STD_DEV_X 0
|
||||
//#define BSM_ACCEL_NOISE_STD_DEV_Y 0
|
||||
//#define BSM_ACCEL_NOISE_STD_DEV_Z 0
|
||||
|
||||
#define BSM_ACCEL_NOISE_STD_DEV_X 5e-1
|
||||
#define BSM_ACCEL_NOISE_STD_DEV_Y 5e-1
|
||||
#define BSM_ACCEL_NOISE_STD_DEV_Z 5e-1
|
||||
|
||||
/* ms-2 */
|
||||
#define BSM_ACCEL_BIAS_X 0
|
||||
#define BSM_ACCEL_BIAS_Y 0
|
||||
#define BSM_ACCEL_BIAS_Z 0
|
||||
/* s */
|
||||
#define BSM_ACCEL_DT (1./1000.)
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Gyrometer
|
||||
*/
|
||||
#define BSM_GYRO_RESOLUTION 65536
|
||||
|
||||
#define BSM_GYRO_SENSITIVITY_PP (-4541.3261)
|
||||
#define BSM_GYRO_SENSITIVITY_QQ (-4651.1628)
|
||||
#define BSM_GYRO_SENSITIVITY_RR ( 4752.8517)
|
||||
|
||||
#define BSM_GYRO_NEUTRAL_P 32768
|
||||
#define BSM_GYRO_NEUTRAL_Q 32768
|
||||
#define BSM_GYRO_NEUTRAL_R 32768
|
||||
|
||||
#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.5)
|
||||
#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.)
|
||||
#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.)
|
||||
|
||||
#define BSM_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0)
|
||||
#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg( .0)
|
||||
#define BSM_GYRO_BIAS_INITIAL_R RadOfDeg( .0)
|
||||
|
||||
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5)
|
||||
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.)
|
||||
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.)
|
||||
|
||||
#define BSM_GYRO_DT (1./1000.)
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Magnetometer
|
||||
*/
|
||||
#define BSM_MAG_RESOLUTION 4096
|
||||
|
||||
#define BSM_MAG_SENSITIVITY_XX (680)
|
||||
#define BSM_MAG_SENSITIVITY_YY (680)
|
||||
#define BSM_MAG_SENSITIVITY_ZZ (680)
|
||||
|
||||
#define BSM_MAG_NEUTRAL_X 0
|
||||
#define BSM_MAG_NEUTRAL_Y 0
|
||||
#define BSM_MAG_NEUTRAL_Z 0
|
||||
|
||||
#define BSM_MAG_NOISE_STD_DEV_X 0
|
||||
#define BSM_MAG_NOISE_STD_DEV_Y 0
|
||||
#define BSM_MAG_NOISE_STD_DEV_Z 0
|
||||
|
||||
//#define BSM_MAG_NOISE_STD_DEV_X 2e-2
|
||||
//#define BSM_MAG_NOISE_STD_DEV_Y 2e-2
|
||||
//#define BSM_MAG_NOISE_STD_DEV_Z 2e-2
|
||||
|
||||
#define BSM_MAG_DT (1./1000.)
|
||||
|
||||
|
||||
/*
|
||||
* Range meter
|
||||
*/
|
||||
#define BSM_RANGEMETER_RESOLUTION (1024)
|
||||
#define BSM_RANGEMETER_SENSITIVITY (1024. / 12.)
|
||||
#define BSM_RANGEMETER_MAX_RANGE (6. * BSM_RANGEMETER_SENSITIVITY)
|
||||
#define BSM_RANGEMETER_DT (1./20.)
|
||||
|
||||
|
||||
/*
|
||||
* Barometer
|
||||
*/
|
||||
#define BSM_BARO_QNH 900.
|
||||
#define BSM_BARO_SENSITIVITY 10.
|
||||
#define BSM_BARO_DT (1./10.)
|
||||
|
||||
|
||||
/*
|
||||
* GPS
|
||||
*/
|
||||
#define BSM_GPS_SPEED_NOISE_STD_DEV 1e-1
|
||||
#define BSM_GPS_SPEED_LATENCY 0.25
|
||||
|
||||
#define BSM_GPS_POS_NOISE_STD_DEV 3e-1
|
||||
#define BSM_GPS_POS_BIAS_INITIAL_X 1e-1
|
||||
#define BSM_GPS_POS_BIAS_INITIAL_Y -1e-1
|
||||
#define BSM_GPS_POS_BIAS_INITIAL_Z -5e-1
|
||||
#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-1
|
||||
#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-1
|
||||
#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-1
|
||||
#define BSM_GPS_POS_LATENCY 0.25
|
||||
#define BSM_GPS_POS_INITIAL_UTM_EAST 37728000
|
||||
#define BSM_GPS_POS_INITIAL_UTM_NORTH 482464300
|
||||
#define BSM_GPS_POS_INITIAL_UTM_ALT 15200
|
||||
|
||||
#define BSM_GPS_DT (1./4.)
|
||||
|
||||
#endif /* BOOZ_SENSORS_MODEL_PARAMS_H */
|
||||
@@ -1,184 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2008 Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef BOOZ_SENSORS_MODEL_PARAMS_H
|
||||
#define BOOZ_SENSORS_MODEL_PARAMS_H
|
||||
|
||||
#include "generated/airframe.h"
|
||||
|
||||
|
||||
#define BSM_BODY_TO_IMU_PHI RadOfDeg(4.)
|
||||
#define BSM_BODY_TO_IMU_THETA RadOfDeg(3.)
|
||||
//#define BSM_BODY_TO_IMU_PHI RadOfDeg(0.)
|
||||
//#define BSM_BODY_TO_IMU_THETA RadOfDeg(0.)
|
||||
#define BSM_BODY_TO_IMU_PSI RadOfDeg(0.)
|
||||
|
||||
/*
|
||||
* Accelerometer
|
||||
*/
|
||||
#define BSM_ACCEL_RESOLUTION (65536)
|
||||
/* ms-2 */
|
||||
/* aka 2^10/ACCEL_X_SENS */
|
||||
#define BSM_ACCEL_SENSITIVITY_XX -408.92695
|
||||
#define BSM_ACCEL_SENSITIVITY_YY -412.69325
|
||||
#define BSM_ACCEL_SENSITIVITY_ZZ -407.32522
|
||||
#define BSM_ACCEL_NEUTRAL_X 32081
|
||||
#define BSM_ACCEL_NEUTRAL_Y 33738
|
||||
#define BSM_ACCEL_NEUTRAL_Z 32441
|
||||
/* m2s-4 */
|
||||
//#define BSM_ACCEL_NOISE_STD_DEV_X 0
|
||||
//#define BSM_ACCEL_NOISE_STD_DEV_Y 0
|
||||
//#define BSM_ACCEL_NOISE_STD_DEV_Z 0
|
||||
|
||||
#define BSM_ACCEL_NOISE_STD_DEV_X 1.e-1
|
||||
#define BSM_ACCEL_NOISE_STD_DEV_Y 1.e-1
|
||||
#define BSM_ACCEL_NOISE_STD_DEV_Z 1.1e-1
|
||||
|
||||
/* ms-2 */
|
||||
#define BSM_ACCEL_BIAS_X 0
|
||||
#define BSM_ACCEL_BIAS_Y 0
|
||||
#define BSM_ACCEL_BIAS_Z 0
|
||||
/* s */
|
||||
#define BSM_ACCEL_DT (1./512.)
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Gyrometer
|
||||
*/
|
||||
#define BSM_GYRO_RESOLUTION 65536
|
||||
|
||||
/* 2^12/GYRO_X_SENS */
|
||||
#define BSM_GYRO_SENSITIVITY_PP ( 4055.)
|
||||
#define BSM_GYRO_SENSITIVITY_QQ (-4055.)
|
||||
#define BSM_GYRO_SENSITIVITY_RR (-4055.)
|
||||
|
||||
#define BSM_GYRO_NEUTRAL_P 33924
|
||||
#define BSM_GYRO_NEUTRAL_Q 33417
|
||||
#define BSM_GYRO_NEUTRAL_R 32809
|
||||
|
||||
//#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.)
|
||||
//#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.)
|
||||
//#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.)
|
||||
|
||||
#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.5)
|
||||
#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.5)
|
||||
#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.5)
|
||||
|
||||
#define BSM_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0)
|
||||
#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg( .0)
|
||||
#define BSM_GYRO_BIAS_INITIAL_R RadOfDeg( .0)
|
||||
|
||||
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.)
|
||||
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.)
|
||||
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.)
|
||||
|
||||
#define BSM_GYRO_DT (1./512.)
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Magnetometer
|
||||
*/
|
||||
//#define BSM_MAG_RESOLUTION 65536
|
||||
|
||||
#define BSM_MAG_IMU_TO_SENSOR_PHI 0.
|
||||
#define BSM_MAG_IMU_TO_SENSOR_THETA 0.
|
||||
#define BSM_MAG_IMU_TO_SENSOR_PSI RadOfDeg(45.)
|
||||
|
||||
#define BSM_MAG_SENSITIVITY_XX (1.*(1<<11)/-4.94075530)
|
||||
#define BSM_MAG_SENSITIVITY_YY (1.*(1<<11)/ 5.10207664)
|
||||
#define BSM_MAG_SENSITIVITY_ZZ (1.*(1<<11)/-4.90788848)
|
||||
|
||||
#define BSM_MAG_NEUTRAL_X 2358
|
||||
#define BSM_MAG_NEUTRAL_Y 2362
|
||||
#define BSM_MAG_NEUTRAL_Z 2119
|
||||
|
||||
//#define BSM_MAG_NOISE_STD_DEV_X 0
|
||||
//#define BSM_MAG_NOISE_STD_DEV_Y 0
|
||||
//#define BSM_MAG_NOISE_STD_DEV_Z 0
|
||||
|
||||
#define BSM_MAG_NOISE_STD_DEV_X 2e-3
|
||||
#define BSM_MAG_NOISE_STD_DEV_Y 2e-3
|
||||
#define BSM_MAG_NOISE_STD_DEV_Z 2e-3
|
||||
|
||||
#define BSM_MAG_DT (1./100.)
|
||||
|
||||
|
||||
/*
|
||||
* Range meter
|
||||
*/
|
||||
#define BSM_RANGEMETER_RESOLUTION (1024)
|
||||
#define BSM_RANGEMETER_SENSITIVITY (1024. / 12.)
|
||||
#define BSM_RANGEMETER_MAX_RANGE (6. * BSM_RANGEMETER_SENSITIVITY)
|
||||
#define BSM_RANGEMETER_DT (1./20.)
|
||||
|
||||
|
||||
/*
|
||||
* Barometer
|
||||
*/
|
||||
/* m */
|
||||
/* aka 2^8/INS_BARO_SENS */
|
||||
#define BSM_BARO_QNH 900.
|
||||
#define BSM_BARO_SENSITIVITY 17.066667
|
||||
#define BSM_BARO_DT (1./100.)
|
||||
#define BSM_BARO_NOISE_STD_DEV 5.e-2
|
||||
|
||||
/*
|
||||
* GPS
|
||||
*/
|
||||
|
||||
#ifdef GPS_PERFECT
|
||||
|
||||
#define BSM_GPS_SPEED_NOISE_STD_DEV 0.
|
||||
#define BSM_GPS_SPEED_LATENCY 0.
|
||||
#define BSM_GPS_POS_NOISE_STD_DEV 0.001
|
||||
#define BSM_GPS_POS_BIAS_INITIAL_X 0.
|
||||
#define BSM_GPS_POS_BIAS_INITIAL_Y 0.
|
||||
#define BSM_GPS_POS_BIAS_INITIAL_Z 0.
|
||||
#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0.
|
||||
#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0.
|
||||
#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0.
|
||||
#define BSM_GPS_POS_LATENCY 0.
|
||||
|
||||
#else
|
||||
|
||||
#define BSM_GPS_SPEED_NOISE_STD_DEV 1e-1
|
||||
#define BSM_GPS_SPEED_LATENCY 0.25
|
||||
#define BSM_GPS_POS_NOISE_STD_DEV 1e-1
|
||||
#define BSM_GPS_POS_BIAS_INITIAL_X 0e-1
|
||||
#define BSM_GPS_POS_BIAS_INITIAL_Y -0e-1
|
||||
#define BSM_GPS_POS_BIAS_INITIAL_Z -0e-1
|
||||
#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3
|
||||
#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3
|
||||
#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3
|
||||
#define BSM_GPS_POS_LATENCY 0.25
|
||||
|
||||
#endif /* GPS_PERFECT */
|
||||
|
||||
#define BSM_GPS_POS_INITIAL_UTM_EAST 37728000
|
||||
#define BSM_GPS_POS_INITIAL_UTM_NORTH 482464300
|
||||
#define BSM_GPS_POS_INITIAL_UTM_ALT 15200
|
||||
|
||||
#define BSM_GPS_DT (1./4.)
|
||||
|
||||
#endif /* BOOZ_SENSORS_MODEL_PARAMS_H */
|
||||
@@ -1,47 +0,0 @@
|
||||
#include "booz_sensors_model.h"
|
||||
|
||||
#include BSM_PARAMS
|
||||
#include "booz_sensors_model_utils.h"
|
||||
#include "booz_flight_model.h"
|
||||
|
||||
bool_t booz_sensors_model_rangemeter_available() {
|
||||
if (bsm.rangemeter_available) {
|
||||
bsm.rangemeter_available = FALSE;
|
||||
return TRUE;
|
||||
}
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
|
||||
void booz_sensors_model_rangemeter_init(double time) {
|
||||
|
||||
bsm.rangemeter = 0.;
|
||||
|
||||
bsm.rangemeter_next_update = time;
|
||||
bsm.rangemeter_available = FALSE;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void booz_sensors_model_rangemeter_run( double time ) {
|
||||
if (time < bsm.rangemeter_next_update)
|
||||
return;
|
||||
|
||||
/* compute dist from ground */
|
||||
double dz = bfm.pos_ltp->ve[AXIS_Z];
|
||||
if (dz > 0.) dz = 0.;
|
||||
double dx = dz * tan(bfm.eulers->ve[EULER_THETA]);
|
||||
double dy = dz * tan(bfm.eulers->ve[EULER_PHI]);
|
||||
double dist = sqrt( dx*dx + dy*dy + dz*dz);
|
||||
dist *= BSM_RANGEMETER_SENSITIVITY;
|
||||
/* add gaussian noise */
|
||||
|
||||
if (dist > BSM_RANGEMETER_MAX_RANGE)
|
||||
dist = BSM_RANGEMETER_MAX_RANGE;
|
||||
dist = rint(dist);
|
||||
bsm.rangemeter = dist;
|
||||
|
||||
bsm.rangemeter_next_update += BSM_RANGEMETER_DT;
|
||||
bsm.rangemeter_available = TRUE;
|
||||
|
||||
}
|
||||
@@ -1,94 +0,0 @@
|
||||
#include "booz_sensors_model_utils.h"
|
||||
|
||||
#include "6dof.h"
|
||||
#include <math.h>
|
||||
|
||||
void UpdateSensorLatency(double time, VEC* cur_reading, GSList **history,
|
||||
double latency, VEC* sensor_reading) {
|
||||
/* add new reading */
|
||||
struct BoozDatedSensor* cur_read = g_new(struct BoozDatedSensor, 1);
|
||||
cur_read->time = time;
|
||||
cur_read->value = v_get(AXIS_NB);
|
||||
v_copy(cur_reading, cur_read->value);
|
||||
*history = g_slist_prepend(*history, cur_read);
|
||||
/* remove old readings */
|
||||
GSList* last = g_slist_last(*history);
|
||||
while (last &&
|
||||
((struct BoozDatedSensor*)last->data)->time < time - latency) {
|
||||
*history = g_slist_remove_link(*history, last);
|
||||
v_free(((struct BoozDatedSensor*)last->data)->value);
|
||||
g_free((struct BoozDatedSensor*)last->data);
|
||||
g_slist_free(last);
|
||||
last = g_slist_last(*history);
|
||||
}
|
||||
/* update sensor */
|
||||
v_copy(((struct BoozDatedSensor*)last->data)->value, sensor_reading);
|
||||
}
|
||||
|
||||
#define THAU 5.
|
||||
VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out) {
|
||||
|
||||
static VEC *drw = VNULL;
|
||||
drw = v_resize(drw, AXIS_NB);
|
||||
v_get_gaussian_noise(std_dev, drw);
|
||||
|
||||
static VEC *tmp = VNULL;
|
||||
tmp = v_resize(tmp, AXIS_NB);
|
||||
v_copy(in, tmp);
|
||||
sv_mlt((-1./THAU), tmp, tmp);
|
||||
v_add(drw, tmp, drw);
|
||||
sv_mlt(dt, drw, drw);
|
||||
v_add(drw, in, out);
|
||||
return out;
|
||||
}
|
||||
|
||||
VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out) {
|
||||
static VEC *tmp = VNULL;
|
||||
tmp = v_resize(tmp, in->dim);
|
||||
unsigned int i;
|
||||
for (i=0; i<in->dim; i++)
|
||||
tmp->ve[i] = get_gaussian_noise() * std_dev->ve[i];
|
||||
v_add(in, tmp, out);
|
||||
return out;
|
||||
}
|
||||
|
||||
VEC* v_get_gaussian_noise(VEC* std_dev, VEC* out) {
|
||||
|
||||
unsigned int i;
|
||||
for (i=0; i<out->dim; i++)
|
||||
out->ve[i] = get_gaussian_noise() * std_dev->ve[i];
|
||||
return out;
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
http://www.taygeta.com/random/gaussian.html
|
||||
*/
|
||||
|
||||
#include "booz_r250.h"
|
||||
|
||||
double get_gaussian_noise(void) {
|
||||
|
||||
double x1;
|
||||
static int nb_call = 0;
|
||||
static double x2, w;
|
||||
if (nb_call==0) r250_init(0);
|
||||
nb_call++;
|
||||
if (nb_call%2) {
|
||||
do {
|
||||
x1 = 2.0 * dr250() - 1.0;
|
||||
x2 = 2.0 * dr250() - 1.0;
|
||||
w = x1 * x1 + x2 * x2;
|
||||
} while ( w >= 1.0 );
|
||||
|
||||
w = sqrt( (-2.0 * log( w ) ) / w );
|
||||
return x1 * w;
|
||||
}
|
||||
else
|
||||
return x2 * w;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,36 +0,0 @@
|
||||
#ifndef BOOZ_SENSORS_MODEL_UTILS_H
|
||||
#define BOOZ_SENSORS_MODEL_UTILS_H
|
||||
|
||||
#include <matrix.h>
|
||||
#include <glib.h>
|
||||
|
||||
struct BoozDatedSensor {
|
||||
VEC* value;
|
||||
double time;
|
||||
};
|
||||
|
||||
extern void UpdateSensorLatency(double time, VEC* cur_reading, GSList **history,
|
||||
double latency, VEC* sensor_reading);
|
||||
extern VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out);
|
||||
extern VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out);
|
||||
extern VEC* v_get_gaussian_noise(VEC* std_dev, VEC* out);
|
||||
extern double get_gaussian_noise();
|
||||
|
||||
#define RoundSensor(_sensor) { \
|
||||
_sensor->ve[AXIS_X] = rint(_sensor->ve[AXIS_X]); \
|
||||
_sensor->ve[AXIS_Y] = rint(_sensor->ve[AXIS_Y]); \
|
||||
_sensor->ve[AXIS_Z] = rint(_sensor->ve[AXIS_Z]); \
|
||||
}
|
||||
|
||||
#define BoundSensor(_sensor, _min, _max) { \
|
||||
if ( _sensor->ve[AXIS_X] < _min) _sensor->ve[AXIS_X] = _min; \
|
||||
if ( _sensor->ve[AXIS_X] > _max) _sensor->ve[AXIS_X] = _max; \
|
||||
if ( _sensor->ve[AXIS_Y] < _min) _sensor->ve[AXIS_Y] = _min; \
|
||||
if ( _sensor->ve[AXIS_Y] > _max) _sensor->ve[AXIS_Y] = _max; \
|
||||
if ( _sensor->ve[AXIS_Z] < _min) _sensor->ve[AXIS_Z] = _min; \
|
||||
if ( _sensor->ve[AXIS_Z] > _max) _sensor->ve[AXIS_Z] = _max; \
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif /* BOOZ_SENSORS_MODEL_UTILS_H */
|
||||
@@ -1,76 +0,0 @@
|
||||
#include "booz_wind_model.h"
|
||||
|
||||
#include "6dof.h"
|
||||
#include "booz_flight_model_utils.h"
|
||||
|
||||
#if 0
|
||||
static void booz_wind_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot);
|
||||
#endif
|
||||
|
||||
struct BoozWindModel bwm;
|
||||
|
||||
#define BWM_STATE_SIZE 6
|
||||
|
||||
#define BWM_WM 1.5
|
||||
#define BWM_ZETA 5e-5
|
||||
#define BWM_STD_DEV 3.3
|
||||
#define BWM_MAX_X 3.
|
||||
#define BWM_MAX_Y 3.
|
||||
#define BWM_MAX_Z 3.
|
||||
|
||||
void booz_wind_model_init( void ) {
|
||||
|
||||
bwm.velocity = v_get(AXIS_NB);
|
||||
v_zero(bwm.velocity);
|
||||
bwm.velocity->ve[AXIS_X] = INIT_WIND_X;
|
||||
bwm.velocity->ve[AXIS_Y] = INIT_WIND_Y;
|
||||
bwm.velocity->ve[AXIS_Z] = INIT_WIND_Z;
|
||||
|
||||
|
||||
bwm.state = v_get(BWM_STATE_SIZE);
|
||||
v_zero(bwm.state);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void booz_wind_model_run( double dt __attribute__ ((unused))) {
|
||||
|
||||
|
||||
#if 0
|
||||
static VEC *u = VNULL;
|
||||
u = v_resize(u, AXIS_NB);
|
||||
u = v_rand(u);
|
||||
static VEC *one = VNULL;
|
||||
one = v_resize(one, AXIS_NB);
|
||||
one = v_ones(one);
|
||||
u = v_mltadd(one, u, -2., u);
|
||||
u = sv_mlt((BWM_STD_DEV * BWM_STD_DEV), u, u);
|
||||
|
||||
rk4(booz_wind_model_get_derivatives, bwm.state, u, dt);
|
||||
|
||||
bwm.velocity->ve[AXIS_X] = bwm.state->ve[0] * BWM_WM * BWM_WM * BWM_MAX_X;
|
||||
bwm.velocity->ve[AXIS_Y] = bwm.state->ve[2] * BWM_WM * BWM_WM * BWM_MAX_Y;
|
||||
bwm.velocity->ve[AXIS_Z] = bwm.state->ve[4] * BWM_WM * BWM_WM * BWM_MAX_Z;
|
||||
#endif
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
#if 0
|
||||
|
||||
static void booz_wind_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot) {
|
||||
|
||||
Xdot->ve[0] = X->ve[1];
|
||||
Xdot->ve[1] = -2. * BWM_ZETA * BWM_WM * X->ve[1] -
|
||||
BWM_WM * BWM_WM * X->ve[0] + u->ve[AXIS_X];
|
||||
Xdot->ve[2] = X->ve[3];
|
||||
Xdot->ve[3] = -2. * BWM_ZETA * BWM_WM * X->ve[3] -
|
||||
BWM_WM * BWM_WM * X->ve[2] + u->ve[AXIS_Y];
|
||||
Xdot->ve[4] = X->ve[5];
|
||||
Xdot->ve[5] = -2. * BWM_ZETA * BWM_WM * X->ve[5] -
|
||||
BWM_WM * BWM_WM * X->ve[4] + u->ve[AXIS_Z];
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -1,21 +0,0 @@
|
||||
#ifndef BOOZ_WIND_MODEL_H
|
||||
#define BOOZ_WIND_MODEL_H
|
||||
|
||||
#include <matrix.h>
|
||||
|
||||
struct BoozWindModel {
|
||||
|
||||
/* velocity in earth tp frame */
|
||||
VEC* velocity;
|
||||
|
||||
/* internal state */
|
||||
VEC* state;
|
||||
|
||||
};
|
||||
|
||||
extern struct BoozWindModel bwm;
|
||||
|
||||
extern void booz_wind_model_init( void );
|
||||
extern void booz_wind_model_run( double dt );
|
||||
|
||||
#endif /* BOOZ_WIND_MODEL_H */
|
||||
@@ -1,76 +0,0 @@
|
||||
#
|
||||
# NPS
|
||||
#
|
||||
#JSBSIM = /usr/local
|
||||
|
||||
Q=@
|
||||
|
||||
#CC = g++
|
||||
#CFLAGS = -Wall -I$(JSBSIM)/include/JSBSim -I../include
|
||||
#LDFLAGS = -L$(JSBSIM)/lib -lJSBSim
|
||||
#CFLAGS += -I/usr/include/meschach -I/usr/local/include/
|
||||
#LDFLAGS += -lmeschach -L/usr/lib
|
||||
#CFLAGS += `pkg-config glib-2.0 --cflags`
|
||||
#LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lglibivy -lpcre
|
||||
|
||||
#test1: nps_test1.c nps_jsbsim.c
|
||||
# $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
|
||||
|
||||
#test2: nps_test2.c
|
||||
# $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
|
||||
|
||||
#test_nps_fdm: test_nps_fdm.c nps_fdm.c
|
||||
# $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
|
||||
|
||||
|
||||
JSBSIM = /home/violato/enac/programs/install_jsbsim
|
||||
|
||||
CFLAGS = -Wall \
|
||||
-I.. \
|
||||
-I../../../var/BOOZ2_A1 \
|
||||
-I../../airborne \
|
||||
-I../../include \
|
||||
-I$(JSBSIM)/include/JSBSim \
|
||||
`pkg-config glib-2.0 --cflags` \
|
||||
|
||||
LDFLAGS = -lm \
|
||||
-lglibivy \
|
||||
-L$(JSBSIM)/lib -lJSBSim \
|
||||
`pkg-config glib-2.0 --libs` \
|
||||
|
||||
SIMDIR = ..
|
||||
|
||||
#
|
||||
#
|
||||
#
|
||||
|
||||
TEST_FDM_SRCS = nps_test_fdm.c \
|
||||
$(SIMDIR)/nps_fdm_jsbsim.c \
|
||||
|
||||
|
||||
nps_test_fdm : $(TEST_FDM_SRCS)
|
||||
g++ $(CFLAGS) -o $@ $^ $(LDFLAGS)
|
||||
|
||||
|
||||
#
|
||||
#
|
||||
#
|
||||
|
||||
TEST_SENSORS_SRCS = test_sensors.c \
|
||||
$(SIMDIR)/booz_flight_model.c \
|
||||
$(SIMDIR)/booz_flight_model_utils.c \
|
||||
$(SIMDIR)/booz_sensors_model.c \
|
||||
$(SIMDIR)/booz_sensors_model_utils.c \
|
||||
$(SIMDIR)/booz_sensors_model_accel.c \
|
||||
$(SIMDIR)/booz_sensors_model_gyro.c \
|
||||
$(SIMDIR)/booz_sensors_model_mag.c \
|
||||
$(SIMDIR)/booz_sensors_model_rangemeter.c \
|
||||
$(SIMDIR)/booz_sensors_model_baro.c \
|
||||
$(SIMDIR)/booz_sensors_model_gps.c \
|
||||
|
||||
test_sensors : $(TEST_SENSORS_SRCS)
|
||||
gcc $(CFLAGS) -o $@ $^ $(LDFLAGS)
|
||||
|
||||
clean:
|
||||
$(Q)rm -f *.o
|
||||
|
||||
@@ -1,24 +0,0 @@
|
||||
#include "booz_flight_model.h"
|
||||
|
||||
|
||||
#define DT_FDM (1./250.)
|
||||
#define NB_ITER 100
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
double actuators_values[] = {0.6, 0.6, 0.6, 0.6};
|
||||
double time = 0.;
|
||||
|
||||
booz_flight_model_init();
|
||||
|
||||
bfm.on_ground = FALSE;
|
||||
|
||||
int i;
|
||||
for (i=0; i<NB_ITER; i++) {
|
||||
booz_flight_model_run(DT_FDM, actuators_values);
|
||||
time += DT_FDM;
|
||||
printf("%f \t %f\n", time, bfm.state->ve[BFMS_Z]);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,32 +0,0 @@
|
||||
#include "booz_flight_model.h"
|
||||
#include "booz_sensors_model.h"
|
||||
|
||||
|
||||
#define DT_FDM (1./250.)
|
||||
#define NB_ITER 100
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
double actuators_values[] = {0.6, 0.6, 0.6, 0.6};
|
||||
double time = 0.;
|
||||
|
||||
booz_flight_model_init();
|
||||
booz_sensors_model_init(time);
|
||||
|
||||
bfm.on_ground = FALSE;
|
||||
|
||||
int i;
|
||||
for (i=0; i<NB_ITER; i++) {
|
||||
booz_flight_model_run(DT_FDM, actuators_values);
|
||||
time += DT_FDM;
|
||||
booz_sensors_model_run(time);
|
||||
// if (booz_sensors_model_baro_available())
|
||||
// printf("%f \t %f\n", time, bsm.baro);
|
||||
// if (booz_sensors_model_gps_available())
|
||||
// printf("%f \t %f \t %f\n", time, bsm.gps_pos->ve[AXIS_Z], bsm.gps_speed->ve[AXIS_Z]);
|
||||
if (booz_sensors_model_accel_available())
|
||||
printf("%f \t %f \t %f \t %f\n", time, bsm.accel->ve[AXIS_X], bsm.accel->ve[AXIS_Y], bsm.accel->ve[AXIS_Z]);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,98 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2008 Gautier Hattenberger
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include "sim_ac_jsbsim.h"
|
||||
#include "firmwares/fixedwing/main_ap.h"
|
||||
#include "firmwares/fixedwing/main_fbw.h"
|
||||
|
||||
using namespace JSBSim;
|
||||
|
||||
//static void sim_gps_feed_data(void);
|
||||
//static void sim_ir_feed_data(void);
|
||||
|
||||
|
||||
void airborne_run_one_step(void) {
|
||||
// SEE sim_run_one_step
|
||||
}
|
||||
|
||||
void sim_autopilot_init(void) {
|
||||
init_fbw();
|
||||
init_ap();
|
||||
}
|
||||
|
||||
void autopilot_periodic_task(void) {
|
||||
periodic_task_ap();
|
||||
periodic_task_fbw();
|
||||
}
|
||||
|
||||
void autopilot_event_task(void) {
|
||||
event_task_ap();
|
||||
event_task_fbw();
|
||||
}
|
||||
|
||||
void copy_inputs_to_jsbsim(FGFDMExec* FDMExec) {
|
||||
|
||||
FGPropertyManager* cur_node;
|
||||
double cur_value;
|
||||
char buf[64];
|
||||
const char* state[] = {"front_motor",
|
||||
"back_motor",
|
||||
"right_motor",
|
||||
"left_motor"};
|
||||
|
||||
for (int i=0; i<4; i++) {
|
||||
sprintf(buf,"fcs/%s",state[i]);
|
||||
FDMExec->GetPropertyManager()->SetDouble(buf,0.5);
|
||||
cur_node = FDMExec->GetPropertyManager()->GetNode(buf);
|
||||
cur_value = cur_node->getDoubleValue();
|
||||
cout << state[i] << " " << cur_value << endl;
|
||||
}
|
||||
}
|
||||
|
||||
void copy_outputs_from_jsbsim(FGFDMExec* FDMExec) {
|
||||
|
||||
FGPropertyManager* cur_node;
|
||||
double cur_value, factor=1;
|
||||
char buf[64];
|
||||
const char* state[] = {"sim-time-sec",
|
||||
"lat-gc-deg","long-gc-deg","h-sl-ft",
|
||||
"u-fps","v-fps","w-fps",
|
||||
"theta-deg","phi-deg","psi-true-deg",
|
||||
"p-rad_sec","q-rad_sec","r-rad_sec"};
|
||||
int i=0;
|
||||
|
||||
cur_node = FDMExec->GetPropertyManager()->GetNode("sim-time-sec");
|
||||
cur_value = cur_node->getDoubleValue();
|
||||
cout << state[i] << cur_value << endl;
|
||||
|
||||
for (i=1; i<12+1; i++) {
|
||||
sprintf(buf,"ic/%s",state[i]);
|
||||
if (strstr(state[i],"rad_")!=NULL) factor=RAD2DEG;
|
||||
if (strstr(state[i],"fps")!=NULL || strstr(state[i],"ft")!=NULL) factor=FT2M;
|
||||
cur_node = FDMExec->GetPropertyManager()->GetNode(buf);
|
||||
cur_value = factor*(cur_node->getDoubleValue());
|
||||
cout << state[i] << " " << cur_value << endl;
|
||||
factor = 1;
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user