diff --git a/conf/boards/opa_ap_1.0.makefile b/conf/boards/opa_ap_1.0.makefile index 7648433c5ba..131c5874740 100644 --- a/conf/boards/opa_ap_1.0.makefile +++ b/conf/boards/opa_ap_1.0.makefile @@ -42,7 +42,7 @@ GPS_PORT ?= UART1 GPS_BAUD ?= B57600 INTERMCU_PORT ?= UART3 -INTERMCU_BAUD ?= B230400 +INTERMCU_BAUD ?= B460800 # # default IMU configuration diff --git a/conf/boards/opa_ftd_1.0.makefile b/conf/boards/opa_ftd_1.0.makefile index e2b9abea6e2..f396daff0cb 100644 --- a/conf/boards/opa_ftd_1.0.makefile +++ b/conf/boards/opa_ftd_1.0.makefile @@ -40,7 +40,10 @@ MODEM_PORT ?= UART3 MODEM_BAUD ?= B19200 INTERMCU_PORT ?= UART2 -INTERMCU_BAUD ?= B230400 +INTERMCU_BAUD ?= B460800 + +GPS_PORT ?= UART4 +GPS_BAUD ?= B57600 # # default actuator configuration diff --git a/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile b/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile index 9ae81994293..3b3a152067a 100644 --- a/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile +++ b/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile @@ -1,6 +1,7 @@ # # Makefile for shared radio_control spektrum susbsytem # +# Define USE_DSMX on STM32 microcontrollers to bind in DSMX instead of DSM2 RADIO_CONTROL_LED ?= none diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h b/sw/airborne/arch/stm32/mcu_periph/uart_arch.h index eff41e75202..28ee4499070 100644 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h +++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.h @@ -39,6 +39,7 @@ #define B100000 100000 #define B115200 115200 #define B230400 230400 +#define B460800 460800 #define B921600 921600 #define B1500000 1500000 #define B3000000 3000000 diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c index e016973889b..ff42b1f1b53 100644 --- a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c +++ b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c @@ -49,8 +49,13 @@ INFO("Radio-Control now follows PPRZ sign convention: this means you might need #define ONE_MHZ 1000000 /* Number of low pulses sent to satellite receivers */ +#ifdef USE_DSMX +#define MASTER_RECEIVER_PULSES 9 +#define SLAVE_RECEIVER_PULSES 10 +#else #define MASTER_RECEIVER_PULSES 5 #define SLAVE_RECEIVER_PULSES 6 +#endif #define TIM_TICS_FOR_100us 100 #define MIN_FRAME_SPACE 70 // 7ms diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 3fd8dc24669..cbcc99f575b 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -307,7 +307,7 @@ void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end) bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time) { - int32_t dist_to_point; + float dist_to_point; struct Int32Vect2 diff; struct EnuCoor_i *pos = stateGetPositionEnu_i(); @@ -326,13 +326,14 @@ bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t VECT2_DIFF(diff, *wp, *pos); } /* compute distance of estimated/current pos to target wp - * distance with half metric precision (6.25 cm) + * POS_FRAC resolution + * convert to float to compute the norm without overflow in 32bit */ - INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2); - dist_to_point = int32_vect2_norm(&diff); + struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)}; + dist_to_point = float_vect2_norm(&diff_f); /* return TRUE if we have arrived */ - if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) { + if (dist_to_point < ARRIVED_AT_WAYPOINT) { return true; } @@ -341,8 +342,8 @@ bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t /* return TRUE if normal line at the end of the segment is crossed */ struct Int32Vect2 from_diff; VECT2_DIFF(from_diff, *wp, *from); - INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC / 2); - return (diff.x * from_diff.x + diff.y * from_diff.y < 0); + struct FloatVect2 from_diff_f = {POS_FLOAT_OF_BFP(from_diff.x), POS_FLOAT_OF_BFP(from_diff.y)}; + return (diff_f.x * from_diff_f.x + diff_f.y * from_diff_f.y < 0); } return false; @@ -351,7 +352,7 @@ bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time) { uint16_t time_at_wp; - uint32_t dist_to_point; + float dist_to_point; static uint16_t wp_entry_time = 0; static bool wp_reached = false; static struct EnuCoor_i wp_last = { 0, 0, 0 }; @@ -361,10 +362,11 @@ bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time) wp_reached = false; wp_last = *wp; } + VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i()); - INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2); - dist_to_point = int32_vect2_norm(&diff); - if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) { + struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)}; + dist_to_point = float_vect2_norm(&diff_f); + if (dist_to_point < ARRIVED_AT_WAYPOINT) { if (!wp_reached) { wp_reached = true; wp_entry_time = autopilot_flight_time; diff --git a/sw/airborne/subsystems/imu/imu_bebop.c b/sw/airborne/subsystems/imu/imu_bebop.c index b22d67f0116..ee7ad825ce8 100644 --- a/sw/airborne/subsystems/imu/imu_bebop.c +++ b/sw/airborne/subsystems/imu/imu_bebop.c @@ -70,6 +70,8 @@ PRINT_CONFIG_VAR(BEBOP_GYRO_RANGE) #endif PRINT_CONFIG_VAR(BEBOP_ACCEL_RANGE) +struct OrientationReps imu_to_mag_bebop; ///< IMU to magneto rotation + /** Basic Navstik IMU data */ struct ImuBebop imu_bebop; @@ -87,6 +89,15 @@ void imu_bebop_init(void) /* AKM8963 */ ak8963_init(&imu_bebop.ak, &(BEBOP_MAG_I2C_DEV), AK8963_ADDR); + +#if BEBOP_VERSION2 + //the magnetometer of the bebop2 is located on the gps board, + //which is under a slight angle + struct FloatEulers imu_to_mag_eulers = + {0.0, RadOfDeg(8.5), 0.0}; + orientationSetEulers_f(&imu_to_mag_bebop, &imu_to_mag_eulers); +#endif + } /** @@ -132,8 +143,13 @@ void imu_bebop_event(void) if (imu_bebop.ak.data_available) { #if BEBOP_VERSION2 + struct Int32Vect3 mag_temp; // In the second bebop version the magneto is turned 90 degrees - VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.x, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.z); + VECT3_ASSIGN(mag_temp, -imu_bebop.ak.data.vect.x, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.z); + + // Rotate the magneto + struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&imu_to_mag_bebop); + int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &mag_temp); #else //BEBOP regular first verion VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.z); #endif diff --git a/sw/lib/ocaml/fp_proc.ml b/sw/lib/ocaml/fp_proc.ml index e7ac6adbeeb..c38819dcda6 100644 --- a/sw/lib/ocaml/fp_proc.ml +++ b/sw/lib/ocaml/fp_proc.ml @@ -121,6 +121,9 @@ let transform_stage = fun prefix reroutes env xml -> | "attitude" -> let attribs = transform_values ["vmode"] env attribs in Xml.Element (tag, attribs, children) + | "manual" -> + let attribs = transform_values [] env attribs in + Xml.Element (tag, attribs, children) | "go" -> assert (children=[]); let attribs = transform_values ["wp";"from";"hmode";"vmode"] env attribs in @@ -226,6 +229,7 @@ let parse_include = fun dir flight_plan include_xml -> let waypoints = get_children "waypoints" proc and exceptions = get_children "exceptions" proc + and modules = get_children "modules" proc and blocks = get_children "blocks" proc and sectors = get_children "sectors" proc and header = get_pc_data "header" proc in @@ -237,6 +241,7 @@ let parse_include = fun dir flight_plan include_xml -> append_children ["waypoints", waypoints; "blocks", blocks; + "modules", modules; "exceptions", exceptions; "sectors", sectors] (append_pc_data "header" header flight_plan)