mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_HAL_ChibiOS: remove HerePro
to be replaced by a HereProAP
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
df45140a56
commit
4113290153
@ -1,3 +0,0 @@
|
|||||||
NTF_LED_OVERRIDE 1
|
|
||||||
SCR_ENABLE 1
|
|
||||||
GPS_INJECT_TO 0
|
|
||||||
@ -1,147 +0,0 @@
|
|||||||
# hw definition file for processing by chibios_hwdef.py
|
|
||||||
# for H743 bootloader
|
|
||||||
|
|
||||||
# MCU class and specific type
|
|
||||||
MCU STM32H7xx STM32H743xx
|
|
||||||
|
|
||||||
FULL_FEATURED_BOOTLOADER 1
|
|
||||||
|
|
||||||
# USB setup
|
|
||||||
USB_VENDOR 0x2DAE # ONLY FOR USE BY HEX! NOBODY ELSE
|
|
||||||
USB_PRODUCT 0x5011
|
|
||||||
USB_STRING_MANUFACTURER "CubePilot"
|
|
||||||
|
|
||||||
# setup build for a peripheral firmware
|
|
||||||
env AP_PERIPH 1
|
|
||||||
|
|
||||||
# crystal frequency
|
|
||||||
OSCILLATOR_HZ 24000000
|
|
||||||
|
|
||||||
# board ID for firmware load
|
|
||||||
APJ_BOARD_ID 1037
|
|
||||||
|
|
||||||
FLASH_SIZE_KB 2048
|
|
||||||
|
|
||||||
# bootloader is installed at zero offset
|
|
||||||
FLASH_RESERVE_START_KB 0
|
|
||||||
|
|
||||||
# the location where the bootloader will put the firmware
|
|
||||||
# the H743 has 128k sectors
|
|
||||||
FLASH_BOOTLOADER_LOAD_KB 256
|
|
||||||
|
|
||||||
PB15 LED_2 OUTPUT LOW
|
|
||||||
PB14 LED OUTPUT LOW
|
|
||||||
|
|
||||||
# board voltage
|
|
||||||
STM32_VDD 330U
|
|
||||||
|
|
||||||
# order of UARTs (and USB)
|
|
||||||
SERIAL_ORDER OTG1 UART7
|
|
||||||
|
|
||||||
define HAL_NO_MONITOR_THREAD
|
|
||||||
|
|
||||||
# USART3 F9
|
|
||||||
PD9 USART3_RX USART3
|
|
||||||
PD8 USART3_TX USART3
|
|
||||||
|
|
||||||
# UART7 F9
|
|
||||||
PE7 UART7_RX UART7
|
|
||||||
PE8 UART7_TX UART7 NODMA
|
|
||||||
|
|
||||||
|
|
||||||
PA11 OTG_FS_DM OTG1
|
|
||||||
PA12 OTG_FS_DP OTG1
|
|
||||||
|
|
||||||
PA13 JTMS-SWDIO SWD
|
|
||||||
PA14 JTCK-SWCLK SWD
|
|
||||||
|
|
||||||
|
|
||||||
define USB_USE_WAIT TRUE
|
|
||||||
define HAL_USE_USB_MSD 1
|
|
||||||
|
|
||||||
define HAL_DEVICE_THREAD_STACK 2048
|
|
||||||
|
|
||||||
#define AP_PARAM_MAX_EMBEDDED_PARAM 0
|
|
||||||
|
|
||||||
define HAL_BARO_ALLOW_INIT_NO_BARO
|
|
||||||
|
|
||||||
define HAL_COMPASS_MAX_SENSORS 1
|
|
||||||
|
|
||||||
# GPS+MAG
|
|
||||||
define HAL_PERIPH_ENABLE_GPS
|
|
||||||
define HAL_PERIPH_ENABLE_MAG
|
|
||||||
|
|
||||||
# Add MAVLink
|
|
||||||
env FORCE_MAVLINK_INCLUDE 1
|
|
||||||
|
|
||||||
# enable CAN support
|
|
||||||
PB8 CAN1_RX CAN1
|
|
||||||
PB9 CAN1_TX CAN1
|
|
||||||
PB5 CAN2_RX CAN2
|
|
||||||
PB6 CAN2_TX CAN2
|
|
||||||
#PB5 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW
|
|
||||||
|
|
||||||
PF8 blank CS
|
|
||||||
SPIDEV profiled SPI5 DEVID1 blank MODE3 1*MHZ 1*MHZ
|
|
||||||
|
|
||||||
PE4 MPU_EXT_CS CS
|
|
||||||
PE2 SPI4_SCK SPI4
|
|
||||||
PE5 SPI4_MISO SPI4
|
|
||||||
PE6 SPI4_MOSI SPI4
|
|
||||||
|
|
||||||
|
|
||||||
PG15 spi6cs CS
|
|
||||||
PG13 SPI6_SCK SPI6
|
|
||||||
PG12 SPI6_MISO SPI6
|
|
||||||
PG14 SPI6_MOSI SPI6
|
|
||||||
|
|
||||||
PF7 SPI5_SCK SPI5
|
|
||||||
PF9 SPI5_MOSI SPI5
|
|
||||||
|
|
||||||
define BOARD_SER1_RTSCTS_DEFAULT 0
|
|
||||||
|
|
||||||
#gps f9
|
|
||||||
PD10 F91TXR INPUT
|
|
||||||
PD11 F92RST INPUT
|
|
||||||
PD13 F93SB INPUT
|
|
||||||
PD14 F94EXTINT INPUT
|
|
||||||
PD15 F95RSV1 INPUT
|
|
||||||
PG2 F97 INPUT GPIO(100)
|
|
||||||
PG3 F96RSV2 INPUT
|
|
||||||
PD4 RTKSTAT INPUT
|
|
||||||
PD5 GEOFENCESTAT INPUT
|
|
||||||
|
|
||||||
#others
|
|
||||||
PG10 can2int INPUT
|
|
||||||
PG11 can2sleep OUTPUT PUSHPULL SPEED_LOW LOW
|
|
||||||
PF1 GNDDET2 INPUT
|
|
||||||
PF2 GNDDET1 INPUT
|
|
||||||
PC6 EXTERN_GPIO1 OUTPUT GPIO(1)
|
|
||||||
PC7 EXTERN_GPIO2 OUTPUT GPIO(2)
|
|
||||||
#sensor enable
|
|
||||||
PC0 SENSEN OUTPUT HIGH
|
|
||||||
PB1 IMUINT INPUT
|
|
||||||
PB3 QCAN2TERM OUTPUT LOW GPIO(4)
|
|
||||||
PB4 QCAN2INT0 INPUT
|
|
||||||
PC13 QCAN1SLEEP OUTPUT PUSHPULL SPEED_LOW LOW
|
|
||||||
PE0 QCAN1INT INPUT
|
|
||||||
PE1 QCAN1INT0 INPUT
|
|
||||||
PE3 QCAN1INT1 INPUT
|
|
||||||
|
|
||||||
PD6 QCAN1TERM OUTPUT LOW GPIO(3)
|
|
||||||
PD7 QCAN2INT1 INPUT
|
|
||||||
PA8 QCAN12OSC1 INPUT
|
|
||||||
|
|
||||||
# This input pin is used to detect that power is valid on USB.
|
|
||||||
PA9 VBUS INPUT
|
|
||||||
PH13 VDD_33V_SENS INPUT
|
|
||||||
|
|
||||||
# This defines some input pins, currently unused.
|
|
||||||
PB2 BOOT1 INPUT
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
# passthrough CAN1
|
|
||||||
define HAL_DEFAULT_CPORT 1
|
|
||||||
|
|
||||||
FULL_CHIBIOS_BOOTLOADER 1
|
|
||||||
@ -1,221 +0,0 @@
|
|||||||
# hw definition file for processing by chibios_hwdef.py
|
|
||||||
# for H743 bootloader
|
|
||||||
|
|
||||||
# MCU class and specific type
|
|
||||||
MCU STM32H7xx STM32H743xx
|
|
||||||
|
|
||||||
# USB setup
|
|
||||||
USB_VENDOR 0x2DAE # ONLY FOR USE BY HEX! NOBODY ELSE
|
|
||||||
USB_PRODUCT 0x5001
|
|
||||||
USB_STRING_MANUFACTURER "CubePilot"
|
|
||||||
|
|
||||||
# setup build for a peripheral firmware
|
|
||||||
env AP_PERIPH 1
|
|
||||||
env AP_PERIPH_HEAVY 1
|
|
||||||
|
|
||||||
# crystal frequency
|
|
||||||
OSCILLATOR_HZ 24000000
|
|
||||||
|
|
||||||
# board ID for firmware load
|
|
||||||
APJ_BOARD_ID 1037
|
|
||||||
|
|
||||||
FLASH_SIZE_KB 2048
|
|
||||||
|
|
||||||
# the location where the bootloader will put the firmware
|
|
||||||
# the H743 has 128k sectors, we leave 2 sectors for BL
|
|
||||||
FLASH_BOOTLOADER_LOAD_KB 256
|
|
||||||
|
|
||||||
PB15 LED_SWITCH_2 OUTPUT HIGH
|
|
||||||
PB14 LED_SWITCH_1 OUTPUT HIGH
|
|
||||||
|
|
||||||
# board voltage
|
|
||||||
STM32_VDD 330U
|
|
||||||
|
|
||||||
# order of UARTs (and USB)
|
|
||||||
SERIAL_ORDER OTG1 OTG2 EMPTY USART3 UART7
|
|
||||||
|
|
||||||
define DEFAULT_SERIAL0_PROTOCOL SerialProtocol_None
|
|
||||||
|
|
||||||
# USART3 F9
|
|
||||||
PD9 USART3_RX USART3
|
|
||||||
PD8 USART3_TX USART3
|
|
||||||
|
|
||||||
# UART7 F9
|
|
||||||
PE7 UART7_RX UART7
|
|
||||||
PE8 UART7_TX UART7 NODMA
|
|
||||||
|
|
||||||
|
|
||||||
PA11 OTG_FS_DM OTG1
|
|
||||||
PA12 OTG_FS_DP OTG1
|
|
||||||
|
|
||||||
PA13 JTMS-SWDIO SWD
|
|
||||||
PA14 JTCK-SWCLK SWD
|
|
||||||
|
|
||||||
###########################
|
|
||||||
define AP_NOTIFY_PROFILED_SPI_ENABLED 1
|
|
||||||
define DEFAULT_NTF_LED_TYPES Notify_LED_ProfiLED_SPI
|
|
||||||
define NOTIFY_LED_LEN_DEFAULT 16
|
|
||||||
define CONFIGURE_PPS_PIN TRUE
|
|
||||||
define HAL_PERIPH_ENABLE_NOTIFY
|
|
||||||
define HAL_PERIPH_NOTIFY_WITHOUT_RCOUT
|
|
||||||
|
|
||||||
FLASH_RESERVE_START_KB 256
|
|
||||||
|
|
||||||
define GPS_UBLOX_MOVING_BASELINE TRUE
|
|
||||||
define HAL_GCS_ENABLED 1
|
|
||||||
define HAL_LOGGING_ENABLED TRUE
|
|
||||||
define HAL_PERIPH_ENABLE_RTC 1
|
|
||||||
define HAL_NO_MONITOR_THREAD
|
|
||||||
|
|
||||||
|
|
||||||
define HAL_DISABLE_LOOP_DELAY
|
|
||||||
define USB_USE_WAIT TRUE
|
|
||||||
define HAL_USE_USB_MSD 1
|
|
||||||
|
|
||||||
|
|
||||||
define HAL_DEVICE_THREAD_STACK 2048
|
|
||||||
|
|
||||||
#define AP_PARAM_MAX_EMBEDDED_PARAM 0
|
|
||||||
|
|
||||||
define HAL_BARO_ALLOW_INIT_NO_BARO
|
|
||||||
|
|
||||||
define HAL_STORAGE_SIZE 16384
|
|
||||||
|
|
||||||
# disable dual GPS and GPS blending to save flash space
|
|
||||||
define GPS_MAX_RECEIVERS 1
|
|
||||||
define GPS_MAX_INSTANCES 1
|
|
||||||
define HAL_COMPASS_MAX_SENSORS 1
|
|
||||||
|
|
||||||
# GPS+MAG
|
|
||||||
define HAL_PERIPH_ENABLE_AHRS
|
|
||||||
define HAL_PERIPH_ENABLE_GPS
|
|
||||||
define HAL_PERIPH_ENABLE_MAG
|
|
||||||
define AP_INERTIALSENSOR_ENABLED 1
|
|
||||||
|
|
||||||
|
|
||||||
# enable CAN support
|
|
||||||
PB8 CAN1_RX CAN1
|
|
||||||
PB9 CAN1_TX CAN1
|
|
||||||
PB5 CAN2_RX CAN2
|
|
||||||
PB6 CAN2_TX CAN2
|
|
||||||
|
|
||||||
PA4 MPU_CS CS
|
|
||||||
PA5 SPI1_SCK SPI1
|
|
||||||
PA6 SPI1_MISO SPI1
|
|
||||||
PA7 SPI1_MOSI SPI1
|
|
||||||
SPIDEV icm20948 SPI1 DEVID4 MPU_CS MODE3 4*MHZ 8*MHZ
|
|
||||||
|
|
||||||
PF8 blank CS
|
|
||||||
SPIDEV profiled SPI5 DEVID1 blank MODE3 1*MHZ 1*MHZ
|
|
||||||
|
|
||||||
IMU Invensensev2 SPI:icm20948 ROTATION_YAW_270
|
|
||||||
|
|
||||||
define HAL_DEFAULT_INS_FAST_SAMPLE 5
|
|
||||||
|
|
||||||
# compass as part of ICM20948 on newer cubes
|
|
||||||
COMPASS AK09916:probe_ICM20948 0 ROTATION_ROLL_180_YAW_90
|
|
||||||
|
|
||||||
# one baro
|
|
||||||
#BARO MS56XX I2C:0:0x77
|
|
||||||
|
|
||||||
PE4 CSPI4_CS CS
|
|
||||||
PE2 SPI4_SCK SPI4
|
|
||||||
PE5 SPI4_MISO SPI4
|
|
||||||
PE6 SPI4_MOSI SPI4
|
|
||||||
|
|
||||||
|
|
||||||
PG15 CSPI6_CS CS
|
|
||||||
PG13 SPI6_SCK SPI6
|
|
||||||
PG12 SPI6_MISO SPI6
|
|
||||||
PG14 SPI6_MOSI SPI6
|
|
||||||
|
|
||||||
PF7 SPI5_SCK SPI5
|
|
||||||
PF9 SPI5_MOSI SPI5
|
|
||||||
|
|
||||||
# Now setup the pins for the microSD card, if available.
|
|
||||||
PC8 SDMMC1_D0 SDMMC1
|
|
||||||
PC9 SDMMC1_D1 SDMMC1
|
|
||||||
PC10 SDMMC1_D2 SDMMC1
|
|
||||||
PC11 SDMMC1_D3 SDMMC1
|
|
||||||
PC12 SDMMC1_CK SDMMC1
|
|
||||||
PD2 SDMMC1_CMD SDMMC1
|
|
||||||
|
|
||||||
define BOARD_SER1_RTSCTS_DEFAULT 0
|
|
||||||
|
|
||||||
define HAL_OS_FATFS_IO 1
|
|
||||||
define HAL_WITH_DRONECAN 1
|
|
||||||
env HAL_WITH_DRONECAN 1
|
|
||||||
define HAL_PICCOLO_CAN_ENABLE 0
|
|
||||||
define AP_CAN_SLCAN_ENABLED 1
|
|
||||||
|
|
||||||
PA1 ANALOG_CAN_VOLTAGE1 ADC1 SCALE(21.404)
|
|
||||||
PA2 ANALOG_CAN_VOLTAGE2 ADC1 SCALE(21.404)
|
|
||||||
|
|
||||||
#gps f9
|
|
||||||
PD10 F91TXR INPUT
|
|
||||||
PD11 F92RST INPUT
|
|
||||||
PD13 F93SB INPUT
|
|
||||||
PD14 F94EXTINT INPUT
|
|
||||||
PD15 F95RSV1 INPUT
|
|
||||||
PG2 F97 INPUT GPIO(100)
|
|
||||||
PG3 F96RSV2 INPUT
|
|
||||||
PD4 RTKSTAT INPUT
|
|
||||||
PD5 GEOFENCESTAT INPUT
|
|
||||||
|
|
||||||
#others
|
|
||||||
PG10 can2int INPUT
|
|
||||||
PG11 can2sleep OUTPUT PUSHPULL SPEED_LOW LOW
|
|
||||||
PF1 GNDDET2 INPUT
|
|
||||||
PF2 GNDDET1 INPUT
|
|
||||||
PC6 EXTERN_GPIO1 OUTPUT GPIO(1)
|
|
||||||
PC7 EXTERN_GPIO2 OUTPUT GPIO(2)
|
|
||||||
PD6 TERMCAN1 OUTPUT LOW GPIO(3)
|
|
||||||
PB3 TERMCAN2 OUTPUT LOW GPIO(4)
|
|
||||||
|
|
||||||
#sensor enable
|
|
||||||
PC0 SENSEN OUTPUT HIGH
|
|
||||||
PB1 IMUINT INPUT
|
|
||||||
PB4 QCAN2INT0 INPUT
|
|
||||||
PC13 QCAN1SLEEP OUTPUT PUSHPULL SPEED_LOW LOW
|
|
||||||
PE0 QCAN1INT INPUT
|
|
||||||
PE1 QCAN1INT0 INPUT
|
|
||||||
PE3 QCAN1INT1 INPUT
|
|
||||||
|
|
||||||
PD7 QCAN2INT1 INPUT
|
|
||||||
PA8 QCAN12OSC1 INPUT
|
|
||||||
|
|
||||||
# This defines more ADC inputs.
|
|
||||||
PC3 ANALOG_VCC_1_8V ADC1 SCALE(2)
|
|
||||||
PC4 ANALOG_VCC_5V_PIN ADC1 SCALE(2.044)
|
|
||||||
|
|
||||||
# This input pin is used to detect that power is valid on USB.
|
|
||||||
PA9 VBUS INPUT
|
|
||||||
PH13 VDD_33V_SENS INPUT
|
|
||||||
|
|
||||||
# This defines some input pins, currently unused.
|
|
||||||
PB2 BOOT1 INPUT
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
# disabled compass cal as it doesn't work for now
|
|
||||||
# define COMPASS_CAL_ENABLED 1
|
|
||||||
|
|
||||||
define HAL_ENABLE_SLCAN 1
|
|
||||||
# passthrough CAN1
|
|
||||||
define HAL_DEFAULT_CPORT 1
|
|
||||||
|
|
||||||
define CONFIGURE_PPS_PIN TRUE
|
|
||||||
define AP_PERIPH_HAVE_LED TRUE
|
|
||||||
|
|
||||||
define AP_SCRIPTING_ENABLED 1
|
|
||||||
define SCRIPTING_HEAP_SIZE (64*1024)
|
|
||||||
|
|
||||||
define GPS_MOVING_BASELINE 1
|
|
||||||
|
|
||||||
define AP_RC_CHANNEL_ENABLED 1
|
|
||||||
|
|
||||||
define AP_RELAY_ENABLED 1
|
|
||||||
define AP_SERVORELAYEVENTS_ENABLED 1
|
|
||||||
|
|
||||||
# bootloader embedding / bootloader flashing not available
|
|
||||||
define AP_BOOTLOADER_FLASHING_ENABLED 0
|
|
||||||
@ -1,315 +0,0 @@
|
|||||||
-- This script is a test of led override
|
|
||||||
-- luacheck: only 0
|
|
||||||
|
|
||||||
local count = 0
|
|
||||||
local num_leds = 16
|
|
||||||
local total_time = 1
|
|
||||||
local animation_end = 0
|
|
||||||
local current_anim = 0
|
|
||||||
|
|
||||||
-- constrain a value between limits
|
|
||||||
function constrain(v, vmin, vmax)
|
|
||||||
if v < vmin then
|
|
||||||
v = vmin
|
|
||||||
end
|
|
||||||
if v > vmax then
|
|
||||||
v = vmax
|
|
||||||
|
|
||||||
end
|
|
||||||
return v
|
|
||||||
end
|
|
||||||
|
|
||||||
--[[
|
|
||||||
Table of neon colors on a rainbow, red first
|
|
||||||
--]]
|
|
||||||
-- local rainbow = {
|
|
||||||
-- { 252, 23, 0 },
|
|
||||||
-- { 253, 72, 37 },
|
|
||||||
-- { 250, 237, 39 },
|
|
||||||
-- { 51, 255, 20 },
|
|
||||||
-- { 27, 3, 163 }
|
|
||||||
-- }
|
|
||||||
|
|
||||||
local rainbow = {
|
|
||||||
{ 252, 23, 0 },
|
|
||||||
{ 253, 100, 0 },
|
|
||||||
{ 250, 237, 0 },
|
|
||||||
{ 51, 255, 20 },
|
|
||||||
{ 27, 3, 163 }
|
|
||||||
}
|
|
||||||
|
|
||||||
local led_map = {
|
|
||||||
14,
|
|
||||||
15,
|
|
||||||
2,
|
|
||||||
3,
|
|
||||||
4,
|
|
||||||
5,
|
|
||||||
6,
|
|
||||||
7,
|
|
||||||
8,
|
|
||||||
9,
|
|
||||||
1,
|
|
||||||
0,
|
|
||||||
10,
|
|
||||||
11,
|
|
||||||
12,
|
|
||||||
13}
|
|
||||||
|
|
||||||
function table.contains(table, element)
|
|
||||||
for _, value in pairs(table) do
|
|
||||||
if value == element then
|
|
||||||
return true
|
|
||||||
end
|
|
||||||
end
|
|
||||||
return false
|
|
||||||
end
|
|
||||||
|
|
||||||
--[[
|
|
||||||
Function to set a LED to a color on a classic rainbow spectrum, with v=0 giving red
|
|
||||||
--]]
|
|
||||||
function get_Rainbow(v, num_rows)
|
|
||||||
local row = math.floor(constrain(v * (num_rows-1)+1, 1, num_rows-1))
|
|
||||||
local v0 = (row-1) / (num_rows-1)
|
|
||||||
local v1 = row / (num_rows-1)
|
|
||||||
local p = (v - v0) / (v1 - v0)
|
|
||||||
r = math.max(math.floor(rainbow[row][1] + p * (rainbow[row+1][1] - rainbow[row][1])),0)
|
|
||||||
g = math.max(math.floor(rainbow[row][2] + p * (rainbow[row+1][2] - rainbow[row][2])),0)
|
|
||||||
b = math.max(math.floor(rainbow[row][3] + p * (rainbow[row+1][3] - rainbow[row][3])),0)
|
|
||||||
return r,g,b
|
|
||||||
end
|
|
||||||
|
|
||||||
function do_initialisation()
|
|
||||||
local pos = math.rad(total_time/2)
|
|
||||||
local v =0.5 + 0.5 * math.sin(pos)
|
|
||||||
local invert = 1
|
|
||||||
local r, g, b = get_Rainbow(v, 5)
|
|
||||||
local led_trail_length = 3
|
|
||||||
local softness = 10
|
|
||||||
local bfact = 1
|
|
||||||
local updated_leds = {}
|
|
||||||
if math.cos(pos) > 0 then
|
|
||||||
invert = 1
|
|
||||||
else
|
|
||||||
invert = 0
|
|
||||||
end
|
|
||||||
|
|
||||||
pos = math.floor(6 + (v*8))%16
|
|
||||||
if pos == 6 then
|
|
||||||
animation_end = 1
|
|
||||||
else
|
|
||||||
animation_end = 0
|
|
||||||
end
|
|
||||||
|
|
||||||
for trail = 0, led_trail_length-1 do
|
|
||||||
if invert == 1 then
|
|
||||||
bfact = softness^(2*(led_trail_length - 1 - trail)/(led_trail_length-1))
|
|
||||||
else
|
|
||||||
bfact = softness^(2*trail/(led_trail_length-1))
|
|
||||||
end
|
|
||||||
local led_id = 1 + ((pos + trail) % num_leds)
|
|
||||||
notify:handle_rgb_id(math.floor(r/bfact), math.floor(g/bfact), math.floor(b/bfact), led_map[led_id])
|
|
||||||
table.insert(updated_leds, led_id)
|
|
||||||
end
|
|
||||||
|
|
||||||
pos = math.floor(6 - (v*8))%16
|
|
||||||
for trail = 0, led_trail_length-1 do
|
|
||||||
if invert == 0 then
|
|
||||||
bfact = softness^(2*(led_trail_length - 1 - trail)/(led_trail_length-1))
|
|
||||||
else
|
|
||||||
bfact = softness^(2*trail/(led_trail_length-1))
|
|
||||||
end
|
|
||||||
local led_id = 1 + ((pos + trail) % num_leds)
|
|
||||||
notify:handle_rgb_id(math.floor(r/bfact), math.floor(g/bfact), math.floor(b/bfact), led_map[led_id])
|
|
||||||
table.insert(updated_leds, led_id)
|
|
||||||
end
|
|
||||||
|
|
||||||
for led = 1, num_leds do
|
|
||||||
if not table.contains(updated_leds, led) then
|
|
||||||
notify:handle_rgb_id(0, 0, 0, led_map[led])
|
|
||||||
end
|
|
||||||
end
|
|
||||||
end
|
|
||||||
|
|
||||||
function do_point_north(r, g, b, led_trail_length, softness)
|
|
||||||
local north_bias = 4
|
|
||||||
local yaw = 8 - ((16 * periph:get_yaw_earth()/(2*math.pi))) - north_bias
|
|
||||||
local ofs = yaw - math.floor(yaw+0.5)
|
|
||||||
yaw = math.floor(yaw+0.5) % 16
|
|
||||||
local updated_leds = {}
|
|
||||||
for trail = 0, led_trail_length-1 do
|
|
||||||
local bfact = softness^(2*(math.abs((led_trail_length-1)/2 - trail + ofs))/(led_trail_length-1))
|
|
||||||
local led_id = 1 + ((yaw + trail) % 16)
|
|
||||||
notify:handle_rgb_id(math.floor(r/bfact), math.floor(g/bfact), math.floor(b/bfact), led_map[led_id])
|
|
||||||
table.insert(updated_leds, led_id)
|
|
||||||
end
|
|
||||||
|
|
||||||
for led = 1, num_leds do
|
|
||||||
if not table.contains(updated_leds, led) then
|
|
||||||
notify:handle_rgb_id(0, 0, 0, led_map[led])
|
|
||||||
end
|
|
||||||
end
|
|
||||||
return yaw
|
|
||||||
end
|
|
||||||
|
|
||||||
function finish_initialisation(r, g, b, led_trail_length, softness, speed_factor, next_call)
|
|
||||||
local north_bias = 4
|
|
||||||
local yaw = 8 - ((16 * periph:get_yaw_earth()/(2*math.pi))) - north_bias
|
|
||||||
yaw = math.floor(yaw+0.5) % 16
|
|
||||||
local led_id = math.floor((count/speed_factor) + north_bias) % 16
|
|
||||||
if yaw == led_id then
|
|
||||||
if total_time > 3000 then
|
|
||||||
animation_end = 1
|
|
||||||
end
|
|
||||||
return
|
|
||||||
end
|
|
||||||
count = count + next_call
|
|
||||||
local updated_leds = {}
|
|
||||||
for trail = 0, led_trail_length-1 do
|
|
||||||
local bfact = softness^(2*(math.abs((led_trail_length-1)/2 - trail))/(led_trail_length-1))
|
|
||||||
local this_led_id = 1 + ((led_id + trail) % 16)
|
|
||||||
notify:handle_rgb_id(math.floor(r/bfact), math.floor(g/bfact), math.floor(b/bfact), led_map[this_led_id])
|
|
||||||
table.insert(updated_leds, this_led_id)
|
|
||||||
end
|
|
||||||
|
|
||||||
for led = 1, num_leds do
|
|
||||||
if not table.contains(updated_leds, led) then
|
|
||||||
notify:handle_rgb_id(0, 0, 0, led_map[led])
|
|
||||||
end
|
|
||||||
end
|
|
||||||
end
|
|
||||||
|
|
||||||
function do_arm_spin(r, g, b, softness, speed_factor, arming)
|
|
||||||
-- reset led states
|
|
||||||
local updated_leds = {}
|
|
||||||
|
|
||||||
-- calculate next call based on time
|
|
||||||
local next_call = 1
|
|
||||||
if arming then
|
|
||||||
next_call = speed_factor - math.floor(((total_time)/speed_factor) + 0.5)
|
|
||||||
if next_call < 1 then
|
|
||||||
return next_call
|
|
||||||
end
|
|
||||||
else
|
|
||||||
next_call = 1 + math.floor(((total_time)/speed_factor) + 0.5)
|
|
||||||
if next_call > speed_factor then
|
|
||||||
return next_call
|
|
||||||
end
|
|
||||||
end
|
|
||||||
-- set trail length bassed on spin progress
|
|
||||||
local led_trail_length = 3 + (13 * (1 - (next_call/75)))
|
|
||||||
|
|
||||||
|
|
||||||
-- create a trail
|
|
||||||
for trail = 0, led_trail_length-1 do
|
|
||||||
local bfact = softness^(led_trail_length - trail)
|
|
||||||
local led_id = 1 + (count + trail) % 16
|
|
||||||
notify:handle_rgb_id(math.floor(r/bfact), math.floor(g/bfact), math.floor(b/bfact), led_map[led_id])
|
|
||||||
table.insert(updated_leds, led_id)
|
|
||||||
end
|
|
||||||
|
|
||||||
for led = 1, num_leds do
|
|
||||||
if not table.contains(updated_leds, led) then
|
|
||||||
notify:handle_rgb_id(0, 0, 0, led_map[led])
|
|
||||||
end
|
|
||||||
end
|
|
||||||
return next_call
|
|
||||||
end
|
|
||||||
|
|
||||||
function set_all(r, g, b)
|
|
||||||
for led = 1, num_leds do
|
|
||||||
notify:handle_rgb_id(r, g, b, led_map[led])
|
|
||||||
end
|
|
||||||
end
|
|
||||||
|
|
||||||
function animation_state_machine(vehicle_state)
|
|
||||||
-- change state only when last loop finished
|
|
||||||
if animation_end == 0 then
|
|
||||||
return
|
|
||||||
end
|
|
||||||
|
|
||||||
if ((vehicle_state & 1) == uint32_t(1)) then
|
|
||||||
-- do_initialisation
|
|
||||||
if current_anim ~= 0 then
|
|
||||||
current_anim = 0
|
|
||||||
total_time = 0
|
|
||||||
animation_end = 0
|
|
||||||
end
|
|
||||||
elseif current_anim == 0 then
|
|
||||||
-- do finish by move to north
|
|
||||||
count = 0
|
|
||||||
total_time = 0
|
|
||||||
current_anim = 1
|
|
||||||
animation_end = 0
|
|
||||||
elseif current_anim == 1 then
|
|
||||||
-- do always point north
|
|
||||||
current_anim = 2
|
|
||||||
elseif current_anim ~= 3 and ((vehicle_state & (1 << 1)) ~= uint32_t(0)) then --VEHICLE_STATE_ARMED
|
|
||||||
total_time = 0
|
|
||||||
current_anim = 3
|
|
||||||
animation_end = 0
|
|
||||||
elseif current_anim == 3 and ((vehicle_state & (1 << 1)) == uint32_t(0)) then
|
|
||||||
total_time = 0
|
|
||||||
current_anim = 4
|
|
||||||
animation_end = 0
|
|
||||||
elseif current_anim == 4 then
|
|
||||||
current_anim = 1
|
|
||||||
end
|
|
||||||
end
|
|
||||||
|
|
||||||
function update() -- this is the loop which periodically runs
|
|
||||||
local next_call = 20
|
|
||||||
local vehicle_state = periph:get_vehicle_state()
|
|
||||||
animation_state_machine(vehicle_state)
|
|
||||||
|
|
||||||
-- Initialisation
|
|
||||||
if current_anim == 0 then
|
|
||||||
do_initialisation()
|
|
||||||
total_time = total_time + next_call
|
|
||||||
elseif current_anim == 1 then
|
|
||||||
finish_initialisation(rainbow[1][1], rainbow[1][2], rainbow[1][3], 5, 20, 50, next_call)
|
|
||||||
total_time = total_time + next_call
|
|
||||||
elseif current_anim == 2 then
|
|
||||||
local v
|
|
||||||
if (vehicle_state & (1<<3)) ~= uint32_t(0) or --VEHICLE_STATE_PREARM
|
|
||||||
(vehicle_state & (1<<4)) ~= uint32_t(0) or --VEHICLE_STATE_PREARM_GPS
|
|
||||||
(vehicle_state & (1<<7)) ~= uint32_t(0) or --VEHICLE_STATE_FAILSAFE_RADIO
|
|
||||||
(vehicle_state & (1<<8)) ~= uint32_t(0) or --VEHICLE_STATE_FAILSAFE_BATT
|
|
||||||
(vehicle_state & (1<<11)) ~= uint32_t(0) then --VEHICLE_STATE_EKF_BAD
|
|
||||||
v = 0.0
|
|
||||||
end
|
|
||||||
if (vehicle_state & (1<<17)) == uint32_t(0) then--VEHICLE_STATE_POS_ABS_AVAIL
|
|
||||||
v = 0.5 + (0.5 * math.min(gps:num_sats(0)/20, 1.0))
|
|
||||||
else
|
|
||||||
v = 1.0
|
|
||||||
end
|
|
||||||
local r, g, b = get_Rainbow(v, 4)
|
|
||||||
count = do_point_north(r, g, b, 5, 20)
|
|
||||||
-- --[[ ARM Display
|
|
||||||
elseif current_anim == 3 then
|
|
||||||
if animation_end == 0 then
|
|
||||||
next_call = do_arm_spin(rainbow[4][1], rainbow[4][2], rainbow[4][3], 2, 75, true)
|
|
||||||
count = count + 1
|
|
||||||
end
|
|
||||||
if next_call < 1 then
|
|
||||||
set_all(rainbow[4][1], rainbow[4][2], rainbow[4][3])
|
|
||||||
animation_end = 1
|
|
||||||
else
|
|
||||||
total_time = total_time + next_call
|
|
||||||
end
|
|
||||||
elseif current_anim == 4 then
|
|
||||||
if animation_end == 0 then
|
|
||||||
next_call = do_arm_spin(rainbow[4][1], rainbow[4][2], rainbow[4][3], 2, 75, false)
|
|
||||||
count = count - 1
|
|
||||||
end
|
|
||||||
if next_call > 75 then
|
|
||||||
animation_end = 1
|
|
||||||
else
|
|
||||||
total_time = total_time + next_call
|
|
||||||
end
|
|
||||||
end
|
|
||||||
-- gcs:send_text(0, string.format("NCALL: %s", tostring(next_call)))
|
|
||||||
return update, next_call -- reschedules the loop in next_call milliseconds
|
|
||||||
end
|
|
||||||
|
|
||||||
return update() -- run immediately before starting to reschedule
|
|
||||||
Loading…
Reference in New Issue
Block a user