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

sim getting closer

Browse Source
This commit is contained in:
Antoine Drouin
2009-07-21 03:01:18 +00:00
parent 24ad57ca46
commit 4e10ff2dc5
24 changed files with 215 additions and 182 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/subsystems/booz2_imu_b2v1.makefile
+5
View File
@@ -48,6 +48,7 @@
# imu Booz2 v1
ap.CFLAGS += -DBOOZ_IMU_TYPE_H=\"imu/booz_imu_b2.h\"
ap.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601
ap.CFLAGS += -DSSP_VIC_SLOT=9
ap.srcs += $(SRC_BOOZ)/booz_imu.c \
$(SRC_BOOZ)/imu/booz_imu_b2.c \
@@ -67,6 +68,7 @@ ap.CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2
#
sim.CFLAGS += -DBOOZ_IMU_TYPE_H=\"imu/booz_imu_b2.h\"
sim.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601
sim.srcs += $(SRC_BOOZ)/booz_imu.c \
$(SRC_BOOZ)/imu/booz_imu_b2.c \
$(SRC_BOOZ_SIM)/imu/booz_imu_b2_arch.c
@@ -75,3 +77,6 @@ sim.srcs += $(SRC_BOOZ)/booz_imu.c \
sim.srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c \
$(SRC_BOOZ_SIM)/peripherals/booz_max1168_arch.c
sim.CFLAGS += -DUSE_AMI601
sim.srcs += $(SRC_BOOZ)/peripherals/booz_ami601.c
sim.CFLAGS += -DUSE_I2C1
conf/messages.xml
+2 -2
View File
@@ -461,11 +461,11 @@
<field name="blmc_nb_err" type="uint8"/>
<field name="rc_status" type="uint8" values="OK|LOST|REALLY_LOST"/>
<field name="gps_status" type="uint8" values="NO_FIX|NA|NA|3Dfix"/>
<field name="ap_mode" type="uint8" values="FAILSAFE|KILL|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_Z_HOLD|NAV"/>
<field name="ap_mode" type="uint8" values="FAILSAFE|KILL|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|ATTITUDE_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_CLIMB|HOVER_Z_HOLD|NAV"/>
<field name="ap_in_flight" type="uint8" values="ON_GROUND|IN_FLIGHT"/>
<field name="ap_motors_on" type="uint8" values="MOTORS_OFF|MOTORS_ON"/>
<field name="ap_h_mode" type="uint8" values="KILL|RATE|ATTITUDE|HOVER|NAV"/>
<field name="ap_v_mode" type="uint8" values="KILL|RC_DIRECT|RC_CLIMB|CLIMB|HOVER"/>
<field name="ap_v_mode" type="uint8" values="KILL|RC_DIRECT|RC_CLIMB|CLIMB|HOVER|NAV"/>
<field name="vsupply" type="uint8" unit="decivolt"/>
<field name="cpu_time" type="uint16" unit="s"></field>
</message>
conf/simulator/jsbsim/aircraft/reset00.xml
+2 -1
View File
@@ -8,7 +8,8 @@
<psi unit="DEG"> 0.0 </psi>
<latitude unit="DEG"> 37.6136 </latitude>
<longitude unit="DEG">-122.3569 </longitude>
<altitude unit="M"> 0.11 </altitude>
<!-- <altitude unit="M"> 0.11 </altitude> -->
<altitude unit="M"> 0.9 </altitude>
<winddir unit="DEG"> 0.0 </winddir>
<vwind unit="FT/SEC"> 0.0 </vwind>
</initialize>
conf/telemetry/telemetry_booz2.xml
+3 -2
View File
@@ -16,8 +16,9 @@
</mode>
<mode name="ppm">
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="BOOZ2_RADIO_CONTROL" period="0.5"/>
<message name="BOOZ_STATUS" period="1"/>
</mode>
sw/airborne/booz/arch/sim/imu/booz_imu_b2_arch.c
+28
View File
@@ -23,6 +23,8 @@
#include "booz_imu.h"
#include "airframe.h"
void booz_imu_b2_arch_init(void) {
}
@@ -31,3 +33,29 @@ void booz_imu_periodic(void) {
}
#include "nps_sensors.h"
void booz_imu_feed_gyro_accel(void) {
booz_max1168_values[IMU_GYRO_P_CHAN] = sensors.gyro.value.x;
booz_max1168_values[IMU_GYRO_Q_CHAN] = sensors.gyro.value.y;
booz_max1168_values[IMU_GYRO_R_CHAN] = sensors.gyro.value.z;
booz_max1168_values[IMU_ACCEL_X_CHAN] = sensors.accel.value.x;
booz_max1168_values[IMU_ACCEL_Y_CHAN] = sensors.accel.value.y;
booz_max1168_values[IMU_ACCEL_Z_CHAN] = sensors.accel.value.z;
booz_max1168_status = STA_MAX1168_DATA_AVAILABLE;
}
void booz_imu_feed_mag(void) {
#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
ms2001_values[IMU_MAG_X_CHAN] = sensors.mag.value.x;
ms2001_values[IMU_MAG_Y_CHAN] = sensors.mag.value.y;
ms2001_values[IMU_MAG_Z_CHAN] = sensors.mag.value.z;
ms2001_status = MS2001_DATA_AVAILABLE;
#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
ami601_values[IMU_MAG_X_CHAN] = sensors.mag.value.x;
ami601_values[IMU_MAG_Y_CHAN] = sensors.mag.value.y;
ami601_values[IMU_MAG_Z_CHAN] = sensors.mag.value.z;
ami601_status = AMI601_DATA_AVAILABLE;
#endif
}
sw/airborne/booz/arch/sim/imu/booz_imu_b2_arch.h
+2 -9
View File
@@ -30,15 +30,8 @@
#define BOOZ2_IMU_B2_ARCH_H
#define booz_imu_feed_gyro_accel() { \
booz2_max1168_values[IMU_GYRO_P_CHAN] = bsm.gyro->ve[AXIS_P]; \
booz2_max1168_values[IMU_GYRO_Q_CHAN] = bsm.gyro->ve[AXIS_Q]; \
booz2_max1168_values[IMU_GYRO_R_CHAN] = bsm.gyro->ve[AXIS_R]; \
booz2_max1168_values[IMU_ACCEL_X_CHAN] = bsm.accel->ve[AXIS_X]; \
booz2_max1168_values[IMU_ACCEL_Y_CHAN] = bsm.accel->ve[AXIS_Y]; \
booz2_max1168_values[IMU_ACCEL_Z_CHAN] = bsm.accel->ve[AXIS_Z]; \
booz2_max1168_status = STA_MAX1168_DATA_AVAILABLE; \
}
extern void booz_imu_feed_gyro_accel(void);
extern void booz_imu_feed_mag(void);
#endif /* BOOZ2_IMU_B2_HW_H */
sw/airborne/booz/arch/sim/radio_control/booz_radio_control_ppm_arch.c
+15
View File
@@ -23,6 +23,21 @@
#include "booz_radio_control.h"
#include "nps_radio_control.h"
void booz_radio_control_ppm_arch_init ( void ) {
}
#define PPM_OF_NPS(_nps, _neutral, _min, _max) \
((_nps) >= 0 ? (_neutral) + (_nps) * ((_max)-(_neutral)) : (_neutral) + (_nps) * ((_neutral)- (_min)))
void booz_radio_control_feed(void) {
booz_radio_control_ppm_pulses[RADIO_CONTROL_ROLL] = PPM_OF_NPS(nps_radio_control.roll, 1500, 950, 2050);
booz_radio_control_ppm_pulses[RADIO_CONTROL_PITCH] = PPM_OF_NPS(nps_radio_control.pitch, 1500, 2050, 950);
booz_radio_control_ppm_pulses[RADIO_CONTROL_YAW] = PPM_OF_NPS(nps_radio_control.yaw, 1500, 2050, 950);
booz_radio_control_ppm_pulses[RADIO_CONTROL_THROTTLE] = PPM_OF_NPS(nps_radio_control.throttle, 1223, 1223, 2050);
booz_radio_control_ppm_pulses[RADIO_CONTROL_MODE] = PPM_OF_NPS(nps_radio_control.mode, 1500, 2050, 940);
booz_radio_control_ppm_frame_available = TRUE;
}
sw/airborne/booz/arch/sim/radio_control/booz_radio_control_ppm_arch.h
+1 -1
View File
@@ -24,6 +24,6 @@
#ifndef BOOZ_RADIO_CONTROL_PPM_ARCH_H
#define BOOZ_RADIO_CONTROL_PPM_ARCH_H
extern void booz_radio_control_feed(void);
#endif /* BOOZ_RADIO_CONTROL_PPM_ARCH_H */
sw/airborne/booz/booz2_analog_baro.c
+19
View File
@@ -50,6 +50,25 @@ void booz2_analog_baro_init( void ) {
#endif
}
/* decrement offset until adc reading is over a threshold */
void booz2_analog_baro_calibrate(void) {
if (booz2_analog_baro_value_filtered < 850 && booz2_analog_baro_offset >= 1) {
if (booz2_analog_baro_value_filtered == 0)
booz2_analog_baro_offset -= 15;
else
booz2_analog_baro_offset--;
Booz2AnalogSetDAC(booz2_analog_baro_offset);
#ifdef BOOZ2_ANALOG_BARO_LED
LED_TOGGLE(BOOZ2_ANALOG_BARO_LED);
#endif
}
else {
booz2_analog_baro_status = BOOZ2_ANALOG_BARO_RUNNING;
#ifdef BOOZ2_ANALOG_BARO_LED
LED_ON(BOOZ2_ANALOG_BARO_LED);
#endif
}
}
sw/airborne/booz/booz2_analog_baro.h
+6 -24
View File
@@ -40,6 +40,8 @@ extern uint16_t booz2_analog_baro_value;
extern uint16_t booz2_analog_baro_value_filtered;
extern bool_t booz2_analog_baro_data_available;
extern void booz2_analog_baro_calibrate(void);
#define Booz2AnalogBaroEvent(_handler) { \
if (booz2_analog_baro_data_available) { \
_handler(); \
@@ -55,32 +57,12 @@ extern bool_t booz2_analog_baro_data_available;
#define Booz2BaroISRHandler(_val) { \
booz2_analog_baro_value = _val; \
booz2_analog_baro_value_filtered = (3*booz2_analog_baro_value_filtered + booz2_analog_baro_value)/4; \
if (booz2_analog_baro_status == BOOZ2_ANALOG_BARO_UNINIT) \
Booz2AnalogBaroCalibrate(); \
if (booz2_analog_baro_status == BOOZ2_ANALOG_BARO_UNINIT) { \
RunOnceEvery(10, { booz2_analog_baro_calibrate();}); \
} \
/* else */ \
booz2_analog_baro_data_available = TRUE; \
}
}
/* decrease offset until adc reading is over a threshold */
#define Booz2AnalogBaroCalibrate() { \
RunOnceEvery(10, { \
if (booz2_analog_baro_value_filtered < 850 && booz2_analog_baro_offset >= 1) { \
if (booz2_analog_baro_value_filtered == 0) \
booz2_analog_baro_offset -= 15; \
else \
booz2_analog_baro_offset--; \
Booz2AnalogSetDAC(booz2_analog_baro_offset); \
/* #ifdef BOOZ2_ANALOG_BARO_LED */ \
LED_TOGGLE(BOOZ2_ANALOG_BARO_LED); \
/* #endif */ \
} \
else { \
booz2_analog_baro_status = BOOZ2_ANALOG_BARO_RUNNING; \
/* #ifdef BOOZ2_ANALOG_BARO_LED */ \
LED_ON(BOOZ2_ANALOG_BARO_LED); \
/* #endif */ \
} \
}); \
} \
#endif /* BOOZ2_ANALOG_BARO_H */
sw/airborne/booz/booz2_telemetry.h
+8
View File
@@ -80,6 +80,14 @@
#ifdef USE_RADIO_CONTROL
#define PERIODIC_SEND_RC() DOWNLINK_SEND_RC(RADIO_CONTROL_NB_CHANNEL, radio_control.values)
#define PERIODIC_SEND_BOOZ2_RADIO_CONTROL() { \
DOWNLINK_SEND_BOOZ2_RADIO_CONTROL(&radio_control.values[RADIO_CONTROL_ROLL], \
&radio_control.values[RADIO_CONTROL_PITCH], \
&radio_control.values[RADIO_CONTROL_YAW], \
&radio_control.values[RADIO_CONTROL_THROTTLE], \
&radio_control.values[RADIO_CONTROL_MODE], \
&radio_control.status);}
#else
#define PERIODIC_SEND_RC() {}
#endif
sw/airborne/booz/imu/booz_imu_b2.h
+5 -5
View File
@@ -46,11 +46,11 @@
#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
#include "peripherals/booz_ami601.h"
#define BoozImuMagEvent(_mag_handler) { \
if (booz_ami601_status == STA_AMI601_DATA_AVAILABLE) { \
booz_imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \
booz_imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \
booz_imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \
booz_ami601_status = STA_AMI601_IDLE; \
if (ami601_status == AMI601_DATA_AVAILABLE) { \
booz_imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \
booz_imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \
booz_imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \
ami601_status = AMI601_IDLE; \
_mag_handler(); \
} \
}
sw/airborne/booz/peripherals/booz_ami601.c
+1 -1
View File
@@ -1,4 +1,4 @@
#include "booz_ami601.h"
#include "peripherals/booz_ami601.h"
uint8_t ami601_foo1;
uint8_t ami601_foo2;
sw/airborne/booz/peripherals/booz_ami601.h
+2 -2
View File
@@ -29,8 +29,8 @@ extern volatile uint32_t ami601_nb_err;
#ifdef SITL
#define AMI601Event(_handler) { \
ami601_status = AMI601_DATA_AVAILABLE; \
_handler(); \
if (ami601_status == AMI601_DATA_AVAILABLE) \
_handler(); \
}
#else
#define AMI601Event(_handler) { \
sw/simulator/nps_autopilot_booz.c
+43 -14
View File
@@ -3,6 +3,11 @@
#include "booz2_main.h"
#include "nps_sensors.h"
#include "nps_radio_control.h"
#include "booz_radio_control.h"
#include "booz_imu.h"
#include "booz2_analog_baro.h"
#include "actuators.h"
struct NpsAutopilot autopilot;
@@ -15,33 +20,57 @@ void nps_autopilot_init(void) {
}
#include <stdio.h>
void nps_autopilot_run_step(double time __attribute__ ((unused))) {
#if 0
if (nps_radio_control_available(time)) {
booz2_radio_control_feed();
booz_radio_control_feed();
booz2_main_event();
}
#endif
if (nps_sensors_gyro_available()) {
// booz2_imu_feed_data();
// booz2_main_event();
booz_imu_feed_gyro_accel();
booz2_main_event();
}
if (nps_sensors_mag_available()) {
// printf("in mag %f %f %f\n", sensors.mag.value.x, sensors.mag.value.y, sensors.mag.value.z);
booz_imu_feed_mag();
booz2_main_event();
}
if (nps_sensors_baro_available()) {
Booz2BaroISRHandler(sensors.baro.value);
booz2_main_event();
}
booz2_main_periodic();
// double hover = 0.25;
double hover = 0.2493;
// double hover = 0.23;
// double hover = 0.;
// if (time > 20) hover = 0.25;
autopilot.commands[SERVO_FRONT] = hover;
autopilot.commands[SERVO_BACK] = hover;
autopilot.commands[SERVO_RIGHT] = hover;
autopilot.commands[SERVO_LEFT] = hover;
if (time < 5) {
double hover = 0.25;
//double hover = 0.2493;
// double hover = 0.23;
// double hover = 0.;
// if (time > 20) hover = 0.25;
autopilot.commands[SERVO_FRONT] = hover;
autopilot.commands[SERVO_BACK] = hover;
autopilot.commands[SERVO_RIGHT] = hover;
autopilot.commands[SERVO_LEFT] = hover;
}
else {
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;
autopilot.commands[SERVO_FRONT] = (double)ut_front / SUPERVISION_MAX_MOTOR;
autopilot.commands[SERVO_BACK] = (double)ut_back / SUPERVISION_MAX_MOTOR;
autopilot.commands[SERVO_RIGHT] = (double)ut_right / SUPERVISION_MAX_MOTOR;
autopilot.commands[SERVO_LEFT] = (double)ut_left / SUPERVISION_MAX_MOTOR;
}
printf("%f %f %f %f\n", autopilot.commands[SERVO_FRONT], autopilot.commands[SERVO_BACK],
autopilot.commands[SERVO_RIGHT], autopilot.commands[SERVO_LEFT]);
}
sw/simulator/nps_fdm_jsbsim.c
+10 -1
View File
@@ -119,7 +119,7 @@ static void test123(LlaCoor_d* fdm_lla, FGPropagate* propagate) {
fdm_lla->lat = propagate->GetLatitude();
fdm_lla->lon = propagate->GetLongitude();
// FIXME
// fdm_lla->alt = MetersOfFeet(propagate->Geth());
fdm_lla->alt = MetersOfFeet(propagate->GetDistanceAGL());
}
@@ -176,6 +176,15 @@ static void init_ltp(void) {
ltp_def_from_ecef_d(&ltpdef_d,&fdm.ecef_pos);
ltpdef_copy(&ltpdef, &ltpdef_d);
fdm.ltp_g.x = 0.;
fdm.ltp_g.y = 0.;
fdm.ltp_g.z = 9.81;
fdm.ltp_h.x = 1.;
fdm.ltp_h.y = 0.;
fdm.ltp_h.z = 1.;
}
static void jsbsimloc_to_loc(EcefCoor_d* fdm_location, FGLocation* jsb_location){
sw/simulator/nps_ivy.c
+19 -3
View File
@@ -9,18 +9,34 @@
#include "nps_autopilot.h"
#include "nps_fdm.h"
static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
extern void nps_ivy_init(void) {
void nps_ivy_init(void) {
const char* agent_name = AIRFRAME_NAME"_NPS";
const char* ready_msg = AIRFRAME_NAME"_NPS Ready";
IvyInit(agent_name, ready_msg, NULL, NULL, NULL, NULL);
//IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)");
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");
}
extern void nps_ivy_display(void) {
#include "settings.h"
#include "dl_protocol.h"
#include "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);
}
void nps_ivy_display(void) {
/*
sw/simulator/nps_jsbsim.c
-95
View File
@@ -1,95 +0,0 @@
#include "nps_jsbsim.h"
#include <FGState.h>
#include <math/FGColumnVector3.h>
#include <math/FGMatrix33.h>
#include <math/FGQuaternion.h>
static void cp_fgvec_to_vec(JSBSim::FGColumnVector3 jsbvec, VEC* vec);
static void cp_fgquat_to_quat(JSBSim::FGQuaternion jsbquat, VEC* quat);
static void cp_fgvec_to_vec(JSBSim::FGColumnVector3 jsbvec, VEC* vec) {
while(int i=0 < 3) {vec->ve[i] = jsbvec.Entry(i); }
}
static void cp_fgquat_to_quat(JSBSim::FGQuaternion jsbquat, VEC* quat) {
while(int i=0 < 4) {quat->ve[i] = jsbquat.Entry(i); }
}
int JSBInit(double sim_dt) {
bool result = false;
JSBSim::FGFDMExec* fdmex;
fdmex = new JSBSim::FGFDMExec();
fdmex->DisableOutput();
fdmex->SetDebugLevel(0);
JSBSim::FGState State(fdmex);
State.Setdt(sim_dt);
if ( ! fdmex->LoadModel( RootDir + "aircraft",
RootDir + "engine",
RootDir + "systems",
AircraftName,
1)) {
cerr << "JSBSim could not load model" << endl << endl;
delete fdmex;
exit(-1);
}
JSBSim::FGInitialCondition* IC = fdmex->GetIC();
if ( ! IC->Load(ResetName)) {
delete fdmex;
cerr << "JSBSim could not initialize state" << endl;
exit(-1);
}
result = fdmex->RunIC();
return(result);
}
void nps_jsbsim_feed_inputs(JSBSim::FGFDMExec* fdmex, struct NpsFdmState* fdm_state) {
fdmex->SetPropertyValue("/fdm/jsbsim/fcs/control_force", fdm_state->vehicle.dummy.f_input);
}
void nps_jsbsim_fetch_state(JSBSim::FGFDMExec* fdmex, struct NpsFdmState* fdm_state) {
double radius;
double lon;
double lat;
radius = FT2M*(fdmex->GetPropagate()->GetRadius()); // meters
lon = fdmex->GetPropagate()->GetLongitude(); // radians
lat = fdmex->GetPropagate()->GetLatitude(); // radians
JSBSim::FGColumnVector3 ecef_pos (radius*cos(lon)*cos(lat),
radius*sin(lon)*cos(lat),radius*sin(lat));
JSBSim::FGColumnVector3 bdy_vel;
JSBSim::FGColumnVector3 bdy_accel;
JSBSim::FGMatrix33 Tb2ec;
JSBSim::FGColumnVector3 ecef_vel;
JSBSim::FGColumnVector3 ecef_accel;
bdy_vel = fdmex->GetPropagate()->GetUVW();
bdy_accel = fdmex->GetPropagate()->GetUVWdot();
Tb2ec = fdmex->GetPropagate()->GetTb2ec();
ecef_vel = (Tb2ec*bdy_vel)*FT2M;
ecef_accel = (Tb2ec*bdy_accel)*FT2M;
fdm_state->on_ground = fdmex->GetPropertyValue("gear/unit[0]/WOW");
cp_fgvec_to_vec(ecef_pos, fdm_state->ecef_pos);
cp_fgvec_to_vec(ecef_vel, fdm_state->ecef_vel);
cp_fgvec_to_vec(ecef_accel, fdm_state->ecef_accel);
cp_fgquat_to_quat(fdmex->GetPropagate()->GetQuaternion(), fdm_state->ltp_to_body_quat);
cp_fgvec_to_vec(fdmex->GetPropagate()->GetPQR(), fdm_state->ltp_body_rate);
cp_fgvec_to_vec(fdmex->GetPropagate()->GetPQRdot(), fdm_state->ltp_body_accel);
}
sw/simulator/nps_jsbsim.h
-12
View File
@@ -1,12 +0,0 @@
#ifndef NPS_JSBSIM_H
#define NPS_JSBSIM_H
#include <FGFDMExec.h>
#include "nps_fdm.h"
extern void nps_jsbsim_feed_inputs(JSBSim::FGFDMExec* fdmex, struct NpsFdmState* fdm_state);
extern void nps_jsbsim_fetch_state(JSBSim::FGFDMExec* fdmex, struct NpsFdmState* fdm_state);
extern int JSBInit(double sim_dt);
#endif /* NPS_JSBSIM_H */
sw/simulator/nps_radio_control.c
+27 -5
View File
@@ -8,15 +8,37 @@ void nps_radio_control_init(void) {
nps_radio_control.next_update = 0.;
}
#define MODE_SWITCH_MANUAL -1.0
#define MODE_SWITCH_AUTO1 0.0
#define MODE_SWITCH_AUTO2 1.0
void nps_radio_control_run_script(double time) {
nps_radio_control.roll = 0.;
nps_radio_control.pitch = 0.;
/* starts motors */
if (time < 1.) {
nps_radio_control.yaw = 1.;
nps_radio_control.throttle = 0.;
nps_radio_control.mode = MODE_SWITCH_MANUAL;
}
else if (time < 3.5) {
nps_radio_control.yaw = 0.;
nps_radio_control.throttle = 0.;
nps_radio_control.mode = MODE_SWITCH_MANUAL;
}
else {
nps_radio_control.yaw = 0.;
// nps_radio_control.throttle = 0.99;
// nps_radio_control.mode = MODE_SWITCH_AUTO2;
nps_radio_control.throttle = 0.265;
nps_radio_control.mode = MODE_SWITCH_MANUAL;
}
}
bool_t nps_radio_control_available(double time) {
if (time >= nps_radio_control.next_update) {
nps_radio_control.next_update += RADIO_CONTROL_DT;
nps_radio_control_run_script(time);
return TRUE;
}
return FALSE;
sw/simulator/nps_sensor_accel.c
+8 -2
View File
@@ -21,6 +21,7 @@ void nps_sensor_accel_init(struct NpsSensorAccel* accel, double time) {
accel->data_available = FALSE;
}
//#include <stdio.h>
void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, struct DoubleRMat* body_to_imu) {
@@ -30,18 +31,23 @@ void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, stru
/* transform gravity to body frame */
struct DoubleVect3 g_body;
FLOAT_QUAT_VMULT(g_body, fdm.ltp_to_body_quat, fdm.ltp_g);
// printf(" g_body %f %f %f\n", g_body.x, g_body.y, g_body.z);
// printf(" accel_fdm %f %f %f\n", fdm.body_ecef_accel.x, fdm.body_ecef_accel.y, fdm.body_ecef_accel.z);
/* substract gravity to acceleration in body frame */
struct DoubleVect3 accelero_body;
FLOAT_VECT3_DIFF(accelero_body, fdm.body_ecef_accel, g_body);
// printf(" accelero body %f %f %f\n", accelero_body.x, accelero_body.y, accelero_body.z);
/* transform to imu frame */
struct DoubleVect3 accelero_imu;
MAT33_VECT3_MUL(accelero_imu, *body_to_imu, accelero_body );
/* compute accelero readings */
MAT33_VECT3_MUL(accel->value, accel->sensitivity, accelero_imu);
VECT3_ADD(accel->value, accel->neutral);
/* Compute sensor error */
struct DoubleVect3 accelero_error;
/* constant bias */
sw/simulator/nps_sensor_baro.c
+7 -2
View File
@@ -1,10 +1,14 @@
#include "nps_sensor_baro.h"
#include "airframe.h"
#include "std.h"
#include "nps_fdm.h"
#include "nps_random.h"
#include NPS_SENSORS_PARAMS
void nps_sensor_baro_init(struct NpsSensorBaro* baro, double time) {
baro->value = 0.;
baro->next_update = time;
@@ -17,13 +21,14 @@ void nps_sensor_baro_run_step(struct NpsSensorBaro* baro, double time) {
if (time < baro->next_update)
return;
if (time < 12.5)
baro->value = 840;
if (time < 10.)
baro->value = rint(time*90);
else {
double z = fdm.ltpprz_pos.z + get_gaussian_noise()*NPS_BARO_NOISE_STD_DEV;
double baro_reading = NPS_BARO_QNH + z * NPS_BARO_SENSITIVITY;
baro_reading = rint(baro_reading);
baro->value = baro_reading;
Bound(baro->value, 0, 1024);
}
baro->next_update += NPS_BARO_DT;
sw/simulator/nps_sensor_gyro.c
+1
View File
@@ -35,6 +35,7 @@ void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct Do
MAT33_VECT3_MUL(rate_imu, *body_to_imu, *rate_body );
/* compute gyros readings */
MAT33_VECT3_MUL(gyro->value, gyro->sensitivity, rate_imu);
VECT3_ADD(gyro->value, gyro->neutral);
/* compute gyro error readings */
/* round signal to account for adc discretisation */
sw/simulator/nps_sensor_mag.c
+1 -1
View File
@@ -39,7 +39,7 @@ void nps_sensor_mag_run_step(struct NpsSensorMag* mag, double time, struct Doubl
/* compute magnetometer reading */
MAT33_VECT3_MUL(mag->value, mag->sensitivity, h_sensor);
VECT3_ADD(mag->value, mag->neutral);
/* FIXME: ADD error reading */
/* round signal to account for adc discretisation */