Skip to content

Commit

Permalink
AP_HAL_ChibiOS: add sw-boom-f407 and supporting scripts
Browse files Browse the repository at this point in the history
  • Loading branch information
WickedShell authored and peterbarker committed Apr 4, 2024
1 parent 81d7240 commit 84cef5b
Show file tree
Hide file tree
Showing 4 changed files with 208 additions and 0 deletions.
22 changes: 22 additions & 0 deletions libraries/AP_HAL_ChibiOS/hwdef/sw-boom-f407/defaults.parm
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
BATT_CAPACITY 6000
BATT_VOLT_PIN 2
CAN_MIRROR_PORTS 3
ESC_NUM_POLES 42
ESC_NUM_POLES2 42
ESC_PWM_TYPE 1
OUT1_MAX 2000
OUT1_MIN 1000
OUT2_MAX 2000
OUT2_MIN 1000
RELAY1_DEFAULT 1
RELAY2_DEFAULT 0
SCR_ENABLE 1
TEMP1_TYPE 5
TEMP1_PIN 14
TEMP1_SRC 3
TEMP1_SRC_ID 1
TEMP1_A0 -49.504657745
TEMP1_A1 75.849555969
TEMP1_A2 -22.5933513
TEMP1_A3 -0.18553680181
TEMP1_A4 1.5855249629
39 changes: 39 additions & 0 deletions libraries/AP_HAL_ChibiOS/hwdef/sw-boom-f407/hwdef-bl.dat
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
# MCU class and specific type
MCU STM32F4xx STM32F407xx

FLASH_RESERVE_START_KB 0
FLASH_BOOTLOADER_LOAD_KB 64
FLASH_SIZE_KB 512

# board ID for firmware load
APJ_BOARD_ID 6001

env AP_PERIPH 1

define CAN_APP_NODE_NAME "sw-boom-f407-bl"

# crystal frequency
OSCILLATOR_HZ 8000000

# activity led
PC8 LED_BOOTLOADER OUTPUT LOW
define HAL_LED_ON 1

PA13 JTMS-SWDIO SWD
PA14 JTCK-SWCLK SWD

PD0 CAN1_RX CAN1
PD1 CAN1_TX CAN1
PD5 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW

# can2 is the tail boom section, disable it
#PB5 CAN2_RX CAN2
#PB6 CAN2_TX CAN2
PC14 GPIO_CAN2_SILENT OUTPUT PUSHPULL SPEED_LOW HIGH

# setup important outputs
PA5 SAGETECH_MAINT OUTPUT GPIO(0) HIGH # force sagetech to maintenance mode
PD15 CHARGER_EN OUTPUT GPIO(3) LOW # disable the charger
PB10 BAT_HEATER OUTPUT PUSHPULL SPEED_LOW LOW # disable the battery heater

define HAL_USE_SERIAL FALSE
119 changes: 119 additions & 0 deletions libraries/AP_HAL_ChibiOS/hwdef/sw-boom-f407/hwdef.dat
Original file line number Diff line number Diff line change
@@ -0,0 +1,119 @@
# MCU class and specific type
MCU STM32F4xx STM32F407xx

FLASH_RESERVE_START_KB 64
FLASH_SIZE_KB 512

define HAL_STORAGE_SIZE 15360
STORAGE_FLASH_PAGE 2

# board ID for firmware load
APJ_BOARD_ID 6001

env AP_PERIPH 1

define STM32_ST_USE_TIMER 5

define CAN_APP_NODE_NAME "sw-boom-f407"

# crystal frequency
OSCILLATOR_HZ 8000000

# activity led
PC8 LED OUTPUT LOW
define HAL_LED_ON 1

# JTAG
PA13 JTMS-SWDIO SWD
PA14 JTCK-SWCLK SWD

PD0 CAN1_RX CAN1
PD1 CAN1_TX CAN1
PD2 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW

PB5 CAN2_RX CAN2
PB6 CAN2_TX CAN2
PB7 GPIO_CAN2_SILENT OUTPUT PUSHPULL SPEED_LOW LOW

# we want to be able to mirror CAN onto both buses
define HAL_PERIPH_CAN_MIRROR 1

# UARTs
# | nav light | esc front | esc rear | transponder |
SERIAL_ORDER USART6 USART2 USART3 UART4

# USART6 nav light
PC6 USART6_TX USART6 SPEED_HIGH DMA

# USART2 front esc
PD6 USART2_RX USART2 SPEED_HIGH DMA

# USART3 rear esc
PD9 USART3_RX USART3 SPEED_HIGH DMA

# USART4 transponder
PA0 UART4_TX UART4 SPEED_HIGH DMA
PA1 UART4_RX UART4 SPEED_HIGH DMA

# APD telem
define HAL_PERIPH_ENABLE_ESC_APD 1
define APD_ESC_INSTANCES 2
define APD_ESC_SERIAL_0 1
define APD_ESC_SERIAL_1 2

# pwm
define HAL_PERIPH_ENABLE_RC_OUT
PE9 TIM1_CH1 TIM1 PWM(1) GPIO(51) # esc front
PE11 TIM1_CH2 TIM1 PWM(2) GPIO(52) # esc rear
PB11 TIM2_CH4 TIM2 PWM(4) GPIO(54) # aux 3

# gpio
PA5 SAGETECH_MAINT OUTPUT GPIO(0) LOW
PB3 POWER_GOOD2 INPUT GPIO(1) # 7.4v secondary
PD7 POWER_GOOD1 INPUT GPIO(2) # 28v primary
PD15 CHARGER_EN OUTPUT GPIO(3) low # charger
PE4 ID_PIN INPUT GPIO(4) # high on left, low on right
PB10 HEAT_PIN OUTPUT GPIO(5) LOW # heater control, low is heater off, high is heater on
PB12 AUX3 OUTPUT GPIO(6) # aux 3

define HAL_PERIPH_ENABLE_RELAY 1
define AP_RELAY_ENABLED 1
define RELAY1_PIN_DEFAULT 3
define RELAY2_PIN_DEFAULT 5


# battery
define HAL_PERIPH_ENABLE_BATTERY
define HAL_PERIPH_BATTERY_SKIP_NAME
define HAL_USE_ADC TRUE
define STM32_ADC_USE_ADC1 TRUE
define AP_BATT_MONITOR_MAX_INSTANCES 4
define HAL_BATT_MONITOR_DEFAULT 4

# vtol voltage
PA2 BATT_VOLTAGE_SENS ADC1 SCALE(1) # pin 2
# vtol current
PA3 BATT_CURRENT_SENS ADC1 SCALE(1) # pin 3
# servo voltage
PA4 SERVO_VOLTAGE_SENS ADC1 SCALE(1) # pin 4
# 5V voltage
PA6 SUPPLY_VOLTAGE_SENS ADC1 SCALE(1) # pin 6
# 28V voltage
PB1 BATT_28V_SENS ADC1 SCALE(1) # pin 9
# 5v backup voltage
PC0 BATT_5V_SEC_SENS ADC1 SCALE(1) # pin 10
# 5v primary
PC2 BATT_5V_PRIM_SENS ADC1 SCALE(1) # pin 12
# battery temperature
PC4 BATT_TEMP_SENS ADC1 SCALE(1) # pin 14

# temperature
define AP_TEMPERATURE_SENSOR_ENABLED 1
define AP_TEMPERATURE_SENSOR_MAX_INSTANCES 1

# Scripting
define AP_SCRIPTING_ENABLED 1
define AP_FILESYSTEM_ROMFS_ENABLED 1

# don't build on firmware.ardupilot.org
AUTOBUILD_TARGETS None
28 changes: 28 additions & 0 deletions libraries/AP_HAL_ChibiOS/hwdef/sw-boom-f407/scripts/params.lua
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
-- load default parameters depending upon if the board is a left or a right

defaults = {}

forced = {{"BATT_SERIAL_NUM", 2, 3},
{"CAN_NODE", 109, 110},
{"ESC_NUMBER", 2, 0},
{"ESC_NUMBER2", 1, 3},
{"RELAY1_FUNCTION", 17, 18},
{"RELAY2_FUNCTION", 19, 20},
{"OUT1_FUNCTION", 35, 33},
{"OUT2_FUNCTION", 34, 36}}

local index
if gpio:read (4) then -- id pin is pin 4, high if it's a left
index = 2
else
index = 3
end

function set_table(fn, table)
for _,values in ipairs (table) do
assert(fn(param, values[1], values[index]), "not set the parameter: " .. values[index])
end
end

set_table(param.set_default, defaults)
set_table(param.set_and_save, forced)

0 comments on commit 84cef5b

Please sign in to comment.