From 80d7b9f81045d92b9b11344d4ce62bf019528dbe Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Thu, 25 Oct 2007 21:19:30 +0000 Subject: [PATCH] *** empty log message *** --- conf/airframes/booz.xml | 21 ++-- conf/airframes/quadRotorJP.xml | 116 ------------------ conf/messages.xml | 41 +++++-- conf/settings/booz.xml | 10 +- conf/telemetry/booz.xml | 4 +- sw/airborne/arm7/actuators_buss_twi_blmc_hw.c | 18 +++ sw/airborne/arm7/actuators_buss_twi_blmc_hw.h | 54 ++++++++ sw/airborne/arm7/i2c_hw.c | 16 ++- sw/airborne/arm7/i2c_hw.h | 14 ++- sw/airborne/booz_autopilot.c | 13 ++ sw/airborne/booz_autopilot.h | 12 +- sw/airborne/booz_control.c | 12 +- sw/airborne/booz_telemetry.h | 10 +- sw/airborne/main_booz.c | 15 ++- sw/airborne/pprz_transport.h | 2 +- sw/simulator/booz_flight_model.c | 19 ++- sw/simulator/booz_flight_model.h | 1 + sw/simulator/booz_flight_model_params.h | 26 ++-- sw/simulator/booz_flightgear.c | 4 +- sw/simulator/main_booz_sim.c | 61 +++++++-- 20 files changed, 266 insertions(+), 203 deletions(-) delete mode 100644 conf/airframes/quadRotorJP.xml create mode 100644 sw/airborne/arm7/actuators_buss_twi_blmc_hw.c create mode 100644 sw/airborne/arm7/actuators_buss_twi_blmc_hw.h diff --git a/conf/airframes/booz.xml b/conf/airframes/booz.xml index 320f8a25ba..7003dc6c38 100644 --- a/conf/airframes/booz.xml +++ b/conf/airframes/booz.xml @@ -1,10 +1,10 @@ - - - - - + + + + + @@ -52,7 +52,7 @@ ctl.TARGETDIR = ctl ctl.CFLAGS += -DCONTROLLER -DCONFIG=\"conf_booz.h\" ctl.srcs = main_booz.c -ctl.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(4e-3)' +ctl.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(4e-3)' -DTIME_LED=1 ctl.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c ctl.CFLAGS += -DLED @@ -73,10 +73,13 @@ ctl.srcs += spi.c $(SRC_ARCH)/spi_hw.c link_imu.c ctl.srcs += booz_estimator.c booz_control.c booz_autopilot.c -ctl.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 -DSERVOS_4017_CLOCK_FALLING -ctl.srcs += $(SRC_ARCH)/servos_4017_hw.c actuators.c +#ctl.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 -DSERVOS_4017_CLOCK_FALLING +#ctl.srcs += $(SRC_ARCH)/servos_4017_hw.c actuators.c -#ctl.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c $(SRC_ARCH)/mb_twi_controller.c +ctl.CFLAGS += -DACTUATORS=\"actuators_buss_twi_blmc_hw.h\" -DUSE_BUSS_TWI_BLMC +ctl.srcs += $(SRC_ARCH)/actuators_buss_twi_blmc_hw.c actuators.c +ctl.CFLAGS += -DI2C_SCLL=150 -DI2C_SCLH=150 -DI2C_VIC_SLOT=10 +ctl.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c ctl.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA -DRC_LED=4 ctl.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c diff --git a/conf/airframes/quadRotorJP.xml b/conf/airframes/quadRotorJP.xml deleted file mode 100644 index 65b865c4eb..0000000000 --- a/conf/airframes/quadRotorJP.xml +++ /dev/null @@ -1,116 +0,0 @@ - -
- - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
- - - - - - - - - -
-
- - - -
-
- - - -
-
- -
- - - - - - - - - - - - - - - - -
- - - - - - - - -
-
- - - - - - - -
-
- - - - - - - - -
-
- - - -
-
- - - - - -
-
- - -
- diff --git a/conf/messages.xml b/conf/messages.xml index ad7c8c3e1d..2f40334d3f 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -430,11 +430,13 @@ - - - - - + + + + + + + @@ -444,16 +446,31 @@ - + + + + + + + + + + + + + + + + - + @@ -461,12 +478,12 @@ - + - + @@ -478,14 +495,14 @@ - + - + @@ -497,7 +514,7 @@ - + diff --git a/conf/settings/booz.xml b/conf/settings/booz.xml index c1d69145e4..1e426b4f15 100644 --- a/conf/settings/booz.xml +++ b/conf/settings/booz.xml @@ -3,10 +3,10 @@ - - - - - + + + + + diff --git a/conf/telemetry/booz.xml b/conf/telemetry/booz.xml index 547f3b367f..6089ce2d1f 100644 --- a/conf/telemetry/booz.xml +++ b/conf/telemetry/booz.xml @@ -5,10 +5,12 @@ - + + + diff --git a/sw/airborne/arm7/actuators_buss_twi_blmc_hw.c b/sw/airborne/arm7/actuators_buss_twi_blmc_hw.c new file mode 100644 index 0000000000..bd99640489 --- /dev/null +++ b/sw/airborne/arm7/actuators_buss_twi_blmc_hw.c @@ -0,0 +1,18 @@ +#include "actuators.h" + +uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB]; +volatile bool_t buss_twi_blmc_status; +volatile bool_t buss_twi_blmc_i2c_done; +volatile uint8_t buss_twi_blmc_nb_err; +volatile uint8_t buss_twi_blmc_idx; + +const uint8_t buss_twi_blmc_addr[BUSS_TWI_BLMC_NB] = { 0x52, 0x54, 0x56, 0x58 }; + +void actuators_init ( void ) { + uint8_t i; + for (i=0; i +#include "std.h" +#include "i2c.h" + +#define ChopServo(x,a,b) Chop(x, a, b) +#define Actuator(i) buss_twi_blmc_motor_power[i] +#define ActuatorsCommit() { \ + if ( buss_twi_blmc_status == BUSS_TWI_BLMC_STATUS_IDLE) { \ + buss_twi_blmc_idx = 0; \ + buss_twi_blmc_motor_power[0] = 0; \ + buss_twi_blmc_motor_power[1] = 0; \ + buss_twi_blmc_motor_power[2] = 0; \ + buss_twi_blmc_motor_power[3] = 0; \ + buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_BUSY; \ + ActuatorsBussTwiBlmcSend(); \ + } \ + else \ + buss_twi_blmc_nb_err++; \ + } + +#define SERVOS_TICS_OF_USEC(s) ((uint8_t)(s)) + +#define BUSS_TWI_BLMC_STATUS_IDLE 0 +#define BUSS_TWI_BLMC_STATUS_BUSY 1 +#define BUSS_TWI_BLMC_NB 4 +extern uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB]; +extern volatile bool_t buss_twi_blmc_status; +extern volatile bool_t buss_twi_blmc_i2c_done; +extern volatile uint8_t buss_twi_blmc_nb_err; +extern volatile uint8_t buss_twi_blmc_idx; +extern const uint8_t buss_twi_blmc_addr[]; + +#define ActuatorsBussTwiBlmcNext() { \ + /* buss_twi_blmc_idx++; */ \ + /* if (buss_twi_blmc_idx < BUSS_TWI_BLMC_NB) { */ \ + /* ActuatorsBussTwiBlmcSend(); */ \ + /* } */ \ + /* else */ \ + buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE; \ + } + +#define ActuatorsBussTwiBlmcSend() { \ + const uint8_t msg[] = { buss_twi_blmc_addr[buss_twi_blmc_idx], \ + buss_twi_blmc_motor_power[buss_twi_blmc_idx]}; \ + memcpy((void*)i2c_buf, msg, sizeof(msg)); \ + i2c_transmit(buss_twi_blmc_addr[buss_twi_blmc_idx], sizeof(msg), &buss_twi_blmc_i2c_done); \ +} + + + +#endif /* ACTUATORS_BUSS_TWI_BLMC_HW_H */ diff --git a/sw/airborne/arm7/i2c_hw.c b/sw/airborne/arm7/i2c_hw.c index 25dee1fda9..ede12c2ee0 100644 --- a/sw/airborne/arm7/i2c_hw.c +++ b/sw/airborne/arm7/i2c_hw.c @@ -16,6 +16,18 @@ #define I2C_SCLH 200 #endif +#ifndef I2C_VIC_SLOT +#define I2C_VIC_CNTL VICVectCntl9 +#define I2C_VIC_ADDR VICVectAddr9 +#else +#define __I2C_VIC_CNTL(idx) VICVectCntl##idx +#define __I2C_VIC_ADDR(idx) VICVectAddr##idx +#define _I2C_VIC_CNTL(idx) __I2C_VIC_CNTL(idx) +#define _I2C_VIC_ADDR(idx) __I2C_VIC_ADDR(idx) +#define I2C_VIC_CNTL _I2C_VIC_CNTL(I2C_VIC_SLOT) +#define I2C_VIC_ADDR _I2C_VIC_ADDR(I2C_VIC_SLOT) +#endif + void i2c0_ISR(void) __attribute__((naked)); @@ -36,8 +48,8 @@ void i2c_hw_init ( void ) { // initialize the interrupt vector VICIntSelect &= ~VIC_BIT(VIC_I2C0); // I2C0 selected as IRQ VICIntEnable = VIC_BIT(VIC_I2C0); // I2C0 interrupt enabled - VICVectCntl9 = VIC_ENABLE | VIC_I2C0; - VICVectAddr9 = (uint32_t)i2c0_ISR; // address of the ISR + I2C_VIC_CNTL = VIC_ENABLE | VIC_I2C0; + I2C_VIC_ADDR = (uint32_t)i2c0_ISR; // address of the ISR } #define I2C_DATA_REG I2C0DAT diff --git a/sw/airborne/arm7/i2c_hw.h b/sw/airborne/arm7/i2c_hw.h index c7a38610a7..20297028b5 100644 --- a/sw/airborne/arm7/i2c_hw.h +++ b/sw/airborne/arm7/i2c_hw.h @@ -4,10 +4,22 @@ #include "LPC21xx.h" +#ifdef USE_BUSS_TWI_BLMC +#include "actuators_buss_twi_blmc_hw.h" +#define I2cStopHandler() ActuatorsBussTwiBlmcNext() +#else +#define I2cStopHandler() {} +#endif + extern void i2c_hw_init(void); #define I2cSendAck() { I2C0CONSET = _BV(AA); } -#define I2cSendStop() { I2C0CONSET = _BV(STO); if (i2c_finished) *i2c_finished = TRUE; i2c_status = I2C_IDLE; } +#define I2cSendStop() { \ + I2C0CONSET = _BV(STO); \ + if (i2c_finished) *i2c_finished = TRUE; \ + i2c_status = I2C_IDLE; \ + I2cStopHandler(); \ + } #define I2cSendStart() { I2C0CONSET = _BV(STA); } #define I2cSendByte(b) { I2C_DATA_REG = b; } diff --git a/sw/airborne/booz_autopilot.c b/sw/airborne/booz_autopilot.c index c815ca596e..18d2981491 100644 --- a/sw/airborne/booz_autopilot.c +++ b/sw/airborne/booz_autopilot.c @@ -10,6 +10,7 @@ void booz_autopilot_init(void) { } void booz_autopilot_periodic_task(void) { + switch (booz_autopilot_mode) { case BOOZ_AP_MODE_FAILSAFE: SetCommands(commands_failsafe); @@ -27,3 +28,15 @@ void booz_autopilot_periodic_task(void) { } +void booz_autopilot_event_task(void) { + + switch (booz_autopilot_mode) { + case BOOZ_AP_MODE_RATE: + booz_control_rate_compute_setpoints(); + break; + case BOOZ_AP_MODE_ATTITUDE: + booz_control_attitude_compute_setpoints(); + break; + } + +} diff --git a/sw/airborne/booz_autopilot.h b/sw/airborne/booz_autopilot.h index 58d0b9d42c..224f87eedc 100644 --- a/sw/airborne/booz_autopilot.h +++ b/sw/airborne/booz_autopilot.h @@ -11,10 +11,14 @@ extern uint8_t booz_autopilot_mode; extern void booz_autopilot_init(void); -void booz_autopilot_periodic_task(void); - - - +extern void booz_autopilot_periodic_task(void); +extern void booz_autopilot_event_task(void); +#define TRESHOLD_RATE_PPRZ (MIN_PPRZ / 2) +#define TRESHOLD_ATTITUDE_PPRZ (MAX_PPRZ/2) +#define BOOZ_AP_MODE_OF_PPRZ(mode) \ + ((mode) < TRESHOLD_RATE_PPRZ ? BOOZ_AP_MODE_RATE : \ + (mode) < TRESHOLD_ATTITUDE_PPRZ ? BOOZ_AP_MODE_ATTITUDE : \ + BOOZ_AP_MODE_ATTITUDE ) #endif /* BOOZ_AUTOPILOT_H */ diff --git a/sw/airborne/booz_control.c b/sw/airborne/booz_control.c index 8c686197f4..d5f0867ce4 100644 --- a/sw/airborne/booz_control.c +++ b/sw/airborne/booz_control.c @@ -18,11 +18,11 @@ float booz_control_rate_last_err_r; pprz_t booz_control_commands[COMMANDS_NB]; -#define BOOZ_CONTROL_RATE_PQ_PGAIN -1000. -#define BOOZ_CONTROL_RATE_PQ_DGAIN 0. +#define BOOZ_CONTROL_RATE_PQ_PGAIN -350. +#define BOOZ_CONTROL_RATE_PQ_DGAIN 30. -#define BOOZ_CONTROL_RATE_R_PGAIN -100. -#define BOOZ_CONTROL_RATE_R_DGAIN 0. +#define BOOZ_CONTROL_RATE_R_PGAIN -170. +#define BOOZ_CONTROL_RATE_R_DGAIN 10. /* setpoints for max stick throw in degres per second */ #define BOOZ_CONTROL_RATE_PQ_MAX_SP 200. @@ -34,8 +34,8 @@ float booz_control_theta_sp; float booz_control_attitude_phi_theta_pgain; float booz_control_attitude_phi_theta_dgain; -#define BOOZ_CONTROL_ATTITUDE_PHI_THETA_PGAIN -15000. -#define BOOZ_CONTROL_ATTITUDE_PHI_THETA_DGAIN -15000. +#define BOOZ_CONTROL_ATTITUDE_PHI_THETA_PGAIN -1200. +#define BOOZ_CONTROL_ATTITUDE_PHI_THETA_DGAIN -600. /* setpoints for max stick throw in degres */ #define BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP 30. diff --git a/sw/airborne/booz_telemetry.h b/sw/airborne/booz_telemetry.h index eea0cb7d68..cc005afd4a 100644 --- a/sw/airborne/booz_telemetry.h +++ b/sw/airborne/booz_telemetry.h @@ -13,18 +13,24 @@ #include "booz_autopilot.h" #include "booz_control.h" +#include "actuators_buss_twi_blmc_hw.h" + +#include "settings.h" + #include "downlink.h" -#define PERIODIC_SEND_BOOZ_STATUS() DOWNLINK_SEND_BOOZ_STATUS(&link_imu_nb_err, &link_imu_status, &rc_status, &booz_autopilot_mode) +#define PERIODIC_SEND_BOOZ_STATUS() DOWNLINK_SEND_BOOZ_STATUS(&link_imu_nb_err, &link_imu_status, &rc_status, &booz_autopilot_mode, &buss_twi_blmc_idx, &cpu_time_sec, &buss_twi_blmc_nb_err) #define PERIODIC_SEND_BOOZ_FD() DOWNLINK_SEND_BOOZ_FD(&booz_estimator_p, &booz_estimator_q, &booz_estimator_r,\ &booz_estimator_phi, &booz_estimator_theta, &booz_estimator_psi); #define PERIODIC_SEND_BOOZ_DEBUG() DOWNLINK_SEND_BOOZ_DEBUG(&booz_control_p_sp, &booz_control_q_sp, &booz_control_r_sp, &booz_control_power_sp); #define PERIODIC_SEND_ACTUATORS() DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators); +#define PERIODIC_SEND_BOOZ_RATE_LOOP() DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_p, &booz_control_p_sp, &booz_estimator_q, &booz_control_q_sp, &booz_estimator_r, &booz_control_r_sp ); + #define PERIODIC_SEND_BOOZ_ATT_LOOP() DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_phi_sp, &booz_estimator_theta, &booz_control_theta_sp); - +#define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue() extern uint8_t telemetry_mode_Main; diff --git a/sw/airborne/main_booz.c b/sw/airborne/main_booz.c index 89c0a5316a..3318e5e9a1 100644 --- a/sw/airborne/main_booz.c +++ b/sw/airborne/main_booz.c @@ -5,6 +5,7 @@ #include "led.h" #include "commands.h" +#include "i2c.h" #include "actuators.h" #include "radio_control.h" @@ -28,6 +29,7 @@ static inline void main_event_task( void ); int16_t trim_p = 0; int16_t trim_q = 0; int16_t trim_r = 0; +uint8_t vbat = 0; int main( void ) { main_init(); @@ -44,6 +46,7 @@ static inline void main_init( void ) { led_init(); sys_time_init(); + i2c_init(); actuators_init(); SetCommands(commands_failsafe); @@ -60,6 +63,8 @@ static inline void main_init( void ) { uart1_init_tx(); int_enable(); + + DOWNLINK_SEND_BOOT(&cpu_time_sec); } static inline void main_periodic_task( void ) { @@ -103,12 +108,10 @@ static inline void main_event_task( void ) { if (ppm_valid) { ppm_valid = FALSE; radio_control_event_task(); - // if (rc_values_contains_avg_channels) { - // fbw_mode = FBW_MODE_OF_PPRZ(rc_values[RADIO_MODE]); - // } - booz_autopilot_mode = BOOZ_AP_MODE_RATE; - /* setpoints */ - booz_control_rate_compute_setpoints(); + if (rc_values_contains_avg_channels) { + booz_autopilot_mode = BOOZ_AP_MODE_OF_PPRZ(rc_values[RADIO_MODE]); + } + booz_autopilot_event_task(); } } diff --git a/sw/airborne/pprz_transport.h b/sw/airborne/pprz_transport.h index defa4a881a..6d16cd593f 100644 --- a/sw/airborne/pprz_transport.h +++ b/sw/airborne/pprz_transport.h @@ -61,7 +61,7 @@ extern uint8_t ck_a, ck_b; #define PprzTransportHeader(payload_len) { \ PprzTransportPut1Byte(STX); \ uint8_t msg_len = PprzTransportSizeOf(payload_len); \ - PprzTransportPut1Byte(msg_len); \ + PprzTransportPut1Byte(msg_len); \ ck_a = msg_len; ck_b = msg_len; \ } diff --git a/sw/simulator/booz_flight_model.c b/sw/simulator/booz_flight_model.c index 8ab6ce4222..0ffe7d6dc8 100644 --- a/sw/simulator/booz_flight_model.c +++ b/sw/simulator/booz_flight_model.c @@ -11,7 +11,6 @@ struct BoozFlightModel bfm; -//static void motor_model_run( double dt ); //static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot); static VEC* booz_get_forces_body_frame( VEC* X, VEC* M, MAT* dcm, VEC* omega_square); @@ -19,6 +18,7 @@ static VEC* booz_get_moments_body_frame( VEC* X, VEC* M, VEC* omega_square ); static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot); void booz_flight_model_init( void ) { + bfm.time = 0.; bfm.bat_voltage = BAT_VOLTAGE; bfm.mot_voltage = v_get(SERVOS_NB); @@ -39,10 +39,10 @@ void booz_flight_model_init( void ) { bfm.props_moment_matrix->me[AXIS_X][SERVO_MOTOR_RIGHT] = -L * bfm.thrust_factor; bfm.props_moment_matrix->me[AXIS_Y][SERVO_MOTOR_BACK] = -L * bfm.thrust_factor; bfm.props_moment_matrix->me[AXIS_Y][SERVO_MOTOR_FRONT] = L * bfm.thrust_factor; - bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_LEFT] = -bfm.torque_factor; - bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_RIGHT] = -bfm.torque_factor; - bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_BACK] = bfm.torque_factor; - bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_FRONT] = bfm.torque_factor; + bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_LEFT] = bfm.torque_factor; + bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_RIGHT] = bfm.torque_factor; + bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_BACK] = -bfm.torque_factor; + bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_FRONT] = -bfm.torque_factor; bfm.Inert = m_get(AXIS_NB, AXIS_NB); m_zero(bfm.Inert); @@ -64,8 +64,9 @@ void booz_flight_model_run( double dt, double* commands ) { int i; for (i=0; ive[i] = bfm.bat_voltage * commands[i]; - // motor_model_run(dt); - rk4(booz_flight_model_get_derivatives, bfm.state, bfm.mot_voltage, dt); + // rk4(motor_model_derivative, bfm.mot_omega, bfm.mot_voltage, dt); + rk4(booz_flight_model_get_derivatives, bfm.state, bfm.mot_voltage, dt); + bfm.time += dt; } @@ -223,10 +224,6 @@ static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot) { #if 0 -static void motor_model_run( double dt ) { - rk4(motor_model_derivative, bfm.mot_omega, bfm.mot_voltage, dt); -} - static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot) { static VEC *temp1 = VNULL; static VEC *temp2 = VNULL; diff --git a/sw/simulator/booz_flight_model.h b/sw/simulator/booz_flight_model.h index b32d3bb4e7..5e0d95bf09 100644 --- a/sw/simulator/booz_flight_model.h +++ b/sw/simulator/booz_flight_model.h @@ -23,6 +23,7 @@ struct BoozFlightModel { + double time; /* battery voltage in V */ double bat_voltage; /* motors supply voltage in V */ diff --git a/sw/simulator/booz_flight_model_params.h b/sw/simulator/booz_flight_model_params.h index 529ac87f0b..2812cd20b7 100644 --- a/sw/simulator/booz_flight_model_params.h +++ b/sw/simulator/booz_flight_model_params.h @@ -2,27 +2,27 @@ #define BOOZ_FLIGHT_MODEL_PARAMS_H /* drag coefficient of the body */ -#define C_d_body .2 +#define C_d_body .005 /* thrust aerodynamic coefficient */ -#define C_t 0.132 +#define C_t 0.297 /* moment aerodynamic coefficient */ -#define C_q 0.01 +#define C_q 0.0276 /* propeller radius in m */ #define PROP_RADIUS 0.125 /* propeller area in m2 */ -#define PROP_AREA (M_PI * PROP_RADIUS * PROP_RADIUS) +#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.5 -/* inertia on x axis in m2 */ -#define Ix .5 -/* inertia on y axis in m2 */ -#define Iy .5 -/* inertia on z axis in m2 */ -#define Iz 1. +/* inertia on x axis in kg * m2 */ +#define Ix .007 +/* inertia on y axis in kg * m2 */ +#define Iy .0137 +/* inertia on z axis in kg * m2 */ +#define Iz .0073 /* lenght between centers of vehicle and prop in m */ #define L 0.25 /* height between cg and prop plane in m */ @@ -38,9 +38,9 @@ */ #define BAT_VOLTAGE 11. -#define THAU 25. -#define Kq 0.02 -#define Kv 1500. +#define THAU 10. +#define Kq 0.0079 +#define Kv 1000. diff --git a/sw/simulator/booz_flightgear.c b/sw/simulator/booz_flightgear.c index 9da2bfee8b..58f474de88 100644 --- a/sw/simulator/booz_flightgear.c +++ b/sw/simulator/booz_flightgear.c @@ -44,12 +44,14 @@ void booz_flightgear_send() { gui.latitude = lat; gui.longitude = lon; - gui.altitude = 10 - bfm.state->ve[BFMS_Z]; + gui.altitude = 30 - 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"); diff --git a/sw/simulator/main_booz_sim.c b/sw/simulator/main_booz_sim.c index 1e5d4cf4c6..193cc5e86f 100644 --- a/sw/simulator/main_booz_sim.c +++ b/sw/simulator/main_booz_sim.c @@ -3,13 +3,15 @@ #include #include +#include + #include "booz_flight_model.h" #include "booz_flightgear.h" -char* fg_host = "127.0.0.1"; -//char* fg_host = "10.31.4.107"; +//char* fg_host = "127.0.0.1"; +char* fg_host = "10.31.4.107"; unsigned int fg_port = 5501; /* 250Hz <-> 4ms */ @@ -20,7 +22,6 @@ double sim_time; #define DT_DISPLAY 0.04 double disp_time; - double foo_commands[] = {0., 0., 0., 0.}; static gboolean timeout_callback(gpointer data); @@ -40,6 +41,9 @@ static void airborne_periodic_task(void); static void airborne_event_task(void); #include "booz_estimator.h" +#include "radio_control.h" +volatile bool_t ppm_valid; +#define RPM_OF_RAD_S(a) ((a)*60./M_PI) static gboolean timeout_callback(gpointer data) { @@ -49,6 +53,11 @@ static gboolean timeout_callback(gpointer data) { if (sim_time >= disp_time) { disp_time+= DT_DISPLAY; booz_flightgear_send(); + IvySendMsg("148 BOOZ_RPMS %f %f %f %f", + RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_F]), + RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_B]), + RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_L]), + RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_R]) ); } booz_estimator_p = bfm.state->ve[BFMS_P]; @@ -59,7 +68,10 @@ static gboolean timeout_callback(gpointer data) { booz_estimator_theta = bfm.state->ve[BFMS_THETA]; booz_estimator_psi = bfm.state->ve[BFMS_PSI]; + /* post a radio control event */ + ppm_valid = TRUE; airborne_event_task(); + airborne_periodic_task(); @@ -125,7 +137,7 @@ int main ( int argc, char** argv) { #include static void joystick_init(void) { - const char* device = "/dev/input/js1"; + const char* device = "/dev/input/js0"; int fd = open(device, O_RDONLY | O_NONBLOCK); if (fd == -1) { printf("opening joystick serial device %s : %s\n", device, strerror(errno)); @@ -231,6 +243,11 @@ uint16_t ppm_pulses[PPM_NB_PULSES]; static void airborne_init(void) { + ppm_pulses[RADIO_THROTTLE] = 1223 + 0.62 * (2050-1223); + ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); + ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); + ppm_pulses[RADIO_YAW] = 1500 + 0. * (2050-950); + ppm_pulses[RADIO_MODE] = 1500 + 0.25 * (2050-950); ppm_init(); radio_control_init(); @@ -244,6 +261,18 @@ static void airborne_init(void) { static void airborne_periodic_task(void) { +#if 0 + int foo = sim_time/10; + if (!(foo%2)) { + ppm_pulses[RADIO_YAW] = 1500 + 0.1 * (2050-950); + ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); + } + else { + ppm_pulses[RADIO_YAW] = 1500 - 0. * (2050-950); + ppm_pulses[RADIO_PITCH] = 1498 + 0.1 * (2050-950); + } +#endif + booz_autopilot_periodic_task(); SetActuatorsFromCommands(commands); @@ -277,15 +306,16 @@ static void airborne_event_task(void) { // DlEventCheckAndHandle(); - // if (ppm_valid) { - // ppm_valid = FALSE; - radio_control_event_task(); - booz_autopilot_mode = BOOZ_AP_MODE_ATTITUDE; - if (booz_autopilot_mode == BOOZ_AP_MODE_RATE) - booz_control_rate_compute_setpoints(); - else if (booz_autopilot_mode == BOOZ_AP_MODE_ATTITUDE) - booz_control_attitude_compute_setpoints(); - //} + if (ppm_valid) { + ppm_valid = FALSE; + radio_control_event_task(); + booz_autopilot_mode = BOOZ_AP_MODE_ATTITUDE; + if (rc_values_contains_avg_channels) { + booz_autopilot_mode = BOOZ_AP_MODE_OF_PPRZ(rc_values[RADIO_MODE]); + } + booz_autopilot_event_task(); + } + } @@ -299,6 +329,7 @@ static void airborne_event_task(void) { #define JS_ROLL 0 #define JS_PITCH 1 #define JS_YAW 5 +#define JS_MODE 2 static gboolean on_js_data_received(GIOChannel *source, GIOCondition condition, gpointer data) { @@ -322,6 +353,10 @@ static gboolean on_js_data_received(GIOChannel *source, GIOCondition condition, 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; } }