forked from Archive/PX4-Autopilot
Adding USVCAN bootloader support
This commit is contained in:
parent
ced8c6a2ef
commit
c417a1be7b
|
@ -16,6 +16,9 @@ exec find src \
|
|||
-path src/lib/external_lgpl -prune -o \
|
||||
-path src/lib/mathlib -prune -o \
|
||||
-path src/lib/matrix -prune -o \
|
||||
-path src/drivers/bootloaders -o \
|
||||
-path src/modules/uavcanesc -o \
|
||||
-path src/modules/uavcannode -o \
|
||||
-path src/modules/attitude_estimator_ekf -prune -o \
|
||||
-path src/modules/commander -prune -o \
|
||||
-path src/modules/mavlink -prune -o \
|
||||
|
@ -25,4 +28,3 @@ exec find src \
|
|||
-path src/modules/uavcan -prune -o \
|
||||
-path src/modules/uavcan/libuavcan -prune -o \
|
||||
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN
|
||||
|
||||
|
|
|
@ -0,0 +1,335 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
import os
|
||||
import sys
|
||||
import subprocess
|
||||
import struct
|
||||
import optparse
|
||||
import binascii
|
||||
import cStringIO
|
||||
|
||||
class GitWrapper:
|
||||
@classmethod
|
||||
def command(cls, txt):
|
||||
cmd = "git " + txt
|
||||
pr = subprocess.Popen( cmd , shell = True, stdout = subprocess.PIPE, stderr = subprocess.PIPE )
|
||||
(out, error) = pr.communicate()
|
||||
if len(error):
|
||||
raise Exception(cmd +" failed with [" + error.strip() + "]")
|
||||
return out
|
||||
|
||||
class AppDescriptor(object):
|
||||
"""
|
||||
UAVCAN firmware image descriptor format:
|
||||
uint64_t signature (bytes [7:0] set to 'APDesc00' by linker script)
|
||||
uint64_t image_crc (set to 0 by linker script)
|
||||
uint32_t image_size (set to 0 by linker script)
|
||||
uint32_t vcs_commit (set in source or by this tool)
|
||||
uint8_t version_major (set in source)
|
||||
uint8_t version_minor (set in source)
|
||||
uint8_t reserved[6] (set to 0xFF by linker script)
|
||||
"""
|
||||
|
||||
LENGTH = 8 + 8 + 4 + 4 + 1 + 1 + 6
|
||||
SIGNATURE = b"APDesc00"
|
||||
RESERVED = b"\xFF" * 6
|
||||
|
||||
def __init__(self, bytes=None):
|
||||
self.signature = AppDescriptor.SIGNATURE
|
||||
self.image_crc = 0
|
||||
self.image_size = 0
|
||||
self.vcs_commit = 0
|
||||
self.version_major = 0
|
||||
self.version_minor = 0
|
||||
self.reserved = AppDescriptor.RESERVED
|
||||
|
||||
if bytes:
|
||||
try:
|
||||
self.unpack(bytes)
|
||||
except Exception:
|
||||
raise ValueError("Invalid AppDescriptor: {0}".format(
|
||||
binascii.b2a_hex(bytes)))
|
||||
|
||||
def pack(self):
|
||||
return struct.pack("<8sQLLBB6s", self.signature, self.image_crc,
|
||||
self.image_size, self.vcs_commit,
|
||||
self.version_major, self.version_minor,
|
||||
self.reserved)
|
||||
|
||||
def unpack(self, bytes):
|
||||
(self.signature, self.image_crc, self.image_size, self.vcs_commit,
|
||||
self.version_major, self.version_minor, self.reserved) = \
|
||||
struct.unpack("<8sQLLBB6s", bytes)
|
||||
|
||||
if not self.empty and not self.valid:
|
||||
raise ValueError()
|
||||
|
||||
@property
|
||||
def empty(self):
|
||||
return (self.signature == AppDescriptor.SIGNATURE and
|
||||
self.image_crc == 0 and self.image_size == 0 and
|
||||
self.reserved == AppDescriptor.RESERVED)
|
||||
|
||||
@property
|
||||
def valid(self):
|
||||
return (self.signature == AppDescriptor.SIGNATURE and
|
||||
self.image_crc != 0 and self.image_size > 0 and
|
||||
self.reserved == AppDescriptor.RESERVED)
|
||||
|
||||
|
||||
class FirmwareImage(object):
|
||||
def __init__(self, path_or_file, mode="r"):
|
||||
if getattr(path_or_file, "read", None):
|
||||
self._file = path_or_file
|
||||
self._do_close = False
|
||||
self._padding = 0
|
||||
else:
|
||||
self._file = open(path_or_file, mode + "b")
|
||||
self._do_close = True
|
||||
self._padding = 4
|
||||
|
||||
if "r" in mode:
|
||||
self._contents = cStringIO.StringIO(self._file.read())
|
||||
else:
|
||||
self._contents = cStringIO.StringIO()
|
||||
self._do_write = False
|
||||
|
||||
self._length = None
|
||||
self._descriptor_offset = None
|
||||
self._descriptor_bytes = None
|
||||
self._descriptor = None
|
||||
|
||||
def __enter__(self):
|
||||
return self
|
||||
|
||||
def __getattr__(self, attr):
|
||||
if attr == "write":
|
||||
self._do_write = True
|
||||
return getattr(self._contents, attr)
|
||||
|
||||
def __iter__(self):
|
||||
return iter(self._contents)
|
||||
|
||||
def __exit__(self, *args):
|
||||
if self._do_write:
|
||||
if getattr(self._file, "seek", None):
|
||||
self._file.seek(0)
|
||||
self._file.write(self._contents.getvalue())
|
||||
if self._padding:
|
||||
self._file.write(b'\xff' * self._padding)
|
||||
|
||||
if self._do_close:
|
||||
self._file.close()
|
||||
|
||||
def _write_descriptor_raw(self):
|
||||
# Seek to the appropriate location, write the serialized
|
||||
# descriptor, and seek back.
|
||||
prev_offset = self._contents.tell()
|
||||
self._contents.seek(self._descriptor_offset)
|
||||
self._contents.write(self._descriptor.pack())
|
||||
self._contents.seek(prev_offset)
|
||||
|
||||
def write_descriptor(self):
|
||||
# Set the descriptor's length and CRC to the values required for
|
||||
# CRC computation
|
||||
self.app_descriptor.image_size = self.length
|
||||
self.app_descriptor.image_crc = 0
|
||||
|
||||
self._write_descriptor_raw()
|
||||
|
||||
# Update the descriptor's CRC based on the computed value and write
|
||||
# it out again
|
||||
self.app_descriptor.image_crc = self.crc
|
||||
|
||||
self._write_descriptor_raw()
|
||||
|
||||
@property
|
||||
def crc(self):
|
||||
MASK = 0xFFFFFFFFFFFFFFFF
|
||||
POLY = 0x42F0E1EBA9EA3693
|
||||
|
||||
# Calculate the image CRC with the image_crc field in the app
|
||||
# descriptor zeroed out.
|
||||
crc_offset = self.app_descriptor_offset + len(AppDescriptor.SIGNATURE)
|
||||
content = bytearray(self._contents.getvalue())
|
||||
content[crc_offset:crc_offset + 8] = bytearray("\x00" * 8)
|
||||
if self._padding:
|
||||
content += bytearray("\xff" * self._padding)
|
||||
val = MASK
|
||||
for byte in content:
|
||||
val ^= (byte << 56) & MASK
|
||||
for bit in range(8):
|
||||
if val & (1 << 63):
|
||||
val = ((val << 1) & MASK) ^ POLY
|
||||
else:
|
||||
val <<= 1
|
||||
|
||||
return (val & MASK) ^ MASK
|
||||
|
||||
@property
|
||||
def padding(self):
|
||||
return self._padding
|
||||
|
||||
@property
|
||||
def length(self):
|
||||
if not self._length:
|
||||
# Find the length of the file by seeking to the end and getting
|
||||
# the offset
|
||||
prev_offset = self._contents.tell()
|
||||
self._contents.seek(0, os.SEEK_END)
|
||||
self._length = self._contents.tell()
|
||||
if self._padding:
|
||||
fill = self._length % self._padding
|
||||
if fill:
|
||||
self._length += fill
|
||||
self._padding = fill
|
||||
self._contents.seek(prev_offset)
|
||||
|
||||
return self._length
|
||||
|
||||
@property
|
||||
def app_descriptor_offset(self):
|
||||
if not self._descriptor_offset:
|
||||
# Save the current position
|
||||
prev_offset = self._contents.tell()
|
||||
# Check each byte in the file to see if a valid descriptor starts
|
||||
# at that location. Slow, but not slow enough to matter.
|
||||
offset = 0
|
||||
while offset < self.length - AppDescriptor.LENGTH:
|
||||
self._contents.seek(offset)
|
||||
try:
|
||||
# If this throws an exception, there isn't a valid
|
||||
# descriptor at this offset
|
||||
AppDescriptor(self._contents.read(AppDescriptor.LENGTH))
|
||||
except Exception:
|
||||
offset += 1
|
||||
else:
|
||||
self._descriptor_offset = offset
|
||||
break
|
||||
# Go back to the previous position
|
||||
self._contents.seek(prev_offset)
|
||||
if not self._descriptor_offset:
|
||||
raise Exception('AppDescriptor not found')
|
||||
|
||||
return self._descriptor_offset
|
||||
|
||||
@property
|
||||
def app_descriptor(self):
|
||||
if not self._descriptor:
|
||||
# Save the current position
|
||||
prev_offset = self._contents.tell()
|
||||
# Jump to the descriptor adn parse it
|
||||
self._contents.seek(self.app_descriptor_offset)
|
||||
self._descriptor_bytes = self._contents.read(AppDescriptor.LENGTH)
|
||||
self._descriptor = AppDescriptor(self._descriptor_bytes)
|
||||
# Go back to the previous offset
|
||||
self._contents.seek(prev_offset)
|
||||
|
||||
return self._descriptor
|
||||
|
||||
@app_descriptor.setter
|
||||
def app_descriptor(self, value):
|
||||
self._descriptor = value
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = optparse.OptionParser(usage="usage: %prog [options] [IN OUT]")
|
||||
parser.add_option("--vcs-commit", dest="vcs_commit", default=None,
|
||||
help="set the descriptor's VCS commit value to COMMIT",
|
||||
metavar="COMMIT")
|
||||
parser.add_option("-g", "--use-git-hash", dest="use_git_hash", action="store_true",
|
||||
help="set the descriptor's VCS commit value to the current git hash",
|
||||
metavar="GIT")
|
||||
parser.add_option("--bootloader-size", dest="bootloader_size", default=0,
|
||||
help="don't write the first SIZE bytes of the image",
|
||||
metavar="SIZE")
|
||||
parser.add_option("--bootloader-image", dest="bootloader_image", default=0,
|
||||
help="prepend a bootloader image to the output file",
|
||||
metavar="IMAGE")
|
||||
parser.add_option("-v", "--verbose", dest="verbose", action="store_true",
|
||||
help="show additional firmware information on stdout")
|
||||
|
||||
options, args = parser.parse_args()
|
||||
if len(args) not in (0, 2):
|
||||
parser.error("specify both IN or OUT for file operation, or " +
|
||||
"neither for stdin/stdout operation")
|
||||
|
||||
if options.vcs_commit and options.use_git_hash:
|
||||
parser.error("options --vcs-commit and --use-git-commit are mutually exclusive")
|
||||
|
||||
if options.use_git_hash:
|
||||
try:
|
||||
options.vcs_commit = int(GitWrapper.command("rev-list HEAD --max-count=1 --abbrev=8 --abbrev-commit"),16)
|
||||
except Exception as e:
|
||||
print "Git Command failed "+ str(e) +"- Exiting!"
|
||||
quit()
|
||||
|
||||
if args:
|
||||
in_file = args[0]
|
||||
out_file = args[1]
|
||||
else:
|
||||
in_file = sys.stdin
|
||||
out_file = sys.stdout
|
||||
|
||||
bootloader_image = ""
|
||||
if options.bootloader_image:
|
||||
with open(options.bootloader_image, "rb") as bootloader:
|
||||
bootloader_image = bootloader.read()
|
||||
|
||||
bootloader_size = int(options.bootloader_size)
|
||||
|
||||
with FirmwareImage(in_file, "rb") as in_image:
|
||||
with FirmwareImage(out_file, "wb") as out_image:
|
||||
image = in_image.read()
|
||||
out_image.write(bootloader_image)
|
||||
out_image.write(image[bootloader_size:])
|
||||
if options.vcs_commit:
|
||||
out_image.app_descriptor.vcs_commit = options.vcs_commit
|
||||
out_image.write_descriptor()
|
||||
|
||||
if options.verbose:
|
||||
sys.stderr.write(
|
||||
"""
|
||||
Application descriptor located at offset 0x{0.app_descriptor_offset:08X}
|
||||
|
||||
""".format(in_image, in_image.app_descriptor, out_image.app_descriptor,
|
||||
bootloader_size, len(bootloader_image)))
|
||||
if bootloader_size:
|
||||
sys.stderr.write(
|
||||
"""Ignored the first {3:d} bytes of the input image. Prepended {4:d} bytes of
|
||||
bootloader image to the output image.
|
||||
|
||||
""".format(in_image, in_image.app_descriptor, out_image.app_descriptor,
|
||||
bootloader_size, len(bootloader_image)))
|
||||
sys.stderr.write(
|
||||
"""READ VALUES
|
||||
------------------------------------------------------------------------------
|
||||
|
||||
Field Type Value
|
||||
signature uint64 {1.signature!r}
|
||||
image_crc uint64 0x{1.image_crc:016X}
|
||||
image_size uint32 0x{1.image_size:X} ({1.image_size:d} B)
|
||||
vcs_commit uint32 {1.vcs_commit:08X}
|
||||
version_major uint8 {1.version_major:d}
|
||||
version_minor uint8 {1.version_minor:d}
|
||||
reserved uint8[6] {1.reserved!r}
|
||||
|
||||
WRITTEN VALUES
|
||||
------------------------------------------------------------------------------
|
||||
|
||||
Field Type Value
|
||||
signature uint64 {2.signature!r}
|
||||
image_crc uint64 0x{2.image_crc:016X}
|
||||
image_size uint32 0x{2.image_size:X} ({2.image_size:d} B)
|
||||
vcs_commit uint32 {2.vcs_commit:08X}
|
||||
version_major uint8 {2.version_major:d}
|
||||
version_minor uint8 {2.version_minor:d}
|
||||
reserved uint8[6] {2.reserved!r}
|
||||
|
||||
""".format(in_image, in_image.app_descriptor, out_image.app_descriptor,
|
||||
bootloader_size, len(bootloader_image)))
|
||||
if out_image.padding:
|
||||
sys.stderr.write(
|
||||
"""
|
||||
padding added {}
|
||||
""".format(out_image.padding))
|
|
@ -0,0 +1,3 @@
|
|||
set(uavcanblid_hw_version_major 1)
|
||||
set(uavcanblid_hw_version_minor 0)
|
||||
set(uavcanblid_name "\"com.auav.esc35-v1\"")
|
|
@ -0,0 +1,3 @@
|
|||
set(uavcanblid_hw_version_major 1)
|
||||
set(uavcanblid_hw_version_minor 0)
|
||||
set(uavcanblid_name "\"org.pixhawk.px4cannode-v1\"")
|
|
@ -0,0 +1,3 @@
|
|||
set(uavcanblid_hw_version_major 1)
|
||||
set(uavcanblid_hw_version_minor 6)
|
||||
set(uavcanblid_name "\"org.pixhawk.px4esc-v1\"")
|
|
@ -0,0 +1,3 @@
|
|||
set(uavcanblid_hw_version_major 2)
|
||||
set(uavcanblid_hw_version_minor 0)
|
||||
set(uavcanblid_name "\"org.pixhawk.px4flow-v2\"")
|
|
@ -0,0 +1,3 @@
|
|||
set(uavcanblid_hw_version_major 1)
|
||||
set(uavcanblid_hw_version_minor 0)
|
||||
set(uavcanblid_name "\"com.thiemar.s2740vc-v1\"")
|
|
@ -0,0 +1,3 @@
|
|||
set(uavcanblid_hw_version_major 1)
|
||||
set(uavcanblid_hw_version_minor 0)
|
||||
set(uavcanblid_name "\"com.zubax.gnss\"")
|
|
@ -2,9 +2,11 @@
|
|||
#include <nuttx/config.h>
|
||||
#include <nuttx/binfmt/builtin.h>
|
||||
#include <nuttx/config.h>
|
||||
#if ${command_count}
|
||||
${builtin_apps_decl_string}
|
||||
const struct builtin_s g_builtins[] = {
|
||||
${builtin_apps_string}
|
||||
{NULL, 0, 0, NULL}
|
||||
};
|
||||
const int g_builtin_count = ${command_count};
|
||||
#endif
|
|
@ -0,0 +1,58 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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 board.h
|
||||
*
|
||||
* bootloader board interface
|
||||
* This file contains the common interfaces that all boards
|
||||
* have to supply
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
#define BIT(pos) (1ull<<(pos))
|
||||
#define BIT_MASK(length) (BIT(length)-1)
|
||||
|
||||
#define BITFEILD_MASK(lsb_pos, length) ( BIT_MASK(length) << (lsb_pos))
|
||||
#define BITFEILD_ISOLATE(x, lsb_pos, length) ((x) & (BITFEILD_MASK((lsb_pos), (length))))
|
||||
#define BITFEILD_EXCLUDE(x, lsb_pos, length) ((x) & ~(BITFEILD_MASK((lsb_pos), (length))))
|
||||
#define BITFEILD_GET(y, lsb_pos, length) (((y)>>(lsb_pos)) & BIT_MASK(length))
|
||||
#define BITFEILD_SET(y, x, lsb_pos, length) ( y= ((y) & ~BF_MASK(lsb_pos, length)) | BITFEILD_ISOLATE(x, lsb_pos, length))
|
|
@ -0,0 +1,77 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Type Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Global Variables
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Name: sched_yield()
|
||||
*
|
||||
* Description:
|
||||
* This function should be called in situation were the cpu may be
|
||||
* busy for a long time without interrupts or the ability to run code
|
||||
* to insure that the timer based process will be run.
|
||||
*
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
#if defined(OPT_USE_YIELD)
|
||||
void bl_sched_yield(void);
|
||||
#else
|
||||
# define bl_sched_yield()
|
||||
#endif
|
|
@ -0,0 +1,151 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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 board.h
|
||||
*
|
||||
* bootloader board interface
|
||||
* This file contains the common interfaces that all boards
|
||||
* have to supply
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
#include "uavcan.h"
|
||||
#include <nuttx/compiler.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Type Definitions
|
||||
****************************************************************************/
|
||||
|
||||
typedef enum {
|
||||
|
||||
off,
|
||||
reset,
|
||||
autobaud_start,
|
||||
autobaud_end,
|
||||
allocation_start,
|
||||
allocation_end,
|
||||
fw_update_start,
|
||||
fw_update_erase_fail,
|
||||
fw_update_invalid_response,
|
||||
fw_update_timeout,
|
||||
fw_update_invalid_crc,
|
||||
jump_to_app,
|
||||
} uiindication_t;
|
||||
/************************************************************************************
|
||||
* Public data
|
||||
************************************************************************************/
|
||||
|
||||
#ifndef __ASSEMBLY__
|
||||
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_boarddeinitialize
|
||||
*
|
||||
* Description:
|
||||
* This function is called by the bootloader code priore to booting
|
||||
* the application. Is should place the HW into an benign initialized state.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
void stm32_boarddeinitialize(void);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: board_get_product_name
|
||||
*
|
||||
* Description:
|
||||
* Called to retrive the product name. The retuned alue is a assumed
|
||||
* to be written to a pascal style string that will be length prefixed
|
||||
* and not null terminated
|
||||
*
|
||||
* Input Parameters:
|
||||
* product_name - A pointer to a buffer to write the name.
|
||||
* maxlen - The imum number of chatater that can be written
|
||||
*
|
||||
* Returned Value:
|
||||
* The length of charaacters written to the buffer.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
uint8_t board_get_product_name(uint8_t *product_name, size_t maxlen);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: board_get_hardware_version
|
||||
*
|
||||
* Description:
|
||||
* Called to retrieve the hardware version information. The function
|
||||
* will first initialize the the callers struct to all zeros.
|
||||
*
|
||||
* Input Parameters:
|
||||
* hw_version - A pointer to a uavcan_hardwareversion_t.
|
||||
*
|
||||
* Returned Value:
|
||||
* Length of the unique_id
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
size_t board_get_hardware_version(uavcan_HardwareVersion_t *hw_version);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: board_indicate
|
||||
*
|
||||
* Description:
|
||||
* Provides User feedback to indicate the state of the bootloader
|
||||
* on board specific hardware.
|
||||
*
|
||||
* Input Parameters:
|
||||
* indication - A member of the uiindication_t
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void board_indicate(uiindication_t indication);
|
||||
|
||||
|
||||
#endif /* __ASSEMBLY__ */
|
|
@ -0,0 +1,237 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/compiler.h>
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/* Define the signature for the Application descriptor as 'APDesc' and a
|
||||
* revision number of 00 used in app_descriptor_t
|
||||
*/
|
||||
|
||||
#define APP_DESCRIPTOR_SIGNATURE_ID 'A','P','D','e','s','c'
|
||||
#define APP_DESCRIPTOR_SIGNATURE_REV '0','0'
|
||||
#define APP_DESCRIPTOR_SIGNATURE APP_DESCRIPTOR_SIGNATURE_ID, APP_DESCRIPTOR_SIGNATURE_REV
|
||||
|
||||
/* N.B. the .ld file must emit this sections */
|
||||
# define boot_app_shared_section __attribute__((section(".app_descriptor")))
|
||||
|
||||
/****************************************************************************
|
||||
* Public Type Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/* eRole defines the role of the bootloader_app_shared_t structure */
|
||||
|
||||
typedef enum eRole {
|
||||
Invalid,
|
||||
App,
|
||||
BootLoader
|
||||
} eRole_t;
|
||||
|
||||
/****************************************************************************
|
||||
*
|
||||
* Bootloader and Application shared structure.
|
||||
*
|
||||
* The data in this structure is passed in SRAM or the the CAN filter
|
||||
* registers from bootloader to application and application to bootloader.
|
||||
*
|
||||
* Do not assume any mapping or location for the passing of this data
|
||||
* that is done in the read and write routines and is abstracted by design.
|
||||
*
|
||||
* For reference, the following is performed based on eRole in API calls
|
||||
* defined below:
|
||||
*
|
||||
* The application must write BOOTLOADER_COMMON_APP_SIGNATURE to the
|
||||
* signature field when passing data to the bootloader; when the
|
||||
* bootloader passes data to the application, it must write
|
||||
* BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE to the signature field.
|
||||
*
|
||||
* The CRC is calculated over the structure from signature to the
|
||||
* last byte. The resulting values are then copied to the CAN filter
|
||||
* registers by bootloader_app_shared_read and
|
||||
* bootloader_app_shared_write.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wattributes"
|
||||
typedef struct packed_struct bootloader_app_shared_t {
|
||||
union {
|
||||
uint64_t ull;
|
||||
uint32_t ul[2];
|
||||
uint8_t valid;
|
||||
} crc;
|
||||
uint32_t signature;
|
||||
uint32_t bus_speed;
|
||||
uint32_t node_id;
|
||||
} bootloader_app_shared_t ;
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
/****************************************************************************
|
||||
*
|
||||
* Application firmware descriptor.
|
||||
*
|
||||
* This structure located by the linker script somewhere after the vector table.
|
||||
* (within the first several kilobytes of the beginning address of the
|
||||
* application);
|
||||
*
|
||||
* This structure must be aligned on an 8-byte boundary.
|
||||
*
|
||||
* The bootloader will scan through the application FLASH image until it
|
||||
* finds the signature.
|
||||
*
|
||||
* The image_crc is calculated as follows:
|
||||
* 1) All fields of this structure must be initialized with the correct
|
||||
* information about the firmware image bin file
|
||||
* (Here after refereed to as image)
|
||||
* 2) image_crc set to 0;
|
||||
* 3) The CRC 64 is calculated over the image from offset 0 up to and including the
|
||||
* last byte of the image file.
|
||||
* 4) The calculated CRC 64 is stored in image_crc
|
||||
* 5) The new image file is then written to a file a ".img" extension.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wattributes"
|
||||
#pragma GCC diagnostic ignored "-Wpacked"
|
||||
typedef struct packed_struct app_descriptor_t {
|
||||
uint8_t signature[sizeof(uint64_t)];
|
||||
uint64_t image_crc;
|
||||
uint32_t image_size;
|
||||
uint32_t vcs_commit;
|
||||
uint8_t major_version;
|
||||
uint8_t minor_version;
|
||||
uint8_t reserved[6];
|
||||
} app_descriptor_t;
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
/****************************************************************************
|
||||
* Global Variables
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Name: bootloader_app_shared_read
|
||||
*
|
||||
* Description:
|
||||
* Based on the role requested, this function will conditionally populate
|
||||
* a bootloader_app_shared_t structure from the physical locations used
|
||||
* to transfer the shared data to/from an application (internal data) .
|
||||
*
|
||||
* The functions will only populate the structure and return a status
|
||||
* indicating success, if the internal data has the correct signature as
|
||||
* requested by the Role AND has a valid crc.
|
||||
*
|
||||
* Input Parameters:
|
||||
* shared - A pointer to a bootloader_app_shared_t return the data in if
|
||||
* the internal data is valid for the requested Role
|
||||
* role - An eRole_t of App or BootLoader to validate the internal data
|
||||
* against. For a Bootloader this would be the value of App to
|
||||
* read the application passed data.
|
||||
*
|
||||
* Returned value:
|
||||
* OK - Indicates that the internal data has been copied to callers
|
||||
* bootloader_app_shared_t structure.
|
||||
*
|
||||
* -EBADR - The Role or crc of the internal data was not valid. The copy
|
||||
* did not occur.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
int bootloader_app_shared_read(bootloader_app_shared_t *shared,
|
||||
eRole_t role);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: bootloader_app_shared_write
|
||||
*
|
||||
* Description:
|
||||
* Based on the role, this function will commit the data passed
|
||||
* into the physical locations used to transfer the shared data to/from
|
||||
* an application (internal data) .
|
||||
*
|
||||
* The functions will populate the signature and crc the data
|
||||
* based on the provided Role.
|
||||
*
|
||||
* Input Parameters:
|
||||
* shared - A pointer to a bootloader_app_shared_t data to commit to
|
||||
* the internal data for passing to/from an application.
|
||||
* role - An eRole_t of App or BootLoader to use in the internal data
|
||||
* to be passed to/from an application. For a Bootloader this
|
||||
* would be the value of Bootloader to write to the passed data.
|
||||
* to the application via the internal data.
|
||||
*
|
||||
* Returned value:
|
||||
* None.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void bootloader_app_shared_write(bootloader_app_shared_t *shared,
|
||||
eRole_t role);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: bootloader_app_shared_invalidate
|
||||
*
|
||||
* Description:
|
||||
* Invalidates the data passed the physical locations used to transfer
|
||||
* the shared data to/from an application (internal data) .
|
||||
*
|
||||
* The functions will invalidate the signature and crc and should be used
|
||||
* to prevent deja vu.
|
||||
*
|
||||
* Input Parameters:
|
||||
* None.
|
||||
*
|
||||
* Returned value:
|
||||
* None.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void bootloader_app_shared_invalidate(void);
|
||||
|
||||
__END_DECLS
|
|
@ -0,0 +1,246 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include "timer.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Type Definitions
|
||||
****************************************************************************/
|
||||
|
||||
typedef enum {
|
||||
CanPayloadLength = 8,
|
||||
} can_const_t;
|
||||
|
||||
typedef enum {
|
||||
CAN_OK = 0,
|
||||
CAN_BOOT_TIMEOUT,
|
||||
CAN_ERROR
|
||||
} can_error_t;
|
||||
|
||||
typedef enum {
|
||||
CAN_UNKNOWN = 0,
|
||||
CAN_125KBAUD = 1,
|
||||
CAN_250KBAUD = 2,
|
||||
CAN_500KBAUD = 3,
|
||||
CAN_1MBAUD = 4
|
||||
} can_speed_t;
|
||||
|
||||
typedef enum {
|
||||
CAN_Mode_Normal = 0, // Bits 30 and 31 00
|
||||
CAN_Mode_LoopBack = 1, // Bit 30: Loop Back Mode (Debug)
|
||||
CAN_Mode_Silent = 2, // Bit 31: Silent Mode (Debug)
|
||||
CAN_Mode_Silent_LoopBack = 3 // Bits 30 and 31 11
|
||||
} can_mode_t;
|
||||
|
||||
|
||||
/*
|
||||
* Receive from FIFO 1 -- filters are configured to push the messages there,
|
||||
* and there are send/receive functions called off the SysTick ISR so
|
||||
* we partition the usage of the CAN hardware to avoid the same FIFOs/mailboxes
|
||||
* as the rest of the application uses.
|
||||
*/
|
||||
|
||||
typedef enum {
|
||||
|
||||
Fifo0 = 0,
|
||||
MailBox0 = 0,
|
||||
Fifo1 = 1,
|
||||
MailBox1 = 1,
|
||||
Fifo2 = 2,
|
||||
MailBox2 = 2,
|
||||
FifoNone = 3,
|
||||
MailBoxNone = 3,
|
||||
|
||||
} can_fifo_mailbox_t;
|
||||
|
||||
/****************************************************************************
|
||||
* Global Variables
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* 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);
|
||||
|
||||
/****************************************************************************
|
||||
* 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);
|
||||
|
||||
/****************************************************************************
|
||||
* 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);
|
||||
|
||||
/****************************************************************************
|
||||
* 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);
|
||||
|
||||
/****************************************************************************
|
||||
* 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);
|
||||
|
||||
/****************************************************************************
|
||||
* 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);
|
||||
|
||||
/****************************************************************************
|
||||
* 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:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void can_cancel_on_error(uint8_t mailbox);
|
|
@ -0,0 +1,116 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
#define CRC16_INITIAL 0xFFFFu
|
||||
#define CRC16_OUTPUT_XOR 0x0000u
|
||||
#define CRC64_INITIAL 0xFFFFFFFFFFFFFFFFull
|
||||
#define CRC64_OUTPUT_XOR 0xFFFFFFFFFFFFFFFFull
|
||||
|
||||
/****************************************************************************
|
||||
* Public Type Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Global Variables
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Name: crc16_add
|
||||
*
|
||||
* Description:
|
||||
* Use to calculates a CRC-16-CCITT using the polynomial of
|
||||
* 0x1021 by adding a value successive values.
|
||||
*
|
||||
* Input Parameters:
|
||||
* crc - The running total of the crc 16
|
||||
* value - The value to add
|
||||
*
|
||||
* Returned Value:
|
||||
* The current crc16 with the value processed.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
uint16_t crc16_add(uint16_t crc, uint8_t value);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: crc16_signature
|
||||
*
|
||||
* Description:
|
||||
* Calculates a CRC-16-CCITT using the crc16_add
|
||||
* function
|
||||
*
|
||||
* Input Parameters:
|
||||
* initial - The Initial value to uses as the crc's starting point
|
||||
* length - The number of bytes to add to the crc
|
||||
* bytes - A pointer to any array of length bytes
|
||||
*
|
||||
* Returned Value:
|
||||
* The crc16 of the array of bytes
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
uint16_t crc16_signature(uint16_t initial, size_t length,
|
||||
const uint8_t *bytes);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: crc64_add_word
|
||||
*
|
||||
* Description:
|
||||
* Calculates a CRC-64-WE using the polynomial of 0x42F0E1EBA9EA3693
|
||||
* See http://reveng.sourceforge.net/crc-catalogue/17plus.htm#crc.cat-bits.64
|
||||
* Check: 0x62EC59E3F1A4F00A
|
||||
*
|
||||
* Input Parameters:
|
||||
* crc - The running total of the crc 64
|
||||
* value - The value to add
|
||||
*
|
||||
* Returned Value:
|
||||
* The current crc64 with the value processed.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
uint64_t crc64_add_word(uint64_t crc, uint32_t value);
|
|
@ -0,0 +1,106 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Type Definitions
|
||||
****************************************************************************/
|
||||
|
||||
typedef enum {
|
||||
FLASH_OK = 0,
|
||||
FLASH_ERROR,
|
||||
FLASH_ERASE_ERROR,
|
||||
FLASH_ERASE_VERIFY_ERROR,
|
||||
FLASH_ERROR_SUICIDE,
|
||||
FLASH_ERROR_AFU,
|
||||
|
||||
} flash_error_t;
|
||||
|
||||
/****************************************************************************
|
||||
* Global Variables
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Name: bl_flash_erase
|
||||
*
|
||||
* Description:
|
||||
* This function erases the flash starting at address and ending at
|
||||
* address + nbytes.
|
||||
*
|
||||
* Input Parameters:
|
||||
* address - A word-aligned address within the first page of flash to erase
|
||||
* nbytes - The number of bytes to erase, rounding up to the next page.
|
||||
*
|
||||
*
|
||||
*
|
||||
* Returned value:
|
||||
* On success FLASH_OK On Error one of the flash_error_t
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
flash_error_t bl_flash_erase(size_t address, size_t nbytes);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: bl_flash_write
|
||||
*
|
||||
* Description:
|
||||
* This function writes the flash starting at the given address
|
||||
*
|
||||
* Input Parameters:
|
||||
* flash_address - The address of the flash to write
|
||||
* must be word aligned
|
||||
* data - A pointer to a buffer count bytes to be written
|
||||
* to the flash.
|
||||
* count - Number of bytes to write
|
||||
*
|
||||
* Returned value:
|
||||
* On success FLASH_OK On Error one of the flash_error_t
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
flash_error_t bl_flash_write(uint32_t flash_address, uint8_t *data, ssize_t count);
|
|
@ -0,0 +1,92 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
#define CRC16_INITIAL 0xFFFFu
|
||||
#define CRC16_OUTPUT_XOR 0x0000u
|
||||
#define CRC64_INITIAL 0xFFFFFFFFFFFFFFFFull
|
||||
#define CRC64_OUTPUT_XOR 0xFFFFFFFFFFFFFFFFull
|
||||
|
||||
/****************************************************************************
|
||||
* Public Type Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Global Variables
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Name: util_srand
|
||||
*
|
||||
* Description:
|
||||
* This function seeds the random number generator
|
||||
*
|
||||
*
|
||||
* Input Parameters:
|
||||
* seed - The seed
|
||||
*
|
||||
* Returned value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
void util_srand(uint16_t seed);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: util_random
|
||||
*
|
||||
* Description:
|
||||
* This function returns a random number between min and max
|
||||
*
|
||||
*
|
||||
* Input Parameters:
|
||||
* min - The minimum value the return value can be.
|
||||
* max - The maximum value the return value can be.
|
||||
*
|
||||
* Returned value:
|
||||
* A random number
|
||||
*
|
||||
****************************************************************************/
|
||||
uint16_t util_random(uint16_t min, uint16_t max);
|
|
@ -0,0 +1,451 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
/*
|
||||
* We support two classes of timer interfaces. The first one is for structured
|
||||
* timers that have an API for life cycle management and use. (timer_xx)
|
||||
* The Second type of methods are for interfacing to a high resolution
|
||||
* counter with fast access and are provided via an in line API (timer_hrt)
|
||||
*/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include "stm32.h"
|
||||
#include "nvic.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
#define TIMER_HRT_CYCLES_PER_US (STM32_HCLK_FREQUENCY/1000000)
|
||||
#define TIMER_HRT_CYCLES_PER_MS (STM32_HCLK_FREQUENCY/1000)
|
||||
|
||||
/****************************************************************************
|
||||
* Public Type Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/* Types for timer access */
|
||||
typedef uint8_t bl_timer_id; /* A timer handle */
|
||||
typedef uint32_t time_ms_t; /* A timer value */
|
||||
typedef volatile time_ms_t *time_ref_t; /* A pointer to the internal
|
||||
counter in the structure of a timer
|
||||
used to do a time out test value */
|
||||
|
||||
typedef uint32_t time_hrt_cycles_t; /* A timer value type of the hrt */
|
||||
|
||||
|
||||
/*
|
||||
* Timers
|
||||
*
|
||||
* There are 3 modes of operation for the timers.
|
||||
* All modes support a call back on expiration.
|
||||
*
|
||||
*/
|
||||
typedef enum {
|
||||
/* Specifies a one-shot timer. After notification timer is discarded. */
|
||||
modeOneShot = 1,
|
||||
/* Specifies a repeating timer. */
|
||||
modeRepeating = 2,
|
||||
/* Specifies a persistent start / stop timer. */
|
||||
modeTimeout = 3,
|
||||
/* Or'ed in to start the timer when allocated */
|
||||
modeStarted = 0x40
|
||||
} bl_timer_modes_t;
|
||||
|
||||
|
||||
/* The call back function signature type */
|
||||
|
||||
typedef void (*bl_timer_ontimeout)(bl_timer_id id, void *context);
|
||||
|
||||
/*
|
||||
* A helper type for timer allocation to setup a callback
|
||||
* There is a null_cb object (see below) that can be used to
|
||||
* a bl_timer_cb_t.
|
||||
*
|
||||
* Typical usage is:
|
||||
*
|
||||
* void my_process(bl_timer_id id, void *context) {
|
||||
* ...
|
||||
* };
|
||||
*
|
||||
* bl_timer_cb_t mycallback = null_cb;
|
||||
* mycallback.cb = my_process;
|
||||
* bl_timer_id mytimer = timer_allocate(modeRepeating|modeStarted, 100, &mycallback);
|
||||
*/
|
||||
|
||||
typedef struct {
|
||||
void *context;
|
||||
bl_timer_ontimeout cb;
|
||||
|
||||
} bl_timer_cb_t;
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Global Variables
|
||||
****************************************************************************/
|
||||
|
||||
extern const bl_timer_cb_t null_cb;
|
||||
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Name: timer_init
|
||||
*
|
||||
* Description:
|
||||
* Called early in os_start to initialize the data associated with
|
||||
* the timers
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void timer_init(void);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_allocate
|
||||
*
|
||||
* Description:
|
||||
* Is used to allocate a timer. Allocation does not involve memory
|
||||
* allocation as the data for the timer are compile time generated.
|
||||
* See OPT_BL_NUMBER_TIMERS
|
||||
*
|
||||
* There is an inherent priority to the timers in that the first timer
|
||||
* allocated is the first timer run per tick.
|
||||
*
|
||||
* There are 3 modes of operation for the timers. All modes support an
|
||||
* optional call back on expiration.
|
||||
*
|
||||
* modeOneShot - Specifies a one-shot timer. After notification timer
|
||||
* is resource is freed.
|
||||
* modeRepeating - Specifies a repeating timer that will reload and
|
||||
* call an optional.
|
||||
* modeTimeout - Specifies a persistent start / stop timer.
|
||||
*
|
||||
* modeStarted - Or'ed in to start the timer when allocated
|
||||
*
|
||||
*
|
||||
* Input Parameters:
|
||||
* mode - One of bl_timer_modes_t with the Optional modeStarted
|
||||
* msfromnow - The reload and initial value for the timer in Ms.
|
||||
* fc - A pointer or NULL (0). If it is non null it can be any
|
||||
* of the following:
|
||||
*
|
||||
* a) A bl_timer_cb_t populated on the users stack or
|
||||
* in the data segment. The values are copied into the
|
||||
* internal data structure of the timer and therefore do
|
||||
* not have to persist after the call to timer_allocate
|
||||
*
|
||||
* b) The address of null_cb. This is identical to passing
|
||||
* null for the value of fc.
|
||||
*
|
||||
* Returned Value:
|
||||
* On success a value from 0 - OPT_BL_NUMBER_TIMERS-1 that is
|
||||
* the bl_timer_id for subsequent timer operations
|
||||
* -1 on failure. This indicates there are no free timers.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
bl_timer_id timer_allocate(bl_timer_modes_t mode, time_ms_t msfromnow,
|
||||
bl_timer_cb_t *fc);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_free
|
||||
*
|
||||
* Description:
|
||||
* Is used to free a timer. Freeing a timer does not involve memory
|
||||
* deallocation as the data for the timer are compile time generated.
|
||||
* See OPT_BL_NUMBER_TIMERS
|
||||
*
|
||||
* Input Parameters:
|
||||
* id - Returned from timer_allocate;
|
||||
*
|
||||
* Returned Value:
|
||||
* None.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void timer_free(bl_timer_id id);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_start
|
||||
*
|
||||
* Description:
|
||||
* Is used to Start a timer. The reload value is copied to the counter.
|
||||
* And the running bit it set. There is no problem in Starting a running
|
||||
* timer. But it will restart the timeout.
|
||||
*
|
||||
* Input Parameters:
|
||||
* id - Returned from timer_allocate;
|
||||
*
|
||||
* Returned Value:
|
||||
* None.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void timer_start(bl_timer_id id);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_restart
|
||||
*
|
||||
* Description:
|
||||
* Is used to re start a timer with a new reload count. The reload value
|
||||
* is copied to the counter and the running bit it set. There is no
|
||||
* problem in restarting a running timer.
|
||||
*
|
||||
* Input Parameters:
|
||||
* id - Returned from timer_allocate;
|
||||
* ms - Is a time_ms_t and the new reload value to use.
|
||||
*
|
||||
* Returned Value:
|
||||
* None.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void timer_restart(bl_timer_id id, time_ms_t ms);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_stop
|
||||
*
|
||||
* Description:
|
||||
* Is used to stop a timer. It is Ok to stop a stopped timer.
|
||||
*
|
||||
* Input Parameters:
|
||||
* id - Returned from timer_allocate;
|
||||
*
|
||||
* Returned Value:
|
||||
* None.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void timer_stop(bl_timer_id id);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_expired
|
||||
*
|
||||
* Description:
|
||||
* Test if a timer that was configured as a modeTimeout timer is expired.
|
||||
* To be expired the time has to be running and have a count of 0.
|
||||
*
|
||||
* Input Parameters:
|
||||
* id - Returned from timer_allocate;
|
||||
*
|
||||
* Returned Value:
|
||||
* No Zero if the timer is expired otherwise zero.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
int timer_expired(bl_timer_id id);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_ref
|
||||
*
|
||||
* Description:
|
||||
* Returns an time_ref_t that is a reference (pointer) to the internal counter
|
||||
* of the timer selected by id. It should only be used with calls to
|
||||
* timer_ref_expired.
|
||||
*
|
||||
* Input Parameters:
|
||||
* id - Returned from timer_allocate;
|
||||
*
|
||||
* Returned Value:
|
||||
* An internal reference that should be treated as opaque by the caller and
|
||||
* should only be used with calls to timer_ref_expired.
|
||||
* There is no reference counting on the reference and therefore does not
|
||||
* require any operation to free it.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
time_ref_t timer_ref(bl_timer_id id);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_ref_expired
|
||||
*
|
||||
* Description:
|
||||
* Test if a timer, that was configured as a modeTimeout timer is expired
|
||||
* based on the reference provided.
|
||||
*
|
||||
* Input Parameters:
|
||||
* ref - Returned timer_ref;
|
||||
*
|
||||
* Returned Value:
|
||||
* Non Zero if the timer is expired otherwise zero.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static inline int timer_ref_expired(time_ref_t ref)
|
||||
{
|
||||
return *ref == 0;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_tic
|
||||
*
|
||||
* Description:
|
||||
* Returns the system tic counter that counts in units of
|
||||
* (CONFIG_USEC_PER_TICK/1000). By default 10 Ms.
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
time_ms_t timer_tic(void);
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_hrt_read
|
||||
*
|
||||
* Description:
|
||||
* Returns the hardware SysTic counter that counts in units of
|
||||
* STM32_HCLK_FREQUENCY. This file defines TIMER_HRT_CYCLES_PER_US
|
||||
* and TIMER_HRT_CYCLES_PER_MS that should be used to define times.
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* The current value of the HW counter in the type of time_hrt_cycles_t.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static inline time_hrt_cycles_t timer_hrt_read(void)
|
||||
{
|
||||
return getreg32(NVIC_SYSTICK_CURRENT);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_hrt_clear_wrap
|
||||
*
|
||||
* Description:
|
||||
* Clears the wrap flag by reading the timer
|
||||
*
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static inline void timer_hrt_clear_wrap(void)
|
||||
{
|
||||
(void)timer_hrt_read();
|
||||
}
|
||||
/****************************************************************************
|
||||
* Name: timer_hrt_wrap
|
||||
*
|
||||
* Description:
|
||||
* Returns true if SysTic counted to 0 since last time it was
|
||||
* read.
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* Returns true if timer counted to 0 since last time this was read.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static inline bool timer_hrt_wrap(void)
|
||||
{
|
||||
uint32_t rv = getreg32(NVIC_SYSTICK_CTRL);
|
||||
return ((rv & NVIC_SYSTICK_CTRL_COUNTFLAG) ? true : false);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_hrt_max
|
||||
*
|
||||
* Description:
|
||||
* Returns the hardware SysTic reload value +1
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* The current SysTic reload of the HW counter in the type of
|
||||
* time_hrt_cycles_t.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static inline time_hrt_cycles_t timer_hrt_max(void)
|
||||
{
|
||||
return getreg32(NVIC_SYSTICK_RELOAD) + 1;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_hrt_elapsed
|
||||
*
|
||||
* Description:
|
||||
* Returns the difference between 2 time values, taking into account
|
||||
* the way the timer wrap.
|
||||
*
|
||||
* Input Parameters:
|
||||
* begin - Beginning timer count.
|
||||
* end - Ending timer count.
|
||||
*
|
||||
* Returned Value:
|
||||
* The difference from begin to end
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static inline time_hrt_cycles_t timer_hrt_elapsed(time_hrt_cycles_t begin, time_hrt_cycles_t end)
|
||||
{
|
||||
/* It is a down count from NVIC_SYSTICK_RELOAD */
|
||||
|
||||
time_hrt_cycles_t elapsed = begin - end;
|
||||
time_hrt_cycles_t reload = timer_hrt_max();
|
||||
|
||||
/* Did it wrap */
|
||||
if (elapsed > reload) {
|
||||
elapsed += reload;
|
||||
}
|
||||
|
||||
return elapsed;
|
||||
}
|
|
@ -0,0 +1,842 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
/*
|
||||
* Uavcan protocol CAN ID formats and Tail byte component definitions
|
||||
*
|
||||
* Most all the constants below are auto generated by the compiler in the
|
||||
* section above.
|
||||
|
||||
* For all UAVCAN_BIT_DEFINE({item name}, lsb_pos, length) entries
|
||||
* it defines the following enumeration constants:
|
||||
*
|
||||
* Mask{item name} - The mask that is positioned at the lsb and
|
||||
* length long
|
||||
* BitPos{item name} - The bit position of the lsb
|
||||
* Length{item name} - The number of bits in the field
|
||||
*
|
||||
*
|
||||
* For the UAVCAN_DSDL_TYPE_DEF(name, dtid, service, signature, packsize,
|
||||
* mailbox, fifo, inbound, outbound)
|
||||
* it defines:
|
||||
*
|
||||
* uavcan dtid definitions are in the form nnnn.type_name.uavcan
|
||||
*
|
||||
* DSDL{type_name} - An internal zero based ID for the transfer
|
||||
* format E.G DSDLNodeInfo would return the internal
|
||||
* constant index derived from the line that
|
||||
* UAVCAN_DSDL_TYPE_DEF(GetNodeInfo... is defined on
|
||||
* in uavcan_dsdl_defs.h (as of this writing it would
|
||||
* be the value 1)
|
||||
*
|
||||
* DTID{type_name} - The uavcan Number nnn.name E.G DTIDNodeInfo
|
||||
*
|
||||
* ServiceNotMessage{type_name} - True for DTID that are uavcan Services Type
|
||||
* Defined Transfer Type
|
||||
*
|
||||
* SignatureCRC16{type_name} - Required for multi-frame Transfer Type
|
||||
*
|
||||
* PackedSize{type_name} - The packed size of the Transfer Type
|
||||
*
|
||||
* MailBox{type_name} - When sending this Transfer Type, use this
|
||||
* mailbox
|
||||
*
|
||||
* Fifo{type_name} - When receiving this Transfer Type, use this
|
||||
* fifo. - Filers need to be configured
|
||||
*
|
||||
* Fifo{type_name} - The uavcan Number nnn.name E.G DTIDNodeInfo
|
||||
*
|
||||
* InTailInit{type_name} - The value of the tail byte to expect at
|
||||
* the end of a transfer
|
||||
*
|
||||
* OutTailInit{type_name} - The value of the tail byte to to use when
|
||||
* sending this transfer
|
||||
*
|
||||
* For all UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length)
|
||||
* it defines the following enumeration constants:
|
||||
*
|
||||
* Mask{item name} - The mask that is positioned at the lsb and
|
||||
* length long
|
||||
* BitPos{item name} - The bit position of the lsb
|
||||
* Length{item name} - The number of bits in the field
|
||||
* PayloadOffset{<name><field_name>} - The offset in the payload
|
||||
* PayloadLength{<name><field_name>} - The number of bytes
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/compiler.h>
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "bitminip.h"
|
||||
#include "can.h"
|
||||
#include "px4_macros.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
#define sizeof_member(t, m) sizeof(((t *)0)->m)
|
||||
#define UAVCAN_STRLEN(x) sizeof((x))-1
|
||||
#define uavcan_exclude(x, name) BITFEILD_EXCLUDE((x), BitPos##name,Length##name)
|
||||
#define uavcan_make_uint16(d0, d1) (uint16_t)(((d1) << 8u) | (d0))
|
||||
|
||||
#define uavcan_dsdl_field(op, data_typ_name, field_name) (op##data_typ_name##field_name)
|
||||
#define uavcan_bit_pos(data_typ_name, field_name) uavcan_dsdl_field(BitPos, data_typ_name, field_name)
|
||||
#define uavcan_bit_mask(data_typ_name, field_name) uavcan_dsdl_field(Mask, data_typ_name, field_name)
|
||||
#define uavcan_bit_count(data_typ_name, field_name) uavcan_dsdl_field(Length, data_typ_name, field_name)
|
||||
|
||||
#define uavcan_byte_offset(data_typ_name, field_name) uavcan_dsdl_field(PayloadOffset, data_typ_name, field_name)
|
||||
#define uavcan_byte_count(data_typ_name, field_name) uavcan_dsdl_field(PayloadLength, data_typ_name, field_name)
|
||||
|
||||
#define uavcan_pack(d, data_typ_name, field_name) \
|
||||
(((d) << uavcan_bit_pos(data_typ_name, field_name)) & uavcan_bit_mask(data_typ_name, field_name))
|
||||
#define uavcan_ppack(d, data_typ_name, field_name) uavcan_pack(d->field_name, data_typ_name, field_name)
|
||||
#define uavcan_rpack(d, data_typ_name, field_name) uavcan_pack(d.field_name, data_typ_name, field_name)
|
||||
|
||||
#define uavcan_unpack(d, data_typ_name, field_name) \
|
||||
(((d) & uavcan_bit_mask(data_typ_name, field_name)) >> uavcan_bit_pos(data_typ_name, field_name))
|
||||
#define uavcan_punpack(d, data_typ_name, field_name) uavcan_unpack(d->field_name, data_typ_name, field_name)
|
||||
#define uavcan_runpack(d, data_typ_name, field_name) uavcan_unpack(d.field_name, data_typ_name, field_name)
|
||||
|
||||
#define uavcan_protocol_mask(field_name) (Mask##field_name)
|
||||
|
||||
/****************************************************************************
|
||||
* Auto Generated Public Type Definitions
|
||||
****************************************************************************/
|
||||
/* CAN ID fields */
|
||||
#define UAVCAN_BIT_DEFINE(field_name, lsb_pos, length) Mask##field_name = BITFEILD_MASK((lsb_pos), (length)),
|
||||
typedef enum uavcan_can_id_mask_t {
|
||||
#include "uavcan_can_id_defs.h"
|
||||
} uavcan_can_id_mask_t;
|
||||
#undef UAVCAN_BIT_DEFINE
|
||||
|
||||
#define UAVCAN_BIT_DEFINE(field_name, lsb_pos, length) BitPos##field_name = (lsb_pos),
|
||||
typedef enum uavcan_can_id_pos_t {
|
||||
#include "uavcan_can_id_defs.h"
|
||||
} uavcan_can_id_pos_t;
|
||||
#undef UAVCAN_BIT_DEFINE
|
||||
|
||||
#define UAVCAN_BIT_DEFINE(field_name, lsb_pos, length) Length##field_name = (length),
|
||||
typedef enum uavcan_can_id_length_t {
|
||||
#include "uavcan_can_id_defs.h"
|
||||
} uavcan_can_id_length_t;
|
||||
#undef UAVCAN_BIT_DEFINE
|
||||
|
||||
/****************************************************************************
|
||||
* Public Type Definitions
|
||||
****************************************************************************/
|
||||
typedef enum uavcan_direction_t {
|
||||
InBound = true,
|
||||
OutBound = false,
|
||||
|
||||
MessageIn = InBound,
|
||||
MessageOut = OutBound,
|
||||
} uavcan_direction_t;
|
||||
|
||||
/*
|
||||
* Uavcan protocol CAN ID formats and Tail byte component definitions
|
||||
*
|
||||
* Most all the constants below are auto generated by the compiler in the
|
||||
* section above.
|
||||
* It defines Mask{item name}, BitPos{item name} and Length{item name}
|
||||
* for all UAVCAN_BIT_DEFINE({item name}, lsb_pos, length) entries
|
||||
*
|
||||
* [CAN ID[4]][data[0-7][tail[1]]]
|
||||
*
|
||||
*/
|
||||
|
||||
/* General UAVCAN Constants */
|
||||
|
||||
typedef enum uavcan_general_t {
|
||||
UavcanPriorityMax = 0,
|
||||
UavcanPriorityMin = 31,
|
||||
|
||||
} uavcan_general_t;
|
||||
|
||||
|
||||
/* CAN ID definitions for native data manipulation */
|
||||
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wattributes"
|
||||
#pragma GCC diagnostic ignored "-Wpacked"
|
||||
typedef struct packed_struct can_id_t {
|
||||
union {
|
||||
uint32_t u32;
|
||||
uint8_t b[sizeof(uint32_t)];
|
||||
};
|
||||
} can_id_t;
|
||||
|
||||
/* UAVCAN CAN ID Usage: Message definition */
|
||||
|
||||
typedef struct uavcan_message_t {
|
||||
uint32_t source_node_id : LengthUavCanMessageSourceNodeID;
|
||||
uint32_t service_not_message : LengthUavCanMessageServiceNotMessage;
|
||||
uint32_t type_id : LengthUavCanMessageTypeID;
|
||||
uint32_t priority : LengthUavCanMessagePriority;
|
||||
} uavcan_message_t;
|
||||
|
||||
/* UAVCAN CAN ID Usage: Anonymous Message Constants */
|
||||
|
||||
typedef enum uavcan_anon_const_t {
|
||||
UavcanAnonymousNodeID = 0,
|
||||
|
||||
} uavcan_anon_const_t;
|
||||
|
||||
|
||||
/* UAVCAN CAN ID Usage: Anonymous Message definition */
|
||||
|
||||
typedef struct uavcan_anonymous_message_t {
|
||||
uint32_t source_node_id : LengthUavCanAnonMessageSourceNodeID;
|
||||
uint32_t service_not_message : LengthUavCanAnonMessageServiceNotMessage;
|
||||
uint32_t type_id : LengthUavCanAnonMessageTypeID;
|
||||
uint32_t discriminator : LengthUavCanAnonMessageDiscriminator;
|
||||
uint32_t priority : LengthUavCanAnonMessagePriority;
|
||||
} uavcan_anonymous_message_t;
|
||||
|
||||
/* UAVCAN CAN ID Usage: Service Constants */
|
||||
|
||||
typedef enum uavcan_service_const_t {
|
||||
UavcanServiceRetries = 3,
|
||||
UavcanServiceTimeOutMs = 1000,
|
||||
|
||||
} uavcan_service_const_t;
|
||||
|
||||
/* UAVCAN CAN ID Usage: Service definition */
|
||||
|
||||
typedef struct uavcan_service_t {
|
||||
uint32_t source_node_id : LengthUavCanServiceSourceNodeID;
|
||||
uint32_t service_not_message : LengthUavCanServiceServiceNotMessage;
|
||||
uint32_t dest_node_id : LengthUavCanServiceDestinationNodeID;
|
||||
uint32_t request_not_response : LengthUavCanServiceRequestNotResponse;
|
||||
uint32_t type_id : LengthUavCanServiceTypeID;
|
||||
uint32_t priority : LengthUavCanServicePriority;
|
||||
} uavcan_service_t;
|
||||
|
||||
/* UAVCAN Tail Byte definitions for native data manipulation */
|
||||
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wpacked"
|
||||
typedef struct packed_struct can_tail_t {
|
||||
union {
|
||||
uint8_t u8;
|
||||
};
|
||||
} can_tail_t;
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
/* UAVCAN Tail Byte definitions */
|
||||
|
||||
typedef struct uavcan_tail_t {
|
||||
uint8_t transfer_id : LengthUavCanTransferID;
|
||||
uint8_t toggle : LengthUavCanToggle;
|
||||
uint8_t eot : LengthUavCanEndOfTransfer;
|
||||
uint8_t sot : LengthUavCanStartOfTransfer;
|
||||
} uavcan_tail_t;
|
||||
|
||||
/* UAVCAN Tail Byte Initialization constants */
|
||||
|
||||
typedef enum uavcan_tail_init_t {
|
||||
SingleFrameTailInit = (MaskUavCanStartOfTransfer | MaskUavCanEndOfTransfer),
|
||||
MultiFrameTailInit = (MaskUavCanStartOfTransfer),
|
||||
BadTailState = (MaskUavCanStartOfTransfer | MaskUavCanToggle),
|
||||
MaxUserPayloadLength = CanPayloadLength - sizeof(uavcan_tail_t),
|
||||
} uavcan_tail_init_t;
|
||||
|
||||
/*
|
||||
* Assert that assumptions in code are true
|
||||
* The code assumes it can manipulate a ALL sub protocol objects
|
||||
* using MaskUavCanMessageServiceNotMessage, MaskUavCanMessagePriority
|
||||
* and MaskUavCanMessageSourceNodeID
|
||||
*/
|
||||
|
||||
CCASSERT(MaskUavCanServicePriority == MaskUavCanAnonMessagePriority);
|
||||
CCASSERT(MaskUavCanServicePriority == MaskUavCanMessagePriority);
|
||||
CCASSERT(MaskUavCanServiceSourceNodeID == MaskUavCanAnonMessageSourceNodeID);
|
||||
CCASSERT(MaskUavCanServiceSourceNodeID == MaskUavCanMessageSourceNodeID);
|
||||
CCASSERT(MaskUavCanMessageServiceNotMessage == MaskUavCanAnonMessageServiceNotMessage);
|
||||
CCASSERT(MaskUavCanMessageServiceNotMessage == MaskUavCanMessageServiceNotMessage);
|
||||
|
||||
/****************************************************************************
|
||||
* Auto Generated Public Type Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/* UAVCAN DSDL Type Definitions*/
|
||||
#define NA 0
|
||||
#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length)
|
||||
|
||||
#define UAVCAN_DSDL_MESG_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \
|
||||
UAVCAN_DSDL_TYPE_DEF(Msg##name, (dtid), (signature), (packsize), (mailbox), (fifo), (inbound), (outbound))
|
||||
|
||||
#define UAVCAN_DSDL_SREQ_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \
|
||||
UAVCAN_DSDL_TYPE_DEF(Req##name, (dtid), (signature), (packsize), (mailbox), (fifo), (inbound), (outbound))
|
||||
|
||||
#define UAVCAN_DSDL_SRSP_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \
|
||||
UAVCAN_DSDL_TYPE_DEF(Rsp##name, (dtid), (signature), (packsize), (mailbox), (fifo), (inbound), (outbound))
|
||||
|
||||
#define END_COMPONENTS SizeDSDLComponents,
|
||||
#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \
|
||||
DSDL##name,
|
||||
typedef enum uavcan_dsdl_t {
|
||||
#include "uavcan_dsdl_defs.h"
|
||||
SizeDSDL,
|
||||
} uavcan_dsdl_t;
|
||||
#undef UAVCAN_DSDL_TYPE_DEF
|
||||
#undef END_COMPONENTS
|
||||
#define END_COMPONENTS
|
||||
|
||||
#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length)
|
||||
#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \
|
||||
DTID##name = (dtid),
|
||||
typedef enum uavcan_dsdl_dtid_t {
|
||||
#include "uavcan_dsdl_defs.h"
|
||||
} uavcan_dsdl_dtid_t;
|
||||
#undef UAVCAN_DSDL_TYPE_DEF
|
||||
|
||||
#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \
|
||||
SignatureCRC16##name = (signature),
|
||||
typedef enum uavcan_dsdl_sig_crc16_t {
|
||||
#include "uavcan_dsdl_defs.h"
|
||||
} uavcan_dsdl_sig_crc16_t;
|
||||
#undef UAVCAN_DSDL_TYPE_DEF
|
||||
|
||||
#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \
|
||||
PackedSize##name = (packsize),
|
||||
typedef enum uavcan_dsdl_packedsize_t {
|
||||
#include "uavcan_dsdl_defs.h"
|
||||
} uavcan_dsdl_packedsize_t;
|
||||
#undef UAVCAN_DSDL_TYPE_DEF
|
||||
|
||||
#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \
|
||||
MailBox##name = (mailbox),
|
||||
typedef enum uavcan_dsdl_mb_t {
|
||||
#include "uavcan_dsdl_defs.h"
|
||||
} uavcan_dsdl_mb_t;
|
||||
#undef UAVCAN_DSDL_TYPE_DEF
|
||||
|
||||
#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \
|
||||
Fifo##name = (fifo),
|
||||
typedef enum uavcan_dsdl_fifo_t {
|
||||
#include "uavcan_dsdl_defs.h"
|
||||
} uavcan_dsdl_fifo_t;
|
||||
#undef UAVCAN_DSDL_TYPE_DEF
|
||||
|
||||
#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \
|
||||
InTailInit##name = (inbound),
|
||||
typedef enum uavcan_dsdl_inbound_t {
|
||||
#include "uavcan_dsdl_defs.h"
|
||||
} uavcan_dsdl_inbound_t;
|
||||
#undef UAVCAN_DSDL_TYPE_DEF
|
||||
|
||||
#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \
|
||||
OutTailInit##name = (outbound),
|
||||
typedef enum uavcan_dsdl_outbound_t {
|
||||
#include "uavcan_dsdl_defs.h"
|
||||
} uavcan_dsdl_outbound_t;
|
||||
#undef UAVCAN_DSDL_TYPE_DEF
|
||||
#undef UAVCAN_DSDL_SRSP_DEF
|
||||
#undef UAVCAN_DSDL_SREQ_DEF
|
||||
#undef UAVCAN_DSDL_MESG_DEF
|
||||
#undef UAVCAN_DSDL_BIT_DEF
|
||||
|
||||
/* UAVCAN DSDL Fields of Type Definitions definitions*/
|
||||
|
||||
#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound)
|
||||
#define UAVCAN_DSDL_MESG_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound)
|
||||
#define UAVCAN_DSDL_SREQ_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound)
|
||||
#define UAVCAN_DSDL_SRSP_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound)
|
||||
|
||||
#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) \
|
||||
Mask##data_typ_name##field_name = BITFEILD_MASK((lsb_pos), (length)),
|
||||
typedef enum uavcan_dsdl_mask_t {
|
||||
#include "uavcan_dsdl_defs.h"
|
||||
} uavcan_dsdl_mask_t;
|
||||
#undef UAVCAN_DSDL_BIT_DEF
|
||||
|
||||
#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) \
|
||||
BitPos##data_typ_name##field_name = (lsb_pos),
|
||||
typedef enum uavcan_dsdl_pos_t {
|
||||
#include "uavcan_dsdl_defs.h"
|
||||
} uavcan_dsdl_pos_t;
|
||||
#undef UAVCAN_DSDL_BIT_DEF
|
||||
|
||||
#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) \
|
||||
Length##data_typ_name##field_name = (length),
|
||||
typedef enum uavcan_dsdl_length_t {
|
||||
#include "uavcan_dsdl_defs.h"
|
||||
} uavcan_dsdl_length_t;
|
||||
#undef UAVCAN_DSDL_BIT_DEF
|
||||
|
||||
#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) \
|
||||
PayloadOffset##data_typ_name##field_name = (payload_offset),
|
||||
typedef enum uavcan_dsdl_payload_offset_t {
|
||||
#include "uavcan_dsdl_defs.h"
|
||||
} uavcan_dsdl_payload_offset_t;
|
||||
#undef UAVCAN_DSDL_BIT_DEF
|
||||
|
||||
#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) \
|
||||
PayloadLength##data_typ_name##field_name = (payload_length),
|
||||
typedef enum uavcan_dsdl_payload_length_t {
|
||||
#include "uavcan_dsdl_defs.h"
|
||||
} uavcan_dsdl_payload_length_t;
|
||||
#undef UAVCAN_DSDL_TYPE_DEF
|
||||
#undef UAVCAN_DSDL_SRSP_DEF
|
||||
#undef UAVCAN_DSDL_SREQ_DEF
|
||||
#undef UAVCAN_DSDL_MESG_DEF
|
||||
#undef UAVCAN_DSDL_BIT_DEF
|
||||
#undef END_COMPONENTS
|
||||
#undef NA
|
||||
|
||||
/****************************************************************************
|
||||
* Public Type Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/* Uavcan function return values */
|
||||
|
||||
typedef enum uavcan_error_t {
|
||||
UavcanOk = 0,
|
||||
UavcanBootTimeout = 1,
|
||||
UavcanError = 3
|
||||
} uavcan_error_t;
|
||||
|
||||
|
||||
/*
|
||||
* Uavcan protocol CAN ID formats and Tail byte
|
||||
*
|
||||
* [CAN ID[4]][data[0-7][tail[1]]]
|
||||
*
|
||||
*/
|
||||
|
||||
typedef struct packed_struct uavcan_protocol_t {
|
||||
union {
|
||||
can_id_t id;
|
||||
uavcan_message_t msg;
|
||||
uavcan_anonymous_message_t ana;
|
||||
uavcan_service_t ser;
|
||||
};
|
||||
union {
|
||||
can_tail_t tail_init;
|
||||
uavcan_tail_t tail;
|
||||
};
|
||||
} uavcan_protocol_t;
|
||||
|
||||
|
||||
/*
|
||||
* Uavcan protocol DSDL Type Definitions
|
||||
*/
|
||||
|
||||
/****************************************
|
||||
* Uavcan NodeStatus
|
||||
****************************************/
|
||||
|
||||
typedef enum uavcan_NodeStatusConsts_t {
|
||||
MAX_BROADCASTING_PERIOD_MS = 1000,
|
||||
MIN_BROADCASTING_PERIOD_MS = 2,
|
||||
OFFLINE_TIMEOUT_MS = 2000,
|
||||
|
||||
HEALTH_OK = 0,
|
||||
HEALTH_WARNING = 1,
|
||||
HEALTH_ERROR = 2,
|
||||
HEALTH_CRITICAL = 3,
|
||||
|
||||
MODE_OPERATIONAL = 0,
|
||||
MODE_INITIALIZATION = 1,
|
||||
MODE_MAINTENANCE = 2,
|
||||
MODE_SOFTWARE_UPDATE = 3,
|
||||
|
||||
MODE_OFFLINE = 7,
|
||||
} uavcan_NodeStatusConsts_t;
|
||||
|
||||
typedef struct packed_struct uavcan_NodeStatus_t {
|
||||
uint32_t uptime_sec;
|
||||
union {
|
||||
uint8_t u8;
|
||||
struct {
|
||||
uint8_t sub_mode: LengthNodeStatussub_mode;
|
||||
uint8_t mode : LengthNodeStatusmode;
|
||||
uint8_t health : LengthNodeStatushealth;
|
||||
};
|
||||
};
|
||||
uint16_t vendor_specific_status_code;
|
||||
} uavcan_NodeStatus_t;
|
||||
|
||||
/****************************************
|
||||
* Uavcan GetNodeInfo composition
|
||||
****************************************/
|
||||
|
||||
/* SoftwareVersion */
|
||||
typedef enum uavcan_SoftwareVersionConsts_t {
|
||||
OPTIONAL_FIELD_FLAG_VCS_COMMIT = 1,
|
||||
OPTIONAL_FIELD_FLAG_IMAGE_CRC = 2,
|
||||
} uavcan_SoftwareVersionConsts_t;
|
||||
|
||||
typedef struct packed_struct uavcan_SoftwareVersion_t {
|
||||
uint8_t major;
|
||||
uint8_t minor;
|
||||
uint8_t optional_field_flags;
|
||||
uint32_t vcs_commit;
|
||||
uint64_t image_crc;
|
||||
} uavcan_SoftwareVersion_t;
|
||||
|
||||
CCASSERT(PackedSizeSoftwareVersion == sizeof(uavcan_SoftwareVersion_t));
|
||||
|
||||
/* HardwareVersion */
|
||||
|
||||
typedef struct packed_struct uavcan_HardwareVersion_t {
|
||||
uint8_t major;
|
||||
uint8_t minor;
|
||||
uint8_t unique_id[PayloadLengthHardwareVersionunique_id];
|
||||
uint8_t certificate_of_authenticity_length;
|
||||
uint8_t certificate_of_authenticity[PayloadLengthHardwareVersioncertificate_of_authenticity];
|
||||
} uavcan_HardwareVersion_t;
|
||||
|
||||
typedef enum uavcan_HardwareVersionConsts_t {
|
||||
FixedSizeHardwareVersion = sizeof_member(uavcan_HardwareVersion_t, major) + \
|
||||
sizeof_member(uavcan_HardwareVersion_t, minor) + \
|
||||
sizeof_member(uavcan_HardwareVersion_t, unique_id) + \
|
||||
sizeof_member(uavcan_HardwareVersion_t, certificate_of_authenticity_length),
|
||||
} uavcan_HardwareVersionConsts_t;
|
||||
|
||||
typedef struct packed_struct uavcan_GetNodeInfo_request_t {
|
||||
uint8_t empty[CanPayloadLength];
|
||||
} uavcan_GetNodeInfo_request_t;
|
||||
|
||||
/* GetNodeInfo Response */
|
||||
|
||||
typedef struct packed_struct uavcan_GetNodeInfo_response_t {
|
||||
|
||||
uavcan_NodeStatus_t nodes_status;;
|
||||
uavcan_SoftwareVersion_t software_version;
|
||||
uavcan_HardwareVersion_t hardware_version;
|
||||
|
||||
uint8_t name[PayloadLengthGetNodeInfoname];
|
||||
uint8_t name_length;
|
||||
} uavcan_GetNodeInfo_response_t;
|
||||
|
||||
typedef enum uavcan_GetNodeInfoConsts_t {
|
||||
FixedSizeGetNodeInfo = PackedSizeMsgNodeStatus + PackedSizeSoftwareVersion + FixedSizeHardwareVersion,
|
||||
|
||||
} uavcan_GetNodeInfoConsts_t;
|
||||
|
||||
/****************************************
|
||||
* Uavcan LogMessage
|
||||
****************************************/
|
||||
|
||||
typedef enum uavcan_LogMessageConsts_t {
|
||||
LOGMESSAGE_LEVELDEBUG = 0,
|
||||
LOGMESSAGE_LEVELINFO = 1,
|
||||
LOGMESSAGE_LEVELWARNING = 2,
|
||||
LOGMESSAGE_LEVELERROR = 3,
|
||||
} uavcan_LogMessageConsts_t;
|
||||
|
||||
typedef struct packed_struct uavcan_LogMessage_t {
|
||||
uint8_t level;
|
||||
uint8_t source[uavcan_byte_count(LogMessage, source)];
|
||||
uint8_t text[uavcan_byte_count(LogMessage, text)];
|
||||
} uavcan_LogMessage_t;
|
||||
|
||||
CCASSERT(sizeof(uavcan_LogMessage_t) == PackedSizeMsgLogMessage);
|
||||
|
||||
|
||||
/****************************************
|
||||
* Uavcan Allocation
|
||||
****************************************/
|
||||
|
||||
typedef enum uavcan_AllocationConsts_t {
|
||||
MAX_REQUEST_PERIOD_MS = 1400,
|
||||
MIN_REQUEST_PERIOD_MS = 600,
|
||||
MAX_FOLLOWUP_DELAY_MS = 400,
|
||||
MIN_FOLLOWUP_DELAY_MS = 0,
|
||||
FOLLOWUP_TIMEOUT_MS = 500,
|
||||
MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST = 6,
|
||||
ANY_NODE_ID = 0,
|
||||
PriorityAllocation = UavcanPriorityMin - 1,
|
||||
} uavcan_AllocationConsts_t;
|
||||
|
||||
typedef struct packed_struct uavcan_Allocation_t {
|
||||
uint8_t node_id; /* bottom bit is the first part flag */
|
||||
uint8_t unique_id[PayloadLengthAllocationunique_id];
|
||||
} uavcan_Allocation_t;
|
||||
|
||||
|
||||
/****************************************
|
||||
* Uavcan Path
|
||||
****************************************/
|
||||
|
||||
|
||||
typedef struct packed_struct uavcan_Path_t {
|
||||
uint8_t u8[PayloadLengthPathpath];
|
||||
} uavcan_Path_t;
|
||||
|
||||
typedef enum uavcan_PathConst_t {
|
||||
SEPARATOR = '/',
|
||||
} uavcan_PathConst_t;
|
||||
|
||||
/****************************************
|
||||
* Uavcan GetInfo Composition
|
||||
****************************************/
|
||||
|
||||
typedef enum uavcan_ErrorConst_t {
|
||||
FILE_ERROR_OK = 0,
|
||||
FILE_ERROR_UNKNOWN_ERROR = 32767,
|
||||
FILE_ERROR_NOT_FOUND = 2,
|
||||
FILE_ERROR_IO_ERROR = 5,
|
||||
FILE_ERROR_ACCESS_DENIED = 13,
|
||||
FILE_ERROR_IS_DIRECTORY = 21,
|
||||
FILE_ERROR_INVALID_VALUE = 22,
|
||||
FILE_ERROR_FILE_TOO_LARGE = 27,
|
||||
FILE_ERROR_OUT_OF_SPACE = 28,
|
||||
FILE_ERROR_NOT_IMPLEMENTED = 38,
|
||||
} uavcan_ErrorConst_t;
|
||||
|
||||
typedef struct packed_struct uavcan_Error_t {
|
||||
uint16_t value;
|
||||
} uavcan_Error_t;
|
||||
|
||||
typedef enum uavcan_EntryTypeConst_t {
|
||||
ENTRY_TYPE_FLAG_FILE = 1,
|
||||
ENTRY_TYPE_FLAG_DIRECTORY = 2,
|
||||
ENTRY_TYPE_FLAG_SYMLINK = 4,
|
||||
ENTRY_TYPE_FLAG_READABLE = 8,
|
||||
ENTRY_TYPE_FLAG_WRITEABLE = 16,
|
||||
} uavcan_EntryTypeConst_t;
|
||||
|
||||
typedef struct packed_struct uavcan_EntryType_t {
|
||||
uint8_t flags;
|
||||
} uavcan_EntryType_t;
|
||||
|
||||
|
||||
/****************************************
|
||||
* Uavcan BeginFirmwareUpdate
|
||||
****************************************/
|
||||
|
||||
typedef enum uavcan_BeginFirmwareUpdateConst_t {
|
||||
ERROR_OK = 0,
|
||||
ERROR_INVALID_MODE = 1,
|
||||
ERROR_IN_PROGRESS = 2,
|
||||
ERROR_UNKNOWN = 255,
|
||||
} uavcan_BeginFirmwareUpdateConst_t;
|
||||
|
||||
typedef struct packed_struct uavcan_BeginFirmwareUpdate_request {
|
||||
uint8_t source_node_id;
|
||||
uavcan_Path_t image_file_remote_path;
|
||||
} uavcan_BeginFirmwareUpdate_request;
|
||||
|
||||
typedef struct packed_struct uavcan_BeginFirmwareUpdate_response {
|
||||
uint8_t error;
|
||||
} uavcan_BeginFirmwareUpdate_response;
|
||||
|
||||
|
||||
|
||||
/****************************************
|
||||
* Uavcan GetInfo
|
||||
****************************************/
|
||||
|
||||
typedef struct packed_struct uavcan_GetInfo_request_t {
|
||||
uavcan_Path_t path;
|
||||
} uavcan_GetInfo_request_t;
|
||||
typedef enum uavcan_GetInfo_requestConst_t {
|
||||
FixedSizeGetInfoRequest = 0,
|
||||
|
||||
} uavcan_GetInfo_requestConst_t;
|
||||
|
||||
typedef struct packed_struct uavcan_GetInfo_response_t {
|
||||
uint32_t size;
|
||||
uint8_t msbsize;
|
||||
uavcan_Error_t error;
|
||||
uavcan_EntryType_t entry_type;
|
||||
} uavcan_GetInfo_response_t;
|
||||
|
||||
|
||||
/****************************************
|
||||
* Uavcan Read Composition
|
||||
****************************************/
|
||||
|
||||
typedef struct packed_struct uavcan_Read_request_t {
|
||||
uint32_t offset;
|
||||
uint8_t msboffset;
|
||||
uavcan_Path_t path;
|
||||
} uavcan_Read_request_t;
|
||||
|
||||
|
||||
typedef enum uavcan_ReadRequestConsts_t {
|
||||
FixedSizeReadRequest = sizeof_member(uavcan_Read_request_t, offset) + \
|
||||
sizeof_member(uavcan_Read_request_t, msboffset),
|
||||
} uavcan_ReadRequestConsts_t;
|
||||
|
||||
|
||||
typedef struct packed_struct uavcan_Read_response_t {
|
||||
uavcan_Error_t error;
|
||||
uint8_t data[PayloadLengthReaddata];
|
||||
} uavcan_Read_response_t;
|
||||
|
||||
/****************************************************************************
|
||||
* Global Variables
|
||||
****************************************************************************/
|
||||
extern uint8_t g_this_node_id;
|
||||
extern uint8_t g_server_node_id;
|
||||
extern uint8_t g_uavcan_priority;
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Name: uavcan_pack_GetNodeInfo_response
|
||||
*
|
||||
* Description:
|
||||
* This function packs the data of a uavcan_NodeStatus_t into
|
||||
* a uavcan_GetNodeInfo_response_t structure as array of bytes.
|
||||
* Then it packs the uavcan_GetNodeInfo_response_t
|
||||
*
|
||||
* Input Parameters:
|
||||
* response The uavcan_GetNodeInfo_response_t to be packed
|
||||
* node_status - The uavcan_NodeStatus_t that is part of the composition
|
||||
*
|
||||
* Returned value:
|
||||
* Number of bytes written.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
size_t uavcan_pack_GetNodeInfo_response(uavcan_GetNodeInfo_response_t
|
||||
*response);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: uavcan_tx_dsdl
|
||||
*
|
||||
* Description:
|
||||
* This helper function sends a uavcan service protocol, it
|
||||
* assumes the protocol object has the destination node id set.
|
||||
*
|
||||
* Input Parameters:
|
||||
* dsdl - An Uavcan DSDL Identifier (Auto Generated)
|
||||
* protocol - A pointer to a uavcan_protocol_t to configure the send,
|
||||
* the transfer with the dest_node_id set to that of the
|
||||
* node we are making the request of.
|
||||
* transfer - A pointer to the packed data of the transfer to be sent.
|
||||
* length - The number of bytes of data
|
||||
*
|
||||
* Returned value:
|
||||
* The UavcanOk of the data sent. Anything else indicates if a timeout
|
||||
* occurred.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
uavcan_error_t uavcan_tx_dsdl(uavcan_dsdl_t dsdl, uavcan_protocol_t *protocol,
|
||||
const uint8_t *transfer, size_t transfer_length);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: uavcan_rx_dsdl
|
||||
*
|
||||
* Description:
|
||||
* This function receives a uavcan Service response protocol transfer
|
||||
*
|
||||
* Input Parameters:
|
||||
* dsdl - An Uavcan DSDL Identifier (Auto Generated)
|
||||
* protocol - A pointer to a uavcan_protocol_t to configure the receive,
|
||||
* based the dsdl for the DTID Service.
|
||||
* If the request must come from a specific server
|
||||
* then protocol->ser.source_node_id, should be set
|
||||
* to that node id;
|
||||
*
|
||||
* in_out_transfer_length - The number of bytes of data to receive and the
|
||||
* number received.
|
||||
* timeout_ms - The amount of time in mS to wait for the initial transfer
|
||||
*
|
||||
* Returned value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
uavcan_error_t uavcan_rx_dsdl(uavcan_dsdl_t dsdl, uavcan_protocol_t *protocol,
|
||||
uint8_t *transfer, size_t *in_out_transfer_length,
|
||||
uint32_t timeout_ms);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: uavcan_tx_log_message
|
||||
*
|
||||
* Description:
|
||||
* This functions sends uavcan LogMessage type data. The Source will be
|
||||
* taken from the application defined debug_log_source
|
||||
*
|
||||
* Input Parameters:
|
||||
* level - Log Level of the LogMessage Constants DEBUG, INFO, WARN, ERROR
|
||||
* stage - The Stage the application is at. see Aplication defined
|
||||
* LOGMESSAGE_STAGE_x
|
||||
* status - The status of that stage. See Application defined
|
||||
* LOGMESSAGE_RESULT_x
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
/* The application must define this */
|
||||
extern const uint8_t debug_log_source[uavcan_byte_count(LogMessage, source)];
|
||||
|
||||
void uavcan_tx_log_message(uavcan_LogMessageConsts_t level, uint8_t stage,
|
||||
uint8_t status);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: uavcan_tx_allocation_message
|
||||
*
|
||||
* Description:
|
||||
* This function sends a uavcan allocation message.
|
||||
*
|
||||
* Input Parameters:
|
||||
* requested_node_id - This node's preferred node id 0 for no preference.
|
||||
* unique_id_length - This node's length of it's unique identifier.
|
||||
* unique_id - A pointer to the bytes that represent unique
|
||||
* identifier.
|
||||
* unique_id_offset - The offset equal 0 or the number of bytes in the
|
||||
* the last received message that matched the unique ID
|
||||
* field.
|
||||
* random - The value to use as the discriminator of the
|
||||
* anonymous message
|
||||
*
|
||||
* Returned value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void uavcan_tx_allocation_message(uint8_t requested_node_id,
|
||||
size_t unique_id_length,
|
||||
const uint8_t *unique_id,
|
||||
uint8_t unique_id_offset,
|
||||
uint16_t random);
|
|
@ -0,0 +1,66 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* This file not a typical h file, is defines the UAVCAN dsdl
|
||||
* usage and may be included several times in header or source
|
||||
* file
|
||||
*/
|
||||
|
||||
/* UAVCAN_BIT_DEFINE( field_name, lsb_pos, length) */
|
||||
|
||||
/* Message Frame Format */
|
||||
UAVCAN_BIT_DEFINE(UavCanMessagePriority, 24, 5)
|
||||
UAVCAN_BIT_DEFINE(UavCanMessageTypeID, 8, 16)
|
||||
UAVCAN_BIT_DEFINE(UavCanMessageServiceNotMessage, 7, 1)
|
||||
UAVCAN_BIT_DEFINE(UavCanMessageSourceNodeID, 0, 7)
|
||||
/* Anonymous message Frame Format */
|
||||
UAVCAN_BIT_DEFINE(UavCanAnonMessagePriority, 24, 5)
|
||||
UAVCAN_BIT_DEFINE(UavCanAnonMessageDiscriminator, 10, 14)
|
||||
UAVCAN_BIT_DEFINE(UavCanAnonMessageTypeID, 8, 2)
|
||||
UAVCAN_BIT_DEFINE(UavCanAnonMessageServiceNotMessage, 7, 1)
|
||||
UAVCAN_BIT_DEFINE(UavCanAnonMessageSourceNodeID, 0, 7)
|
||||
/* Service Frame Format */
|
||||
UAVCAN_BIT_DEFINE(UavCanServicePriority, 24, 5)
|
||||
UAVCAN_BIT_DEFINE(UavCanServiceTypeID, 16, 8)
|
||||
UAVCAN_BIT_DEFINE(UavCanServiceRequestNotResponse, 15, 1)
|
||||
UAVCAN_BIT_DEFINE(UavCanServiceDestinationNodeID, 8, 7)
|
||||
UAVCAN_BIT_DEFINE(UavCanServiceServiceNotMessage, 7, 1)
|
||||
UAVCAN_BIT_DEFINE(UavCanServiceSourceNodeID, 0, 7)
|
||||
/* Tail Format */
|
||||
UAVCAN_BIT_DEFINE(UavCanStartOfTransfer, 7, 1)
|
||||
UAVCAN_BIT_DEFINE(UavCanEndOfTransfer, 6, 1)
|
||||
UAVCAN_BIT_DEFINE(UavCanToggle, 5, 1)
|
||||
UAVCAN_BIT_DEFINE(UavCanTransferID, 0, 5)
|
|
@ -0,0 +1,187 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* This file not a typical h file, is defines the UAVCAN dsdl
|
||||
* usage and may be included several times in header or source
|
||||
* file
|
||||
*/
|
||||
|
||||
/* Components */
|
||||
|
||||
/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */
|
||||
UAVCAN_DSDL_BIT_DEF(CRC, lsb, 0, 8, 0, 1)
|
||||
UAVCAN_DSDL_BIT_DEF(CRC, msb, 0, 8, 1, 1)
|
||||
UAVCAN_DSDL_BIT_DEF(CRC, data, 0, 8, 2, 4)
|
||||
|
||||
/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */
|
||||
UAVCAN_DSDL_TYPE_DEF(Path, 0, 0, 200, NA, NA,
|
||||
NA, NA)
|
||||
/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */
|
||||
UAVCAN_DSDL_BIT_DEF(Path, path, 0, 8, 0,
|
||||
200)
|
||||
|
||||
/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */
|
||||
UAVCAN_DSDL_TYPE_DEF(SoftwareVersion, 0, 0, 15, NA, NA,
|
||||
NA, NA)
|
||||
/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */
|
||||
UAVCAN_DSDL_BIT_DEF(SoftwareVersion, major, 0, 8, 0,
|
||||
1)
|
||||
UAVCAN_DSDL_BIT_DEF(SoftwareVersion, minor, 0, 8, 1,
|
||||
1)
|
||||
UAVCAN_DSDL_BIT_DEF(SoftwareVersion, optional_field_flags, 0, 8, 2,
|
||||
1)
|
||||
UAVCAN_DSDL_BIT_DEF(SoftwareVersion, vcs_commit, 0, 32, 3,
|
||||
4)
|
||||
UAVCAN_DSDL_BIT_DEF(SoftwareVersion, image_crc, 0, NA, 7,
|
||||
8) // NA becuase bit mask is 64
|
||||
|
||||
/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */
|
||||
UAVCAN_DSDL_TYPE_DEF(HardwareVersion, 0, 0, NA, NA, NA,
|
||||
NA, NA)
|
||||
/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */
|
||||
UAVCAN_DSDL_BIT_DEF(HardwareVersion, unique_id, 0, 8, 0,
|
||||
16)
|
||||
UAVCAN_DSDL_BIT_DEF(HardwareVersion, certificate_of_authenticity, 0, 8, 0,
|
||||
255)
|
||||
END_COMPONENTS
|
||||
|
||||
/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */
|
||||
UAVCAN_DSDL_MESG_DEF(Allocation, 1, 0xf258, 0, MailBox0, Fifo0,
|
||||
MultiFrameTailInit, SingleFrameTailInit) /* Message */
|
||||
/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */
|
||||
UAVCAN_DSDL_BIT_DEF(Allocation, node_id, 1, 7, 0,
|
||||
1)
|
||||
UAVCAN_DSDL_BIT_DEF(Allocation, first_part_of_unique_id, 0, 1, 0,
|
||||
1)
|
||||
UAVCAN_DSDL_BIT_DEF(Allocation, unique_id, 0, 8, 1,
|
||||
16)
|
||||
|
||||
|
||||
/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */
|
||||
UAVCAN_DSDL_MESG_DEF(NodeStatus, 341, 0xbe5f, 7, MailBox1, FifoNone,
|
||||
SingleFrameTailInit, SingleFrameTailInit) /* Message */
|
||||
/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */
|
||||
UAVCAN_DSDL_BIT_DEF(NodeStatus, uptime_sec, 0, 32, 0,
|
||||
4)
|
||||
UAVCAN_DSDL_BIT_DEF(NodeStatus, health, 6, 2, 5,
|
||||
1)
|
||||
UAVCAN_DSDL_BIT_DEF(NodeStatus, mode, 3, 3, 5,
|
||||
1)
|
||||
UAVCAN_DSDL_BIT_DEF(NodeStatus, sub_mode, 0, 3, 5,
|
||||
1)
|
||||
UAVCAN_DSDL_BIT_DEF(NodeStatus, vendor_specific_status_code, 0, 16, 6,
|
||||
2)
|
||||
|
||||
/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */
|
||||
UAVCAN_DSDL_SREQ_DEF(GetNodeInfo, 1, 0xd9a7, 0, MailBox1, Fifo1,
|
||||
SingleFrameTailInit, MultiFrameTailInit) /* Request */
|
||||
/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) Empty */
|
||||
|
||||
UAVCAN_DSDL_SRSP_DEF(GetNodeInfo, 1, 0xd9a7, 0, MailBox1, Fifo1,
|
||||
SingleFrameTailInit, MultiFrameTailInit) /* Response */
|
||||
UAVCAN_DSDL_BIT_DEF(GetNodeInfo, status, 0, 8, 0,
|
||||
PackedSizeMsgNodeStatus)
|
||||
UAVCAN_DSDL_BIT_DEF(GetNodeInfo, software_version, 0, 8, NA,
|
||||
NA)
|
||||
UAVCAN_DSDL_BIT_DEF(GetNodeInfo, hardware_version, 0, 8, NA,
|
||||
NA)
|
||||
UAVCAN_DSDL_BIT_DEF(GetNodeInfo, name, 0, 8, NA,
|
||||
80)
|
||||
|
||||
/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */
|
||||
UAVCAN_DSDL_SREQ_DEF(BeginFirmwareUpdate, 40, 0x729e, 0, MailBox0, Fifo0,
|
||||
MultiFrameTailInit, SingleFrameTailInit) /* Request */
|
||||
/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */
|
||||
UAVCAN_DSDL_BIT_DEF(BeginFirmwareUpdate, source_node_id, 0, 8, 0,
|
||||
1)
|
||||
UAVCAN_DSDL_BIT_DEF(BeginFirmwareUpdate, image_file_remote_path, 0, 8, 1,
|
||||
PayloadLengthPathpath)
|
||||
|
||||
UAVCAN_DSDL_SRSP_DEF(BeginFirmwareUpdate, 40, 0x729e, 0, MailBox0, Fifo0,
|
||||
MultiFrameTailInit, SingleFrameTailInit) /* Response */
|
||||
UAVCAN_DSDL_BIT_DEF(BeginFirmwareUpdate, error, 0, 8, 0,
|
||||
1)
|
||||
UAVCAN_DSDL_BIT_DEF(BeginFirmwareUpdate, optional_error_message, 0, 8, 1,
|
||||
128)
|
||||
|
||||
/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */
|
||||
UAVCAN_DSDL_SREQ_DEF(GetInfo, 45, 0x14b9, 0, MailBox0, Fifo0,
|
||||
SingleFrameTailInit, MultiFrameTailInit) /* Request */
|
||||
/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */
|
||||
UAVCAN_DSDL_BIT_DEF(GetInfo, path, 0, 8, 0,
|
||||
PayloadLengthPathpath)
|
||||
|
||||
UAVCAN_DSDL_SRSP_DEF(GetInfo, 45, 0x14b9, 0, MailBox0, Fifo0,
|
||||
SingleFrameTailInit, MultiFrameTailInit) /* Response */
|
||||
UAVCAN_DSDL_BIT_DEF(GetInfo, size, 0, 32, 0,
|
||||
4) /* Response */
|
||||
UAVCAN_DSDL_BIT_DEF(GetInfo, sizemsb, 0, 8, 4,
|
||||
1)
|
||||
UAVCAN_DSDL_BIT_DEF(GetInfo, error, 0, 16, 5,
|
||||
2)
|
||||
UAVCAN_DSDL_BIT_DEF(GetInfo, entry_type, 0, 8, 7,
|
||||
1)
|
||||
|
||||
/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */
|
||||
UAVCAN_DSDL_SREQ_DEF(Read, 48, 0x2f12, 0, MailBox0, Fifo0,
|
||||
SingleFrameTailInit, MultiFrameTailInit) /* Request */
|
||||
/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */
|
||||
UAVCAN_DSDL_BIT_DEF(Read, offset, 0, 32, 0,
|
||||
4) /* Request */
|
||||
UAVCAN_DSDL_BIT_DEF(Read, msboffset, 0, 8, 4,
|
||||
1)
|
||||
UAVCAN_DSDL_BIT_DEF(Read, path, 0, 8, 5,
|
||||
PayloadLengthPathpath)
|
||||
|
||||
UAVCAN_DSDL_SRSP_DEF(Read, 48, 0x2f12, 0, MailBox0, Fifo0,
|
||||
SingleFrameTailInit, MultiFrameTailInit) // Responce
|
||||
UAVCAN_DSDL_BIT_DEF(Read, error, 0, 16, 0,
|
||||
2) /* Response */
|
||||
UAVCAN_DSDL_BIT_DEF(Read, data, 0, 8, 2,
|
||||
256)
|
||||
|
||||
/*UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packed size mailbox, fifo, inbound, outbound) */
|
||||
UAVCAN_DSDL_MESG_DEF(LogMessage, 16383, 0x4570, 7, MailBox0, Fifo0,
|
||||
NA, SingleFrameTailInit) /* Message */
|
||||
/* UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length) */
|
||||
UAVCAN_DSDL_BIT_DEF(LogMessage, level, 5, 3, 0,
|
||||
1)
|
||||
UAVCAN_DSDL_BIT_DEF(LogMessage, source_length, 0, 5, 0,
|
||||
1)
|
||||
UAVCAN_DSDL_BIT_DEF(LogMessage, source, 0, 8, 1,
|
||||
4) // Bootloader specific uses is 4
|
||||
UAVCAN_DSDL_BIT_DEF(LogMessage, text, 0, 8, 5,
|
||||
2) // Bootloader specific uses is 2
|
|
@ -0,0 +1,616 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#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 "stm32.h"
|
||||
#include <chip/stm32_can.h>
|
||||
#include "nvic.h"
|
||||
|
||||
#include "board.h"
|
||||
#include "px4_macros.h"
|
||||
#include "can.h"
|
||||
#include "crc.h"
|
||||
#include "timer.h"
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
#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 == 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
|
||||
# error Undefined QUANTA bsed on Clock Rate
|
||||
#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
|
||||
|
||||
/****************************************************************************
|
||||
* Private Types
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Name: read_msr_rx
|
||||
****************************************************************************/
|
||||
|
||||
static inline uint32_t read_msr_rx(void)
|
||||
{
|
||||
return getreg32(STM32_CAN1_MSR) & CAN_MSR_RX;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: read_msr
|
||||
****************************************************************************/
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: read_bits_times
|
||||
****************************************************************************/
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* 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);
|
||||
}
|
||||
}
|
|
@ -0,0 +1,265 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "chip.h"
|
||||
#include "stm32.h"
|
||||
|
||||
#include <errno.h>
|
||||
#include "boot_app_shared.h"
|
||||
#include "crc.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
#define BOOTLOADER_COMMON_APP_SIGNATURE 0xB0A04150u
|
||||
#define BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE 0xB0A0424Cu
|
||||
|
||||
|
||||
/* CAN_FiRx where (i=0..27|13, x=1, 2)
|
||||
* STM32_CAN1_FIR(i,x)
|
||||
* Using i = 2 does not requier there block
|
||||
* to be enabled nor FINIT in CAN_FMR to be set.
|
||||
* todo:Validate this claim on F2, F3
|
||||
*/
|
||||
|
||||
#define crc_HiLOC STM32_CAN1_FIR(2,1)
|
||||
#define crc_LoLOC STM32_CAN1_FIR(2,2)
|
||||
#define signature_LOC STM32_CAN1_FIR(3,1)
|
||||
#define bus_speed_LOC STM32_CAN1_FIR(3,2)
|
||||
#define node_id_LOC STM32_CAN1_FIR(4,1)
|
||||
#define CRC_H 1
|
||||
#define CRC_L 0
|
||||
|
||||
/****************************************************************************
|
||||
* Private Types
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: read
|
||||
****************************************************************************/
|
||||
|
||||
inline static void read(bootloader_app_shared_t *pshared)
|
||||
{
|
||||
pshared->signature = getreg32(signature_LOC);
|
||||
pshared->bus_speed = getreg32(bus_speed_LOC);
|
||||
pshared->node_id = getreg32(node_id_LOC);
|
||||
pshared->crc.ul[CRC_L] = getreg32(crc_LoLOC);
|
||||
pshared->crc.ul[CRC_H] = getreg32(crc_HiLOC);
|
||||
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: write
|
||||
****************************************************************************/
|
||||
|
||||
inline static void write(bootloader_app_shared_t *pshared)
|
||||
{
|
||||
putreg32(pshared->signature, signature_LOC);
|
||||
putreg32(pshared->bus_speed, bus_speed_LOC);
|
||||
putreg32(pshared->node_id, node_id_LOC);
|
||||
putreg32(pshared->crc.ul[CRC_L], crc_LoLOC);
|
||||
putreg32(pshared->crc.ul[CRC_H], crc_HiLOC);
|
||||
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: calulate_signature
|
||||
****************************************************************************/
|
||||
|
||||
static uint64_t calulate_signature(bootloader_app_shared_t *pshared)
|
||||
{
|
||||
uint64_t crc;
|
||||
crc = crc64_add_word(CRC64_INITIAL, pshared->signature);
|
||||
crc = crc64_add_word(crc, pshared->bus_speed);
|
||||
crc = crc64_add_word(crc, pshared->node_id);
|
||||
crc ^= CRC64_OUTPUT_XOR;
|
||||
return crc;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: bootloader_app_shared_init
|
||||
****************************************************************************/
|
||||
static void bootloader_app_shared_init(bootloader_app_shared_t *pshared, eRole_t role)
|
||||
{
|
||||
memset(pshared, 0, sizeof(bootloader_app_shared_t));
|
||||
|
||||
if (role != Invalid) {
|
||||
pshared->signature =
|
||||
(role ==
|
||||
App ? BOOTLOADER_COMMON_APP_SIGNATURE :
|
||||
BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Name: bootloader_app_shared_read
|
||||
*
|
||||
* Description:
|
||||
* Based on the role requested, this function will conditionally populate
|
||||
* a bootloader_app_shared_t structure from the physical locations used
|
||||
* to transfer the shared data to/from an application (internal data) .
|
||||
*
|
||||
* The functions will only populate the structure and return a status
|
||||
* indicating success, if the internal data has the correct signature as
|
||||
* requested by the Role AND has a valid crc.
|
||||
*
|
||||
* Input Parameters:
|
||||
* shared - A pointer to a bootloader_app_shared_t return the data in if
|
||||
* the internal data is valid for the requested Role
|
||||
* role - An eRole_t of App or BootLoader to validate the internal data
|
||||
* against. For a Bootloader this would be the value of App to
|
||||
* read the application passed data.
|
||||
*
|
||||
* Returned value:
|
||||
* OK - Indicates that the internal data has been copied to callers
|
||||
* bootloader_app_shared_t structure.
|
||||
*
|
||||
* -EBADR - The Role or crc of the internal data was not valid. The copy
|
||||
* did not occur.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
__EXPORT
|
||||
int bootloader_app_shared_read(bootloader_app_shared_t *shared,
|
||||
eRole_t role)
|
||||
{
|
||||
int rv = -EBADR;
|
||||
bootloader_app_shared_t working;
|
||||
|
||||
read(&working);
|
||||
|
||||
if ((role == App ? working.signature == BOOTLOADER_COMMON_APP_SIGNATURE
|
||||
: working.signature == BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE)
|
||||
&& (working.crc.ull == calulate_signature(&working))) {
|
||||
*shared = working;
|
||||
rv = OK;
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: bootloader_app_shared_write
|
||||
*
|
||||
* Description:
|
||||
* Based on the role, this function will commit the data passed
|
||||
* into the physical locations used to transfer the shared data to/from
|
||||
* an application (internal data) .
|
||||
*
|
||||
* The functions will populate the signature and crc the data
|
||||
* based on the provided Role.
|
||||
*
|
||||
* Input Parameters:
|
||||
* shared - A pointer to a bootloader_app_shared_t data to commit to
|
||||
* the internal data for passing to/from an application.
|
||||
* role - An eRole_t of App or BootLoader to use in the internal data
|
||||
* to be passed to/from an application. For a Bootloader this
|
||||
* would be the value of Bootloader to write to the passed data.
|
||||
* to the application via the internal data.
|
||||
*
|
||||
* Returned value:
|
||||
* None.
|
||||
*
|
||||
****************************************************************************/
|
||||
__EXPORT
|
||||
void bootloader_app_shared_write(bootloader_app_shared_t *shared,
|
||||
eRole_t role)
|
||||
{
|
||||
bootloader_app_shared_t working = *shared;
|
||||
working.signature =
|
||||
(role ==
|
||||
App ? BOOTLOADER_COMMON_APP_SIGNATURE :
|
||||
BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE);
|
||||
working.crc.ull = calulate_signature(&working);
|
||||
write(&working);
|
||||
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: bootloader_app_shared_invalidate
|
||||
*
|
||||
* Description:
|
||||
* Invalidates the data passed the physical locations used to transfer
|
||||
* the shared data to/from an application (internal data) .
|
||||
*
|
||||
* The functions will invalidate the signature and crc and shoulf be used
|
||||
* to prevent deja vu.
|
||||
*
|
||||
* Input Parameters:
|
||||
* None.
|
||||
*
|
||||
* Returned value:
|
||||
* None.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
__EXPORT
|
||||
void bootloader_app_shared_invalidate(void)
|
||||
{
|
||||
bootloader_app_shared_t working;
|
||||
bootloader_app_shared_init(&working, Invalid);
|
||||
write(&working);
|
||||
}
|
|
@ -0,0 +1,177 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include "boot_config.h"
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <nuttx/progmem.h>
|
||||
|
||||
#include "chip.h"
|
||||
#include "stm32.h"
|
||||
|
||||
#include "flash.h"
|
||||
#include "blsched.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Types
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Name: bl_flash_erase
|
||||
*
|
||||
* Description:
|
||||
* This function erases the flash starting at address and ending at
|
||||
* address + nbytes.
|
||||
*
|
||||
* Input Parameters:
|
||||
* address - A word-aligned address within the first page of flash to erase
|
||||
* nbytes - The number of bytes to erase, rounding up to the next page.
|
||||
*
|
||||
*
|
||||
*
|
||||
* Returned value:
|
||||
* On success FLASH_OK On Error one of the flash_error_t
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
flash_error_t bl_flash_erase(size_t address, size_t nbytes)
|
||||
{
|
||||
/*
|
||||
* FIXME (?): this may take a long time, and while flash is being erased it
|
||||
* might not be possible to execute interrupts, send NodeStatus messages etc.
|
||||
* We can pass a per page callback or yeild */
|
||||
|
||||
flash_error_t status = FLASH_ERROR_AFU;
|
||||
|
||||
|
||||
ssize_t bllastpage = up_progmem_getpage(address - 1);
|
||||
|
||||
if (bllastpage < 0) {
|
||||
return FLASH_ERROR_AFU;
|
||||
}
|
||||
|
||||
ssize_t appstartpage = up_progmem_getpage(address);
|
||||
ssize_t appendpage = up_progmem_getpage(address + nbytes - 4);
|
||||
|
||||
if (appendpage < 0 || appstartpage < 0) {
|
||||
return FLASH_ERROR_AFU;
|
||||
}
|
||||
|
||||
status = FLASH_ERROR_SUICIDE;
|
||||
|
||||
if (bllastpage >= 0 && appstartpage > bllastpage) {
|
||||
|
||||
/* Erase the whole application flash region */
|
||||
|
||||
status = FLASH_OK;
|
||||
|
||||
while (status == FLASH_OK && appstartpage <= appendpage) {
|
||||
bl_sched_yield();
|
||||
ssize_t ps = up_progmem_erasepage(appstartpage++);
|
||||
|
||||
if (ps <= 0) {
|
||||
status = FLASH_ERASE_ERROR;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: bl_flash_write
|
||||
*
|
||||
* Description:
|
||||
* This function writes the flash starting at the given address
|
||||
*
|
||||
* Input Parameters:
|
||||
* flash_address - The address of the flash to write
|
||||
* must be word aligned
|
||||
* data - A pointer to a buffer count bytes to be written
|
||||
* to the flash.
|
||||
* count - Number of bytes to write
|
||||
*
|
||||
* Returned value:
|
||||
* On success FLASH_OK On Error one of the flash_error_t
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
flash_error_t bl_flash_write(uint32_t flash_address, uint8_t *data, ssize_t count)
|
||||
{
|
||||
|
||||
flash_error_t status = FLASH_ERROR;
|
||||
|
||||
if (flash_address >= APPLICATION_LOAD_ADDRESS &&
|
||||
(flash_address + count) <= (uint32_t) APPLICATION_LAST_8BIT_ADDRRESS) {
|
||||
if (count ==
|
||||
up_progmem_write((size_t) flash_address, (void *)data, count)) {
|
||||
status = FLASH_OK;
|
||||
}
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
|
@ -0,0 +1,885 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include "boot_config.h"
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "chip.h"
|
||||
#include "stm32.h"
|
||||
|
||||
#include "timer.h"
|
||||
#include "uavcan.h"
|
||||
#include "can.h"
|
||||
#include "crc.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
#define CAN_REQUEST_TIMEOUT 1000
|
||||
#define ANY_NODE_ID 0
|
||||
#define THIS_NODE 1
|
||||
#define FW_SERVER 2
|
||||
#define REQ_NODE 3
|
||||
|
||||
/****************************************************************************
|
||||
* Private Types
|
||||
****************************************************************************/
|
||||
typedef struct packed_struct dsdl_t {
|
||||
uavcan_protocol_t prototype;
|
||||
uint16_t signature_crc16;
|
||||
uint8_t intail;
|
||||
uint8_t outtail;
|
||||
uint8_t mailbox : 2;
|
||||
uint8_t fifo : 2;
|
||||
} dsdl_t;
|
||||
|
||||
/* Values used in filter initialization */
|
||||
|
||||
typedef enum uavcan_transfer_stage_t {
|
||||
Initialization,
|
||||
OnStartOfTransfer
|
||||
} uavcan_transfer_stage_t;
|
||||
|
||||
typedef enum uavcan_dsdl_ignore_t {
|
||||
sizeDSDLTransfers = SizeDSDL - SizeDSDLComponents + 1
|
||||
} uavcan_dsdl_ignore_t;
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
/* Forward declaration */
|
||||
extern const dsdl_t dsdl_table[sizeDSDLTransfers];
|
||||
|
||||
/****************************************************************************
|
||||
* Private Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
uint8_t g_uavcan_priority = PriorityAllocation;
|
||||
uint8_t g_this_node_id = 0;
|
||||
uint8_t g_server_node_id = 0;
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
static inline uavcan_dsdl_t getDSDLOffset(uavcan_dsdl_t full) { return full - (SizeDSDLComponents + 1); }
|
||||
|
||||
/****************************************************************************
|
||||
* Name: uavcan_init_comapare_masks
|
||||
*
|
||||
* Description:
|
||||
* This function builds the masks needed for filtering Transfers
|
||||
*
|
||||
* Input Parameters:
|
||||
* stage - Either Initialization, OnStartOfTransfer
|
||||
* protocol - A pointer to a uavcan_protocol_t to configure the filters
|
||||
* masks - A pointer to a uavcan_protocol_t return the filters
|
||||
*
|
||||
* Returned value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static void uavcan_init_comapare_masks(uavcan_transfer_stage_t stage,
|
||||
uavcan_protocol_t *protocol,
|
||||
uavcan_protocol_t *masks)
|
||||
{
|
||||
|
||||
switch (stage) {
|
||||
|
||||
default:
|
||||
break;
|
||||
|
||||
case OnStartOfTransfer:
|
||||
masks->id.u32 |= uavcan_protocol_mask(UavCanMessageSourceNodeID);
|
||||
masks->tail_init.u8 |= uavcan_protocol_mask(UavCanTransferID);
|
||||
break;
|
||||
|
||||
|
||||
case Initialization:
|
||||
|
||||
masks->tail_init.u8 = uavcan_protocol_mask(UavCanToggle)
|
||||
| uavcan_protocol_mask(UavCanStartOfTransfer);
|
||||
|
||||
if (protocol->msg.service_not_message) {
|
||||
|
||||
/* Specific Filter initialization for a Services */
|
||||
|
||||
|
||||
masks->id.u32 = uavcan_protocol_mask(UavCanServiceTypeID)
|
||||
| uavcan_protocol_mask(UavCanServiceRequestNotResponse)
|
||||
| uavcan_protocol_mask(UavCanServiceDestinationNodeID)
|
||||
| uavcan_protocol_mask(UavCanServiceServiceNotMessage);
|
||||
|
||||
if (protocol->ser.source_node_id != ANY_NODE_ID) {
|
||||
|
||||
masks->id.u32 |= uavcan_protocol_mask(UavCanMessageSourceNodeID);
|
||||
|
||||
}
|
||||
|
||||
|
||||
} else {
|
||||
|
||||
/* Specific Filter initialization for a Message */
|
||||
|
||||
/* Is it anonymous? */
|
||||
|
||||
if (protocol->msg.source_node_id == ANY_NODE_ID) {
|
||||
|
||||
/* Intentional use of UavCanMessageTypeID because the response
|
||||
* to the anonymous message is a message and the discriminator
|
||||
* will be 0
|
||||
*/
|
||||
masks->id.u32 = uavcan_protocol_mask(UavCanMessageTypeID)
|
||||
| uavcan_protocol_mask(UavCanAnonMessageServiceNotMessage);
|
||||
|
||||
} else {
|
||||
|
||||
|
||||
masks->id.u32 = uavcan_protocol_mask(UavCanMessageTypeID)
|
||||
| uavcan_protocol_mask(UavCanMessageServiceNotMessage)
|
||||
| uavcan_protocol_mask(UavCanMessageSourceNodeID);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: load_dsdl_protocol
|
||||
*
|
||||
* Description:
|
||||
* This function builds the protocol for a given dsdl
|
||||
* N.B The value set for a service is request.
|
||||
*
|
||||
* Input Parameters:
|
||||
* dsdl - An uavcan DSDL Identifier (Auto Generated)
|
||||
* direction_in_not_out - in or out bound
|
||||
* protocol - A pointer to a uavcan_protocol_t to configure the filters
|
||||
* masks - A pointer to a uavcan_protocol_t return the filters
|
||||
*
|
||||
* Returned value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static const dsdl_t *load_dsdl_protocol(uavcan_dsdl_t dsdl,
|
||||
uavcan_direction_t rx_not_tx,
|
||||
uavcan_protocol_t *protocol,
|
||||
uint8_t that_node_id)
|
||||
{
|
||||
|
||||
const dsdl_t *pdsdl = &dsdl_table[getDSDLOffset(dsdl)];
|
||||
|
||||
protocol->id.u32 = pdsdl->prototype.id.u32;
|
||||
|
||||
protocol->msg.priority = g_uavcan_priority;
|
||||
|
||||
/* Preserver the transfer_id */
|
||||
|
||||
protocol->tail_init.u8 &= uavcan_protocol_mask(UavCanTransferID);
|
||||
|
||||
if (rx_not_tx) {
|
||||
|
||||
/* Rx */
|
||||
|
||||
protocol->tail_init.u8 |= pdsdl->intail;
|
||||
|
||||
/* Service */
|
||||
|
||||
if (pdsdl->prototype.msg.service_not_message) {
|
||||
|
||||
protocol->ser.dest_node_id = g_this_node_id;
|
||||
protocol->ser.source_node_id = that_node_id;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
/*
|
||||
* Tx
|
||||
* All Transfers sent have .source_node_id set
|
||||
* to this node's id
|
||||
* During allocation this value will be
|
||||
* 0
|
||||
*/
|
||||
|
||||
protocol->tail_init.u8 |= pdsdl->outtail;
|
||||
protocol->msg.source_node_id = g_this_node_id;
|
||||
|
||||
/*
|
||||
* All Service Transfer sent have
|
||||
* The ser.dest_node_id
|
||||
*/
|
||||
|
||||
if (pdsdl->prototype.msg.service_not_message) {
|
||||
protocol->ser.dest_node_id = that_node_id;
|
||||
}
|
||||
}
|
||||
|
||||
return pdsdl;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: uavcan_tx
|
||||
*
|
||||
* Description:
|
||||
* This function sends a single uavcan protocol based frame applying
|
||||
* the tail byte.
|
||||
*
|
||||
* Input Parameters:
|
||||
* protocol - A pointer to a uavcan_protocol_t to configure the send
|
||||
* frame_data - 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.
|
||||
* length - The number of bytes of the frame_date (DLC field)
|
||||
* mailbox - A can_fifo_mailbox_t MBxxx value to choose the outgoing
|
||||
* mailbox.
|
||||
*
|
||||
* Returned value:
|
||||
* The UavcanOk of the data sent. Anything else indicates if a timeout
|
||||
* occurred.
|
||||
*
|
||||
****************************************************************************/
|
||||
CCASSERT((int)UavcanOk == (int)CAN_OK);
|
||||
static uavcan_error_t uavcan_tx(uavcan_protocol_t *protocol, uint8_t *frame_data,
|
||||
size_t length, uint8_t mailbox)
|
||||
{
|
||||
frame_data[length++] = protocol->tail_init.u8;
|
||||
return can_tx(protocol->id.u32, length, frame_data, mailbox);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: uavcan_tx_dsdl
|
||||
*
|
||||
* Description:
|
||||
* This function sends a uavcan protocol transfer. For a Service
|
||||
* The natural case for a Service Response is to send it to the Requestor
|
||||
*
|
||||
* Therefore, this function assumes that id this is a Service Transfer,
|
||||
* the protocol object has the destination node id set in
|
||||
* protocol->ser.source_node_id.
|
||||
*
|
||||
* Input Parameters:
|
||||
* dsdl - An Uavcan DSDL Identifier (Auto Generated)
|
||||
* protocol - A pointer to a uavcan_protocol_t to configure the send,
|
||||
* For a service transfer the desination node id set in
|
||||
* ser.source_node_id to that of the node we are sending to.
|
||||
* transfer - A pointer to the packed data of the transfer to be sent.
|
||||
* length - The number of bytes of data
|
||||
*
|
||||
* Returned value:
|
||||
* The UavcanOk of the data sent. Anything else indicates if a timeout
|
||||
* occurred.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
uavcan_error_t uavcan_tx_dsdl(uavcan_dsdl_t dsdl, uavcan_protocol_t *protocol,
|
||||
const uint8_t *transfer, size_t transfer_length)
|
||||
{
|
||||
|
||||
/*
|
||||
* Since we do not discriminate between sending a Message or a
|
||||
* Service (Request or Response)
|
||||
* We assume this is a response from a rx request and msg.source_node_id
|
||||
* is that of the requester and that will become the ser.dest_node_id
|
||||
*/
|
||||
const dsdl_t *pdsdl = load_dsdl_protocol(dsdl, OutBound, protocol, protocol->ser.source_node_id);
|
||||
|
||||
uint8_t payload[CanPayloadLength];
|
||||
|
||||
/*
|
||||
* Only prepend CRC if the transfer will not fit within a single frame
|
||||
*/
|
||||
uint32_t m = 0;
|
||||
|
||||
if (transfer_length > MaxUserPayloadLength) {
|
||||
uint16_t transfer_crc = crc16_signature(pdsdl->signature_crc16, transfer_length, transfer);
|
||||
payload[PayloadOffsetCRClsb] = (uint8_t)transfer_crc;
|
||||
payload[PayloadOffsetCRCmsb] = (uint8_t)(transfer_crc >> 8u);
|
||||
m = PayloadOffsetCRCdata;
|
||||
}
|
||||
|
||||
uint32_t i;
|
||||
|
||||
for (i = 0; i < transfer_length; i++) {
|
||||
|
||||
payload[m++] = transfer[i];
|
||||
|
||||
/* Is this the last byte? */
|
||||
|
||||
protocol->tail.eot = (i == (transfer_length - 1));
|
||||
|
||||
/* Either end of user portion of payload or last byte */
|
||||
if (m == MaxUserPayloadLength || protocol->tail.eot) {
|
||||
uavcan_error_t rv = uavcan_tx(protocol, payload, m, pdsdl->mailbox);
|
||||
|
||||
if (rv != UavcanOk) {
|
||||
return rv;
|
||||
}
|
||||
|
||||
/* Increment 1 bit frame sequence number */
|
||||
|
||||
protocol->tail.toggle ^= true;
|
||||
protocol->tail.sot = false;
|
||||
|
||||
m = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return UavcanOk;
|
||||
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: uavcan_rx
|
||||
*
|
||||
* Description:
|
||||
* This function is called to receive a single CAN frame from the supplied
|
||||
* fifo. It does not block if there is not available, but returns 0
|
||||
*
|
||||
* Input Parameters:
|
||||
* protocol - A pointer to a uavcan_protocol_t to return the CAN ID and
|
||||
* Tail byte
|
||||
* frame_data - A pointer to return 8 bytes of the frame's data
|
||||
* (all 8 bytes will be read from the CAN fifo but
|
||||
* only length bytes will valid.
|
||||
* length - A pointer to return the number of bytes of the frame_data
|
||||
* (DLC field)
|
||||
* 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
|
||||
*
|
||||
****************************************************************************/
|
||||
//#define DEBUG_DTID 1
|
||||
#define DEBUG_DTID_TRIGGER 4
|
||||
#ifdef DEBUG_DTID
|
||||
int trigger = DEBUG_DTID_TRIGGER;
|
||||
int id = DEBUG_DTID;
|
||||
bool msg = true;
|
||||
bool svc = false;
|
||||
|
||||
#endif
|
||||
|
||||
static uint8_t uavcan_rx(uavcan_protocol_t *protocol, uint8_t *frame_data,
|
||||
size_t *length, uint8_t fifo)
|
||||
{
|
||||
uint8_t rv = can_rx(&protocol->id.u32, length, frame_data, fifo);
|
||||
|
||||
if (rv) {
|
||||
|
||||
/* Remove the tail byte from length */
|
||||
|
||||
*length -= 1;
|
||||
|
||||
/* Save the tail byte from the last byte of frame*/
|
||||
|
||||
protocol->tail_init.u8 = frame_data[*length];
|
||||
#ifdef DEBUG_DTID
|
||||
#pragma message("!!!!!!! DEBUG_DTID is enabled !!!!")
|
||||
static volatile int count = 0;
|
||||
|
||||
if ((msg && protocol->msg.type_id == id)
|
||||
|| (svc && protocol->ser.type_id == id)
|
||||
) {
|
||||
if (count++ == trigger) {
|
||||
count = 0;
|
||||
static volatile int j = 0;
|
||||
j++;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Name: uavcan_rx_dsdl
|
||||
*
|
||||
* Description:
|
||||
* This function receives a uavcan protocol transfer.
|
||||
* For a Service the natural case for a Service Request is to receive from
|
||||
* 1) Any Node
|
||||
* 2) The established Server.
|
||||
*
|
||||
* Therefore, this function assumes that id this is a Service Transfer,
|
||||
* the protocol object has the destination node id set in
|
||||
* protocol->ser.source_node_id.
|
||||
*
|
||||
*
|
||||
* Input Parameters:
|
||||
* dsdl - An Uavcan DSDL Identifier (Auto Generated)
|
||||
* protocol - A pointer to a uavcan_protocol_t to configure the receive,
|
||||
* based the dsdl for the DTID Service.
|
||||
* If the request must come from a specific server
|
||||
* then protocol->ser.source_node_id, should be set
|
||||
* to that node id;
|
||||
*
|
||||
* in_out_transfer_length - The number of bytes of data to receive and the
|
||||
* number received.
|
||||
* timeout_ms - The amount of time in mS to wait for the initial transfer
|
||||
*
|
||||
* Returned value:
|
||||
* uavcan_error_t
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
uavcan_error_t uavcan_rx_dsdl(uavcan_dsdl_t dsdl, uavcan_protocol_t *protocol,
|
||||
uint8_t *transfer, size_t *in_out_transfer_length,
|
||||
uint32_t timeout_ms)
|
||||
{
|
||||
const dsdl_t *pdsdl = load_dsdl_protocol(dsdl, InBound, protocol, protocol->msg.source_node_id);
|
||||
bl_timer_id timer = timer_allocate(modeTimeout | modeStarted, timeout_ms, 0);
|
||||
|
||||
/*
|
||||
* Set up comparison masks
|
||||
* In the tail we care about the initial state sot, eot and toggle
|
||||
* In the CAN ID We care about the Service Type ID, RequestNotResponse,
|
||||
* ServiceNotMessage, and the destination id
|
||||
* If the source id is not ANY_NODE_ID, that that must match too.
|
||||
*/
|
||||
|
||||
uavcan_protocol_t masks;
|
||||
|
||||
uavcan_init_comapare_masks(Initialization, protocol, &masks);
|
||||
|
||||
uint8_t timeout = false;
|
||||
uint16_t transfer_crc = 0;
|
||||
size_t total_rx = 0;
|
||||
uavcan_protocol_t rx_protocol;
|
||||
|
||||
do {
|
||||
uint8_t payload[CanPayloadLength];
|
||||
size_t rx_length;
|
||||
|
||||
if (!uavcan_rx(&rx_protocol, payload, &rx_length, pdsdl->fifo)
|
||||
|| BadTailState == (rx_protocol.tail_init.u8 & BadTailState)
|
||||
|| ((rx_protocol.id.u32 ^ protocol->id.u32) & masks.id.u32)
|
||||
|| ((rx_protocol.tail_init.u8 ^ protocol->tail_init.u8) & masks.tail_init.u8)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
|
||||
timer_restart(timer, timeout_ms);
|
||||
|
||||
/*
|
||||
If this is the first frame, capture the actual source node ID and
|
||||
transfer ID for future comparisons
|
||||
*/
|
||||
|
||||
size_t payload_index = 0;
|
||||
|
||||
/* Is this the start of transfer? */
|
||||
|
||||
if (rx_protocol.tail.sot) {
|
||||
|
||||
/*
|
||||
* We expect only one Start Of Transfer per transfer
|
||||
* So knock it down
|
||||
*/
|
||||
|
||||
protocol->tail.sot = false;
|
||||
|
||||
/* Discard any data */
|
||||
|
||||
total_rx = 0;
|
||||
|
||||
/*
|
||||
* Capture the source node id
|
||||
* and the Transfer Id
|
||||
*/
|
||||
|
||||
protocol->msg.source_node_id = rx_protocol.msg.source_node_id;
|
||||
protocol->tail.transfer_id = rx_protocol.tail.transfer_id;
|
||||
|
||||
/*
|
||||
* This is the start of transfer - update the
|
||||
* masks for this phase or the transfer were
|
||||
* source_node_id is known and transfer_id
|
||||
* matters. From now on match source node and
|
||||
* transfer ID too
|
||||
*/
|
||||
|
||||
uavcan_init_comapare_masks(OnStartOfTransfer, protocol, &masks);
|
||||
|
||||
|
||||
/* Is this a multi-frame transfer? */
|
||||
|
||||
if (rx_protocol.tail.eot == false) {
|
||||
|
||||
/* Capture the frame CRC */
|
||||
|
||||
transfer_crc = uavcan_make_uint16(payload[PayloadOffsetCRClsb], payload[PayloadOffsetCRCmsb]);
|
||||
|
||||
/*
|
||||
* When the CRC is prepended to the payload
|
||||
* the index of the data is past the CRC
|
||||
*/
|
||||
|
||||
payload_index = PayloadOffsetCRCdata;
|
||||
rx_length -= PayloadOffsetCRCdata;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
if (rx_protocol.tail.transfer_id > protocol->tail.transfer_id
|
||||
|| total_rx >= *in_out_transfer_length) {
|
||||
|
||||
uavcan_init_comapare_masks(Initialization, protocol, &masks);
|
||||
protocol->tail.sot = true;
|
||||
protocol->tail.toggle = false;
|
||||
|
||||
} else if (rx_protocol.tail.transfer_id < protocol->tail.transfer_id) {
|
||||
continue;
|
||||
}
|
||||
|
||||
/* Copy transfer bytes to the transfer */
|
||||
|
||||
if (total_rx + rx_length <= *in_out_transfer_length) {
|
||||
memcpy(&transfer[total_rx], &payload[payload_index], rx_length);
|
||||
total_rx += rx_length;
|
||||
}
|
||||
|
||||
/* Increment 1 bit frame sequence number */
|
||||
|
||||
protocol->tail.toggle ^= true;
|
||||
|
||||
/* Is this the end of the transfer ?*/
|
||||
|
||||
if (rx_protocol.tail.eot) {
|
||||
|
||||
/* Return length of data received */
|
||||
|
||||
*in_out_transfer_length = total_rx;
|
||||
|
||||
break;
|
||||
}
|
||||
} while (!(timeout = timer_expired(timer)));
|
||||
|
||||
timer_free(timer);
|
||||
|
||||
return (!timeout && (rx_protocol.tail.sot
|
||||
|| transfer_crc == crc16_signature(pdsdl->signature_crc16, total_rx, transfer))
|
||||
? UavcanOk : UavcanError);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Name: uavcan_pack_GetNodeInfo_response
|
||||
*
|
||||
* Description:
|
||||
* This function packs the data of a uavcan_NodeStatus_t into
|
||||
* a uavcan_GetNodeInfo_response_t structure as array of bytes.
|
||||
* Then it packs the uavcan_GetNodeInfo_response_t
|
||||
*
|
||||
* Input Parameters:
|
||||
* response The uavcan_GetNodeInfo_response_t to be packed
|
||||
* node_status - The uavcan_NodeStatus_t that is part of the composition
|
||||
*
|
||||
* Returned value:
|
||||
* Number of bytes written.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
size_t uavcan_pack_GetNodeInfo_response(uavcan_GetNodeInfo_response_t *response)
|
||||
{
|
||||
size_t contiguous_length = FixedSizeGetNodeInfo + \
|
||||
response->hardware_version.certificate_of_authenticity_length;
|
||||
/*
|
||||
* Move name so it's contiguous with the end of certificate_of_authenticity[length]
|
||||
* which very well may be just after certificate_of_authenticity_length if the length
|
||||
* of the certificate_of_authenticity is 0
|
||||
*/
|
||||
memcpy(&((uint8_t *)response)[contiguous_length], response->name, response->name_length);
|
||||
return contiguous_length + response->name_length;
|
||||
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: uavcan_pack_LogMessage
|
||||
*
|
||||
* Description:
|
||||
* This function formats the data of a uavcan_logmessage_t structure into
|
||||
* an array of bytes.
|
||||
*
|
||||
* Input Parameters:
|
||||
* external - The array of bytes to populate.
|
||||
* internal - The uavcan_logmessage_t to pack into the data
|
||||
*
|
||||
* Returned value:
|
||||
* Number of bytes written.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static size_t uavcan_pack_LogMessage(uint8_t *external,
|
||||
const uavcan_LogMessage_t *internal)
|
||||
{
|
||||
/* Pack the 3 bit level in top bits followed by the length of source */
|
||||
external[uavcan_byte_offset(LogMessage, level)] = uavcan_ppack(internal, LogMessage, level) \
|
||||
| uavcan_pack(uavcan_byte_count(LogMessage, source), LogMessage, source_length);
|
||||
memcpy(&external[uavcan_byte_offset(LogMessage, source)], internal->source,
|
||||
PackedSizeMsgLogMessage - sizeof_member(uavcan_LogMessage_t, level));
|
||||
return PackedSizeMsgLogMessage;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: uavcan_tx_log_message
|
||||
*
|
||||
* Description:
|
||||
* This functions sends uavcan LogMessage type data. The Source will be
|
||||
* taken from the application defined debug_log_source
|
||||
*
|
||||
* Input Parameters:
|
||||
* level - Log Level of the LogMessage Constants DEBUG, INFO, WARN, ERROR
|
||||
* stage - The Stage the application is at. see Aplication defined
|
||||
* LOGMESSAGE_STAGE_x
|
||||
* status - The status of that stage. See Application defined
|
||||
* LOGMESSAGE_RESULT_x
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
void uavcan_tx_log_message(uavcan_LogMessageConsts_t level, uint8_t stage,
|
||||
uint8_t status)
|
||||
{
|
||||
static uint8_t transfer_id;
|
||||
|
||||
uavcan_LogMessage_t message;
|
||||
uavcan_protocol_t protocol;
|
||||
|
||||
protocol.tail.transfer_id = transfer_id++;
|
||||
|
||||
const dsdl_t *dsdl = load_dsdl_protocol(DSDLMsgLogMessage, MessageOut, &protocol, 0);
|
||||
|
||||
|
||||
message.level = level;
|
||||
memcpy(&message.source, debug_log_source, sizeof(message.source));
|
||||
message.text[0] = stage;
|
||||
message.text[1] = status;
|
||||
|
||||
uint8_t payload[CanPayloadLength];
|
||||
size_t frame_len = uavcan_pack_LogMessage(payload, &message);
|
||||
uavcan_tx(&protocol, payload, frame_len, dsdl->mailbox);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: uavcan_tx_allocation_message
|
||||
*
|
||||
* Description:
|
||||
* This function sends a uavcan allocation message.
|
||||
*
|
||||
* Input Parameters:
|
||||
* requested_node_id - This node's preferred node id 0 for no preference.
|
||||
* unique_id_length - This node's length of it's unique identifier.
|
||||
* unique_id - A pointer to the bytes that represent unique
|
||||
* identifier.
|
||||
* unique_id_offset - The offset equal 0 or the number of bytes in the
|
||||
* the last received message that matched the unique ID
|
||||
* field.
|
||||
* random - The value to use as the discriminator of the
|
||||
* anonymous message
|
||||
*
|
||||
* Returned value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void uavcan_tx_allocation_message(uint8_t requested_node_id,
|
||||
size_t unique_id_length,
|
||||
const uint8_t *unique_id,
|
||||
uint8_t unique_id_offset,
|
||||
uint16_t random)
|
||||
{
|
||||
static uint8_t transfer_id;
|
||||
|
||||
uavcan_protocol_t protocol;
|
||||
|
||||
protocol.tail.transfer_id = transfer_id++;
|
||||
|
||||
const dsdl_t *dsdl = load_dsdl_protocol(DSDLMsgAllocation, MessageOut, &protocol, 0);
|
||||
|
||||
/* Override defaults */
|
||||
|
||||
protocol.ana.discriminator = random;
|
||||
|
||||
uint8_t payload[CanPayloadLength];
|
||||
|
||||
size_t max_copy = MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST;
|
||||
size_t remaining = unique_id_length - unique_id_offset;
|
||||
|
||||
if (max_copy > remaining) {
|
||||
max_copy = remaining;
|
||||
}
|
||||
|
||||
payload[uavcan_byte_offset(Allocation, node_id)] = uavcan_pack(requested_node_id, Allocation, node_id) |
|
||||
(unique_id_offset ? 0 : uavcan_bit_mask(Allocation, first_part_of_unique_id));
|
||||
/*
|
||||
* Copy in the remaining bytes of payload, either filling it
|
||||
* or on the last chunk partially filling it
|
||||
*/
|
||||
memcpy(&payload[uavcan_byte_offset(Allocation, unique_id)], &unique_id[unique_id_offset], max_copy);
|
||||
|
||||
/* Account for the payload[0] */
|
||||
max_copy++;
|
||||
uavcan_tx(&protocol, payload, max_copy, dsdl->mailbox);
|
||||
can_cancel_on_error(dsdl->mailbox);
|
||||
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data - This table is positioned here to not mess up the line
|
||||
* numbers for the debugger.
|
||||
****************************************************************************/
|
||||
#define NA 0
|
||||
#define END_COMPONENTS
|
||||
|
||||
#define UAVCAN_DSDL_BIT_DEF(data_typ_name, field_name, lsb_pos, length, payload_offset, payload_length)
|
||||
|
||||
#define UAVCAN_DSDL_MESG_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \
|
||||
{ \
|
||||
{\
|
||||
.msg = \
|
||||
{ \
|
||||
.source_node_id = 0, \
|
||||
.service_not_message = (false), \
|
||||
.type_id = (dtid), \
|
||||
.priority = 0, \
|
||||
}, \
|
||||
{ \
|
||||
.tail_init = \
|
||||
{ \
|
||||
.u8 = (outbound), \
|
||||
} \
|
||||
} \
|
||||
}, \
|
||||
signature, \
|
||||
inbound, \
|
||||
outbound, \
|
||||
mailbox, \
|
||||
fifo, \
|
||||
},
|
||||
|
||||
#define UAVCAN_DSDL_SREQ_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \
|
||||
{ \
|
||||
{\
|
||||
.ser = \
|
||||
{ \
|
||||
.source_node_id = 0, \
|
||||
.service_not_message = (true), \
|
||||
.dest_node_id = 0, \
|
||||
.request_not_response = (true), \
|
||||
.type_id = (dtid), \
|
||||
.priority = 0, \
|
||||
}, \
|
||||
{ \
|
||||
.tail_init = \
|
||||
{ \
|
||||
.u8 = (outbound), \
|
||||
} \
|
||||
} \
|
||||
}, \
|
||||
signature, \
|
||||
inbound, \
|
||||
outbound, \
|
||||
mailbox, \
|
||||
fifo, \
|
||||
},
|
||||
|
||||
#define UAVCAN_DSDL_SRSP_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound) \
|
||||
{ \
|
||||
{\
|
||||
.ser = \
|
||||
{ \
|
||||
.source_node_id = 0, \
|
||||
.service_not_message = (true), \
|
||||
.dest_node_id = 0, \
|
||||
.request_not_response = (false), \
|
||||
.type_id = (dtid), \
|
||||
.priority = 0, \
|
||||
}, \
|
||||
{ \
|
||||
.tail_init = \
|
||||
{ \
|
||||
.u8 = (outbound), \
|
||||
} \
|
||||
} \
|
||||
}, \
|
||||
signature, \
|
||||
inbound, \
|
||||
outbound, \
|
||||
mailbox, \
|
||||
fifo, \
|
||||
},
|
||||
|
||||
|
||||
#define UAVCAN_DSDL_TYPE_DEF(name, dtid, signature, packsize, mailbox, fifo, inbound, outbound)
|
||||
|
||||
const dsdl_t dsdl_table[sizeDSDLTransfers] = {
|
||||
#include "uavcan_dsdl_defs.h"
|
||||
};
|
||||
#undef UAVCAN_DSDL_TYPE_DEF
|
||||
#undef UAVCAN_DSDL_SRSP_DEF
|
||||
#undef UAVCAN_DSDL_SREQ_DEF
|
||||
#undef UAVCAN_DSDL_MESG_DEF
|
||||
#undef UAVCAN_DSDL_BIT_DEF
|
||||
#undef FW_SERVER
|
||||
#undef REQ_NODE
|
||||
#undef THIS_NODE
|
||||
#undef ANY_NODE_ID
|
||||
#undef END_COMPONENTS
|
||||
#undef NA
|
|
@ -0,0 +1,491 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
// Turn off Probes in this module
|
||||
#undef CONFIG_BOARD_USE_PROBES
|
||||
|
||||
#include <boot_config.h>
|
||||
|
||||
#include <systemlib/visibility.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <nuttx/arch.h>
|
||||
#include <arch/irq.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include "px4_macros.h"
|
||||
#include "timer.h"
|
||||
#include "nvic.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Types
|
||||
****************************************************************************/
|
||||
|
||||
typedef enum {
|
||||
OneShot = modeOneShot,
|
||||
Repeating = modeRepeating,
|
||||
Timeout = modeTimeout,
|
||||
|
||||
modeMsk = 0x3 ,
|
||||
running = modeStarted,
|
||||
inuse = 0x80,
|
||||
|
||||
} bl_timer_ctl_t;
|
||||
|
||||
typedef struct {
|
||||
bl_timer_cb_t usr;
|
||||
time_ms_t count;
|
||||
time_ms_t reload;
|
||||
bl_timer_ctl_t ctl;
|
||||
} bl_timer_t;
|
||||
|
||||
/****************************************************************************
|
||||
* Private Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
|
||||
static time_ms_t sys_tic;
|
||||
static bl_timer_t timers[OPT_BL_NUMBER_TIMERS];
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
|
||||
/* Use to initialize */
|
||||
|
||||
const bl_timer_cb_t null_cb = { 0, 0 };
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/* We use the linker --wrap ability to wrap the NuttX stm32 call out to
|
||||
* the sceduler's sched_process_timer and service it here. Thus replacing
|
||||
* the NuttX scheduler with are timer driven scheduling.
|
||||
*/
|
||||
void __wrap_sched_process_timer(void);
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Name: timer_tic
|
||||
*
|
||||
* Description:
|
||||
* Returns the system tic counter that counts in units of
|
||||
* (CONFIG_USEC_PER_TICK/1000). By default 10 Ms.
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
time_ms_t timer_tic(void)
|
||||
{
|
||||
return sys_tic;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: sched_process_timer
|
||||
*
|
||||
* Description:
|
||||
* Called by Nuttx on the ISR of the SysTic. This function run the list of
|
||||
* timers. It deducts that amount of the time of a system tick from the
|
||||
* any timers that are in use and running.
|
||||
*
|
||||
* Depending on the mode of the timer, the appropriate actions is taken on
|
||||
* expiration.
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
__EXPORT
|
||||
void __wrap_sched_process_timer(void)
|
||||
{
|
||||
PROBE(1, true);
|
||||
PROBE(1, false);
|
||||
|
||||
/* Increment the per-tick system counter */
|
||||
|
||||
sys_tic++;
|
||||
|
||||
|
||||
/* todo:May need a X tick here is threads run long */
|
||||
|
||||
time_ms_t ms_elapsed = (CONFIG_USEC_PER_TICK / 1000);
|
||||
|
||||
/* Walk the time list from High to low and */
|
||||
|
||||
bl_timer_id t;
|
||||
|
||||
for (t = arraySize(timers) - 1; (int8_t) t >= 0; t--) {
|
||||
|
||||
|
||||
/* Timer in use and running */
|
||||
|
||||
if ((timers[t].ctl & (inuse | running)) == (inuse | running)) {
|
||||
|
||||
/* Is it NOT already expired nor about to expire ?*/
|
||||
|
||||
if (timers[t].count != 0) {
|
||||
|
||||
/* Is it off in future */
|
||||
if (timers[t].count > ms_elapsed) {
|
||||
|
||||
/* Just remove the amount attributed to the tick */
|
||||
|
||||
timers[t].count -= ms_elapsed;
|
||||
continue;
|
||||
}
|
||||
|
||||
/* it has expired now or less than a tick ago */
|
||||
|
||||
/* Mark it expired */
|
||||
|
||||
timers[t].count = 0;
|
||||
|
||||
/* Now perform action based on mode */
|
||||
|
||||
switch (timers[t].ctl & ~(inuse | running)) {
|
||||
|
||||
case OneShot: {
|
||||
bl_timer_cb_t user = timers[t].usr;
|
||||
memset(&timers[t], 0, sizeof(timers[t]));
|
||||
|
||||
if (user.cb) {
|
||||
user.cb(t, user.context);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case Repeating:
|
||||
timers[t].count = timers[t].reload;
|
||||
|
||||
/* fall through to callback */
|
||||
case Timeout:
|
||||
if (timers[t].usr.cb) {
|
||||
timers[t].usr.cb(t, timers[t].usr.context);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_allocate
|
||||
*
|
||||
* Description:
|
||||
* Is used to allocate a timer. Allocation does not involve memory
|
||||
* allocation as the data for the timer are compile time generated.
|
||||
* See OPT_BL_NUMBER_TIMERS
|
||||
*
|
||||
* There is an inherent priority to the timers in that the first timer
|
||||
* allocated is the first timer run per tick.
|
||||
*
|
||||
* There are 3 modes of operation for the timers. All modes support an
|
||||
* optional call back on expiration.
|
||||
*
|
||||
* modeOneShot - Specifies a one-shot timer. After notification timer
|
||||
* is resource is freed.
|
||||
* modeRepeating - Specifies a repeating timer that will reload and
|
||||
* call an optional.
|
||||
* modeTimeout - Specifies a persistent start / stop timer.
|
||||
*
|
||||
* modeStarted - Or'ed in to start the timer when allocated
|
||||
*
|
||||
*
|
||||
* Input Parameters:
|
||||
* mode - One of bl_timer_modes_t with the Optional modeStarted
|
||||
* msfromnow - The reload and initial value for the timer in Ms.
|
||||
* fc - A pointer or NULL (0). If it is non null it can be any
|
||||
* of the following:
|
||||
*
|
||||
* a) A bl_timer_cb_t populated on the users stack or
|
||||
* in the data segment. The values are copied into the
|
||||
* internal data structure of the timer and therefore do
|
||||
* not have to persist after the call to timer_allocate
|
||||
*
|
||||
* b) The address of null_cb. This is identical to passing
|
||||
* null for the value of fc.
|
||||
*
|
||||
* Returned Value:
|
||||
* On success a value from 0 - OPT_BL_NUMBER_TIMERS-1 that is
|
||||
* the bl_timer_id for subsequent timer operations
|
||||
* -1 on failure. This indicates there are no free timers.
|
||||
*
|
||||
****************************************************************************/
|
||||
bl_timer_id timer_allocate(bl_timer_modes_t mode, time_ms_t msfromnow,
|
||||
bl_timer_cb_t *fc)
|
||||
{
|
||||
bl_timer_id t;
|
||||
irqstate_t s = enter_critical_section();
|
||||
|
||||
for (t = arraySize(timers) - 1; (int8_t)t >= 0; t--) {
|
||||
|
||||
if ((timers[t].ctl & inuse) == 0) {
|
||||
|
||||
timers[t].reload = msfromnow;
|
||||
timers[t].count = msfromnow;
|
||||
timers[t].usr = fc ? *fc : null_cb;
|
||||
timers[t].ctl = (mode & (modeMsk | running)) | (inuse);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
leave_critical_section(s);
|
||||
return t;
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_free
|
||||
*
|
||||
* Description:
|
||||
* Is used to free a timer. Freeing a timer does not involve memory
|
||||
* deallocation as the data for the timer are compile time generated.
|
||||
* See OPT_BL_NUMBER_TIMERS
|
||||
*
|
||||
* Input Parameters:
|
||||
* id - Returned from timer_allocate;
|
||||
*
|
||||
* Returned Value:
|
||||
* None.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void timer_free(bl_timer_id id)
|
||||
{
|
||||
DEBUGASSERT(id >= 0 && id < arraySize(timers));
|
||||
irqstate_t s = enter_critical_section();
|
||||
memset(&timers[id], 0, sizeof(timers[id]));
|
||||
leave_critical_section(s);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_start
|
||||
*
|
||||
* Description:
|
||||
* Is used to Start a timer. The reload value is copied to the counter.
|
||||
* And the running bit it set. There is no problem in Starting a running
|
||||
* timer. But it will restart the timeout.
|
||||
*
|
||||
* Input Parameters:
|
||||
* id - Returned from timer_allocate;
|
||||
*
|
||||
* Returned Value:
|
||||
* None.
|
||||
*
|
||||
****************************************************************************/
|
||||
void timer_start(bl_timer_id id)
|
||||
{
|
||||
DEBUGASSERT(id >= 0 && id < arraySize(timers) && (timers[id].ctl & inuse));
|
||||
irqstate_t s = enter_critical_section();
|
||||
timers[id].count = timers[id].reload;
|
||||
timers[id].ctl |= running;
|
||||
leave_critical_section(s);
|
||||
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_stop
|
||||
*
|
||||
* Description:
|
||||
* Is used to stop a timer. It is Ok to stop a stopped timer.
|
||||
*
|
||||
* Input Parameters:
|
||||
* id - Returned from timer_allocate;
|
||||
*
|
||||
* Returned Value:
|
||||
* None.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void timer_stop(bl_timer_id id)
|
||||
{
|
||||
DEBUGASSERT(id >= 0 && id < arraySize(timers) && (timers[id].ctl & inuse));
|
||||
irqstate_t s = enter_critical_section();
|
||||
timers[id].ctl &= ~running;
|
||||
leave_critical_section(s);
|
||||
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_expired
|
||||
*
|
||||
* Description:
|
||||
* Test if a timer that was configured as a modeTimeout timer is expired.
|
||||
* To be expired the time has to be running and have a count of 0.
|
||||
*
|
||||
* Input Parameters:
|
||||
* id - Returned from timer_allocate;
|
||||
*
|
||||
* Returned Value:
|
||||
* Non Zero if the timer is expired otherwise zero.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
int timer_expired(bl_timer_id id)
|
||||
{
|
||||
DEBUGASSERT(id >= 0 && id < arraySize(timers) && (timers[id].ctl & inuse));
|
||||
irqstate_t s = enter_critical_section();
|
||||
int rv = ((timers[id].ctl & running) && timers[id].count == 0);
|
||||
leave_critical_section(s);
|
||||
return rv;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_restart
|
||||
*
|
||||
* Description:
|
||||
* Is used to re start a timer with a new reload count. The reload value
|
||||
* is copied to the counter and the running bit it set. There is no
|
||||
* problem in restarting a running timer.
|
||||
*
|
||||
* Input Parameters:
|
||||
* id - Returned from timer_allocate;
|
||||
* ms - Is a time_ms_t and the new reload value to use.
|
||||
*
|
||||
* Returned Value:
|
||||
* None.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void timer_restart(bl_timer_id id, time_ms_t ms)
|
||||
{
|
||||
DEBUGASSERT(id >= 0 && id < arraySize(timers) && (timers[id].ctl & inuse));
|
||||
irqstate_t s = enter_critical_section();
|
||||
timers[id].count = timers[id].reload = ms;
|
||||
timers[id].ctl |= running;
|
||||
leave_critical_section(s);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_ref
|
||||
*
|
||||
* Description:
|
||||
* Returns an time_ref_t that is a reference (ponter) to the internal counter
|
||||
* of the timer selected by id. It should only be used with calls to
|
||||
* timer_ref_expired.
|
||||
*
|
||||
* Input Parameters:
|
||||
* id - Returned from timer_allocate;
|
||||
*
|
||||
* Returned Value:
|
||||
* An internal reference that should be treated as opaque by the caller and
|
||||
* should only be used with calls to timer_ref_expired.
|
||||
* There is no reference counting on the reference and therefore does not
|
||||
* require any operation to free it.
|
||||
*
|
||||
*************************************************************************/
|
||||
|
||||
time_ref_t timer_ref(bl_timer_id id)
|
||||
{
|
||||
DEBUGASSERT(id >= 0 && id < arraySize(timers) && (timers[id].ctl & inuse));
|
||||
return (time_ref_t) &timers[id].count;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: timer_init
|
||||
*
|
||||
* Description:
|
||||
* Called early in os_start to initialize the data associated with
|
||||
* the timers
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
__EXPORT
|
||||
void timer_init(void)
|
||||
{
|
||||
/* For system timing probing see bord.h and
|
||||
* CONFIG_BOARD_USE_PROBES
|
||||
*/
|
||||
PROBE_INIT(7);
|
||||
PROBE(1, true);
|
||||
PROBE(2, true);
|
||||
PROBE(3, true);
|
||||
PROBE(1, false);
|
||||
PROBE(2, false);
|
||||
PROBE(3, false);
|
||||
/* This is the lowlevel IO if needed to instrument timing
|
||||
* with the smallest impact
|
||||
* *((uint32_t *)0x40011010) = 0x100; // PROBE(3,true);
|
||||
* *((uint32_t *)0x40011014) = 0x100; // PROBE(3,false);
|
||||
*/
|
||||
|
||||
/* Initialize timer data */
|
||||
|
||||
sys_tic = 0;
|
||||
memset(timers, 0, sizeof(timers));
|
||||
}
|
File diff suppressed because it is too large
Load Diff
|
@ -0,0 +1,174 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include "crc.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Types
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: crc16_add
|
||||
*
|
||||
* Description:
|
||||
* Use to calculates a CRC-16-CCITT using the polynomial of
|
||||
* 0x1021 by adding a value successive values.
|
||||
*
|
||||
* Input Parameters:
|
||||
* crc - The running total of the crc 16
|
||||
* value - The value to add
|
||||
*
|
||||
* Returned Value:
|
||||
* The current crc16 with the value processed.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
uint16_t crc16_add(uint16_t crc, uint8_t value)
|
||||
{
|
||||
uint32_t i;
|
||||
const uint16_t poly = 0x1021u;
|
||||
crc ^= (uint16_t)((uint16_t) value << 8u);
|
||||
|
||||
for (i = 0; i < 8; i++) {
|
||||
if (crc & (1u << 15u)) {
|
||||
crc = (uint16_t)((crc << 1u) ^ poly);
|
||||
|
||||
} else {
|
||||
crc = (uint16_t)(crc << 1u);
|
||||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: crc16_signature
|
||||
*
|
||||
* Description:
|
||||
* Calculates a CRC-16-CCITT using the crc16_add
|
||||
* function
|
||||
*
|
||||
* Input Parameters:
|
||||
* initial - The Initial value to uses as the crc's starting point
|
||||
* length - The number of bytes to add to the crc
|
||||
* bytes - A pointer to any array of length bytes
|
||||
*
|
||||
* Returned Value:
|
||||
* The crc16 of the array of bytes
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
uint16_t crc16_signature(uint16_t initial, size_t length,
|
||||
const uint8_t *bytes)
|
||||
{
|
||||
size_t i;
|
||||
|
||||
for (i = 0u; i < length; i++) {
|
||||
initial = crc16_add(initial, bytes[i]);
|
||||
}
|
||||
|
||||
return initial ^ CRC16_OUTPUT_XOR;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: crc64_add_word
|
||||
*
|
||||
* Description:
|
||||
* Calculates a CRC-64-WE using the polynomial of 0x42F0E1EBA9EA3693
|
||||
* See http://reveng.sourceforge.net/crc-catalogue/17plus.htm#crc.cat-bits.64
|
||||
* Check: 0x62EC59E3F1A4F00A
|
||||
*
|
||||
* Input Parameters:
|
||||
* crc - The running total of the crc 64
|
||||
* value - The value to add
|
||||
*
|
||||
* Returned Value:
|
||||
* The current crc64 with the value processed.
|
||||
*
|
||||
****************************************************************************/
|
||||
__EXPORT
|
||||
uint64_t crc64_add_word(uint64_t crc, uint32_t value)
|
||||
{
|
||||
uint32_t i, j;
|
||||
uint8_t byte;
|
||||
const uint64_t poly = 0x42F0E1EBA9EA3693ull;
|
||||
|
||||
for (j = 0; j < 4; j++) {
|
||||
byte = ((uint8_t *) &value)[j];
|
||||
crc ^= (uint64_t) byte << 56u;
|
||||
|
||||
for (i = 0; i < 8; i++) {
|
||||
if (crc & (1ull << 63u)) {
|
||||
crc = (uint64_t)(crc << 1u) ^ poly;
|
||||
|
||||
} else {
|
||||
crc = (uint64_t)(crc << 1u);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
|
@ -0,0 +1,115 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <stdint.h>
|
||||
#include "random.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
#define RAND_MAX_32 ((1U << 31) - 1)
|
||||
#define RAND_MAX ((1U << 15) - 1)
|
||||
|
||||
/****************************************************************************
|
||||
* Private Types
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
static uint32_t rseed;
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
* Name: util_srand
|
||||
*
|
||||
* Description:
|
||||
* This function seeds the random number generator
|
||||
*
|
||||
*
|
||||
* Input Parameters:
|
||||
* seed - The seed
|
||||
*
|
||||
* Returned value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
void util_srand(uint16_t seed)
|
||||
{
|
||||
rseed = seed;
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Name: util_random
|
||||
*
|
||||
* Description:
|
||||
* This function returns a random number between min and max
|
||||
*
|
||||
*
|
||||
* Input Parameters:
|
||||
* min - The minimum value the return value can be.
|
||||
* max - The maximum value the return value can be.
|
||||
*
|
||||
* Returned value:
|
||||
* A random number
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
uint16_t util_random(uint16_t min, uint16_t max)
|
||||
{
|
||||
uint16_t rand = (rseed = (rseed * 214013 + 2531011) & RAND_MAX_32) >> 16;
|
||||
return rand % (max - min) + min;
|
||||
}
|
Loading…
Reference in New Issue