Skip to content

Commit 9835a63

Browse files
Ar-Ray-codeAr-Ray-code
committed
Add PAA5160E1 driver and bringup packages with initial implementation
1 parent a34e44e commit 9835a63

File tree

15 files changed

+587
-0
lines changed

15 files changed

+587
-0
lines changed

‎firmware/paa5160e1.ino‎

Lines changed: 98 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,98 @@
1+
/*
2+
* Copyright (c) 2025 NITK.K ROS-Team
3+
*
4+
* SPDX-License-Identifier: Apache-2.0
5+
*/
6+
7+
#include <Arduino.h>
8+
9+
#include "SparkFun_Qwiic_OTOS_Arduino_Library.h"
10+
#include "Wire.h"
11+
12+
#include <CRC8.h> // Arduino CRC library (Rob Tillaart)
13+
14+
QwiicOTOS myOtos;
15+
const int ledPin = 13;
16+
17+
static const char SERIAL_HEADER = 'X';
18+
19+
union float2byte {
20+
float f;
21+
uint8_t b[4];
22+
};
23+
24+
static void get_time(int32_t &sec, int32_t &msec)
25+
{
26+
unsigned long ms = millis();
27+
sec = ms / 1000;
28+
msec = ms % 1000;
29+
}
30+
31+
void setup()
32+
{
33+
pinMode(ledPin, OUTPUT);
34+
35+
Serial.begin(115200);
36+
Wire.begin();
37+
38+
// Attempt to begin the sensor
39+
while (myOtos.begin() == false)
40+
{
41+
delay(1000);
42+
}
43+
44+
myOtos.calibrateImu();
45+
myOtos.resetTracking();
46+
}
47+
48+
void loop()
49+
{
50+
while (Serial.available())
51+
{
52+
int ch = Serial.read();
53+
if (ch == 'r' || ch == 'R')
54+
{
55+
myOtos.resetTracking();
56+
digitalWrite(ledPin, LOW);
57+
delay(30);
58+
digitalWrite(ledPin, HIGH);
59+
}
60+
}
61+
62+
sfe_otos_pose2d_t myPosition;
63+
myOtos.getPosition(myPosition);
64+
65+
// Send binary packet: 'X' header, sec, msec, x, y, yaw, CRC8, then newline
66+
{
67+
int32_t sec = 0, msec = 0;
68+
get_time(sec, msec);
69+
70+
float2byte fx, fy, fyaw;
71+
fx.f = myPosition.x;
72+
fy.f = myPosition.y;
73+
fyaw.f = myPosition.h;
74+
75+
CRC8 crc(CRC8_DALLAS_MAXIM_POLYNOME,
76+
CRC8_DALLAS_MAXIM_INITIAL,
77+
CRC8_DALLAS_MAXIM_XOR_OUT,
78+
CRC8_DALLAS_MAXIM_REV_IN,
79+
CRC8_DALLAS_MAXIM_REV_OUT);
80+
crc.restart();
81+
crc.add((uint8_t)SERIAL_HEADER);
82+
crc.add(reinterpret_cast<const uint8_t*>(&sec), 4);
83+
crc.add(reinterpret_cast<const uint8_t*>(&msec), 4);
84+
crc.add(fx.b, 4);
85+
crc.add(fy.b, 4);
86+
crc.add(fyaw.b, 4);
87+
uint8_t crc8 = crc.calc();
88+
89+
Serial.write(static_cast<uint8_t>(SERIAL_HEADER));
90+
Serial.write(reinterpret_cast<uint8_t*>(&sec), 4);
91+
Serial.write(reinterpret_cast<uint8_t*>(&msec), 4);
92+
Serial.write(fx.b, 4);
93+
Serial.write(fy.b, 4);
94+
Serial.write(fyaw.b, 4);
95+
Serial.write(crc8);
96+
Serial.write('\n');
97+
}
98+
}

‎paa5160e1_driver/CMakeLists.txt‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(paa5160e1_driver)
3+
4+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
find_package(ament_cmake REQUIRED)
9+
10+
if(BUILD_TESTING)
11+
find_package(ament_lint_auto REQUIRED)
12+
set(ament_cmake_copyright_FOUND TRUE)
13+
set(ament_cmake_cpplint_FOUND TRUE)
14+
ament_lint_auto_find_test_dependencies()
15+
endif()
16+
17+
ament_package()
18+

‎paa5160e1_driver/package.xml‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>paa5160e1_driver</name>
5+
<version>0.0.0</version>
6+
<description>paa5160e1_driver Base Package</description>
7+
<maintainer email="ray255ar@gmail.com">Ar-Ray-code</maintainer>
8+
<license>Apache License 2.0</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
12+
<depend>paa5160e1_driver_bringup</depend>
13+
<depend>paa5160e1_driver_node</depend>
14+
15+
<test_depend>ament_lint_auto</test_depend>
16+
<test_depend>ament_lint_common</test_depend>
17+
18+
<export>
19+
<build_type>ament_cmake</build_type>
20+
</export>
21+
</package>
22+

‎paa5160e1_driver_bringup/launch/paa5160e1.launch.py‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
from launch import LaunchDescription
2+
from launch_ros.actions import Node
3+
4+
5+
def generate_launch_description():
6+
node = Node(
7+
package='paa5160e1_driver_node',
8+
executable='paa5160e1_driver_node_exec',
9+
name='paa5160e1_driver_node',
10+
output='screen',
11+
parameters=[{
12+
'dev': '/dev/ttyACM0',
13+
'baudrate': 115200,
14+
'timeout_ms': 100,
15+
'spin_ms': 1,
16+
'odom_frame_id': 'odom',
17+
'base_frame_id': 'base_link',
18+
'device_frame_id': 'device',
19+
}]
20+
)
21+
22+
return LaunchDescription([node])

‎paa5160e1_driver_bringup/paa5160e1_driver_bringup/__init__.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
# empty package marker

‎paa5160e1_driver_bringup/package.xml‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>paa5160e1_driver_bringup</name>
5+
<version>0.0.0</version>
6+
<description>Bringup for PAA5160E1 driver</description>
7+
<maintainer email="ray255ar@gmail.com">Ar-Ray-code</maintainer>
8+
<license>Apache-2.0</license>
9+
10+
<exec_depend>launch</exec_depend>
11+
<exec_depend>launch_ros</exec_depend>
12+
<exec_depend>paa5160e1_driver_node</exec_depend>
13+
14+
<export>
15+
<build_type>ament_python</build_type>
16+
</export>
17+
</package>

‎paa5160e1_driver_bringup/resource/paa5160e1_driver_bringup‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
paa5160e1_driver_bringup

‎paa5160e1_driver_bringup/setup.cfg‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
[develop]
2+
script_dir=$base/lib/paa5160e1_driver_bringup
3+
[install]
4+
install_scripts=$base/lib/paa5160e1_driver_bringup
5+

‎paa5160e1_driver_bringup/setup.py‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
from setuptools import setup
2+
3+
package_name = 'paa5160e1_driver_bringup'
4+
5+
setup(
6+
name=package_name,
7+
version='0.0.0',
8+
packages=[package_name],
9+
data_files=[
10+
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
11+
('share/' + package_name, ['package.xml']),
12+
('share/' + package_name + '/launch', ['launch/paa5160e1.launch.py']),
13+
],
14+
install_requires=['setuptools'],
15+
zip_safe=True,
16+
maintainer='Ar-Ray-code',
17+
maintainer_email='ray255ar@gmail.com',
18+
description='Bringup launch for PAA5160E1 driver',
19+
license='Apache-2.0',
20+
tests_require=['pytest'],
21+
)
22+

‎paa5160e1_driver_node/CMakeLists.txt‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
# Copyright (c) 2025 NITK.K ROS-Team.
2+
# SPDX-License-Identifier: Apache-2.0
3+
4+
cmake_minimum_required(VERSION 3.8)
5+
project(paa5160e1_driver_node)
6+
7+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
8+
add_compile_options(-Wall -Wextra -Wpedantic)
9+
endif()
10+
11+
find_package(ament_cmake_auto REQUIRED)
12+
ament_auto_find_build_dependencies()
13+
14+
include_directories(include)
15+
16+
set(TARGET paa5160e1_driver_node)
17+
set(MY_LIB_NAME ${PROJECT_NAME}_${TARGET})
18+
ament_auto_add_library(${MY_LIB_NAME} SHARED
19+
src/${TARGET}.cpp
20+
src/odom_class.cpp
21+
)
22+
23+
rclcpp_components_register_node(
24+
${MY_LIB_NAME}
25+
PLUGIN "${PROJECT_NAME}::Paa5160e1DriverNode"
26+
EXECUTABLE ${TARGET}_exec)
27+
28+
if(BUILD_TESTING)
29+
find_package(ament_lint_auto REQUIRED)
30+
set(ament_cmake_copyright_FOUND TRUE)
31+
set(ament_cmake_cpplint_FOUND TRUE)
32+
ament_lint_auto_find_test_dependencies()
33+
endif()
34+
35+
ament_auto_package()

0 commit comments

Comments
 (0)