Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 17:49:49 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

simulator using jsbsim (ground reaction problem)

Browse Source
This commit is contained in:
Gautier Hattenberger
2009-06-19 08:34:42 +00:00
parent f571a56248
commit d4577d8693
7 changed files with 348 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/sim/jsbsim_gps.c
+78
View File
@@ -0,0 +1,78 @@
/* OCaml binding to link the simulator to autopilot functions. */
#include <assert.h>
#include <math.h>
#include <inttypes.h>
/** From airborne/autopilot/ */
#include "airframe.h"
#include "flight_plan.h"
#include "autopilot.h"
#include "gps.h"
#include "estimator.h"
#include "latlong.h"
#include "common_nav.h"
uint8_t gps_mode;
uint32_t gps_itow; /* ms */
int32_t gps_alt; /* cm */
uint16_t gps_gspeed; /* cm/s */
int16_t gps_climb; /* cm/s */
int16_t gps_course; /* decideg */
int32_t gps_utm_east, gps_utm_north;
uint8_t gps_utm_zone;
int32_t gps_lat, gps_lon;
struct svinfo gps_svinfos[GPS_NB_CHANNELS];
uint8_t gps_nb_channels = 0;
uint16_t gps_PDOP;
uint32_t gps_Pacc, gps_Sacc;
uint8_t gps_numSV;
uint16_t gps_reset;
void sim_use_gps_pos(double lat, double lon, double alt, double course, double gspeed, double climb, double time) {
gps_mode = 3; // Mode 3D
gps_course = DegOfRad(course) * 10.;
gps_alt = alt * 100.;
gps_gspeed = gspeed * 100.;
gps_climb = climb * 100.;
gps_itow = time * 1000.;
gps_lat = DegOfRad(lat)*1e7;
gps_lon = DegOfRad(lon)*1e7;
latlong_utm_of(lat, lon, nav_utm_zone0);
gps_utm_east = latlong_utm_x * 100;
gps_utm_north = latlong_utm_y * 100;
gps_utm_zone = nav_utm_zone0;
}
/** Space vehicle info simulation */
void sim_update_sv(void) {
gps_nb_channels=7;
int i;
static int time;
time++;
for(i = 0; i < gps_nb_channels; i++) {
gps_svinfos[i].svid = 7 + i;
gps_svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45;
gps_svinfos[i].azim = (time/gps_nb_channels + 50 * i) % 360;
gps_svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.;
gps_svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01);
gps_svinfos[i].qi = (int)((time / 1000.) + i) % 8;
}
gps_PDOP = gps_Sacc = gps_Pacc = 500+200*sin(time/100.);
gps_numSV = 7;
gps_verbose_downlink = !launch;
UseGpsPosNoSend(estimator_update_state_gps);
gps_downlink();
}
void ubxsend_cfg_rst(uint16_t a __attribute__ ((unused)), uint8_t b __attribute__ ((unused))) {
return;
}
sw/airborne/sim/jsbsim_hw.c
+35
View File
@@ -0,0 +1,35 @@
/* Definitions and declarations required to compile autopilot code on a
i386 architecture */
#include "jsbsim_hw.h"
#include <stdio.h>
#include <assert.h>
#include <sys/time.h>
#include <sys/stat.h>
#include <time.h>
#include <string.h>
/* Dummy definitions to replace the ones from the files not compiled in the simulator */
//uint8_t ir_estim_mode;
//uint8_t vertical_mode;
//uint8_t inflight_calib_mode;
//bool_t rc_event_1, rc_event_2;
uint8_t gps_nb_ovrn, modem_nb_ovrn, link_fbw_fbw_nb_err, link_fbw_nb_err;
//float alt_roll_pgain;
//float roll_rate_pgain;
//bool_t gpio1_status;
uint16_t adc_generic_val1;
uint16_t adc_generic_val2;
uint16_t ppm_pulses[ PPM_NB_PULSES ];
volatile bool_t ppm_valid;
uint8_t ac_id;
void update_bat(double bat) {
fbw_vsupply_decivolt = (int) (bat * 10.);
}
void adc_generic_init( void ) {}
void adc_generic_periodic( void ) {}
sw/airborne/sim/jsbsim_hw.h
+62
View File
@@ -0,0 +1,62 @@
/*
*
* Copyright (C) 2009 Enac
*
* 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.
*
*/
/** \file jsbsim_hw.h
*/
#ifndef JSBSIM_HW_H
#define JSBSIM_HW_H
#include <inttypes.h>
#include "std.h"
#include "inter_mcu.h"
#include "autopilot.h"
#include "estimator.h"
#include "gps.h"
#include "traffic_info.h"
#include "flight_plan.h"
#include "airframe.h"
#include "settings.h"
#include "nav.h"
#include "fw_h_ctl.h"
#include "fw_v_ctl.h"
#include "infrared.h"
#include "cam.h"
#include "commands.h"
#include "main_ap.h"
#include "ap_downlink.h"
#include "sim_uart.h"
#include "latlong.h"
#include "datalink.h"
#include "adc_generic.h"
#include "ppm.h"
void sim_use_gps_pos(double lat, double lon, double alt, double course, double gspeed, double climb, double time);
void sim_update_sv(void);
void set_ir(double roll, double pitch);
void update_bat(double bat);
#endif
sw/airborne/sim/jsbsim_ir.c
+28
View File
@@ -0,0 +1,28 @@
/**
* \brief Regroup functions to simulate autopilot/infrared.c
*
* Infrared soft simulation.
*/
#include "jsbsim_hw.h"
#include <math.h>
void set_ir(double roll, double pitch) {
double ir_contrast = 150; //FIXME
double roll_sensor = roll + ir_roll_neutral;
double pitch_sensor = pitch + ir_pitch_neutral;
#ifdef INFRARED
ir_roll = sin(roll_sensor) * ir_contrast;
ir_pitch = sin(pitch_sensor) * ir_contrast;
ir_top = cos(roll_sensor) * cos(pitch_sensor) * ir_contrast;
#endif
}
void ir_gain_calib(void) {}
/** Required by infrared.c:ir_init() */
void adc_buf_channel(uint8_t adc_channel __attribute__ ((unused)), struct adc_buf* s __attribute__ ((unused)), uint8_t av_nb_sample __attribute__ ((unused))) {}
sw/simulator/sim_ac_fw.c
+58 -26
View File
@@ -26,12 +26,10 @@
#include "sim_ac_jsbsim.h"
#include "main_ap.h"
#include "main_fbw.h"
#include "jsbsim_hw.h"
using namespace JSBSim;
//static void sim_gps_feed_data(void);
//static void sim_ir_feed_data(void);
void autopilot_init(void) {
init_fbw();
init_ap();
@@ -47,36 +45,70 @@ void autopilot_event_task(void) {
event_task_fbw();
}
void print(FGFDMExec* FDMExec) {
FGPropertyManager* cur_node;
double cur_value, factor=1;
const char* state[] = {"sim-time-sec",
"position/lat-gc-deg","position/long-gc-deg","position/h-sl-meters",/*
"ic/lat-gc-deg","ic/long-gc-deg","ic/h-sl-ft",
"velocities/v-north-fps","velocities/v-east-fps","velocities/v-down-fps","velocities/vg-fps",
"attitude/roll-rad","attitude/pitch-rad","attitude/heading-true-rad",
"velocities/p-rad_sec","velocities/q-rad_sec","velocities/r-rad_sec",*/
"forces/fbx-gear-lbs","forces/fby-gear-lbs","forces/fbz-gear-lbs"};
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<6+1; 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(state[i]);
cur_value = factor*(cur_node->getDoubleValue());
cout << string(state[i]) << " " << cur_value << endl;
factor = 1;
}
}
void copy_inputs_to_jsbsim(FGFDMExec* FDMExec) {
}
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;
double get_value(FGFDMExec* FDMExec, string name) {
return FDMExec->GetPropertyManager()->GetNode(name)->getDoubleValue();
}
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;
#define GPS_PERIOD (1./4.)
void copy_outputs_from_jsbsim(FGFDMExec* FDMExec) {
static double gps_period = 0.;
// copy GPS pos
gps_period += DT;
if (gps_period > GPS_PERIOD) {
double lat = get_value(FDMExec, "position/lat-gc-rad");
double lon = get_value(FDMExec, "position/long-gc-rad");
double alt = get_value(FDMExec, "position/h-sl-meters");
double course = get_value(FDMExec, "attitude/heading-true-rad");
double gspeed = get_value(FDMExec, "velocities/vg-fps") * FT2M;
double climb = get_value(FDMExec, "velocities/v-down-fps") * (-FT2M);
double time = get_value(FDMExec, "sim-time-sec");
sim_use_gps_pos(lat, lon, alt, course, gspeed, climb, time);
sim_update_sv();
gps_period = 0.;
}
print(FDMExec);
// copy IR
double roll = get_value(FDMExec, "attitude/roll-rad");
double pitch = get_value(FDMExec, "attitude/pitch-rad");
set_ir(roll, pitch);
// copy Bat level
double bat = 12.; // get_value(FDMExec, "propulsion/total-fuel-lbs");
update_bat(bat);
}
sw/simulator/sim_ac_jsbsim.c
+86 -55
View File
@@ -24,6 +24,7 @@
#include "sim_ac_jsbsim.h"
#include <stdlib.h>
#include <glib.h>
#include <getopt.h>
@@ -34,16 +35,15 @@
GLOBAL DATA
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
string RootDir = "";
string ICName;
string AircraftName;
string LogOutputName;
JSBSim::FGFDMExec* FDMExec;
static void sim_parse_options(int argc, char** argv);
static void sim_init(void);
static gboolean sim_periodic(gpointer data);
string ivyBus = "127.255.255.255";
static void ivy_transport_init(void);
//static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
// void *user_data __attribute__ ((unused)),
@@ -51,7 +51,7 @@ static void ivy_transport_init(void);
static void sim_init(void) {
jsbsim_init();
// init sensors ? or discribe them in jSBSim
@@ -69,19 +69,21 @@ static gboolean sim_periodic(gpointer data __attribute__ ((unused))) {
copy_inputs_to_jsbsim(FDMExec);
/* run JSBSim flight model */
bool result = FDMExec->Run();
//sim_time += DT;
bool result = true;
//if (check_crash_jsbsim(FDMExec)) {
result = FDMExec->Run();
//}
/* check if still flying */
result = check_crash_jsbsim(FDMExec);
//result = check_crash_jsbsim(FDMExec);
/* read outputs from model state (and display ?) */
copy_outputs_from_jsbsim(FDMExec);
/* run the airborne code */
airborne_run_one_step();
// airborne_run_one_step();
autopilot_event_task();
autopilot_periodic_task();
return result;
}
@@ -89,11 +91,7 @@ static gboolean sim_periodic(gpointer data __attribute__ ((unused))) {
int main ( int argc, char** argv) {
ICName = "reset00";
AircraftName = "quad";
LogOutputName = "out.csv";
//sim_parse_options(argc, argv);
sim_parse_options(argc, argv);
sim_init();
@@ -103,21 +101,20 @@ int main ( int argc, char** argv) {
g_main_loop_run(ml);
//delete FDMExec;
delete FDMExec;
return 0;
}
static void ivy_transport_init(void) {
IvyInit ("Paparazzi sim " + AC_ID, "READY", NULL, NULL, NULL, NULL);
IvyStart("127.255.255.255");
IvyStart(ivyBus.c_str());
}
void print_help() {
//cout << "Usage: " << argv[0] << " [options]" << endl;
cout << "Usage: simsitl [options]" << endl;
cout << " Options :" << endl;
cout << " -a <aircraft name>" << endl;
cout << " -a <aircraft name>\tUnused, only for compatibility" << endl;
cout << " -b <Ivy bus>\tdefault is 127.255.255.255:2010" << endl;
cout << " -fg <flight gear client address>" << endl;
cout << " -h --help show this help" << endl;
@@ -125,26 +122,34 @@ void print_help() {
static void sim_parse_options(int argc, char** argv) {
if (argc == 1) {
print_help();
exit(0);
}
if (argc == 1) return;
int i;
for (i = 1; i < argc; ++i) {
string argument = string(argv[i]);
if (argument == "--help" || argument == "-h") {
print_help();
exit(0);
} else if (argument == "-a") {
} else if (argument == "-b") {
} else if (argument == "-fg") {
} else {
}
else if (argument == "-a") {
// Compatibility with ocaml
i++;
}
else if (argument == "-boot") {
// Compatibility with ocaml
}
else if (argument == "-norc") {
// Compatibility with ocaml
}
else if (argument == "-b") {
ivyBus = string(argv[++i]);
}
else if (argument == "-fg") {
// TODO
i++;
}
else {
cerr << "Unknown argument" << endl;
print_help();
exit(0);
@@ -155,30 +160,37 @@ static void sim_parse_options(int argc, char** argv) {
void jsbsim_init(void) {
JSBSim::FGState* State;
bool result;
// *** SET UP JSBSIM *** //
char* root = getenv("PAPARAZZI_HOME");
if (root == NULL) {
cerr << "PAPARAZZI_HOME is not defined" << endl;
exit(0);
}
string pprzRoot = string(root);
#ifdef JSBSIM_MODEL
AircraftName = JSBSIM_MODEL;
#endif
#ifdef JSBSIM_INIT
ICName = JSBSIM_INIT;
#endif
FDMExec = new JSBSim::FGFDMExec();
State = FDMExec->GetState();
State->Setsim_time(0.);
State->Setdt(DT);
cout << "Simulation elapsed time: " << FDMExec->GetSimTime() << endl;
/* Set simulation time step */
FDMExec->GetState()->Setsim_time(0.);
FDMExec->GetState()->Setdt(DT);
cout << "Simulation delta " << FDMExec->GetDeltaT() << endl;
#ifdef JSBSIM_ROOT_DIR
RootDir = JSBSIM_ROOT_DIR;
#endif
FDMExec->DisableOutput();
FDMExec->SetDebugLevel(0); // No DEBUG messages
if (!AircraftName.empty()) {
FDMExec->DisableOutput();
FDMExec->SetDebugLevel(0); // No DEBUG messages
if ( ! FDMExec->LoadModel( RootDir + "aircraft",
RootDir + "engine",
RootDir + "systems",
if ( ! FDMExec->LoadModel( pprzRoot + "/conf/simulator",
pprzRoot + "/conf/simulator",
pprzRoot + "/conf/simulator",
AircraftName)){
cerr << " JSBSim could not be started" << endl << endl;
delete FDMExec;
@@ -186,11 +198,27 @@ void jsbsim_init(void) {
}
JSBSim::FGInitialCondition *IC = FDMExec->GetIC();
if ( ! IC->Load(ICName)) {
delete FDMExec;
cerr << "Initialization unsuccessful" << endl;
exit(-1);
if(!ICName.empty()) {
if (!IC->Load(ICName)) {
delete FDMExec;
cerr << "Initialization from file unsuccessful" << endl;
exit(-1);
}
}
else {
// Use flight plan initial conditions
IC->SetLatitudeDegIC(NAV_LAT0 / 1e7);
IC->SetLongitudeDegIC(NAV_LON0 / 1e7);
IC->SetAltitudeFtIC((GROUND_ALT+1) / FT2M);
IC->SetTerrainAltitudeFtIC(GROUND_ALT / FT2M);
IC->SetPsiDegIC(QFU);
IC->SetVgroundFpsIC(15./FT2M);
if (!FDMExec->RunIC()) {
cerr << "Initialization from flight plan unsuccessful" << endl;
exit(-1);
}
}
//FDMExec->GetGroundReactions()->InitModel();
} else {
cerr << " No Aircraft given" << endl << endl;
@@ -198,8 +226,11 @@ void jsbsim_init(void) {
exit(-1);
}
result = FDMExec->Run();
if (result) cout << "Made Initial Run" << endl;
//if (FDMExec->Run()) cout << "Made Initial Run" << endl;
//else {
// cerr << "Initial run failed " << endl;
// exit(-1);
//}
}
@@ -208,9 +239,9 @@ bool check_crash_jsbsim(JSBSim::FGFDMExec* FDMExec) {
JSBSim::FGPropertyManager* cur_node;
double cur_value;
cur_node = FDMExec->GetPropertyManager()->GetNode("ic/h-agl-ft");
cur_node = FDMExec->GetPropertyManager()->GetNode("position/h-agl-ft");
cur_value = cur_node->getDoubleValue();
if (cur_value>0) return true;
if (cur_value>=0) return true;
else return false;
}
sw/simulator/sim_ac_jsbsim.h
+1
View File
@@ -29,6 +29,7 @@
#include <FGJSBBase.h>
#include <FGState.h>
#include "std.h"
#include "airframe.h"
#include "flight_plan.h"