mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-17 17:17:34 +08:00
CAN node STM32F7 support and Freefly RTK GPS CAN node
This commit is contained in:
Daniel Agar
@@ -40,6 +40,7 @@ px4_add_library(canbootloader
|
||||
sched/timer.c
|
||||
uavcan/main.c
|
||||
util/random.c
|
||||
util/cxx_init.c
|
||||
)
|
||||
|
||||
include_directories(include)
|
||||
|
||||
@@ -0,0 +1,44 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
include_directories(../../../include)
|
||||
|
||||
px4_add_library(arch_canbootloader
|
||||
board_identity.c
|
||||
drivers/can/driver.c
|
||||
)
|
||||
|
||||
target_link_libraries(arch_canbootloader
|
||||
PRIVATE
|
||||
arch_watchdog_iwdg
|
||||
)
|
||||
@@ -0,0 +1,56 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2017 PX4 Development Team. All rights reserved.
|
||||
* Author: @author David Sidrane <david_s5@nscdg.com>
|
||||
*
|
||||
* 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 board_identity.c
|
||||
* Implementation of STM32 based Board identity API
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#define SWAP_UINT32(x) (((x) >> 24) | (((x) & 0x00ff0000) >> 8) | (((x) & 0x0000ff00) << 8) | ((x) << 24))
|
||||
|
||||
int board_get_mfguid(mfguid_t mfgid)
|
||||
{
|
||||
uint32_t *chip_uuid = (uint32_t *) STM32_SYSMEM_UID;
|
||||
uint32_t *rv = (uint32_t *) &mfgid[0];
|
||||
|
||||
for (unsigned int i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
|
||||
*rv++ = SWAP_UINT32(chip_uuid[(PX4_CPU_UUID_WORD32_LENGTH - 1) - i]);
|
||||
}
|
||||
|
||||
return PX4_CPU_MFGUID_BYTE_LENGTH;
|
||||
}
|
||||
@@ -0,0 +1,566 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: Ben Dyer <ben_dyer@mac.com>
|
||||
* Pavel Kirienko <pavel.kirienko@zubax.com>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include "boot_config.h"
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <limits.h>
|
||||
|
||||
#include "chip.h"
|
||||
#include <hardware/stm32_can.h>
|
||||
#include "nvic.h"
|
||||
|
||||
#include "board.h"
|
||||
#include <systemlib/px4_macros.h>
|
||||
#include "can.h"
|
||||
#include "timer.h"
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <lib/systemlib/crc.h>
|
||||
|
||||
#define INAK_TIMEOUT 65535
|
||||
|
||||
#define CAN_TX_TIMEOUT_MS (200 /(1000/(1000000/CONFIG_USEC_PER_TICK)))
|
||||
|
||||
#define SJW_POS 24
|
||||
#define BS1_POS 16
|
||||
#define BS2_POS 20
|
||||
|
||||
#define CAN_TSR_RQCP_SHFTS 8
|
||||
|
||||
#define FILTER_ID 1
|
||||
#define FILTER_MASK 2
|
||||
|
||||
#if STM32_PCLK1_FREQUENCY == 54000000
|
||||
/* Sample 85.7 % */
|
||||
# define QUANTA 18
|
||||
# define BS1_VALUE 14
|
||||
# define BS2_VALUE 1
|
||||
#elif STM32_PCLK1_FREQUENCY == 48000000
|
||||
/* Sample 85.7 % */
|
||||
# define QUANTA 16
|
||||
# define BS1_VALUE 12
|
||||
# define BS2_VALUE 1
|
||||
#elif STM32_PCLK1_FREQUENCY == 45000000 || STM32_PCLK1_FREQUENCY == 36000000
|
||||
/* Sample 88.9 % */
|
||||
# define QUANTA 9
|
||||
# define BS1_VALUE 6
|
||||
# define BS2_VALUE 0
|
||||
#elif STM32_PCLK1_FREQUENCY == 42000000
|
||||
/* Sample 85.7 % */
|
||||
# define QUANTA 14
|
||||
# define BS1_VALUE 10
|
||||
# define BS2_VALUE 1
|
||||
#else
|
||||
# warning Undefined QUANTA bsed on Clock Rate
|
||||
/* Sample 85.7 % */
|
||||
# define QUANTA 14
|
||||
# define BS1_VALUE 10
|
||||
# define BS2_VALUE 1
|
||||
#endif
|
||||
|
||||
#define CAN_1MBAUD_SJW 0
|
||||
#define CAN_1MBAUD_BS1 BS1_VALUE
|
||||
#define CAN_1MBAUD_BS2 BS2_VALUE
|
||||
#define CAN_1MBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/1000000/QUANTA)
|
||||
|
||||
#define CAN_500KBAUD_SJW 0
|
||||
#define CAN_500KBAUD_BS1 BS1_VALUE
|
||||
#define CAN_500KBAUD_BS2 BS2_VALUE
|
||||
#define CAN_500KBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/500000/QUANTA)
|
||||
|
||||
#define CAN_250KBAUD_SJW 0
|
||||
#define CAN_250KBAUD_BS1 BS1_VALUE
|
||||
#define CAN_250KBAUD_BS2 BS2_VALUE
|
||||
#define CAN_250KBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/250000/QUANTA)
|
||||
|
||||
#define CAN_125KBAUD_SJW 0
|
||||
#define CAN_125KBAUD_BS1 BS1_VALUE
|
||||
#define CAN_125KBAUD_BS2 BS2_VALUE
|
||||
#define CAN_125KBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/125000/QUANTA)
|
||||
|
||||
#define CAN_BTR_LBK_SHIFT 30
|
||||
|
||||
// Number of CPU cycles for a single bit time at the supported speeds
|
||||
#define CAN_125KBAUD_BIT_CYCLES (8*(TIMER_HRT_CYCLES_PER_US))
|
||||
|
||||
#define CAN_BAUD_TIME_IN_MS 200
|
||||
#define CAN_BAUD_SAMPLES_NEEDED 32
|
||||
#define CAN_BAUD_SAMPLES_DISCARDED 8
|
||||
|
||||
static inline uint32_t read_msr_rx(void)
|
||||
{
|
||||
return getreg32(STM32_CAN1_MSR) & CAN_MSR_RX;
|
||||
}
|
||||
|
||||
static uint32_t read_msr(time_hrt_cycles_t *now)
|
||||
{
|
||||
__asm__ __volatile__("\tcpsid i\n");
|
||||
*now = timer_hrt_read();
|
||||
uint32_t msr = read_msr_rx();
|
||||
__asm__ __volatile__("\tcpsie i\n");
|
||||
return msr;
|
||||
}
|
||||
|
||||
static int read_bits_times(time_hrt_cycles_t *times, size_t max)
|
||||
{
|
||||
uint32_t samplecnt = 0;
|
||||
bl_timer_id ab_timer = timer_allocate(modeTimeout | modeStarted, CAN_BAUD_TIME_IN_MS, 0);
|
||||
time_ref_t ab_ref = timer_ref(ab_timer);
|
||||
uint32_t msr;
|
||||
uint32_t last_msr = read_msr(times);
|
||||
|
||||
while (samplecnt < max && !timer_ref_expired(ab_ref)) {
|
||||
do {
|
||||
msr = read_msr(×[samplecnt]);
|
||||
} while (!(msr ^ last_msr) && !timer_ref_expired(ab_ref));
|
||||
|
||||
last_msr = msr;
|
||||
samplecnt++;
|
||||
}
|
||||
|
||||
timer_free(ab_timer);
|
||||
return samplecnt;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: can_speed2freq
|
||||
*
|
||||
* Description:
|
||||
* This function maps a can_speed_t to a bit rate in Hz
|
||||
*
|
||||
* Input Parameters:
|
||||
* can_speed - A can_speed_t from CAN_125KBAUD to CAN_1MBAUD
|
||||
*
|
||||
* Returned value:
|
||||
* Bit rate in Hz
|
||||
*
|
||||
****************************************************************************/
|
||||
int can_speed2freq(can_speed_t speed)
|
||||
|
||||
{
|
||||
return 1000000 >> (CAN_1MBAUD - speed);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: can_speed2freq
|
||||
*
|
||||
* Description:
|
||||
* This function maps a frequency in Hz to a can_speed_t in the range
|
||||
* CAN_125KBAUD to CAN_1MBAUD.
|
||||
*
|
||||
* Input Parameters:
|
||||
* freq - Bit rate in Hz
|
||||
*
|
||||
* Returned value:
|
||||
* A can_speed_t from CAN_125KBAUD to CAN_1MBAUD
|
||||
*
|
||||
****************************************************************************/
|
||||
can_speed_t can_freq2speed(int freq)
|
||||
{
|
||||
return (freq == 1000000u ? CAN_1MBAUD : freq == 500000u ? CAN_500KBAUD : freq == 250000u ? CAN_250KBAUD : CAN_125KBAUD);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: can_tx
|
||||
*
|
||||
* Description:
|
||||
* This function is called to transmit a CAN frame using the supplied
|
||||
* mailbox. It will busy wait on the mailbox if not available.
|
||||
*
|
||||
* Input Parameters:
|
||||
* message_id - The CAN message's EXID field
|
||||
* length - The number of bytes of data - the DLC field
|
||||
* message - A pointer to 8 bytes of data to be sent (all 8 bytes will be
|
||||
* loaded into the CAN transmitter but only length bytes will
|
||||
* be sent.
|
||||
* mailbox - A can_fifo_mailbox_t MBxxx value to choose the outgoing
|
||||
* mailbox.
|
||||
*
|
||||
* Returned value:
|
||||
* The CAN_OK of the data sent or CAN_ERROR if a time out occurred
|
||||
*
|
||||
****************************************************************************/
|
||||
uint8_t can_tx(uint32_t message_id, size_t length, const uint8_t *message, uint8_t mailbox)
|
||||
{
|
||||
uint32_t data[2];
|
||||
|
||||
memcpy(data, message, sizeof(data));
|
||||
|
||||
/*
|
||||
* Just block while waiting for the mailbox.
|
||||
*/
|
||||
|
||||
uint32_t mask = CAN_TSR_TME0 << mailbox;
|
||||
|
||||
/* Reset the indication of timer expired */
|
||||
|
||||
timer_hrt_clear_wrap();
|
||||
uint32_t cnt = CAN_TX_TIMEOUT_MS;
|
||||
|
||||
while ((getreg32(STM32_CAN1_TSR) & mask) == 0) {
|
||||
if (timer_hrt_wrap()) {
|
||||
timer_hrt_clear_wrap();
|
||||
|
||||
if (--cnt == 0) {
|
||||
return CAN_ERROR;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* To allow detection of completion - Set the LEC to
|
||||
* 'No error' state off all 1s
|
||||
*/
|
||||
|
||||
putreg32(CAN_ESR_LEC_MASK, STM32_CAN1_ESR);
|
||||
|
||||
putreg32(length & CAN_TDTR_DLC_MASK, STM32_CAN1_TDTR(mailbox));
|
||||
putreg32(data[0], STM32_CAN1_TDLR(mailbox));
|
||||
putreg32(data[1], STM32_CAN1_TDHR(mailbox));
|
||||
putreg32((message_id << CAN_TIR_EXID_SHIFT) | CAN_TIR_IDE | CAN_TIR_TXRQ,
|
||||
STM32_CAN1_TIR(mailbox));
|
||||
return CAN_OK;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: can_rx
|
||||
*
|
||||
* Description:
|
||||
* This function is called to receive a CAN frame from a supplied fifo.
|
||||
* It does not block if there is not available, but returns 0
|
||||
*
|
||||
* Input Parameters:
|
||||
* message_id - A pointer to return the CAN message's EXID field
|
||||
* length - A pointer to return the number of bytes of data - the DLC field
|
||||
* message - A pointer to return 8 bytes of data to be sent (all 8 bytes will
|
||||
* be written from the CAN receiver but only length bytes will be sent.
|
||||
* fifo A can_fifo_mailbox_t fifixxx value to choose the incoming fifo.
|
||||
*
|
||||
* Returned value:
|
||||
* The length of the data read or 0 if the fifo was empty
|
||||
*
|
||||
****************************************************************************/
|
||||
uint8_t can_rx(uint32_t *message_id, size_t *length, uint8_t *message, uint8_t fifo)
|
||||
{
|
||||
uint32_t data[2];
|
||||
uint8_t rv = 0;
|
||||
const uint32_t fifos[] = { STM32_CAN1_RF0R, STM32_CAN1_RF1R };
|
||||
|
||||
if (getreg32(fifos[fifo & 1]) & CAN_RFR_FMP_MASK) {
|
||||
|
||||
rv = 1;
|
||||
/* If so, process it */
|
||||
|
||||
*message_id = (getreg32(STM32_CAN1_RIR(fifo)) & CAN_RIR_EXID_MASK) >>
|
||||
CAN_RIR_EXID_SHIFT;
|
||||
*length = (getreg32(STM32_CAN1_RDTR(fifo)) & CAN_RDTR_DLC_MASK) >>
|
||||
CAN_RDTR_DLC_SHIFT;
|
||||
data[0] = getreg32(STM32_CAN1_RDLR(fifo));
|
||||
data[1] = getreg32(STM32_CAN1_RDHR(fifo));
|
||||
|
||||
putreg32(CAN_RFR_RFOM, fifos[fifo & 1]);
|
||||
|
||||
memcpy(message, data, sizeof(data));
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: can_autobaud
|
||||
*
|
||||
* Description:
|
||||
* This function will attempt to detect the bit rate in use on the CAN
|
||||
* interface until the timeout provided expires or the successful detection
|
||||
* occurs.
|
||||
*
|
||||
* It will initialize the CAN block for a given bit rate
|
||||
* to test that a message can be received. The CAN interface is left
|
||||
* operating at the detected bit rate and in CAN_Mode_Normal mode.
|
||||
*
|
||||
* Input Parameters:
|
||||
* can_speed - A pointer to return detected can_speed_t from CAN_UNKNOWN to
|
||||
* CAN_1MBAUD
|
||||
* timeout - The timer id of a timer to use as the maximum time to wait for
|
||||
* successful bit rate detection. This timer may be not running
|
||||
* in which case the auto baud code will try indefinitely to
|
||||
* detect the bit rate.
|
||||
*
|
||||
* Returned value:
|
||||
* CAN_OK - on Success or a CAN_BOOT_TIMEOUT
|
||||
*
|
||||
****************************************************************************/
|
||||
int can_autobaud(can_speed_t *can_speed, bl_timer_id timeout)
|
||||
{
|
||||
*can_speed = CAN_UNKNOWN;
|
||||
|
||||
volatile int attempt = 0;
|
||||
/* Threshold are at 1.5 Bit times */
|
||||
|
||||
/*
|
||||
* We are here because there was a reset or the app invoked
|
||||
* the bootloader with no bit rate set.
|
||||
*/
|
||||
|
||||
time_hrt_cycles_t bit_time;
|
||||
time_hrt_cycles_t min_cycles;
|
||||
int sample;
|
||||
can_speed_t speed = CAN_125KBAUD;
|
||||
|
||||
time_hrt_cycles_t samples[128];
|
||||
|
||||
while (1) {
|
||||
|
||||
|
||||
while (1) {
|
||||
|
||||
min_cycles = ULONG_MAX;
|
||||
int samplecnt = read_bits_times(samples, arraySize(samples));
|
||||
|
||||
if (timer_expired(timeout)) {
|
||||
return CAN_BOOT_TIMEOUT;
|
||||
}
|
||||
|
||||
if ((getreg32(STM32_CAN1_RF0R) | getreg32(STM32_CAN1_RF1R)) &
|
||||
CAN_RFR_FMP_MASK) {
|
||||
*can_speed = speed;
|
||||
can_init(speed, CAN_Mode_Normal);
|
||||
return CAN_OK;
|
||||
}
|
||||
|
||||
if (samplecnt < CAN_BAUD_SAMPLES_NEEDED) {
|
||||
continue;
|
||||
}
|
||||
|
||||
for (sample = 0; sample < samplecnt; sample += 2) {
|
||||
bit_time = samples[sample] = timer_hrt_elapsed(samples[sample], samples[sample + 1]);
|
||||
|
||||
if (sample > CAN_BAUD_SAMPLES_DISCARDED && bit_time < min_cycles) {
|
||||
min_cycles = bit_time;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
uint32_t bit34 = CAN_125KBAUD_BIT_CYCLES - CAN_125KBAUD_BIT_CYCLES / 4;
|
||||
samples[1] = min_cycles;
|
||||
speed = CAN_125KBAUD;
|
||||
|
||||
while (min_cycles < bit34 && speed < CAN_1MBAUD) {
|
||||
speed++;
|
||||
bit34 /= 2;
|
||||
}
|
||||
|
||||
attempt++;
|
||||
can_init(speed, CAN_Mode_Silent);
|
||||
|
||||
|
||||
} /* while(1) */
|
||||
|
||||
return CAN_OK;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: can_init
|
||||
*
|
||||
* Description:
|
||||
* This function is used to initialize the CAN block for a given bit rate and
|
||||
* mode.
|
||||
*
|
||||
* Input Parameters:
|
||||
* speed - A can_speed_t from CAN_125KBAUD to CAN_1MBAUD
|
||||
* mode - One of the can_mode_t of Normal, LoopBack and Silent or
|
||||
* combination thereof.
|
||||
*
|
||||
* Returned value:
|
||||
* OK - on Success or a negate errno value
|
||||
*
|
||||
****************************************************************************/
|
||||
int can_init(can_speed_t speed, can_mode_t mode)
|
||||
{
|
||||
int speedndx = speed - 1;
|
||||
/*
|
||||
* TODO: use full-word writes to reduce the number of loads/stores.
|
||||
*
|
||||
* Also consider filter use -- maybe set filters for all the message types we
|
||||
* want. */
|
||||
const uint32_t bitrates[] = {
|
||||
(CAN_125KBAUD_SJW << SJW_POS) |
|
||||
(CAN_125KBAUD_BS1 << BS1_POS) |
|
||||
(CAN_125KBAUD_BS2 << BS2_POS) | (CAN_125KBAUD_PRESCALER - 1),
|
||||
|
||||
(CAN_250KBAUD_SJW << SJW_POS) |
|
||||
(CAN_250KBAUD_BS1 << BS1_POS) |
|
||||
(CAN_250KBAUD_BS2 << BS2_POS) | (CAN_250KBAUD_PRESCALER - 1),
|
||||
|
||||
(CAN_500KBAUD_SJW << SJW_POS) |
|
||||
(CAN_500KBAUD_BS1 << BS1_POS) |
|
||||
(CAN_500KBAUD_BS2 << BS2_POS) | (CAN_500KBAUD_PRESCALER - 1),
|
||||
|
||||
(CAN_1MBAUD_SJW << SJW_POS) |
|
||||
(CAN_1MBAUD_BS1 << BS1_POS) |
|
||||
(CAN_1MBAUD_BS2 << BS2_POS) | (CAN_1MBAUD_PRESCALER - 1)
|
||||
};
|
||||
|
||||
/* Remove Unknow Offset */
|
||||
if (speedndx < 0 || speedndx > (int)arraySize(bitrates)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
uint32_t timeout;
|
||||
/*
|
||||
* Reset state is 0x0001 0002 CAN_MCR_DBF|CAN_MCR_SLEEP
|
||||
* knock down Sleep and raise CAN_MCR_INRQ
|
||||
*/
|
||||
|
||||
putreg32(CAN_MCR_DBF | CAN_MCR_INRQ, STM32_CAN1_MCR);
|
||||
|
||||
/* Wait until initialization mode is acknowledged */
|
||||
|
||||
for (timeout = INAK_TIMEOUT; timeout > 0; timeout--) {
|
||||
if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) != 0) {
|
||||
/* We are in initialization mode */
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (timeout < 1) {
|
||||
/*
|
||||
* Initialization failed, not much we can do now other than try a normal
|
||||
* startup. */
|
||||
return -ETIME;
|
||||
}
|
||||
|
||||
|
||||
putreg32(bitrates[speedndx] | mode << CAN_BTR_LBK_SHIFT, STM32_CAN1_BTR);
|
||||
putreg32(CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_DBF | CAN_MCR_TXFP, STM32_CAN1_MCR);
|
||||
|
||||
for (timeout = INAK_TIMEOUT; timeout > 0; timeout--) {
|
||||
if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) == 0) {
|
||||
/* We are in initialization mode */
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (timeout < 1) {
|
||||
return -ETIME;
|
||||
}
|
||||
|
||||
/*
|
||||
* CAN filter initialization -- accept everything on RX FIFO 0, and only
|
||||
* GetNodeInfo requests on RX FIFO 1. */
|
||||
|
||||
/* Set FINIT - Initialization mode for the filters- */
|
||||
putreg32(CAN_FMR_FINIT, STM32_CAN1_FMR);
|
||||
|
||||
putreg32(0, STM32_CAN1_FA1R); /* Disable all filters */
|
||||
|
||||
putreg32(3, STM32_CAN1_FS1R); /* Single 32-bit scale configuration for filters 0 and 1 */
|
||||
|
||||
/* Filter 0 masks -- DTIDGetNodeInfo requests only */
|
||||
|
||||
uavcan_protocol_t protocol;
|
||||
|
||||
protocol.id.u32 = 0;
|
||||
protocol.ser.type_id = DTIDReqGetNodeInfo;
|
||||
protocol.ser.service_not_message = true;
|
||||
protocol.ser.request_not_response = true;
|
||||
|
||||
/* Set the Filter ID */
|
||||
putreg32(protocol.id.u32 << CAN_RIR_EXID_SHIFT, STM32_CAN1_FIR(0, FILTER_ID));
|
||||
|
||||
/* Set the Filter Mask */
|
||||
protocol.ser.type_id = BIT_MASK(LengthUavCanServiceTypeID);
|
||||
|
||||
putreg32(protocol.id.u32 << CAN_RIR_EXID_SHIFT, STM32_CAN1_FIR(0, FILTER_MASK));
|
||||
|
||||
/* Filter 1 masks -- everything is don't-care */
|
||||
putreg32(0, STM32_CAN1_FIR(1, FILTER_ID));
|
||||
putreg32(0, STM32_CAN1_FIR(1, FILTER_MASK));
|
||||
|
||||
putreg32(0, STM32_CAN1_FM1R); /* Mask mode for all filters */
|
||||
putreg32(1, STM32_CAN1_FFA1R); /* FIFO 1 for filter 0, FIFO 0 for the
|
||||
* rest of filters */
|
||||
putreg32(3, STM32_CAN1_FA1R); /* Enable filters 0 and 1 */
|
||||
|
||||
/* Clear FINIT - Active mode for the filters- */
|
||||
|
||||
putreg32(0, STM32_CAN1_FMR);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: can_cancel_on_error
|
||||
*
|
||||
* Description:
|
||||
* This function will test for transition completion or any error.
|
||||
* If the is a error the the transmit will be aborted.
|
||||
*
|
||||
* Input Parameters:
|
||||
* mailbox - A can_fifo_mailbox_t MBxxx value to choose the outgoing
|
||||
* mailbox.
|
||||
*
|
||||
* Returned value:
|
||||
* CAN_OK - on Success or a CAN_ERROR if the cancellation was needed
|
||||
*
|
||||
****************************************************************************/
|
||||
void can_cancel_on_error(uint8_t mailbox)
|
||||
{
|
||||
uint32_t rvalue;
|
||||
|
||||
/* Wait for completion the all 1's wat set in the tx code*/
|
||||
while (CAN_ESR_LEC_MASK == ((rvalue = (getreg32(STM32_CAN1_ESR) & CAN_ESR_LEC_MASK))));
|
||||
|
||||
/* Any errors */
|
||||
if (rvalue) {
|
||||
|
||||
putreg32(0, STM32_CAN1_ESR);
|
||||
|
||||
/* Abort the the TX in case of collision wuth NART false */
|
||||
|
||||
putreg32(CAN_TSR_ABRQ0 << (mailbox * CAN_TSR_RQCP_SHFTS), STM32_CAN1_TSR);
|
||||
}
|
||||
}
|
||||
@@ -40,6 +40,7 @@
|
||||
#include "nuttx/arch.h"
|
||||
#include "arm_internal.h"
|
||||
|
||||
#include "boot_config.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
@@ -81,6 +82,12 @@ int main(int argc, char **argv);
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
int __wrap_nxsem_wait_uninterruptible(void *sem)
|
||||
{
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
int __wrap_nxsem_wait(void *sem)
|
||||
{
|
||||
return 0;
|
||||
@@ -174,13 +181,27 @@ void __wrap_nx_start(void)
|
||||
* Name: malloc
|
||||
*
|
||||
* Description:
|
||||
* This function hijacks the OS's malloc and provides no allocation
|
||||
* This function hijacks the OS's malloc and provides no or small
|
||||
* allocation
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
FAR void *malloc(size_t size)
|
||||
{
|
||||
#if defined(OPT_SIMPLE_MALLOC_HEAP_SIZE)
|
||||
static int aloc = 0;
|
||||
static uint8_t mem[OPT_SIMPLE_MALLOC_HEAP_SIZE];
|
||||
void *rv = &mem[aloc];
|
||||
aloc += size;
|
||||
|
||||
if (aloc > OPT_SIMPLE_MALLOC_HEAP_SIZE) {
|
||||
PANIC();
|
||||
}
|
||||
|
||||
return rv;
|
||||
#else
|
||||
return NULL;
|
||||
#endif
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
|
||||
@@ -0,0 +1,51 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane <David.Sidrnae@nscdg.com>
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
/****************************************************************************
|
||||
* Name: cxx_initialize
|
||||
*
|
||||
* Description:
|
||||
* This function initialises static C++N objects
|
||||
*
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
void cxx_initialize(void);
|
||||
@@ -0,0 +1,126 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 Gregory Nutt. All rights reserved.
|
||||
* Author: Gregory Nutt <gnutt@nuttx.org>
|
||||
*
|
||||
* 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 NuttX 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <sched.h>
|
||||
#include <stdlib.h>
|
||||
#include <assert.h>
|
||||
|
||||
#ifdef CONFIG_HAVE_CXXINITIALIZE
|
||||
#include "cxx_init.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Private Types
|
||||
****************************************************************************/
|
||||
|
||||
/* This type defines one entry in initialization array */
|
||||
|
||||
typedef CODE void (*initializer_t)(void);
|
||||
|
||||
/****************************************************************************
|
||||
* External References
|
||||
****************************************************************************/
|
||||
|
||||
/* _sinit and _einit are symbols exported by the linker script that mark the
|
||||
* beginning and the end of the C++ initialization section.
|
||||
*/
|
||||
|
||||
extern initializer_t _sinit;
|
||||
extern initializer_t _einit;
|
||||
|
||||
/* _stext and _etext are symbols exported by the linker script that mark the
|
||||
* beginning and the end of text.
|
||||
*/
|
||||
|
||||
extern uintptr_t _stext;
|
||||
extern uintptr_t _etext;
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: cxx_initialize
|
||||
*
|
||||
* Description:
|
||||
* If C++ and C++ static constructors are supported, then this function
|
||||
* must be provided by board-specific logic in order to perform
|
||||
* initialization of the static C++ class instances.
|
||||
*
|
||||
* This function should then be called in the application-specific
|
||||
* user_start logic in order to perform the C++ initialization. NOTE
|
||||
* that no component of the core NuttX RTOS logic is involved; this
|
||||
* function definition only provides the 'contract' between application
|
||||
* specific C++ code and platform-specific toolchain support.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void cxx_initialize(void)
|
||||
{
|
||||
static int inited = 0;
|
||||
|
||||
if (inited == 0) {
|
||||
initializer_t *initp;
|
||||
|
||||
sinfo("_sinit: %p _einit: %p _stext: %p _etext: %p\n",
|
||||
&_sinit, &_einit, &_stext, &_etext);
|
||||
|
||||
/* Visit each entry in the initialization table */
|
||||
|
||||
for (initp = &_sinit; initp != &_einit; initp++) {
|
||||
initializer_t initializer = *initp;
|
||||
sinfo("initp: %p initializer: %p\n", initp, initializer);
|
||||
|
||||
/* Make sure that the address is non-NULL and lies in the text
|
||||
* region defined by the linker script. Some toolchains may put
|
||||
* NULL values or counts in the initialization table.
|
||||
*/
|
||||
|
||||
if ((FAR void *)initializer >= (FAR void *)&_stext &&
|
||||
(FAR void *)initializer < (FAR void *)&_etext) {
|
||||
sinfo("Calling %p\n", initializer);
|
||||
initializer();
|
||||
}
|
||||
}
|
||||
|
||||
inited = 1;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user