diff --git a/conf/airframes/matek_f405_wing/Easyglider_matek_f405_autoload.xml b/conf/airframes/matek_f405_wing/Easyglider_matek_f405_autoload.xml
new file mode 100644
index 00000000000..5544a96a321
--- /dev/null
+++ b/conf/airframes/matek_f405_wing/Easyglider_matek_f405_autoload.xml
@@ -0,0 +1,336 @@
+
+
+
+
+ Easyglider from Multiplex
+ BOARD = Matek f405 Wing
+ IMU = MPU6000
+ BARO = BMP280
+ Radio modem: OPENLRSNG
+ Radio control: OPENLRSNG
+ GPS: Ublox
+ On Board OSD
+ On board current sensor
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/matek_f405_wing/Easyglider_matek_f405_wing.xml b/conf/airframes/matek_f405_wing/Easyglider_matek_f405_wing.xml
new file mode 100644
index 00000000000..07391d8bd4d
--- /dev/null
+++ b/conf/airframes/matek_f405_wing/Easyglider_matek_f405_wing.xml
@@ -0,0 +1,349 @@
+
+
+
+
+ Easyglider from Multiplex
+ BOARD = Matek f405 Wing
+ IMU = MPU6000
+ BARO = BMP280
+ Radio modem: OPENLRSNG
+ Radio control: OPENLRSNG
+ GPS: Ublox
+ On Board OSD
+ On board current sensor
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/boards/matek_f405_wing_v1.makefile b/conf/boards/matek_f405_wing_v1.makefile
new file mode 100644
index 00000000000..c9f77bd685f
--- /dev/null
+++ b/conf/boards/matek_f405_wing_v1.makefile
@@ -0,0 +1,57 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# matek_f405_wing_v1.makefile
+#
+# http://www.mateksys.com/?portfolio=f405-wing
+#
+
+BOARD=matek_f405_wing
+BOARD_VERSION=v1
+BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\"
+
+ARCH=stm32
+ARCH_L=f4
+HARD_FLOAT=yes
+$(TARGET).ARCHDIR = $(ARCH)
+$(TARGET).LDSCRIPT=$(SRC_ARCH)/openpilot_revo.ld
+
+# -----------------------------------------------------------------------
+
+# default flash mode is via SWD
+# other possibilities: DFU, DFU-UTIL, SWD, STLINK
+FLASH_MODE ?= DFU-UTIL
+
+
+
+#
+# default LED configuration
+#
+RADIO_CONTROL_LED ?= none
+BARO_LED ?= none
+AHRS_ALIGNER_LED ?= none
+GPS_LED ?=2
+SYS_TIME_LED ?=1
+
+#
+# default uart configuration
+#
+RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART3
+
+MODEM_PORT ?= UART6
+MODEM_BAUD ?= B57600
+
+GPS_PORT ?= UART1
+GPS_BAUD ?= B38400
+
+
+
+
+#
+# default actuator configuration
+#
+# you can use different actuators by adding a configure option to your firmware section
+# e.g.
+#
+ACTUATORS ?= actuators_pwm
diff --git a/conf/modules/baro_bmp280_i2c.xml b/conf/modules/baro_bmp280_i2c.xml
new file mode 100644
index 00000000000..771de804a0b
--- /dev/null
+++ b/conf/modules/baro_bmp280_i2c.xml
@@ -0,0 +1,26 @@
+
+
+
+
+
+ Bosch-Sensortech BMP280xx pressure sensor
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/board_matek_wing.xml b/conf/modules/board_matek_wing.xml
new file mode 100644
index 00000000000..c8433179556
--- /dev/null
+++ b/conf/modules/board_matek_wing.xml
@@ -0,0 +1,36 @@
+
+
+
+
+
+ Autoload several onboard sensors and subsystems
+ for the Matek F405 Wing board with proper configuration.
+ IMU MPU6000
+ Baro (BMP280)
+ OSD
+ Normal front of the board is on the servo S3-S8 servo connector
+ Normal up of the board is on STM32F405 side.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/osd_max7456.xml b/conf/modules/osd_max7456.xml
index ab69dcf63e7..03063ae577f 100644
--- a/conf/modules/osd_max7456.xml
+++ b/conf/modules/osd_max7456.xml
@@ -23,7 +23,6 @@
-
diff --git a/conf/radios/openTX_radio_8ch.xml b/conf/radios/openTX_radio_8ch.xml
new file mode 100644
index 00000000000..febe4959fe8
--- /dev/null
+++ b/conf/radios/openTX_radio_8ch.xml
@@ -0,0 +1,51 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/telemetry/telemetry_openlrsng_19200.xml b/conf/telemetry/telemetry_openlrsng_19200.xml
new file mode 100644
index 00000000000..619a007e01f
--- /dev/null
+++ b/conf/telemetry/telemetry_openlrsng_19200.xml
@@ -0,0 +1,99 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/boards/matek_f405_wing_v1.h b/sw/airborne/boards/matek_f405_wing_v1.h
new file mode 100644
index 00000000000..74b9a73a6c8
--- /dev/null
+++ b/sw/airborne/boards/matek_f405_wing_v1.h
@@ -0,0 +1,468 @@
+/*
+ * Chris Efstathiou hendrixgr@gmail.com
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#ifndef CONFIG_MATEK_F405_WING_1_0_H
+#define CONFIG_MATEK_F405_WING_1_0_H
+
+#define BOARD_MATEK_F405_WING
+
+/* The Matek F405 Wing autopilot has a 8MHz external clock and 168MHz internal. */
+#define EXT_CLK 8000000
+#define AHB_CLK 168000000
+
+// Onboard LEDs
+/* STAT blue, on PB5 */
+#ifndef USE_LED_1
+#define USE_LED_1 1
+#endif
+#define LED_1_GPIO GPIOA
+#define LED_1_GPIO_PIN GPIO14
+#define LED_1_GPIO_ON gpio_clear
+#define LED_1_GPIO_OFF gpio_set
+#define LED_1_AFIO_REMAP ((void)0)
+
+/* WARN red, on PB4 */
+#ifndef USE_LED_2
+#define USE_LED_2 1
+#endif
+#define LED_2_GPIO GPIOA
+#define LED_2_GPIO_PIN GPIO13
+#define LED_2_GPIO_ON gpio_clear
+#define LED_2_GPIO_OFF gpio_set
+#define LED_2_AFIO_REMAP ((void)0)
+
+// LED STRIP 2812
+#ifndef USE_LED_3
+#define USE_LED_3 1
+#endif
+#define LED_3_GPIO GPIOA
+#define LED_3_GPIO_PIN GPIO15
+#define LED_3_GPIO_ON gpio_clear
+#define LED_3_GPIO_OFF gpio_set
+#define LED_3_AFIO_REMAP ((void)0)
+
+// BEEPER
+#ifndef USE_LED_4
+#define USE_LED_4 1
+#endif
+#define LED_4_GPIO GPIOC
+#define LED_4_GPIO_PIN GPIO15
+#define LED_4_GPIO_ON gpio_clear
+#define LED_4_GPIO_OFF gpio_set
+#define LED_4_AFIO_REMAP ((void)0)
+
+
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
+#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
+#define ActuatorsDefaultInit() ActuatorsPwmInit()
+#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
+
+
+#define VBUS_GPIO GPIOC
+#define VBUS_GPIO_PIN GPIO13
+
+/* UART */
+//CAN BE USED AS GPS SERIAL PORT
+#define UART1_GPIO_AF GPIO_AF7
+#define UART1_GPIO_PORT_TX GPIOA
+#define UART1_GPIO_TX GPIO9
+#define UART1_GPIO_PORT_RX GPIOA
+#define UART1_GPIO_RX GPIO10
+
+// UART 2 RX INPUT IS USED AS THE PPM INPUT THUS I WILL USE THE TX OUTPUT AS ADC INPUT
+#define UART2_GPIO_AF GPIO_AF8
+#define UART2_GPIO_PORT_TX GPIOA
+#define UART2_GPIO_TX GPIO2
+#define UART2_GPIO_PORT_RX GPIOA
+#define UART2_GPIO_RX GPIO3
+
+#define UART3_GPIO_AF GPIO_AF8
+#define UART3_GPIO_PORT_TX GPIOC
+#define UART3_GPIO_TX GPIO10
+#define UART3_GPIO_PORT_RX GPIOC
+#define UART3_GPIO_RX GPIO11
+
+#define UART4_GPIO_AF GPIO_AF8
+#define UART4_GPIO_PORT_TX GPIOA
+#define UART4_GPIO_TX GPIO0
+#define UART4_GPIO_PORT_RX GPIOA
+#define UART4_GPIO_RX GPIO1
+
+#define UART5_GPIO_AF GPIO_AF8
+#define UART5_GPIO_PORT_TX GPIOC
+#define UART5_GPIO_TX GPIO12
+#define UART5_GPIO_PORT_RX GPIOD
+#define UART5_GPIO_RX GPIO2
+
+// CAN BE USED AS A MODEM SERIAL PORT
+#define UART6_GPIO_AF GPIO_AF8
+#define UART6_GPIO_PORT_TX GPIOC
+#define UART6_GPIO_TX GPIO6
+#define UART6_GPIO_PORT_RX GPIOC
+#define UART6_GPIO_RX GPIO7
+
+// SPI1, MPU6000 ON SPI1
+#define SPI1_GPIO_AF GPIO_AF5
+
+#define SPI1_GPIO_PORT_SCK GPIOA
+#define SPI1_GPIO_SCK GPIO5
+#define SPI1_GPIO_PORT_MISO GPIOA
+#define SPI1_GPIO_MISO GPIO6
+#define SPI1_GPIO_PORT_MOSI GPIOA
+#define SPI1_GPIO_MOSI GPIO7
+#define SPI1_GPIO_PORT_NSS GPIOA
+#define SPI1_GPIO_NSS GPIO4
+
+#define SPI_SELECT_SLAVE0_PORT GPIOA
+#define SPI_SELECT_SLAVE0_PIN GPIO4
+
+// SPI2 IS USED FOR THE MAX7456 OSD
+#define SPI2_GPIO_AF GPIO_AF5
+
+#define SPI2_GPIO_PORT_SCK GPIOB
+#define SPI2_GPIO_SCK GPIO13
+#define SPI2_GPIO_PORT_MISO GPIOC
+#define SPI2_GPIO_MISO GPIO2
+#define SPI2_GPIO_PORT_MOSI GPIOC
+#define SPI2_GPIO_MOSI GPIO3
+#define SPI2_GPIO_PORT_NSS GPIOB
+#define SPI2_GPIO_NSS GPIO12
+
+#define SPI_SELECT_SLAVE1_PORT GPIOB
+#define SPI_SELECT_SLAVE1_PIN GPIO12
+
+// SDCARD ON SPI3
+#define SPI3_GPIO_AF GPIO_AF5
+
+#define SPI3_GPIO_PORT_SCK GPIOB
+#define SPI3_GPIO_SCK GPIO3
+#define SPI3_GPIO_PORT_MISO GPIOB
+#define SPI3_GPIO_MISO GPIO4
+#define SPI3_GPIO_PORT_MOSI GPIOB
+#define SPI3_GPIO_MOSI GPIO5
+#define SPI3_GPIO_PORT_NSS GPIOC
+#define SPI3_GPIO_NSS GPIO14
+
+#define SPI_SELECT_SLAVE2_PORT GPIOC
+#define SPI_SELECT_SLAVE2_PIN GPIO14
+
+
+/* I2C mapping */
+/* HMC5883L mag on I2C1 with DRDY on PB7 */
+/* MS5611 baro on I2C1 */
+#define I2C1_GPIO_PORT GPIOB
+#define I2C1_GPIO_SCL GPIO8
+#define I2C1_GPIO_SDA GPIO9
+
+#define I2C2_GPIO_PORT GPIOB
+#define I2C2_GPIO_SCL GPIO10
+#define I2C2_GPIO_SDA GPIO11
+
+// ADC
+
+/* Onboard ADCs */
+/*
+ ADC1 PC2/ADC1,2,3 channel 12 (Voltage input 3.3v max)
+ ADC2 PC1/ADC1,2,3 channel 11 (Current input 3.3v max)
+ ADC3 PA3/ADC1,2,3 channel 3
+ ADC4 PA2/ADC1,2,3 channel 2
+*/
+
+/* provide defines that can be used to access the ADC_x in the code or airframe file
+ * these directly map to the index number of the 4 adc channels defined above
+ * 4th (index 3) is used for bat monitoring by default
+ */
+
+#define USE_AD_TIM2 1
+
+#ifndef USE_ADC_1
+#define USE_ADC_1 1
+#endif
+
+#ifndef USE_ADC_2
+#define USE_ADC_2 1
+#endif
+
+#ifndef USE_ADC_3
+#define USE_ADC_3 1
+#endif
+
+#ifndef USE_ADC_4
+#define USE_ADC_4 1
+#endif
+
+// POWER SUPPLY VOLTAGE MEASUREMENT INPUT
+#if USE_ADC_1
+#define AD1_1_CHANNEL 10
+#define ADC_1 AD1_1
+#define ADC_1_GPIO_PORT GPIOC
+#define ADC_1_GPIO_PIN GPIO0
+#endif
+
+// CURRENT MEASUREMENT INPUT
+#if USE_ADC_2
+#define AD1_2_CHANNEL 11
+#define ADC_2 AD1_2
+#define ADC_2_GPIO_PORT GPIOC
+#define ADC_2_GPIO_PIN GPIO1
+#ifndef CURRENT_ADC_IN
+#define CURRENT_ADC_IN ADC_2
+#endif
+#endif
+
+// RSSI MEASUREMENT INPUT
+#if USE_ADC_3
+#define AD1_3_CHANNEL 15
+#define ADC_3 AD1_3
+#define ADC_3_GPIO_PORT GPIOC
+#define ADC_3_GPIO_PIN GPIO5
+#endif
+
+// FREE, LABELED AS UART2 TX PIN
+#if USE_ADC_4
+#define AD1_4_CHANNEL 2
+#define ADC_4 AD1_4
+#define ADC_4_GPIO_PORT GPIOA
+#define ADC_4_GPIO_PIN GPIO2
+#endif
+
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
+#define ADC_CHANNEL_VSUPPLY ADC_1
+#endif
+
+#ifndef CURRENT_ADC_IN
+#define CURRENT_ADC_IN ADC_2
+#endif
+
+/* no voltage divider on board, adjust VoltageOfAdc in airframe file */
+#define DefaultVoltageOfAdc(adc) (0.008830925*adc)
+#define DefaultMilliAmpereOfAdc(adc) (25*adc)
+
+#define UART2_RX 1
+#define SERVO9_PWM_OUT 2
+
+#if defined(RADIO_CONTROL_PPM_PIN) && RADIO_CONTROL_PPM_PIN == UART2_RX
+
+// THE PPM INPUT IS ALSO THE UART2 RX
+#define USE_PPM_TIM9 1
+#define PPM_CHANNEL TIM_IC2
+#define PPM_TIMER_INPUT TIM_IC_IN_TI2
+#define PPM_IRQ NVIC_TIM1_BRK_TIM9_IRQ
+// Capture/Compare InteruptEnable and InterruptFlag
+#define PPM_CC_IE TIM_DIER_CC2IE
+#define PPM_CC_IF TIM_SR_CC2IF
+#define PPM_GPIO_PORT GPIOA
+#define PPM_GPIO_PIN GPIO3
+#define PPM_GPIO_AF GPIO_AF3
+
+#else
+
+#define USE_PPM_TIM1 1
+#define PPM_CHANNEL TIM_IC1
+#define PPM_TIMER_INPUT TIM_IC_IN_TI1
+#define PPM_IRQ NVIC_TIM1_CC_IRQ
+// Capture/Compare InteruptEnable and InterruptFlag
+#define PPM_CC_IE TIM_DIER_CC1IE
+#define PPM_CC_IF TIM_SR_CC1IF
+#define PPM_GPIO_PORT GPIOA
+#define PPM_GPIO_PIN GPIO8
+#define PPM_GPIO_AF GPIO_AF1
+
+#endif
+
+// SERVO DEFINITIONS
+#define PWM_USE_TIM1 0
+#define PWM_USE_TIM3 1
+#define PWM_USE_TIM4 1
+#define PWM_USE_TIM8 1
+#define PWM_USE_TIM12 1
+
+#define USE_PWM1 1
+#define USE_PWM2 1
+#define USE_PWM3 1
+#define USE_PWM4 1
+#define USE_PWM5 1
+#define USE_PWM6 1
+#define USE_PWM7 1
+#define USE_PWM8 1
+#define USE_PWM9 0
+
+// PWM_SERVO_x is the index of the servo in the actuators_pwm_values array
+#if USE_PWM1
+#define PWM_SERVO_1 0
+#define PWM_SERVO_1_TIMER TIM4
+#define PWM_SERVO_1_GPIO GPIOB
+#define PWM_SERVO_1_PIN GPIO7
+#define PWM_SERVO_1_AF GPIO_AF2
+#define PWM_SERVO_1_OC TIM_OC2
+#define PWM_SERVO_1_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_1_OC_BIT 0
+#endif
+
+#if USE_PWM2
+#define PWM_SERVO_2 1
+#define PWM_SERVO_2_TIMER TIM4
+#define PWM_SERVO_2_GPIO GPIOB
+#define PWM_SERVO_2_PIN GPIO6
+#define PWM_SERVO_2_AF GPIO_AF2
+#define PWM_SERVO_2_OC TIM_OC1
+#define PWM_SERVO_2_OC_BIT (1<<0)
+#else
+#define PWM_SERVO_2_OC_BIT 0
+#endif
+
+#if USE_PWM3
+#define PWM_SERVO_3 2
+#define PWM_SERVO_3_TIMER TIM3
+#define PWM_SERVO_3_GPIO GPIOB
+#define PWM_SERVO_3_PIN GPIO0
+#define PWM_SERVO_3_AF GPIO_AF2
+#define PWM_SERVO_3_OC TIM_OC3
+#define PWM_SERVO_3_OC_BIT (1<<2)
+#else
+#define PWM_SERVO_3_OC_BIT 0
+#endif
+
+#if USE_PWM4
+#define PWM_SERVO_4 3
+#define PWM_SERVO_4_TIMER TIM3
+#define PWM_SERVO_4_GPIO GPIOB
+#define PWM_SERVO_4_PIN GPIO1
+#define PWM_SERVO_4_AF GPIO_AF2
+#define PWM_SERVO_4_OC TIM_OC4
+#define PWM_SERVO_4_OC_BIT (1<<3)
+#else
+#define PWM_SERVO_4_OC_BIT 0
+#endif
+
+#if USE_PWM5
+#define PWM_SERVO_5 4
+#define PWM_SERVO_5_TIMER TIM8
+#define PWM_SERVO_5_GPIO GPIOC
+#define PWM_SERVO_5_PIN GPIO8
+#define PWM_SERVO_5_AF GPIO_AF3
+#define PWM_SERVO_5_OC TIM_OC3
+#define PWM_SERVO_5_OC_BIT (1<<2)
+#else
+#define PWM_SERVO_5_OC_BIT 0
+#endif
+
+#if USE_PWM6
+#define PWM_SERVO_6 5
+#define PWM_SERVO_6_TIMER TIM8
+#define PWM_SERVO_6_GPIO GPIOC
+#define PWM_SERVO_6_PIN GPIO9
+#define PWM_SERVO_6_AF GPIO_AF3
+#define PWM_SERVO_6_OC TIM_OC4
+#define PWM_SERVO_6_OC_BIT (1<<3)
+#else
+#define PWM_SERVO_6_OC_BIT 0
+#endif
+
+#if USE_PWM7
+#define PWM_SERVO_7 6
+#define PWM_SERVO_7_TIMER TIM12
+#define PWM_SERVO_7_GPIO GPIOB
+#define PWM_SERVO_7_PIN GPIO14
+#define PWM_SERVO_7_AF GPIO_AF9
+#define PWM_SERVO_7_OC TIM_OC1
+#define PWM_SERVO_7_OC_BIT (1<<0)
+#else
+#define PWM_SERVO_7_OC_BIT 0
+#endif
+
+#if USE_PWM8
+#define PWM_SERVO_8 7
+#define PWM_SERVO_8_TIMER TIM12
+#define PWM_SERVO_8_GPIO GPIOB
+#define PWM_SERVO_8_PIN GPIO15
+#define PWM_SERVO_8_AF GPIO_AF9
+#define PWM_SERVO_8_OC TIM_OC2
+#define PWM_SERVO_8_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_8_OC_BIT 0
+#endif
+
+#if USE_PWM9
+#define PWM_SERVO_9 8
+#define PWM_SERVO_9_TIMER TIM1
+#define PWM_SERVO_9_GPIO GPIOA
+#define PWM_SERVO_9_PIN GPIO8
+#define PWM_SERVO_9_AF GPIO_AF1
+#define PWM_SERVO_9_OC TIM_OC1
+#define PWM_SERVO_9_OC_BIT (1<<0)
+#else
+#define PWM_SERVO_9_OC_BIT 0
+#endif
+
+
+// servos 1-2 on TIM4
+#define PWM_TIM4_CHAN_MASK (PWM_SERVO_1_OC_BIT | PWM_SERVO_2_OC_BIT)
+// servos 3-4 on TIM3
+#define PWM_TIM3_CHAN_MASK (PWM_SERVO_3_OC_BIT | PWM_SERVO_4_OC_BIT)
+// servos 5-6 on TIM8
+#define PWM_TIM8_CHAN_MASK (PWM_SERVO_5_OC_BIT | PWM_SERVO_6_OC_BIT)
+// servos 7-8 on TIM12
+#define PWM_TIM12_CHAN_MASK (PWM_SERVO_7_OC_BIT | PWM_SERVO_8_OC_BIT)
+// servo 9 on TIM1
+#if USE_PWM9
+#define PWM_TIM1_CHAN_MASK (PWM_SERVO_9_OC_BIT)
+#endif
+
+
+/*
+ * Spektrum
+ */
+/* The line that is pulled low at power up to initiate the bind process */
+#define SPEKTRUM_BIND_PIN GPIO0
+#define SPEKTRUM_BIND_PIN_PORT GPIOB
+
+#define SPEKTRUM_UART1_RCC RCC_USART1
+#define SPEKTRUM_UART1_BANK GPIOA
+#define SPEKTRUM_UART1_PIN GPIO10
+#define SPEKTRUM_UART1_AF GPIO_AF7
+#define SPEKTRUM_UART1_IRQ NVIC_USART1_IRQ
+#define SPEKTRUM_UART1_ISR usart1_isr
+#define SPEKTRUM_UART1_DEV USART1
+
+#define SPEKTRUM_UART2_RCC RCC_USART2
+#define SPEKTRUM_UART2_BANK GPIOA
+#define SPEKTRUM_UART2_PIN GPIO3
+#define SPEKTRUM_UART2_AF GPIO_AF8
+#define SPEKTRUM_UART2_IRQ NVIC_USART2_IRQ
+#define SPEKTRUM_UART2_ISR usart2_isr
+#define SPEKTRUM_UART2_DEV USART2
+
+#define SPEKTRUM_UART5_RCC RCC_UART5
+#define SPEKTRUM_UART5_BANK GPIOD
+#define SPEKTRUM_UART5_PIN GPIO2
+#define SPEKTRUM_UART5_AF GPIO_AF8
+#define SPEKTRUM_UART5_IRQ NVIC_UART5_IRQ
+#define SPEKTRUM_UART5_ISR uart5_isr
+#define SPEKTRUM_UART5_DEV UART5
+
+
+
+#endif // CONFIG_MATEK_F405_WING_1_0_H
diff --git a/sw/airborne/modules/display/max7456.c b/sw/airborne/modules/display/max7456.c
index 6ca07fd4058..2d12f9152db 100644
--- a/sw/airborne/modules/display/max7456.c
+++ b/sw/airborne/modules/display/max7456.c
@@ -28,6 +28,8 @@
#include "std.h"
//#include "stdio.h"
+#include "inter_mcu.h"
+
#include "mcu_periph/sys_time.h"
#include "mcu_periph/gpio.h"
#include "mcu_periph/spi.h"
@@ -39,8 +41,9 @@
#include "state.h"
// for GetPosAlt, include correct header until we have unified API
-#ifdef AP
-#include "subsystems/navigation/nav.h"
+#if AP
+//#include "subsystems/navigation/nav.h"
+#include "subsystems/navigation/common_nav.h"
#else
#include "firmwares/rotorcraft/navigation.h"
#endif
@@ -52,6 +55,23 @@
#define OSD_STRING_SIZE 31
#define osd_sprintf _osd_sprintf
+typedef struct {
+ float fx;
+ float fy;
+ float fz;
+} VECTOR;
+
+typedef struct {
+ float fx1; float fx2; float fx3;
+ float fy1; float fy2; float fy3;
+ float fz1; float fz2; float fz3;
+} MATRIX;
+
+
+static void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC);
+static void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC);
+static void check_osd_status(void);
+static float home_direction(void);
static char ascii_to_osd_c(char c);
static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t row, uint8_t column);
static bool _osd_sprintf(char *buffer, char *string, float value);
@@ -84,6 +104,10 @@ enum max7456_osd_status_codes {
enum osd_attributes {
BLINK = OSD_BLINK_CHAR,
INVERT = OSD_INVERT_PIXELS,
+ L_JUST = 0x00,
+ R_JUST = 0x01,
+ C_JUST = 0x02,
+
};
uint8_t max7456_osd_status = OSD_UNINIT;
@@ -91,6 +115,378 @@ uint8_t osd_enable = true;
uint8_t osd_enable_val = OSD_IMAGE_ENABLE;
uint8_t osd_stat_reg = 0;
bool osd_stat_reg_valid = false;
+float home_dir = 0;
+
+
+//*******************************************************************
+// function name: vSubtractVectors
+// description: subtracts two vectors a = b - c
+// parameters:
+//*******************************************************************
+static void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC)
+{
+ svA->fx = svB.fx - svC.fx;
+ svA->fy = svB.fy - svC.fy;
+ svA->fz = svB.fz - svC.fz;
+}
+
+//*******************************************************************
+// function name: vMultiplyMatrixByVector
+// description: multiplies matrix by vector svA = smB * svC
+// parameters:
+//*******************************************************************
+static void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC)
+{
+ svA->fx = smB.fx1 * svC.fx + smB.fx2 * svC.fy + smB.fx3 * svC.fz;
+ svA->fy = smB.fy1 * svC.fx + smB.fy2 * svC.fy + smB.fy3 * svC.fz;
+ svA->fz = smB.fz1 * svC.fx + smB.fz2 * svC.fy + smB.fz3 * svC.fz;
+}
+
+
+static float home_direction(void)
+{
+
+ static VECTOR svPlanePosition,
+ Home_Position,
+ Home_PositionForPlane,
+ Home_PositionForPlane2;
+
+ static MATRIX smRotation;
+
+ /*
+ By swapping coordinates (fx=fPlaneNorth, fy=fPlaneEast) we make the the circle angle go from 0 (0 is to the top of the circle)
+ to 360 degrees or from 0 radians to 2 PI radians in a clockwise rotation. This way the GPS reported angle can be directly
+ applied to the rotation matrices (in radians).
+ In standard mathematical notation 0 is to the right (East) of the circle, -90 is to the bottom, +-180 is to the left
+ and +90 is to the top (counterclockwise rotation).
+ When reading back the actual rotated coordinates fx has the y coordinate and fy has the x
+ represented on a circle in standard mathematical notation.
+ */
+
+ svPlanePosition.fx = stateGetPositionEnu_f()->y;
+ svPlanePosition.fy = stateGetPositionEnu_f()->x;
+ svPlanePosition.fz = stateGetPositionUtm_f()->alt;
+#ifdef AP
+ Home_Position.fx = WaypointY(WP_HOME);
+ Home_Position.fy = WaypointX(WP_HOME);
+ Home_Position.fz = ground_alt;
+#else
+ Home_Position.fx = waypoint_get_x(WP_HOME);
+ Home_Position.fy = waypoint_get_y(WP_HOME);
+ Home_Position.fz = 0;
+#endif
+
+ /* distance between plane and object */
+ vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition);
+
+ /* yaw */
+ smRotation.fx1 = cosf(stateGetHorizontalSpeedDir_f());
+ smRotation.fx2 = sinf(stateGetHorizontalSpeedDir_f());
+ smRotation.fx3 = 0.;
+ smRotation.fy1 = -smRotation.fx2;
+ smRotation.fy2 = smRotation.fx1;
+ smRotation.fy3 = 0.;
+ smRotation.fz1 = 0.;
+ smRotation.fz2 = 0.;
+ smRotation.fz3 = 1.;
+
+ vMultiplyMatrixByVector(&Home_PositionForPlane2, smRotation, Home_PositionForPlane);
+
+ /* DEFAULT ORIENTATION IS 0 = FRONT, 90 = RIGHT, 180 = BACK, -90 = LEFT
+ *
+ * WHEN home_dir = (float)(atan2(Home_PositionForPlane2.fy, (Home_PositionForPlane2.fx)));
+ *
+ * plane front
+ *
+ * 0˚
+ ^
+ * I
+ * -45˚ I 45˚
+ * \ I /
+ * \I/
+ * -90˚-------I------- 90˚
+ * /I\
+ * / I \
+ * -135˚ I 120˚
+ * I
+ * 180
+ * plane back
+ *
+ *
+ */
+
+ /*
+ * WHEN home_dir = (float)(atan2(Home_PositionForPlane2.fx, (Home_PositionForPlane2.fy)));
+ *
+ *
+ * plane front
+ *
+ * 90˚
+ ^
+ * I
+ * 135˚ I 45˚
+ * \ I /
+ * \I/
+ * 180˚-------I------- 0˚
+ * /I\
+ * / I \
+ * -135˚ I -45˚
+ * I
+ * -90
+ * plane back
+ *
+ *
+ */
+
+ /* fPan = 0˚ -> antenna looks along the wing
+ 90˚ -> antenna looks in flight direction
+ -90˚ -> antenna looks backwards
+ */
+ /* fixed to the plane*/
+ home_dir = (float)(atan2(Home_PositionForPlane2.fy, (Home_PositionForPlane2.fx)));
+ home_dir = DegOfRad(home_dir);
+ if (home_dir < 0) { home_dir += 360; }
+
+ return (home_dir);
+}
+
+static void check_osd_status(void)
+{
+
+ osd_stat_reg_valid = FALSE;
+
+ if (max7456_osd_status == OSD_IDLE) {
+ max7456_trans.output_length = 1;
+ max7456_trans.input_length = 1;
+ max7456_trans.output_buf[0] = OSD_STAT_REG;
+ max7456_osd_status = OSD_READ_STATUS;
+ spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
+ }
+
+ return;
+}
+
+static char ascii_to_osd_c(char c)
+{
+
+#if defined USE_MATEK_TYPE_OSD_CHIP && USE_MATEK_TYPE_OSD_CHIP == 1
+PRINT_CONFIG_MSG("OSD USES THE CUSTOM MATEK TYPE OSD CHIP")
+
+ return (c);
+
+#else
+
+ if (c >= '0' && c <= '9') {
+ if (c == '0') { c -= 38; } else { c -= 48; }
+ } else if (c >= 'A' && c <= 'Z') {
+ c -= 54;
+ } else if (c >= 'a' && c <= 'z') {
+ c -= 60;
+
+ } else {
+ switch (c) {
+ case ('('): c = 0x3f; break;
+ case (')'): c = 0x40; break;
+ case ('.'): c = 0x41; break;
+ case ('?'): c = 0x42; break;
+ case (';'): c = 0x43; break;
+ case (':'): c = 0x44; break;
+ case (','): c = 0x45; break;
+ //case('''): c = 0x46; break;
+ case ('/'): c = 0x47; break;
+ case ('"'): c = 0x48; break;
+ case ('-'): c = 0x49; break;
+ case ('<'): c = 0x4A; break;
+ case ('>'): c = 0x4B; break;
+ case ('@'): c = 0x4C; break;
+ case (' '): c = 0x00; break;
+ case ('\0'): c = 0xFF; break;
+ default : break;
+ }
+ }
+
+ return (c);
+
+#endif
+}
+
+static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t row, uint8_t column)
+{
+
+ int8_t x = 0, idx = 0, post_offset = 0, aft_offset = 0, string_len = 0;
+ char osd_buf[OSD_STRING_SIZE];
+
+ if (row > 15) { column = 15; }
+ if (column > 29) { column = 29; }
+
+// translate the string and put it to the "osd_string" '\0' = 0xff
+ x = 0;
+ while (*(string + x) != '\0') { osd_string[x] = ascii_to_osd_c(*(string + x)); x++; }
+ osd_string[x] = 0xff;
+ string_len = x;
+ idx = x;
+
+ if (attributes & C_JUST) {
+ if (char_nb % 2 == 0) { char_nb++; }
+ post_offset = (char_nb - string_len) / 2;
+ aft_offset = char_nb - string_len;
+ if (((int8_t)column - (char_nb / 2)) >= 0) { column -= (char_nb / 2); }
+ for (x = 0; x < 30; x++) { osd_buf[x] = 0; } // FILL WITH SPACES
+ // COPY THE ORIGINAL STRING TO ITS NEW POSITION
+ for (x = 0; x < string_len; x++) { osd_buf[post_offset + x] = osd_string[x]; }
+ osd_buf[string_len + aft_offset] = 0xFF; // TERMINATE THE MODIFIED STRING
+ // COPY THE MODIFIED STRING TO MAIN OSD STRING
+ x = 0;
+ do { osd_string[x] = osd_buf[x]; } while (osd_buf[x++] != 0xFF);
+ } else if (attributes & R_JUST) {
+ //if(x){ x -= 1; }
+ //if (char_nb < string_len){ char_nb = string_len; }
+ if (((int8_t)column - char_nb) >= 0) { column -= char_nb; }
+ if (((int8_t)char_nb - string_len) >= 0) { post_offset = char_nb - string_len; } else {post_offset = 0; }
+ //ADD LEADING SPACES
+ //First shift right the string and then add spaces at the beggining
+ while (idx >= 0) { osd_string[idx + post_offset] = osd_string[idx]; idx--; }
+ idx = 0;
+ while (idx < post_offset) { osd_string[idx] = 0; idx++; }
+ //osd_string[idx] = 0xff;
+
+ } else {
+ //Adjust for the reserved character number.
+ for (x = 0; x < (int8_t)(sizeof(osd_string)); x++) { if (osd_string[x] == 0xFF) { break; } }
+ for (; x < char_nb; x++) { osd_string[x] = 0; }
+ osd_string[x] = 0xff;
+ }
+
+ osd_char_address = ((uint16_t)row * 30) + column;
+ osd_attr = (attributes & (BLINK | INVERT));
+//TRIGGER THE SPI TRANSFERS. The rest of the spi transfers occur in the "max7456_event" function.
+ if (max7456_osd_status == OSD_IDLE) {
+ max7456_trans.output_length = 2;
+ max7456_trans.output_buf[0] = OSD_DMAH_REG;
+ max7456_trans.output_buf[1] = (uint8_t)((osd_char_address >> 8) & 0x0001);
+ max7456_osd_status = OSD_S_STEP1;
+ spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
+
+ }
+
+ return;
+}
+
+
+static bool _osd_sprintf(char *buffer, char *string, float value)
+{
+
+ uint8_t param_start = 0;
+ uint8_t param_end = 0;
+ uint8_t frac_nb = 0;
+ uint8_t digit = 0;
+ uint8_t x = 0, y = 0, z = 0;
+
+ uint16_t i_dec = 0;
+ uint16_t i_frac = 0;
+
+ char to_asc[10] = {48, 48, 48, 48, 48, 48, 48, 48, 48, 48};
+ char string_buf[OSD_STRING_SIZE];
+
+// Clear the osd string.
+ for (x = 0; x < sizeof(osd_string); x++) { osd_string[x] = 0; }
+ for (x = 0; x < sizeof(string_buf); x++) { string_buf[x] = 0; }
+
+//copy the string passed as parameter to a buffer
+ for (x = 0; x < sizeof(string_buf); x++) { string_buf[x] = *(string + x); if (string_buf[x] == '\0') { break; } }
+ x = 0;
+ param_start = 0;
+ param_end = 0;
+//do {
+ //Now check for any special character
+ while (string_buf[x] != '\0') {
+ // EXAMPLE: in "%160c"x is '%' x+4 = 'c' and x+1='1', x+2='6' and x+3='0'
+ if (string_buf[x] == '%') { if (string_buf[x + 4] == 'c') { (param_start = x + 1); param_end = x + 3; break; } }
+ x++;
+ }
+ if (param_end - param_start) {
+ //load the special character value where the % character was
+ string_buf[x] = ((string_buf[param_start] - 48) * 100) + ((string_buf[param_start + 1] - 48) * 10) +
+ (string_buf[param_start + 2] - 48);
+ x++; // increment x to the next character which should be the first special character's digit
+ //Move the rest of the buffer forward so only the special character remains,
+ // for example in %170c '%' now has the special character's code and x now points to '1'
+ // which will be overwritten with the rest of the string after the 'c'
+ for (y = (x + 4); y <= sizeof(string_buf); y++) { string_buf[x++] = string_buf[y]; }
+ }
+
+//}while((param_end-param_start > 0));
+
+// RESET THE USED VARIABLES JUST TO BE SAFE.
+ x = 0;
+ y = 0;
+ param_start = 0;
+ param_end = 0;
+// Search for the prameter start and stop positions.
+ while (string_buf[x] != '\0') {
+ if (string_buf[x] == '%') {
+ param_start = x;
+
+ } else if (string_buf[x] == 'f') { param_end = x; break; }
+ x++;
+ }
+ if (param_end - param_start) {
+ // find and bound the precision specified.
+ frac_nb = string_buf[param_end - 1] - 48; // Convert to number, ASCII 48 = '0'
+ if (frac_nb > 3) { frac_nb = 3; } // Bound value.
+
+ y = (sizeof(to_asc) - 1); // Point y to the end of the array.
+ i_dec = abs((int16_t)value);
+ // Fist we will deal with the fractional part if specified.
+ if (frac_nb > 0 && frac_nb <= 3) {
+ i_frac = abs((int16_t)((value - (int16_t)value) * 1000)); // Max precision is 3 digits.
+ x = 100;
+ z = frac_nb;
+ do { // Example if frac_nb=2 then 952 will show as .95
+ z--;
+ digit = (i_frac / x);
+ to_asc[y + z] = digit + 48; // Convert to ASCII
+ i_frac -= digit * x; // Calculate the remainder.
+ x /= 10; // 952-(9*100) = 52, 52-(10*5)=2 etc.
+
+ } while (z > 0);
+
+ y -= frac_nb; // set y to point where the dot must be placed.
+ to_asc[y] = '.';
+ y--; // Set y to point where the rest of the numbers must be written.
+
+ } // if (frac_nb > 0 && frac_nb <= 3){
+
+ // Now it is time for the integer part. "y" already points to the position just before the dot.
+ do {
+ to_asc[y] = (i_dec % 10) + 48; //Write at least one digit even if value is zero.
+ i_dec /= 10;
+ if (i_dec <= 0) { // This way the leading zero is ommited.
+ if (value < 0) { y--; to_asc[y] = '-'; } // Place the minus sign if needed.
+ break;
+
+ } else { y--; }
+
+ } while (1);
+
+ // Fill the buffer with the characters in the beggining of the string if any.
+ for (x = 0; x < param_start; x++) { *(buffer + x) = string_buf[x]; }
+
+ // x is now pointing to the next character in osd_string.
+ // y is already pointing to the first digit or negative sign in "to_asc" array.
+ while (y < sizeof(to_asc)) { *(buffer + x) = to_asc[y]; x++; y++; }
+ // x is now pointing to the next character in osd_string.
+ // "param_end" is pointing to the last format character in the string.
+ do {
+ param_end++;
+ *(buffer + x++) = string_buf[param_end];
+
+ } while (string_buf[param_end] != '\0'); //Write the rest of the string including the terminating char.
+
+ } // End of if (param_end - param_start)
+
+ return (0);
+}
void max7456_init(void)
{
@@ -120,6 +516,15 @@ void max7456_periodic(void)
{
float temp = 0;
+ struct FloatEulers *att = stateGetNedToBodyEulers_f();
+ struct EnuCoor_f *pos = stateGetPositionEnu_f();
+#if AP
+ float ph_x = waypoints[WP_HOME].x - pos->x;
+ float ph_y = waypoints[WP_HOME].y - pos->y;
+#else
+ float ph_x = waypoint_get_x(WP_HOME) - pos->x;
+ float ph_y = waypoint_get_y(WP_HOME) - pos->y;
+#endif
//This code is executed always and checks if the "osd_enable" var has been changed by telemetry.
//If yes then it commands a reset but this time turns on or off the osd overlay, not the video.
if (max7456_osd_status == OSD_IDLE) {
@@ -148,56 +553,151 @@ void max7456_periodic(void)
max7456_trans.output_buf[0] = OSD_OSDBL_REG_R;
max7456_osd_status = OSD_INIT3;
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
- } else if (max7456_osd_status == OSD_IDLE && osd_enable > 0) { // DRAW THE OSD SCREEN
- //draw_osd();
+ } else if (max7456_osd_status == OSD_IDLE && osd_enable > 0) {
switch (step) {
case (0):
- osd_put_s("HDG", FALSE, 3, 0, 13);
+ osd_put_s("HDG", FALSE, 3, 1, 14);
+ step = 1;
+ break;
+ case (1):
+#if !defined USE_MATEK_TYPE_OSD_CHIP || USE_MATEK_TYPE_OSD_CHIP == 0
+ osd_put_s("DISTANCE", FALSE, 8, 14, 12);
+#endif
step = 10;
break;
case (10):
- osd_sprintf(osd_string, "%.2fV", electrical.vsupply);
- if (electrical.vsupply > LOW_BAT_LEVEL) {
- osd_put_s(osd_string, FALSE, 8, 0, 2);
- } else {
- osd_put_s(osd_string, BLINK | INVERT, 8, 0, 2);
- }
+ osd_put_s("( )", FALSE, 3, 8, 14);
step = 20;
break;
case (20):
-#if MAG_HEADING_AVAILABLE && !defined(SITL)
- temp = DegOfRad(MAG_Heading);
- if (temp < 0) {
- temp += 360;
- }
+ temp = ((float)electrical.vsupply);
+ osd_sprintf(osd_string, "%.1fV", temp);
+ if (temp > LOW_BAT_LEVEL) {
+ osd_put_s(osd_string, L_JUST, 5, 1, 1);
+
+ } else { osd_put_s(osd_string, L_JUST | BLINK | INVERT, 5, 1, 1); }
+ step = 30;
+ break;
+ case (30):
+#if OSD_USE_MAG_COMPASS && !defined(SITL)
+PRINT_CONFIG_MSG("OSD USES THE MAGNETIC HEADING")
+ temp = DegOfRad(MAG_Heading);
+ if (temp < 0) { temp += 360; }
#else
+PRINT_CONFIG_MSG("OSD USES THE GPS HEADING")
temp = DegOfRad(state.h_speed_dir_f);
- if (temp < 0) {
- temp += 360;
- }
+ if (temp < 0) { temp += 360; }
#endif
osd_sprintf(osd_string, "%.0f", temp);
- osd_put_s(osd_string, FALSE, 8, 1, 13);
- step = 30;
- break;
- case (30):
- osd_sprintf(osd_string, "%.0fKm", (state.h_speed_norm_f * 3.6));
- osd_put_s(osd_string, FALSE, 8, 0, 24);
+ osd_put_s(osd_string, C_JUST, 3, 1, 15);
step = 40;
break;
case (40):
- osd_sprintf(osd_string, "%.0fm", GetPosAlt());
- osd_put_s(osd_string, FALSE, 10, 13, 2);
+ osd_sprintf(osd_string, "%.0f KM", (state.h_speed_norm_f * 3.6));
+ osd_put_s(osd_string, R_JUST, 6, 1, 29);
+ step = 42;
+ break;
+ case (42):
+#if AP
+ osd_sprintf(osd_string, "%.0fTHR", (((float)ap_state->commands[COMMAND_THROTTLE] / MAX_PPRZ) * 100));
+#else
+ osd_sprintf(osd_string, "%.0fTHR", (((float)stabilization_cmd[COMMAND_THRUST] / MAX_PPRZ) * 100));
+#endif
+ osd_put_s(osd_string, R_JUST, 5, 2, 29);
step = 50;
break;
case (50):
+#if OSD_USE_BARO_ALTITUDE && !defined(SITL)
+PRINT_CONFIG_MSG("OSD ALTITUDE IS COMING FROM BAROMETER")
+#if defined BARO_ALTITUDE_VAR
+ osd_sprintf(osd_string, "%.0fM", BARO_ALTITUDE_VAR);
+#else
+PRINT_CONFIG_MSG("OSD USES THE DEFAULT BARO ALTITUDE VARIABLE")
+ osd_sprintf(osd_string, "%.0fM", baro_alt);
+#endif
+#else
+PRINT_CONFIG_MSG("ALTITUDE IS COMING FROM GPS")
+ osd_sprintf(osd_string, "%.0fM", GetPosAlt());
+#endif
+ osd_put_s(osd_string, L_JUST, 6, 14, 1); // "FALSE = L_JUST
+ step = 52;
+ break;
+ case (52):
+#if defined USE_MATEK_TYPE_OSD_CHIP && USE_MATEK_TYPE_OSD_CHIP == 1
+ // ANY SPECIAL CHARACTER CODE MUST BE A 3 DIGIT NUMBER WITH THE LEADING ZEROS!!!!
+ // THE SPECIAL CHARACTER CAN BE PLACED BEFORE OR AFTER THE FLOAT OR ANY OTHER CHARACTER
+ osd_sprintf(osd_string, "%191c%.0f", home_direction());
+ osd_put_s(osd_string, C_JUST, 4, 2, 15); // "FALSE = L_JUST
+#else
+ osd_sprintf(osd_string, "H%.0f", home_direction());
+ osd_put_s(osd_string, C_JUST, 4, 2, 15); // "FALSE = L_JUST
+
+#endif
+ step = 60;
+ break;
+
+ case (60):
+#if defined USE_MATEK_TYPE_OSD_CHIP && USE_MATEK_TYPE_OSD_CHIP == 1
+ // ANY SPECIAL CHARACTER CODE MUST BE A 3 DIGIT NUMBER WITH THE LEADING ZEROS!!!!
+ // THE SPECIAL CHARACTER CAN BE PLACED BEFORE OR AFTER THE FLOAT OR ANY OTHER CHARACTER
+ osd_sprintf(osd_string, "%160c%.0fM", (float)(sqrt(ph_x * ph_x + ph_y * ph_y)));
+ osd_put_s(osd_string, C_JUST, 6, 14, 15);
+#else
+ osd_sprintf(osd_string, "%.0fM", (float)(sqrt(ph_x * ph_x + ph_y * ph_y)));
+ osd_put_s(osd_string, C_JUST, 6, 14, 15);
+#endif
+ step = 70;
+ break;
+ case (70):
osd_sprintf(osd_string, "%.1fVZ", stateGetSpeedEnu_f()->z);
- osd_put_s(osd_string, FALSE, 7, 13, 24);
+ osd_put_s(osd_string, R_JUST, 9, 14, 29);
+ step = 80;
+ break;
+ // A Text PFD as graphics are not the strong point of the MAX7456
+ // In order to level the aircraft while fpving
+ // just move the stick to the opposite direction from the angles shown on the osd
+ // and that's why positive pitch (UP) is shown below the OSD center
+ case (80):
+ if (DegOfRad(att->theta) > 2) {
+ osd_sprintf(osd_string, "%.0f", DegOfRad(att->theta));
+ osd_put_s(osd_string, C_JUST, 5, 6, 15);
+
+ } else { osd_put_s(" ", C_JUST, 5, 6, 15); }
+ step = 90;
+ break;
+ case (90):
+ if (DegOfRad(att->theta) < -2) {
+ osd_sprintf(osd_string, "%.0f", DegOfRad(att->theta));
+ osd_put_s(osd_string, C_JUST, 5, 10, 15);
+
+ } else { osd_put_s(" ", C_JUST, 5, 10, 15); }
+ step = 100;
+ break;
+ case (100):
+ if (DegOfRad(att->phi) > 2) {
+ osd_sprintf(osd_string, "%.0f>", DegOfRad(att->phi));
+ osd_put_s(osd_string, FALSE, 5, 8, 18);
+
+ } else { osd_put_s(" ", FALSE, 5, 8, 18); }
+ step = 110;
+ break;
+ case (110):
+ if (DegOfRad(att->phi) < -2) {
+ osd_sprintf(osd_string, "<%.0f", DegOfRad(fabs(att->phi)));
+ osd_put_s(osd_string, R_JUST, 5, 8, 13);
+
+ } else { osd_put_s(" ", R_JUST, 5, 8, 13); }
+ step = 120;
+ break;
+
+ case (120):
+ check_osd_status();
step = 10;
break;
- default: break;
- }
- }
+
+ default: step = 10; break;
+ } // End of switch statement.
+ } // End of if (max7456_osd_status == OSD_UNINIT)
return;
}
@@ -231,11 +731,13 @@ void max7456_event(void)
max7456_osd_status = OSD_FINISHED;
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
break;
- case (OSD_READ_STATUS):
- osd_stat_reg = max7456_trans.input_buf[0];
- osd_stat_reg_valid = true;
- max7456_osd_status = OSD_FINISHED;
- break;
+ /*
+ case (OSD_READ_STATUS):
+ osd_stat_reg = max7456_trans.input_buf[0];
+ osd_stat_reg_valid = true;
+ max7456_osd_status = OSD_FINISHED;
+ break;
+ */
case (OSD_S_STEP1):
max7456_trans.output_length = 2;
max7456_trans.output_buf[0] = OSD_DMAL_REG;
@@ -262,6 +764,12 @@ void max7456_event(void)
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
}
break;
+ case (OSD_READ_STATUS):
+ osd_stat_reg = max7456_trans.input_buf[0];
+ osd_stat_reg_valid = TRUE;
+ max7456_trans.status = SPITransDone;
+ max7456_osd_status = OSD_IDLE;
+ break;
case (OSD_FINISHED):
osd_attr = 0;
max7456_trans.status = SPITransDone;
@@ -273,184 +781,6 @@ void max7456_event(void)
return;
}
-static char ascii_to_osd_c(char c)
-{
-
- if (c >= '0' && c <= '9') {
- if (c == '0') {
- c -= 38;
- } else {
- c -= 48;
- }
- } else {
- if (c >= 'A' && c <= 'Z') {
- c -= 54;
- } else {
- if (c >= 'a' && c <= 'z') {
- c -= 60;
- } else {
- switch (c) {
- case ('('): c = 0x3f; break;
- case (')'): c = 0x40; break;
- case ('.'): c = 0x41; break;
- case ('?'): c = 0x42; break;
- case (';'): c = 0x43; break;
- case (':'): c = 0x44; break;
- case (','): c = 0x45; break;
- //case('''): c = 0x46; break;
- case ('/'): c = 0x47; break;
- case ('"'): c = 0x48; break;
- case ('-'): c = 0x49; break;
- case ('<'): c = 0x4A; break;
- case ('>'): c = 0x4B; break;
- case ('@'): c = 0x4C; break;
- case (' '): c = 0x00; break;
- case ('\0'): c = 0xFF; break;
- default : break;
- }
- }
- }
- }
- return (c);
-}
-
-static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t row, uint8_t column)
-{
-
- uint8_t x = 0;
-
- if (row > 15) {
- column = 15;
- }
- if (column > 29) {
- column = 29;
- }
- osd_char_address = ((uint16_t)row * 30) + column;
-
- // translate the string and put it to the "osd_string" '\0' = 0xff
- x = 0;
- while ( (*(string + x) != '\0') && ( x < (sizeof(osd_string)-1) )) {
- osd_string[x] = ascii_to_osd_c(*(string + x));
- x++;
- }
- osd_string[x] = ascii_to_osd_c(*(string + x));
-
- for (x = 0; x < sizeof(osd_string); x++) {
- if (osd_string[x] == 0xff) {
- break;
- }
- }
-
- // Prevent overflow condition
- if (char_nb >= OSD_STRING_SIZE) {
- char_nb = OSD_STRING_SIZE-1;
- }
- //Adjust for the reserved character number.
- for (; x < char_nb; x++) {
- osd_string[x] = 0;
- }
- osd_string[x] = 0xff;
-
- osd_attr = attributes;
-
- //TRIGGER THE SPI TRANSFERS. The rest of the spi transfers occur in the "max7456_event" function.
- if (max7456_osd_status == OSD_IDLE) {
- max7456_trans.output_length = 2;
- max7456_trans.output_buf[0] = OSD_DMAH_REG;
- max7456_trans.output_buf[1] = (uint8_t)((osd_char_address >> 8) & 0x0001);
- max7456_osd_status = OSD_S_STEP1;
- spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
- }
- return;
-}
-
-// A VERY VERY STRIPED DOWN sprintf function suitable only for the paparazzi OSD.
-static bool _osd_sprintf(char *buffer, char *string, float value)
-{
- uint8_t param_start = 0;
- uint8_t param_end = 0;
- uint8_t frac_nb = 0;
- uint8_t digit = 0;
- uint8_t x = 0, y = 0, z = 0;
- uint16_t i_dec = 0;
- uint16_t i_frac = 0;
-
- char to_asc[10] = {48, 48, 48, 48, 48, 48, 48, 48, 48, 48};
- // Clear the osd string.
- for (x = 0; x < sizeof(osd_string); x++) {
- osd_string[x] = 0;
- }
- x = 0;
- // Search for the prameter start and stop positions.
- while (*(string + x) != '\0') {
- if (*(string + x) == '%') {
- param_start = x;
- } else if (*(string + x) == 'f') {
- param_end = x;
- break;
- }
- x++;
- }
- // find and bound the precision specified.
- frac_nb = *(string + param_end - 1) - 48; // Convert to number, ASCII 48 = '0'
- if (frac_nb > 3) {
- frac_nb = 3; // Bound value.
- }
- // Point y to one before the end of the array
- // Because we will increment it with frac_nb-1, so pointing it to the end would result in overflow
- y = (sizeof(to_asc) - 3);
- i_dec = abs((int16_t)value);
- // Fist we will deal with the fractional part if specified.
- if (frac_nb > 0 && frac_nb <= 3) {
- i_frac = abs((int16_t)((value - (int16_t)value) * 1000)); // Max precision is 3 digits.
- x = 100;
- z = frac_nb;
- do { // Example if frac_nb=2 then 952 will show as .95
- z--;
- digit = (i_frac / x);
- to_asc[y + z] = digit + 48; // Convert to ASCII
- i_frac -= digit * x; // Calculate the remainder.
- x /= 10; // 952-(9*100) = 52, 52-(10*5)=2 etc.
- } while (z > 0);
- y -= frac_nb; // set y to point where the dot must be placed.
- to_asc[y] = '.';
- y--; // Set y to point where the rest of the numbers must be written.
- }
-
- // Now it is time for the integer part. "y" already points to the position just before the dot.
- do {
- to_asc[y] = (i_dec % 10) + 48; //Write at least one digit even if value is zero.
- i_dec /= 10;
- if (i_dec <= 0) { // This way the leading zero is ommited.
- if (value < 0) {
- y--; to_asc[y] = '-'; // Place the minus sign if needed.
- }
- break;
- } else {
- y--;
- }
- } while (1);
-
- // Fill the buffer with the characters in the beggining of the string if any.
- for (x = 0; x < param_start; x++) {
- *(buffer + x) = *(string + x);
- }
-
- // x is now pointing to the next character in osd_string.
- // y is already pointing to the first digit or negative sign in "to_asc" array.
- while (y < sizeof(to_asc)) {
- *(buffer + x) = to_asc[y];
- x++; y++;
- }
- // x is now pointing to the next character in osd_string.
- // "param_end" is pointing to the last format character in the string.
- do {
- param_end++;
- *(buffer + x++) = *(string + param_end);
- } while (*(string + param_end) != '\0'); //Write the rest of the string including the terminating char.
-
- return (0);
-}
diff --git a/sw/airborne/modules/sensors/baro_bmp280_i2c.c b/sw/airborne/modules/sensors/baro_bmp280_i2c.c
new file mode 100644
index 00000000000..e62d522b93c
--- /dev/null
+++ b/sw/airborne/modules/sensors/baro_bmp280_i2c.c
@@ -0,0 +1,78 @@
+/*
+ * Chris Efstathiou hendrixgr@gmail.com
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ *
+ */
+
+/**
+ * @file modules/sensors/baro_bmp280.c
+ * Bosch BMP280 I2C sensor interface.
+ *
+ * This reads the values for pressure and temperature from the Bosch BMP280 sensor through I2C.
+ */
+
+
+#include "baro_bmp280_i2c.h"
+
+#include "subsystems/abi.h"
+#include "mcu_periph/uart.h"
+#include "pprzlink/messages.h"
+#include "subsystems/datalink/downlink.h"
+#include "math/pprz_isa.h"
+
+/** default slave address */
+#ifndef BMP280_SLAVE_ADDR
+#define BMP280_SLAVE_ADDR BMP280_I2C_ADDR
+#endif
+
+float baro_alt = 0;
+bool baro_alt_valid = 0;
+
+struct Bmp280_I2c baro_bmp280;
+
+void baro_bmp280_init(void)
+{
+ bmp280_i2c_init(&baro_bmp280, &BMP280_I2C_DEV, BMP280_SLAVE_ADDR);
+}
+
+void baro_bmp280_periodic(void)
+{
+ bmp280_i2c_periodic(&baro_bmp280);
+}
+
+void baro_bmp280_event(void)
+{
+ bmp280_i2c_event(&baro_bmp280);
+
+ if (baro_bmp280.data_available) {
+ uint32_t now_ts = get_sys_time_usec();
+ // send ABI message
+ AbiSendMsgBARO_ABS(BARO_BMP_SENDER_ID, now_ts, baro_bmp280.pressure);
+ AbiSendMsgTEMPERATURE(BARO_BMP_SENDER_ID, baro_bmp280.temperature);
+ baro_bmp280.data_available = false;
+ baro_alt = pprz_isa_altitude_of_pressure((float)baro_bmp280.pressure);
+ baro_alt_valid = TRUE;
+
+#ifdef BMP280_SYNC_SEND
+ int32_t up = (int32_t)(baro_alt * 100.0);
+ int32_t ut = (int32_t) baro_bmp280.raw_temperature;
+ int32_t p = (int32_t) baro_bmp280.pressure;
+ int32_t t = (int32_t)(baro_bmp280.temperature);
+ DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &up, &ut, &p, &t);
+#endif
+ }
+}
diff --git a/sw/airborne/modules/sensors/baro_bmp280_i2c.h b/sw/airborne/modules/sensors/baro_bmp280_i2c.h
new file mode 100644
index 00000000000..d6487e6647d
--- /dev/null
+++ b/sw/airborne/modules/sensors/baro_bmp280_i2c.h
@@ -0,0 +1,43 @@
+/*
+ * Chris Efstathiou hendrixgr@gmail.com
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ *
+ */
+
+/**
+ * @file modules/sensors/baro_bmp280.h
+ * Bosch BMP280 I2C sensor interface.
+ *
+ * This reads the values for pressure and temperature from the Bosch BMP280 sensor through I2C.
+ */
+
+#ifndef BARO_BMP280_H
+#define BARO_BMP280_H
+
+#include "peripherals/bmp280_i2c.h"
+
+extern struct Bmp280_I2c baro_bmp280;
+
+extern float baro_alt;
+extern bool baro_alt_valid;
+
+void baro_bmp280_init(void);
+void baro_bmp280_periodic(void);
+void baro_bmp280_event(void);
+
+#endif
diff --git a/sw/airborne/peripherals/bmp280_i2c.c b/sw/airborne/peripherals/bmp280_i2c.c
new file mode 100644
index 00000000000..bda0f0e907a
--- /dev/null
+++ b/sw/airborne/peripherals/bmp280_i2c.c
@@ -0,0 +1,248 @@
+/*
+ * Chris Efstathiou hendrixgr@gmail.com
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+/**
+ * @file peripherals/bmp280_i2c.c
+ * @brief Sensor driver for BMP280 sensor via I2C
+ *
+ *
+ */
+
+#include "peripherals/bmp280_i2c.h"
+
+/** local function to extract raw data from i2c buffer
+ * and compute compensation with selected precision
+ */
+static void parse_sensor_data(struct Bmp280_I2c *bmp);
+static void parse_calib_data(struct Bmp280_I2c *bmp);
+PRINT_CONFIG_MSG("BMP280 uses double precision compensation")
+static double compensate_pressure(struct Bmp280_I2c *bmp);
+static double compensate_temperature(struct Bmp280_I2c *bmp);
+
+int64_t t_fine;
+
+// init function
+void bmp280_i2c_init(struct Bmp280_I2c *bmp, struct i2c_periph *i2c_p, uint8_t addr)
+{
+ // set i2c_peripheral
+ bmp->i2c_p = i2c_p;
+
+ // slave address
+ bmp->i2c_trans.slave_addr = addr;
+ // set initial status: Done
+ bmp->i2c_trans.status = I2CTransDone;
+
+ bmp->data_available = false;
+ bmp->initialized = false;
+ bmp->status = BMP280_STATUS_UNINIT;
+}
+
+// Start new measurement if sensor ready
+void bmp280_i2c_periodic(struct Bmp280_I2c *bmp)
+{
+ if (bmp->i2c_trans.status != I2CTransDone) {
+ return; // transaction not finished
+ }
+
+ switch (bmp->status) {
+ case BMP280_STATUS_UNINIT:
+ bmp->data_available = false;
+ bmp->initialized = false;
+ bmp->status = BMP280_STATUS_GET_CALIB;
+ break;
+
+ case BMP280_STATUS_GET_CALIB:
+ // request calibration data
+ bmp->i2c_trans.buf[0] = BMP280_CALIB_LSB_DATA_ADDR;
+ i2c_transceive(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, 1, BMP280_CALIB_DATA_LEN);
+ break;
+
+ case BMP280_STATUS_CONFIGURE:
+ // From datasheet, recommended config for drone usecase:
+ // osrs_p = 8, osrs_t = 1
+ // IIR filter = 2 (note: this one doesn't exist...)
+ bmp->i2c_trans.buf[0] = BMP280_CTRL_MEAS_REG_ADDR;
+ bmp->i2c_trans.buf[1] = (BMP280_OVERSAMPLING_1X_T | BMP280_OVERSAMPLING_8X_P | BMP280_POWER_NORMAL_MODE);
+ bmp->i2c_trans.buf[2] = BMP280_CONFIG_REG_ADDR;
+ bmp->i2c_trans.buf[3] = (BMP280_INACTIVITY_62_5_MS | BMP280_IIR_FILTER_COEFF_2);
+ i2c_transmit(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, (BMP280_CONFIG_LEN * 2));
+ break;
+
+ case BMP280_STATUS_READ_STATUS_REG:
+ // READ THE STATUS BYTE
+ bmp->i2c_trans.buf[0] = BMP280_STATUS_REG_ADDR;
+ i2c_transceive(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, 1, 1);
+ break;
+
+ case BMP280_STATUS_READ_DATA_REGS:
+ // READ ALL 6 DATA REGISTERS
+ bmp->i2c_trans.buf[0] = BMP280_DATA_START_REG_ADDR;
+ i2c_transceive(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, 1, BMP280_P_T_DATA_LEN);
+ break;
+
+ default:
+ break;
+ }
+}
+
+void bmp280_i2c_event(struct Bmp280_I2c *bmp)
+{
+ if (bmp->i2c_trans.status == I2CTransSuccess) {
+ switch (bmp->status) {
+
+ case BMP280_STATUS_GET_CALIB:
+ // compute calib
+ parse_calib_data(bmp);
+ bmp->status = BMP280_STATUS_CONFIGURE;
+ break;
+
+ case BMP280_STATUS_CONFIGURE:
+ // nothing else to do, start reading
+ bmp->status = BMP280_STATUS_READ_STATUS_REG;
+ bmp->initialized = true;
+ break;
+
+ case BMP280_STATUS_READ_STATUS_REG:
+ // check status byte
+ if ((bmp->i2c_trans.buf[0] & (BMP280_EOC_BIT | BMP280_NVRAM_COPY_BIT)) == 0) {
+ bmp->status = BMP280_STATUS_READ_DATA_REGS;
+ }
+ break;
+
+ case BMP280_STATUS_READ_DATA_REGS:
+ // parse sensor data, compensate temperature first, then pressure
+ parse_sensor_data(bmp);
+ compensate_temperature(bmp);
+ compensate_pressure(bmp);
+ bmp->data_available = true;
+ bmp->status = BMP280_STATUS_READ_STATUS_REG;
+ break;
+
+ default:
+ bmp->status = BMP280_STATUS_GET_CALIB; // just to avoid the compiler's warning message
+ break;
+ }
+ bmp->i2c_trans.status = I2CTransDone;
+ } else if (bmp->i2c_trans.status == I2CTransFailed) {
+ /* try again */
+ if (!bmp->initialized) {
+ bmp->status = BMP280_STATUS_UNINIT;
+ }
+ bmp->i2c_trans.status = I2CTransDone;
+ }
+
+ return;
+}
+
+static void parse_sensor_data(struct Bmp280_I2c *bmp)
+{
+ /* Temporary variables to store the sensor data */
+ uint32_t data_xlsb;
+ uint32_t data_lsb;
+ uint32_t data_msb;
+
+ // BMP280 HAS THE 6 DATA REGISTERS START AT F7 AND GOING UP TO FC MSB FIRST THEN LSB AND LAST THE XLSB BYTE.
+ // THE FIRST THREE BYTES ARE THE PRESSURE AND THE NEXT 3 THE TEMPERATURE.
+ /* Store the parsed register values for pressure data */
+ data_msb = (uint32_t)bmp->i2c_trans.buf[0] << 16;
+ data_lsb = (uint32_t)bmp->i2c_trans.buf[1] << 8;
+ data_xlsb = (uint32_t)bmp->i2c_trans.buf[2];
+ bmp->raw_pressure = (int32_t)((data_msb | data_lsb | data_xlsb) >> 4);
+
+ /* Store the parsed register values for temperature data */
+ data_msb = (uint32_t)bmp->i2c_trans.buf[3] << 16;
+ data_lsb = (uint32_t)bmp->i2c_trans.buf[4] << 8;
+ data_xlsb = (uint32_t)bmp->i2c_trans.buf[5];
+ bmp->raw_temperature = (int32_t)((data_msb | data_lsb | data_xlsb) >> 4);
+}
+
+
+/**
+ * @brief This internal API is used to parse the calibration data, compensates
+ * it and store it in device structure (float version)
+ */
+static void parse_calib_data(struct Bmp280_I2c *bmp)
+{
+ uint8_t *data = (uint8_t *)bmp->i2c_trans.buf; // we know that this buffer will not be modified during this call
+
+ bmp->calib.dig_t1 = BMP280_CONCAT_BYTES(data[1], data[0]);
+ bmp->calib.dig_t2 = (int16_t)BMP280_CONCAT_BYTES(data[3], data[2]);
+ bmp->calib.dig_t3 = (int16_t)BMP280_CONCAT_BYTES(data[5], data[4]);
+
+ bmp->calib.dig_p1 = BMP280_CONCAT_BYTES(data[7], data[6]);
+ bmp->calib.dig_p2 = (int16_t)BMP280_CONCAT_BYTES(data[9], data[8]);
+ bmp->calib.dig_p3 = (int16_t)BMP280_CONCAT_BYTES(data[11], data[10]);
+ bmp->calib.dig_p4 = (int16_t)BMP280_CONCAT_BYTES(data[13], data[12]);
+ bmp->calib.dig_p5 = (int16_t)BMP280_CONCAT_BYTES(data[15], data[14]);
+ bmp->calib.dig_p6 = (int16_t)BMP280_CONCAT_BYTES(data[17], data[16]);
+ bmp->calib.dig_p7 = (int16_t)BMP280_CONCAT_BYTES(data[19], data[18]);
+ bmp->calib.dig_p8 = (int16_t)BMP280_CONCAT_BYTES(data[21], data[20]);
+ bmp->calib.dig_p9 = (int16_t)BMP280_CONCAT_BYTES(data[23], data[22]);
+
+ return;
+}
+
+/**
+ * @brief This internal API is used to compensate the raw temperature data and
+ * return the compensated temperature data in float data type.
+ */
+static double compensate_temperature(struct Bmp280_I2c *bmp)
+{
+
+ double var1;
+ double var2;
+
+ var1 = (((double)bmp->raw_temperature / 16384.0) - ((double)bmp->calib.dig_t1 / 1024.0)) * ((double)bmp->calib.dig_t2);
+ var2 = ((double)bmp->raw_temperature / 131072.0) - ((double)bmp->calib.dig_t1 / 8192.0);
+ var2 = (var2 * var2) * (double)bmp->calib.dig_t3;
+ t_fine = (int64_t)(var1 + var2);
+
+ /* Store t_lin in dev. structure for pressure calculation */
+ bmp->calib.t_fine = t_fine;
+ /* Store compensated temperature in float in structure */
+ bmp->temperature = (((var1 + var2) / 5120.f) * 100);
+
+ return (double)bmp->temperature;
+}
+
+/**
+ * @brief This internal API is used to compensate the raw pressure data and
+ * return the compensated pressure data in integer data type.
+ */
+static double compensate_pressure(struct Bmp280_I2c *bmp)
+{
+ double var1;
+ double var2;
+ double p;
+
+ var1 = ((double)t_fine / 2) - 64000.0;
+ var2 = (var1 * var1 * (double)bmp->calib.dig_p5) / 32768.0;
+ var2 = var2 + (var1 * (double)bmp->calib.dig_p5 * 2.0);
+ var2 = (var2 / 4.0) + ((double)bmp->calib.dig_p4 * 65536.0);
+ var1 = (((double)bmp->calib.dig_p3 * var1 * (var1 / 524288.0)) + ((double)bmp->calib.dig_p2 * var1)) / 524288.0;
+ var1 = (1 + (var1 / 32768.0)) * (double)bmp->calib.dig_p1;
+ p = 1048576.0 - (double)bmp->raw_pressure;
+ p = (p - (var2 / 4096.0)) * (6250.0 / var1);
+ var1 = ((double)bmp->calib.dig_p9 * p) * (p / 2147483648.0);
+ var2 = (p * ((double)bmp->calib.dig_p8)) / 32768.0;
+ p = p + ((var1 + var2 + (double)bmp->calib.dig_p7) / 16.0);
+ bmp->pressure = p;
+
+ return (p);
+}
diff --git a/sw/airborne/peripherals/bmp280_i2c.h b/sw/airborne/peripherals/bmp280_i2c.h
new file mode 100644
index 00000000000..3fc45e00dbf
--- /dev/null
+++ b/sw/airborne/peripherals/bmp280_i2c.h
@@ -0,0 +1,53 @@
+/*
+ * Chris Efstathiou hendrixgr@gmail.com
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+/**
+ * @file peripherals/bmp280_i2c.h
+ * @brief Sensor driver for BMP280 sensor via I2C
+ *
+ *
+ */
+
+#ifndef BMP280_I2C_H
+#define BMP280_I2C_H
+
+/* Header includes */
+#include "peripherals/bmp280_regs.h"
+#include "mcu_periph/i2c.h"
+
+
+struct Bmp280_I2c {
+ struct i2c_periph *i2c_p;
+ struct i2c_transaction i2c_trans;
+ enum Bmp280Status status; ///< state machine status
+ bool initialized; ///< config done flag
+ volatile bool data_available; ///< data ready flag
+ struct bmp280_reg_calib_data calib; ///< calibration data
+ uint32_t raw_pressure; ///< uncompensated pressure
+ uint32_t raw_temperature; ///< uncompensated temperature
+ float pressure; ///< pressure in Pascal
+ float temperature; ///< temperature in deg Celcius
+};
+
+extern void bmp280_i2c_read_eeprom_calib(struct Bmp280_I2c *bmp);
+extern void bmp280_i2c_init(struct Bmp280_I2c *bmp, struct i2c_periph *i2c_p, uint8_t addr);
+extern void bmp280_i2c_periodic(struct Bmp280_I2c *bmp);
+extern void bmp280_i2c_event(struct Bmp280_I2c *bmp);
+
+#endif /* BMP280_I2C_H */
diff --git a/sw/airborne/peripherals/bmp280_regs.h b/sw/airborne/peripherals/bmp280_regs.h
new file mode 100644
index 00000000000..9cd2871b167
--- /dev/null
+++ b/sw/airborne/peripherals/bmp280_regs.h
@@ -0,0 +1,174 @@
+/*
+ * Chris Efstathiou hendrixgr@gmail.com
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+/**
+ * @file peripherals/bmp3_regs.h
+ * @brief Sensor driver for BMP280 sensor register definition
+ *
+ * Modified for Paparazzi from SDP3 driver from BoshSensortec
+ * see https://github.com/BoschSensortec/BMP280-Sensor-API
+ * for original code and license
+ *
+ */
+
+#ifndef BMP280_REGS_H
+#define BMP280_REGS_H
+
+#include "std.h"
+
+// I2C addresses (8 bits)
+#define BMP280_I2C_ADDR 0xEC
+#define BMP280_I2C_ADDR_ALT 0xEE
+
+/**\name BMP280 chip identifier */
+#define BMP280_CHIP_ID 0x50
+
+// CALIBRATION REGISTER ADDRESSES
+#define BMP280_CALIB_LSB_DATA_ADDR 0x88
+#define BMP280_CALIB_DATA_LEN 24
+
+#define BMP280_DIG_T1_UINT 0x88
+#define BMP280_DIG_T2_INT 0x8A
+#define BMP280_DIG_T3_INT 0x8C
+#define BMP280_DIG_P1_UINT 0x8E
+#define BMP280_DIG_P2_INT 0x90
+#define BMP280_DIG_P3_INT 0x92
+#define BMP280_DIG_P4_INT 0x94
+#define BMP280_DIG_P5_INT 0x96
+#define BMP280_DIG_P6_INT 0x98
+#define BMP280_DIG_P7_INT 0x9A
+#define BMP280_DIG_P8_INT 0x9C
+#define BMP280_DIG_P9_INT 0x9E
+
+// CONTROL AND VALUES REGISTER ADDRESSES
+#define BMP280_CONFIG_ADDR 0xF4
+#define BMP280_CONFIG_LEN 0x02
+#define BMP280_DATA_START_REG_ADDR 0xF7
+#define BMP280_P_T_DATA_LEN 6
+#define BMP280_P_DATA_LEN 3
+#define BMP280_T_DATA_LEN 3
+
+#define BMP280_CHIP_ID_REG_ADDR 0xD0
+#define BMP280_RESET_REG_ADDR 0xF3
+#define BMP280_STATUS_REG_ADDR 0xF3
+#define BMP280_CTRL_MEAS_REG_ADDR 0xF4
+#define BMP280_CONFIG_REG_ADDR 0xF5
+#define BMP280_P_MSB_REG_ADDR 0xF7
+#define BMP280_P_LSB_REG_ADDR 0xF8
+#define BMP280_P_XLSB_REG_ADDR 0xF9
+#define BMP280_T_MSB_REG_ADDR 0xFA
+#define BMP280_T_LSB_REG_ADDR 0xFB
+#define BMP280_T_XLSB_REG_ADDR 0xFC
+
+// BMP280 ID
+#define BMP280_ID_NB 0x58
+//BMP280 RESET COMMAND VALUE
+#define BMP280_RESET_VAL 0xB6
+
+#define BMP280_EOC_BIT (1<<3)
+#define BMP280_NVRAM_COPY_BIT (1<<0)
+
+// BMP280 CONTROL MEASUREMENT REGISTER BIT VALUES.
+#define BMP280_NO_OVERSAMPLING_T (0x00<<5)
+#define BMP280_OVERSAMPLING_1X_T (0x01<<5)
+#define BMP280_OVERSAMPLING_2X_T (0x02<<5)
+#define BMP280_OVERSAMPLING_4X_T (0x03<<5)
+#define BMP280_OVERSAMPLING_8X_T (0x04<<5)
+#define BMP280_OVERSAMPLING_16X_T (0x05<<5)
+
+#define BMP280_NO_OVERSAMPLING_P (0x00<<2)
+#define BMP280_OVERSAMPLING_1X_P (0x01<<2)
+#define BMP280_OVERSAMPLING_2X_P (0x02<<2)
+#define BMP280_OVERSAMPLING_4X_P (0x03<<2)
+#define BMP280_OVERSAMPLING_8X_P (0x04<<2)
+#define BMP280_OVERSAMPLING_16X_P (0x05<<2)
+
+#define BMP280_POWER_SLEEP_MODE (0x00)
+#define BMP280_POWER_FORCED_MODE (0x01) // OX02 IS EXACTLY THE SAME
+#define BMP280_POWER_NORMAL_MODE (0x03)
+
+// BMP280 CONFIG REGISTER BIT VALUES
+#define BMP280_INACTIVITY_HALF_MS (0x00<<5)
+#define BMP280_INACTIVITY_62_5_MS (0x01<<5)
+#define BMP280_INACTIVITY_125_MS (0x02<<5)
+#define BMP280_INACTIVITY_250_MS (0x03<<5)
+#define BMP280_INACTIVITY_500_MS (0x04<<5)
+#define BMP280_INACTIVITY_1000_MS (0x05<<5)
+#define BMP280_INACTIVITY_2000_MS (0x06<<5)
+#define BMP280_INACTIVITY_4000_MS (0x07<<5)
+
+#define BMP280_IIR_FILTER_COEFF_1 (0x00<<2)
+#define BMP280_IIR_FILTER_COEFF_2 (0x01<<2)
+#define BMP280_IIR_FILTER_COEFF_4 (0x02<<2)
+#define BMP280_IIR_FILTER_COEFF_8 (0x03<<2)
+#define BMP280_IIR_FILTER_COEFF_16 (0x04<<2)
+
+#define BMP280_DISABLE_SPI_IF (0x00) // DEFAULT
+#define BMP280_ENABLE_SPI_IF (0x01)
+
+
+/**\name Power mode macros */
+#define BMP280_SLEEP_MODE 0x00
+#define BMP280_FORCED_MODE 0x01
+#define BMP280_NORMAL_MODE 0x03
+
+
+/**\name Sensor component selection macros */
+#define BMP280_PRESS 0x01
+#define BMP280_TEMP 0x02
+#define BMP280_ALL 0x03
+
+/**\name Macro to combine two 8 bit data's to form a 16 bit data */
+#define BMP280_CONCAT_BYTES(msb, lsb) (((uint16_t)msb << 8) | (uint16_t)lsb)
+
+#ifndef BMP280_COMPENSATION
+#define BMP280_COMPENSATION BMP280_DOUBLE_PRECISION_COMPENSATION
+#endif
+
+/**
+ * @brief Status enum
+ */
+enum Bmp280Status {
+ BMP280_STATUS_UNINIT,
+ BMP280_STATUS_GET_CALIB,
+ BMP280_STATUS_CONFIGURE,
+ BMP280_STATUS_READ_STATUS_REG,
+ BMP280_STATUS_READ_DATA_REGS
+};
+
+// brief Register Trim Variables
+struct bmp280_reg_calib_data {
+ uint16_t dig_t1;
+ int16_t dig_t2;
+ int16_t dig_t3;
+ uint16_t dig_p1;
+ int16_t dig_p2;
+ int16_t dig_p3;
+ int16_t dig_p4;
+ int16_t dig_p5;
+ int16_t dig_p6;
+ int16_t dig_p7;
+ int16_t dig_p8;
+ int16_t dig_p9;
+ int32_t t_fine;
+};
+
+
+#endif /* BMP280_REGS_H */