diff --git a/conf/Makefile.lpc21 b/conf/Makefile.lpc21 index 26c8a2fe32..9f29195229 100644 --- a/conf/Makefile.lpc21 +++ b/conf/Makefile.lpc21 @@ -32,7 +32,7 @@ SRC_ARCH = arch/lpc21 # Define programs and commands. -HAVE_ARM_NONE_EABI_GCC := $(wildcard /usr/bin/arm-none-eabi-gcc) +HAVE_ARM_NONE_EABI_GCC := $(shell which arm-none-eabi-gcc) ifeq ($(strip $(HAVE_ARM_NONE_EABI_GCC)),) CC = arm-elf-gcc LD = $(CC) diff --git a/conf/Makefile.stm32 b/conf/Makefile.stm32 index c9271cde7d..c361279f28 100644 --- a/conf/Makefile.stm32 +++ b/conf/Makefile.stm32 @@ -38,7 +38,7 @@ OPT = s #OPT = 0 # Programs location -TOOLCHAIN_DIR=/opt/paparazzi/stm32 +TOOLCHAIN_DIR=$(shell find -L /opt/paparazzi/stm32 ~/sat -maxdepth 1 -type d -name arm-none-eabi 2>/dev/null | head -n 1 | xargs dirname ) GCC_BIN_DIR=$(TOOLCHAIN_DIR)/bin GCC_LIB_DIR=$(TOOLCHAIN_DIR)/arm-none-eabi/lib diff --git a/conf/airframes/TUDelft/yapa_xsens.xml b/conf/airframes/TUDelft/yapa_xsens.xml new file mode 100644 index 0000000000..f483e42481 --- /dev/null +++ b/conf/airframes/TUDelft/yapa_xsens.xml @@ -0,0 +1,212 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + +
+ + +
+ +
+ + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + +
+ +
+ + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + +
+ +CONFIG = \"tiny_2_1_1.h\" + +include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile + +FLASH_MODE=IAP + +XSENS_UART_NR = 0 + +ap.CFLAGS += -DFBW -DAP -DBOARD_CONFIG=$(CONFIG) -DLED +ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c main_ap.c main.c + +ap.srcs += commands.c + +ap.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 +ap.srcs += $(SRC_ARCH)/servos_4017_hw.c actuators.c + +ap.CFLAGS += -DRADIO_CONTROL +ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c + +#ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B57600 +#ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c pprz_transport.c + +# Maxstream API protocol +ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B9600 -DTELEMETRY_MODE_FBW=1 +ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c xbee.c + +ap.CFLAGS += -DINTER_MCU -DUSE_MODULES +ap.srcs += inter_mcu.c + +ap.CFLAGS += -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_4 -DUSE_ADC_5 +ap.srcs += $(SRC_ARCH)/adc_hw.c + +ap.srcs += gps_xsens.c gps.c latlong.c + +ap.CFLAGS += -DALT_KALMAN +ap.srcs += estimator.c + +ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM +ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c + +ap.srcs += nav_line.c nav_survey_rectangle.c + +# Config for SITL simulation +include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile +sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DTRAFFIC_INFO +sim.srcs += nav_survey_rectangle.c traffic_info.c nav_line.c + + diff --git a/conf/messages.xml b/conf/messages.xml index 3a69a1fb30..1e13a74278 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1615,6 +1615,7 @@ + diff --git a/paparazzi.osx b/paparazzi.osx index 7b9cfbb654..7efa9dc9a6 100755 --- a/paparazzi.osx +++ b/paparazzi.osx @@ -1,4 +1,4 @@ -#!/usr/local/bin/ocamlrun /usr/local/bin/ocaml +#!/opt/local/bin/ocamlrun /opt/local/bin/ocaml #load "unix.cma";; let (//) = Filename.concat let dirname = Filename.dirname Sys.argv.(0) diff --git a/sw/airborne/arch/lpc21/i2c_hw.c b/sw/airborne/arch/lpc21/i2c_hw.c index b3e0c54281..d91352db4b 100644 --- a/sw/airborne/arch/lpc21/i2c_hw.c +++ b/sw/airborne/arch/lpc21/i2c_hw.c @@ -33,16 +33,16 @@ // I2C Automaton // /////////////////// -static inline void I2cSendStart(struct i2c_periph* p) { +__attribute__ ((always_inline)) static inline void I2cSendStart(struct i2c_periph* p) { p->status = I2CStartRequested; ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STA); } -static inline void I2cSendAck(void* reg) { +__attribute__ ((always_inline)) static inline void I2cSendAck(void* reg) { ((i2cRegs_t *)reg)->conset = _BV(AA); } -static inline void I2cEndOfTransaction(struct i2c_periph* p) { +__attribute__ ((always_inline)) static inline void I2cEndOfTransaction(struct i2c_periph* p) { // handle fifo here p->trans_extract_idx++; if (p->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN) @@ -56,18 +56,18 @@ static inline void I2cEndOfTransaction(struct i2c_periph* p) { } } -static inline void I2cFinished(struct i2c_periph* p, struct i2c_transaction* t) { +__attribute__ ((always_inline)) static inline void I2cFinished(struct i2c_periph* p, struct i2c_transaction* t) { // transaction finished with success t->status = I2CTransSuccess; I2cEndOfTransaction(p); } -static inline void I2cSendStop(struct i2c_periph* p, struct i2c_transaction* t) { +__attribute__ ((always_inline)) static inline void I2cSendStop(struct i2c_periph* p, struct i2c_transaction* t) { ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO); I2cFinished(p,t); } -static inline void I2cFail(struct i2c_periph* p, struct i2c_transaction* t) { +__attribute__ ((always_inline)) static inline void I2cFail(struct i2c_periph* p, struct i2c_transaction* t) { ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO); t->status = I2CTransFailed; p->status = I2CFailed; @@ -75,24 +75,24 @@ static inline void I2cFail(struct i2c_periph* p, struct i2c_transaction* t) { I2cEndOfTransaction(p); } -static inline void I2cSendByte(void* reg, uint8_t b) { +__attribute__ ((always_inline)) static inline void I2cSendByte(void* reg, uint8_t b) { ((i2cRegs_t *)reg)->dat = b; } -static inline void I2cReceive(void* reg, bool_t ack) { +__attribute__ ((always_inline)) static inline void I2cReceive(void* reg, bool_t ack) { if (ack) ((i2cRegs_t *)reg)->conset = _BV(AA); else ((i2cRegs_t *)reg)->conclr = _BV(AAC); } -static inline void I2cClearStart(void* reg) { +__attribute__ ((always_inline)) static inline void I2cClearStart(void* reg) { ((i2cRegs_t *)reg)->conclr = _BV(STAC); } -static inline void I2cClearIT(void* reg) { +__attribute__ ((always_inline)) static inline void I2cClearIT(void* reg) { ((i2cRegs_t *)reg)->conclr = _BV(SIC); } -static inline void I2cAutomaton(int32_t state, struct i2c_periph* p) { +__attribute__ ((always_inline)) static inline void I2cAutomaton(int32_t state, struct i2c_periph* p) { struct i2c_transaction* trans = p->trans[p->trans_extract_idx]; switch (state) { case I2C_START: diff --git a/sw/airborne/firmwares/rotorcraft/actuators/supervision.c b/sw/airborne/firmwares/rotorcraft/actuators/supervision.c index 9a114641ee..012b2402a0 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/supervision.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/supervision.c @@ -59,13 +59,13 @@ void supervision_init(void) { supervision.nb_failure = 0; } -static inline void offset_commands(int32_t offset) { +__attribute__ ((always_inline)) static inline void offset_commands(int32_t offset) { uint8_t j; for (j=0; j>(GV_Z_REF_FRAC - INT32_POS_FRAC); diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h index dba603bddb..bcdd473413 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h @@ -96,14 +96,14 @@ int64_t gv_z_ref; int32_t gv_zd_ref; int32_t gv_zdd_ref; -static inline void gv_set_ref(int32_t alt, int32_t speed, int32_t accel) { +__attribute__ ((always_inline)) static inline void gv_set_ref(int32_t alt, int32_t speed, int32_t accel) { int64_t new_z = ((int64_t)alt)<<(GV_Z_REF_FRAC - INT32_POS_FRAC); gv_z_ref = new_z; gv_zd_ref = speed>>(INT32_SPEED_FRAC - GV_ZD_REF_FRAC); gv_zdd_ref = accel>>(INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC); } -static inline void gv_update_ref_from_z_sp(int32_t z_sp) { +__attribute__ ((always_inline)) static inline void gv_update_ref_from_z_sp(int32_t z_sp) { gv_z_ref += gv_zd_ref; gv_zd_ref += gv_zdd_ref; @@ -135,7 +135,7 @@ static inline void gv_update_ref_from_z_sp(int32_t z_sp) { } -static inline void gv_update_ref_from_zd_sp(int32_t zd_sp) { +__attribute__ ((always_inline)) static inline void gv_update_ref_from_zd_sp(int32_t zd_sp) { gv_z_ref += gv_zd_ref; gv_zd_ref += gv_zdd_ref; diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c index 80bdfaa72d..546159d568 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c @@ -154,7 +154,7 @@ void ahrs_update_mag(void) { } /* measures phi and theta assuming no dynamic acceleration ?!! */ -static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel) { +__attribute__ ((always_inline)) static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel) { INT32_ATAN2(*phi_meas, -accel.y, -accel.z); int32_t cphi; @@ -167,7 +167,7 @@ static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_ } /* measure psi by projecting magnetic vector in local tangeant plan */ -static inline void get_psi_measurement_from_mag(int32_t* psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 mag) { +__attribute__ ((always_inline)) static inline void get_psi_measurement_from_mag(int32_t* psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 mag) { int32_t sphi; PPRZ_ITRIG_SIN(sphi, phi_est); @@ -196,7 +196,7 @@ static inline void get_psi_measurement_from_mag(int32_t* psi_meas, int32_t phi_e /* Compute ltp to imu rotation in quaternion and rotation matrice representation from the euler angle representation */ -static inline void compute_imu_quat_and_rmat_from_euler(void) { +__attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_from_euler(void) { /* Compute LTP to IMU quaternion */ INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler); @@ -205,7 +205,7 @@ static inline void compute_imu_quat_and_rmat_from_euler(void) { } -static inline void compute_body_orientation(void) { +__attribute__ ((always_inline)) static inline void compute_body_orientation(void) { /* Compute LTP to BODY quaternion */ INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_to_imu_quat); diff --git a/sw/airborne/subsystems/ins/vf_float.c b/sw/airborne/subsystems/ins/vf_float.c index bc33bb0fcf..8638b93035 100644 --- a/sw/airborne/subsystems/ins/vf_float.c +++ b/sw/airborne/subsystems/ins/vf_float.c @@ -122,7 +122,7 @@ void vff_propagate(float accel) { // update covariance Pp = Pm - K*H*Pm; */ -static inline void update_z_conf(float z_meas, float conf) { +__attribute__ ((always_inline)) static inline void update_z_conf(float z_meas, float conf) { vff_z_meas = z_meas; const float y = z_meas - vff_z; @@ -179,7 +179,7 @@ void vff_update_z_conf(float z_meas, float conf) { // update covariance Pp = Pm - K*H*Pm; */ -static inline void update_vz_conf(float vz, float conf) { +__attribute__ ((always_inline)) static inline void update_vz_conf(float vz, float conf) { const float yd = vz - vff_zdot; const float S = vff_P[1][1] + conf; const float K1 = vff_P[0][1] * 1/S; diff --git a/sw/ground_segment/tmtc/booz_server.ml b/sw/ground_segment/tmtc/booz_server.ml index 98187d7f75..a076cb9d27 100644 --- a/sw/ground_segment/tmtc/booz_server.ml +++ b/sw/ground_segment/tmtc/booz_server.ml @@ -182,6 +182,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> Wind.update ac_name a.gspeed a.course | "ROTORCRAFT_STATUS" -> a.fbw.rc_status <- get_rc_status (ivalue "rc_status"); + a.fbw.rc_rate <- ivalue "frame_rate"; a.gps_mode <- check_index (ivalue "gps_status") gps_modes "GPS_MODE"; a.ap_mode <- check_index (get_pprz_mode (ivalue "ap_mode")) ap_modes "BOOZ_AP_MODE"; a.kill_mode <- ivalue "ap_motors_on" == 0; diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml index 1375c9dd28..ce4a4fe2ba 100644 --- a/sw/ground_segment/tmtc/fw_server.ml +++ b/sw/ground_segment/tmtc/fw_server.ml @@ -103,6 +103,8 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> a.last_msg_date <- U.gettimeofday (); match msg.Pprz.name with "GPS" -> + a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE"; + if a.gps_mode = _3D then begin let p = { LL.utm_x = fvalue "utm_east" /. 100.; utm_y = fvalue "utm_north" /. 100.; utm_zone = ivalue "utm_zone" } in @@ -114,9 +116,9 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> if !heading_from_course then a.heading <- a.course; a.agl <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0); - a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE"; if a.gspeed > 3. && a.ap_mode = _AUTO2 then Wind.update ac_name a.gspeed a.course + end | "GPS_LLA" -> let lat = ivalue "lat" and lon = ivalue "lon" in @@ -143,9 +145,6 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> Some nav_ref -> let x = (try fvalue "x" with _ -> fvalue "desired_x") and y = (try fvalue "y" with _ -> fvalue "desired_y") in - (*let target_utm = LL.utm_add (LL.utm_of nav_ref) (x, y) in - let target_geo = LL.of_utm WGS84 target_utm in - a.desired_pos <- target_geo;*) a.desired_pos <- Aircraft.add_pos_to_nav_ref nav_ref (x, y); | None -> () end; diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml index 33dd4fd378..a66e209bad 100644 --- a/sw/ground_segment/tmtc/server.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -151,8 +151,7 @@ let send_cam_status = fun a -> let alpha = -. a.course in let east = dx *. cos alpha -. dy *. sin alpha and north = dx *. sin alpha +. dy *. cos alpha in - let utm = LL.utm_add (LL.utm_of WGS84 a.pos) (east, north) in - let wgs84 = LL.of_utm WGS84 utm in + let wgs84 = Aircraft.add_pos_to_nav_ref (Geo a.pos) (east, north) in let twgs84 = Aircraft.add_pos_to_nav_ref nav_ref a.cam.target in let values = ["ac_id", Pprz.String a.id; "cam_lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat); diff --git a/sw/ground_segment/tmtc/server_globals.ml b/sw/ground_segment/tmtc/server_globals.ml index 4b6d968488..0255bca0b1 100644 --- a/sw/ground_segment/tmtc/server_globals.ml +++ b/sw/ground_segment/tmtc/server_globals.ml @@ -8,6 +8,7 @@ let _AUTO2 = 2 let gaz_modes = [|"MANUAL";"GAZ";"CLIMB";"ALT"|] let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|] let gps_modes = [|"NOFIX";"DRO";"2D";"3D";"GPSDRO"|] +let _3D = 3 let gps_hybrid_modes = [|"OFF";"ON"|] let horiz_modes = [|"WAYPOINT";"ROUTE";"CIRCLE";"ATTITUDE"|]