Skip to content

Commit

Permalink
MR-CANHUBK3 ADAP board support, add ADC support
Browse files Browse the repository at this point in the history
  • Loading branch information
PetervdPerk-NXP committed May 11, 2023
1 parent be03b7f commit f9c2796
Show file tree
Hide file tree
Showing 17 changed files with 393 additions and 128 deletions.
5 changes: 5 additions & 0 deletions boards/nxp/mr-canhubk3/fmu.px4board
Original file line number Diff line number Diff line change
@@ -1,20 +1,25 @@
# CONFIG_BOARD_ROMFSROOT is not set
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1"
CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4"
CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3"
CONFIG_BOARD_UAVCAN_INTERFACES=1
CONFIG_COMMON_LIGHT=y
CONFIG_CYPHAL_BMS_SUBSCRIBER=y
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_CYPHAL=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IRLOCK=y
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y
CONFIG_DRIVERS_RC_INPUT=y
CONFIG_DRIVERS_SAFETY_BUTTON=y
CONFIG_DRIVERS_UAVCAN=y
CONFIG_EXAMPLES_FAKE_GPS=y
CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
CONFIG_MODULES_BATTERY_STATUS=y
CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_ESC_BATTERY=y
Expand Down
11 changes: 11 additions & 0 deletions boards/nxp/mr-canhubk3/init/rc.board_defaults
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,17 @@ param set-default MAV_1_UDP_PRT 14550
param set-default SENS_EXT_I2C_PRB 0
param set-default CYPHAL_ENABLE 0

if ver hwtypecmp MR-CANHUBK3-ADAP
then
param set-default GPS_1_CONFIG 202
param set-default RC_PORT_CONFIG 104
param set-default SENS_INT_BARO_EN 0
param set-default SYS_HAS_BARO 0
# MR-CANHUBK3-ADAP voltage divider
param set-default BAT1_V_DIV 13.158
safety_button start
fi

if param greater -s UAVCAN_ENABLE 0
then
ifup can0
Expand Down
31 changes: 18 additions & 13 deletions boards/nxp/mr-canhubk3/init/rc.board_sensors
Original file line number Diff line number Diff line change
Expand Up @@ -2,28 +2,33 @@
#
# NXP MR-CANHUBK3 specific board sensors init
#------------------------------------------------------------------------------

#board_adc start FIXME no ADC drivers
if ver hwtypecmp MR-CANHUBK3-ADAP
then
icm42688p -c 2 -b 3 -R 0 -S -f 15000 start
# Internal magnetometer on I2c on ADAP
bmm150 -X -a 18 start
ist8310 -X -b 1 -R 10 start
# ADC for voltage input sensing
board_adc start
else
bmm150 -X start
# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
ist8310 -X -b 2 -R 10 start
fi

#FMUv5Xbase board orientation

# Internal SPI bus ICM20649
icm20649 -s -R 6 start

# Internal SPI bus ICM42688p
icm42688p -R 6 -s start

# Internal magnetometer on I2c
bmm150 -I start
# External SPI bus ICM20649
icm20649 -b 4 -S -R 6 start

# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
ist8310 -X -b 2 -R 10 start
# External SPI bus ICM42688p
icm42688p -c 1 -b 3 -R 6 -S -f 15000 start

# External compass on GPS1/I2C1 (the 3rd external bus): Drotek RTK GPS with LIS3MDL Compass
lis3mdl -X -b 2 -R 2 start

# Disable startup of internal baros if param is set to false
if param compare SENS_INT_BARO_EN 1
then
bmp388 -I -a 0x77 start
bmp388 -X -a 0x77 start
fi
15 changes: 14 additions & 1 deletion boards/nxp/mr-canhubk3/nuttx-config/include/board.h
Original file line number Diff line number Diff line change
Expand Up @@ -152,6 +152,18 @@
#define PIN_LPUART1_RX (PIN_LPUART1_RX_3 | PIN_INPUT_PULLUP) /* PTC6 */
#define PIN_LPUART1_TX PIN_LPUART1_TX_3 /* PTC7 */


/* LPUART4 /dev/ttyS3 P8B 3X7 Pin 3 Single wire RC UART */

#define PIN_LPUART4_RX 0 /* NC */
#define PIN_LPUART4_TX PIN_LPUART4_TX_3 /* PTE11 */


/* LPUART7 /dev/ttyS4 P8B 3X7 Pin 3 and Pin 8 */

#define PIN_LPUART7_RX (PIN_LPUART7_RX_3 | PIN_INPUT_PULLUP) /* PTE0 */
#define PIN_LPUART7_TX PIN_LPUART7_TX_3 /* PTE1 */

/* LPUART9 P24 UART connector */

#define PIN_LPUART9_RX (PIN_LPUART9_RX_1 | PIN_INPUT_PULLUP) /* PTB2 */
Expand Down Expand Up @@ -209,7 +221,8 @@
#define PIN_LPSPI4_PCS0 PIN_LPSPI4_PCS0_1 /* PTB8 */
#define PIN_LPSPI4_PCS3 PIN_LPSPI4_PCS3_1 /* PTA16 */

#define PIN_LPSPI4_PCS (PIN_PTA16 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE) /* PTA16 */
#define PIN_LPSPI4_CS_P26 (PIN_PTA16 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE) /* PTA16 */
#define PIN_LPSPI4_CS_P8B (PIN_PTB8 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE) /* PTB8 */

/* LPSPI5 P26 external IMU connector */

Expand Down
2 changes: 2 additions & 0 deletions boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig
Original file line number Diff line number Diff line change
Expand Up @@ -245,6 +245,8 @@ CONFIG_S32K3XX_LPUART13=y
CONFIG_S32K3XX_LPUART14=y
CONFIG_S32K3XX_LPUART1=y
CONFIG_S32K3XX_LPUART2=y
CONFIG_S32K3XX_LPUART4=y
CONFIG_S32K3XX_LPUART7=y
CONFIG_S32K3XX_LPUART9=y
CONFIG_S32K3XX_LPUART_INVERT=y
CONFIG_S32K3XX_LPUART_SINGLEWIRE=y
Expand Down
3 changes: 3 additions & 0 deletions boards/nxp/mr-canhubk3/src/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader")
clockconfig.c
periphclocks.c
timer_config.cpp
hw_rev_ver_canhubk3.c
)

target_link_libraries(drivers_board
Expand Down Expand Up @@ -65,6 +66,8 @@ else()
spi.cpp
timer_config.cpp
s32k3xx_userleds.c
hw_rev_ver_canhubk3.c
manifest.c
)

target_link_libraries(drivers_board
Expand Down
36 changes: 35 additions & 1 deletion boards/nxp/mr-canhubk3/src/board_config.h
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,7 @@ __BEGIN_DECLS
#define DIRECT_PWM_OUTPUT_CHANNELS 8

#define RC_SERIAL_PORT "/dev/ttyS5"
#define RC_SERIAL_SINGLEWIRE
#define RC_SERIAL_SINGLEWIRE_FORCE
#define RC_SERIAL_INVERT_RX_ONLY

#define BOARD_ENABLE_CONSOLE_BUFFER
Expand All @@ -110,6 +110,40 @@ __BEGIN_DECLS
/* Reboot and ulog we store on a wear-level filesystem */
#define HARDFAULT_REBOOT_PATH "/mnt/progmem/reboot"

/* To detect MR-CANHUBK3-ADAP board */
#define BOARD_HAS_HW_VERSIONING 1
#define CANHUBK3_ADAP_DETECT (PIN_PTA12 | GPIO_INPUT | GPIO_PULLUP)


/*
* ADC channels
*
* These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4
* Firmware in the adc driver. ADC1 has 32 channels, with some a/b selection overlap
* in the AD4-AD7 range on the same ADC.
*
* Only ADC1 is used
* Bits 31:0 are ADC1 channels 31:0
*/

#define ADC1_CH(c) (((c) & 0x1f)) /* Define ADC number Channel number */

/* ADC defines to be used in sensors.cpp to read from a particular channel */

#define ADC_BATTERY_VOLTAGE_CHANNEL ADC1_CH(0) /* BAT_VSENS 85 PTB4 ADC1_SE10 */
#define ADC_BATTERY_CURRENT_CHANNEL ADC1_CH(1) /* Non-existant but needed for compilation */


/* Mask use to initialize the ADC driver */

#define ADC_CHANNELS ((1 << ADC_BATTERY_VOLTAGE_CHANNEL))

/* Safety Switch
* TBD
*/
#define GPIO_LED_SAFETY (PIN_PTE26 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO)
#define GPIO_BTN_SAFETY (PIN_PTA11 | GPIO_INPUT | GPIO_PULLDOWN)

/****************************************************************************
* Public Data
****************************************************************************/
Expand Down
142 changes: 142 additions & 0 deletions boards/nxp/mr-canhubk3/src/hw_rev_ver_canhubk3.c
Original file line number Diff line number Diff line change
@@ -0,0 +1,142 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/

/**
* @file hw_rev_ver_canhubk3.c
* CANHUBK3 Hardware Revision and Version ID API
*/
#include <drivers/drv_adc.h>
#include <px4_arch/adc.h>
#include <px4_platform_common/micro_hal.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform/board_determine_hw_info.h>
#include <stdio.h>
#include <board_config.h>

#include <systemlib/px4_macros.h>

#if defined(BOARD_HAS_HW_VERSIONING)


#define HW_INFO_SIZE HW_INFO_VER_DIGITS + HW_INFO_REV_DIGITS


/****************************************************************************
* Private Data
****************************************************************************/
static int is_adap_connected = 0;

/****************************************************************************
* Public Functions
****************************************************************************/
/************************************************************************************
* Name: board_get_hw_type
*
* Description:
* Optional returns a 0 terminated string defining the HW type.
*
* Input Parameters:
* None
*
* Returned Value:
* a 0 terminated string defining the HW type. This my be a 0 length string ""
*
************************************************************************************/

__EXPORT const char *board_get_hw_type_name()
{
if (is_adap_connected) {
return (const char *)"MR-CANHUBK3-ADAP";

} else {
return (const char *)"MR-CANHUBK344";
}
}

/************************************************************************************
* Name: board_get_hw_version
*
* Description:
* Optional returns a integer HW version
*
* Input Parameters:
* None
*
* Returned Value:
* An integer value of this boards hardware version.
* A value of -1 is the default for boards not supporting the BOARD_HAS_VERSIONING API.
* A value of 0 is the default for boards supporting the API but not having version.
*
************************************************************************************/

__EXPORT int board_get_hw_version()
{
return 0;
}

/************************************************************************************
* Name: board_get_hw_revision
*
* Description:
* Optional returns a integer HW revision
*
* Input Parameters:
* None
*
* Returned Value:
* An integer value of this boards hardware revision.
* A value of -1 is the default for boards not supporting the BOARD_HAS_VERSIONING API.
* A value of 0 is the default for boards supporting the API but not having revision.
*
************************************************************************************/

__EXPORT int board_get_hw_revision()
{
return 0;
}

/************************************************************************************
* Name: board_determine_hw_info
*
* Description:
* Uses GPIO to detect MR-CANHUBK3-ADAP
*
************************************************************************************/

int board_determine_hw_info()
{
s32k3xx_pinconfig(CANHUBK3_ADAP_DETECT);
is_adap_connected = !s32k3xx_gpioread(CANHUBK3_ADAP_DETECT);
return 0;
}
#endif
4 changes: 2 additions & 2 deletions boards/nxp/mr-canhubk3/src/i2c.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,6 @@
#include <px4_arch/i2c_hw_description.h>

constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
initI2CBusInternal(PX4_BUS_NUMBER_TO_PX4(0)),
initI2CBusExternal(PX4_BUS_NUMBER_TO_PX4(1)),
initI2CBusExternal(PX4_BUS_NUMBER_TO_PX4(0)),
initI2CBusInternal(PX4_BUS_NUMBER_TO_PX4(1)),
};
3 changes: 3 additions & 0 deletions boards/nxp/mr-canhubk3/src/init.c
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@
#include "board_config.h"

#include <px4_platform_common/init.h>
#include <px4_platform/board_determine_hw_info.h>

#if defined(CONFIG_S32K3XX_LPSPI1) && defined(CONFIG_MMCSD)
#include <nuttx/mmcsd.h>
Expand Down Expand Up @@ -96,6 +97,8 @@ __EXPORT int board_app_initialize(uintptr_t arg)

int rv;

board_determine_hw_info();


#if defined(CONFIG_S32K3XX_LPSPI1) && defined(CONFIG_MMCSD)
/* LPSPI1 *****************************************************************/
Expand Down
Loading

0 comments on commit f9c2796

Please sign in to comment.