diff --git a/conf/autopilot/subsystems/booz2_imu_b2v1.makefile b/conf/autopilot/subsystems/booz2_imu_b2v1.makefile index cf77e8e012..015d04962f 100644 --- a/conf/autopilot/subsystems/booz2_imu_b2v1.makefile +++ b/conf/autopilot/subsystems/booz2_imu_b2v1.makefile @@ -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 \ No newline at end of file diff --git a/conf/messages.xml b/conf/messages.xml index 3c169abc9e..6b7d0daef8 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -461,11 +461,11 @@ - + - + diff --git a/conf/simulator/jsbsim/aircraft/reset00.xml b/conf/simulator/jsbsim/aircraft/reset00.xml index 5fc3b685fc..99e3ecbe9d 100644 --- a/conf/simulator/jsbsim/aircraft/reset00.xml +++ b/conf/simulator/jsbsim/aircraft/reset00.xml @@ -8,7 +8,8 @@ 0.0 37.6136 -122.3569 - 0.11 + + 0.9 0.0 0.0 diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml index 7dee1d0552..bdcfc2c4ea 100644 --- a/conf/telemetry/telemetry_booz2.xml +++ b/conf/telemetry/telemetry_booz2.xml @@ -16,8 +16,9 @@ - - + + + diff --git a/sw/airborne/booz/arch/sim/imu/booz_imu_b2_arch.c b/sw/airborne/booz/arch/sim/imu/booz_imu_b2_arch.c index 97ab97f782..8ee5c08bbe 100644 --- a/sw/airborne/booz/arch/sim/imu/booz_imu_b2_arch.c +++ b/sw/airborne/booz/arch/sim/imu/booz_imu_b2_arch.c @@ -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 +} diff --git a/sw/airborne/booz/arch/sim/imu/booz_imu_b2_arch.h b/sw/airborne/booz/arch/sim/imu/booz_imu_b2_arch.h index e001586274..cc742a9fab 100644 --- a/sw/airborne/booz/arch/sim/imu/booz_imu_b2_arch.h +++ b/sw/airborne/booz/arch/sim/imu/booz_imu_b2_arch.h @@ -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 */ diff --git a/sw/airborne/booz/arch/sim/radio_control/booz_radio_control_ppm_arch.c b/sw/airborne/booz/arch/sim/radio_control/booz_radio_control_ppm_arch.c index bd958dd546..9490392232 100644 --- a/sw/airborne/booz/arch/sim/radio_control/booz_radio_control_ppm_arch.c +++ b/sw/airborne/booz/arch/sim/radio_control/booz_radio_control_ppm_arch.c @@ -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; +} + diff --git a/sw/airborne/booz/arch/sim/radio_control/booz_radio_control_ppm_arch.h b/sw/airborne/booz/arch/sim/radio_control/booz_radio_control_ppm_arch.h index a08114913e..4810c63721 100644 --- a/sw/airborne/booz/arch/sim/radio_control/booz_radio_control_ppm_arch.h +++ b/sw/airborne/booz/arch/sim/radio_control/booz_radio_control_ppm_arch.h @@ -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 */ diff --git a/sw/airborne/booz/booz2_analog_baro.c b/sw/airborne/booz/booz2_analog_baro.c index 692b6d04e2..4738a6b4f6 100644 --- a/sw/airborne/booz/booz2_analog_baro.c +++ b/sw/airborne/booz/booz2_analog_baro.c @@ -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 + } +} diff --git a/sw/airborne/booz/booz2_analog_baro.h b/sw/airborne/booz/booz2_analog_baro.h index cb6acaf48d..afdf9d4f3e 100644 --- a/sw/airborne/booz/booz2_analog_baro.h +++ b/sw/airborne/booz/booz2_analog_baro.h @@ -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 */ diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h index 1334ec3b13..f3992f82fe 100644 --- a/sw/airborne/booz/booz2_telemetry.h +++ b/sw/airborne/booz/booz2_telemetry.h @@ -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 diff --git a/sw/airborne/booz/imu/booz_imu_b2.h b/sw/airborne/booz/imu/booz_imu_b2.h index ccce8eb066..5d606addca 100644 --- a/sw/airborne/booz/imu/booz_imu_b2.h +++ b/sw/airborne/booz/imu/booz_imu_b2.h @@ -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(); \ } \ } diff --git a/sw/airborne/booz/peripherals/booz_ami601.c b/sw/airborne/booz/peripherals/booz_ami601.c index 044cc7cdaf..aca953e838 100644 --- a/sw/airborne/booz/peripherals/booz_ami601.c +++ b/sw/airborne/booz/peripherals/booz_ami601.c @@ -1,4 +1,4 @@ -#include "booz_ami601.h" +#include "peripherals/booz_ami601.h" uint8_t ami601_foo1; uint8_t ami601_foo2; diff --git a/sw/airborne/booz/peripherals/booz_ami601.h b/sw/airborne/booz/peripherals/booz_ami601.h index 5b30f26d78..3ac4d30f59 100644 --- a/sw/airborne/booz/peripherals/booz_ami601.h +++ b/sw/airborne/booz/peripherals/booz_ami601.h @@ -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) { \ diff --git a/sw/simulator/nps_autopilot_booz.c b/sw/simulator/nps_autopilot_booz.c index de3b6d44fd..7d473d5d38 100644 --- a/sw/simulator/nps_autopilot_booz.c +++ b/sw/simulator/nps_autopilot_booz.c @@ -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 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]); } diff --git a/sw/simulator/nps_fdm_jsbsim.c b/sw/simulator/nps_fdm_jsbsim.c index 201c92965c..2c90e7f1f8 100644 --- a/sw/simulator/nps_fdm_jsbsim.c +++ b/sw/simulator/nps_fdm_jsbsim.c @@ -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(<pdef_d,&fdm.ecef_pos); ltpdef_copy(<pdef, <pdef_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){ diff --git a/sw/simulator/nps_ivy.c b/sw/simulator/nps_ivy.c index 098a474059..f14f9b7616 100644 --- a/sw/simulator/nps_ivy.c +++ b/sw/simulator/nps_ivy.c @@ -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) { /* diff --git a/sw/simulator/nps_jsbsim.c b/sw/simulator/nps_jsbsim.c deleted file mode 100644 index 0b22f8370c..0000000000 --- a/sw/simulator/nps_jsbsim.c +++ /dev/null @@ -1,95 +0,0 @@ -#include "nps_jsbsim.h" - -#include -#include -#include -#include - -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); -} diff --git a/sw/simulator/nps_jsbsim.h b/sw/simulator/nps_jsbsim.h deleted file mode 100644 index 2cb935e962..0000000000 --- a/sw/simulator/nps_jsbsim.h +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef NPS_JSBSIM_H -#define NPS_JSBSIM_H - -#include -#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 */ - diff --git a/sw/simulator/nps_radio_control.c b/sw/simulator/nps_radio_control.c index 8817a61c61..4f8855f9af 100644 --- a/sw/simulator/nps_radio_control.c +++ b/sw/simulator/nps_radio_control.c @@ -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; diff --git a/sw/simulator/nps_sensor_accel.c b/sw/simulator/nps_sensor_accel.c index 75ad5e655f..d85329912f 100644 --- a/sw/simulator/nps_sensor_accel.c +++ b/sw/simulator/nps_sensor_accel.c @@ -21,6 +21,7 @@ void nps_sensor_accel_init(struct NpsSensorAccel* accel, double time) { accel->data_available = FALSE; } +//#include 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 */ diff --git a/sw/simulator/nps_sensor_baro.c b/sw/simulator/nps_sensor_baro.c index 1e96993d85..b5ecad9c5d 100644 --- a/sw/simulator/nps_sensor_baro.c +++ b/sw/simulator/nps_sensor_baro.c @@ -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; diff --git a/sw/simulator/nps_sensor_gyro.c b/sw/simulator/nps_sensor_gyro.c index a118ce0ae2..908eb44922 100644 --- a/sw/simulator/nps_sensor_gyro.c +++ b/sw/simulator/nps_sensor_gyro.c @@ -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 */ diff --git a/sw/simulator/nps_sensor_mag.c b/sw/simulator/nps_sensor_mag.c index 872a5b1725..d7603e183a 100644 --- a/sw/simulator/nps_sensor_mag.c +++ b/sw/simulator/nps_sensor_mag.c @@ -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 */