Skip to content

Commit 2eab16e

Browse files
jgoppertbperseghettichamdanedu
committed
Add attitude controller.
Co-authored-by: Benjamin Perseghetti <bperseghetti@rudislabs.com> Co-authored-by: Chase Hamdan <chamdan@purdue.edu> Signed-off-by: James Goppert <james.goppert@gmail.com>
1 parent 7bfe422 commit 2eab16e

29 files changed

+1924
-3186
lines changed

.gitignore

+1
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
# editors
2+
.gdbinit
23
.vscode
34
*.swp
45
*~

app/rdd2/CMakeLists.txt

+10-5
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@ find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
1515

1616
project(rdd2 LANGUAGES C)
1717

18-
set(SOURCE_FILES src/main.c)
18+
set(SOURCE_FILES src/boot_banner.c)
1919

2020
if (CONFIG_CEREBRI_RDD2_FSM)
2121
list(APPEND SOURCE_FILES src/fsm.c)
@@ -25,8 +25,8 @@ if (CONFIG_CEREBRI_RDD2_ESTIMATE)
2525
list(APPEND SOURCE_FILES src/estimate.c)
2626
endif()
2727

28-
if (CONFIG_CEREBRI_RDD2_MIXING)
29-
list(APPEND SOURCE_FILES src/mixing.c)
28+
if (CONFIG_CEREBRI_RDD2_ALLOCATION)
29+
list(APPEND SOURCE_FILES src/allocation.c)
3030
endif()
3131

3232
if (CONFIG_CEREBRI_RDD2_LIGHTING)
@@ -42,9 +42,14 @@ if (CONFIG_CEREBRI_RDD2_POSITION)
4242
src/position.c)
4343
endif()
4444

45-
if (CONFIG_CEREBRI_RDD2_VELOCITY)
45+
if (CONFIG_CEREBRI_RDD2_ANGULAR_VELOCITY)
4646
list(APPEND SOURCE_FILES
47-
src/velocity.c)
47+
src/angular_velocity.c)
48+
endif()
49+
50+
if (CONFIG_CEREBRI_RDD2_ATTITUDE)
51+
list(APPEND SOURCE_FILES
52+
src/attitude.c)
4853
endif()
4954

5055
if (CONFIG_CEREBRI_RDD2_CASADI)

app/rdd2/Kconfig

+11-22
Original file line numberDiff line numberDiff line change
@@ -22,11 +22,11 @@ config CEREBRI_RDD2_LIGHTING
2222

2323
config CEREBRI_RDD2_MANUAL
2424
bool "enable manual"
25-
depends on CEREBRI_RDD2_MIXING
25+
depends on CEREBRI_RDD2_ALLOCATION
2626
help
2727
Enable manual
2828

29-
config CEREBRI_RDD2_MIXING
29+
config CEREBRI_RDD2_ALLOCATION
3030
bool "enable mixing"
3131
help
3232
Enable mixing
@@ -37,13 +37,20 @@ config CEREBRI_RDD2_POSITION
3737
help
3838
Enable position
3939

40-
config CEREBRI_RDD2_VELOCITY
40+
config CEREBRI_RDD2_ANGULAR_VELOCITY
4141
bool "enable velocity"
42-
depends on CEREBRI_RDD2_MIXING
42+
depends on CEREBRI_RDD2_ALLOCATION
4343
depends on CEREBRI_RDD2_CASADI
4444
help
4545
Enable velocity
4646

47+
config CEREBRI_RDD2_ATTITUDE
48+
bool "enable attitude"
49+
depends on CEREBRI_RDD2_ANGULAR_VELOCITY
50+
depends on CEREBRI_RDD2_CASADI
51+
help
52+
Enable attitude
53+
4754
config CEREBRI_RDD2_CASADI
4855
bool "enable casadi code"
4956
help
@@ -85,30 +92,12 @@ config CEREBRI_RDD2_GAIN_ALONG_TRACK
8592
help
8693
Steering = Along track error * GAIN / 1000
8794

88-
config CEREBRI_RDD2_MAX_TURN_ANGLE_MRAD
89-
int "max turn angle mrad"
90-
default 400
91-
help
92-
Max turn angle in milli-radians
93-
9495
config CEREBRI_RDD2_MAX_VELOCITY_MM_S
9596
int "max velocity, mm/s"
9697
default 1000
9798
help
9899
Max velocity in mm/s
99100

100-
config CEREBRI_RDD2_WHEEL_RADIUS_MM
101-
int "wheel radius, mm"
102-
default 37
103-
help
104-
Wheel radius in mm
105-
106-
config CEREBRI_RDD2_WHEEL_BASE_MM
107-
int "wheel base, mm"
108-
default 226
109-
help
110-
Wheel radius in mm
111-
112101
module = CEREBRI_RDD2
113102
module-str = cerebri_rdd2
114103
source "subsys/logging/Kconfig.template.log_config"

app/rdd2/boards/native_sim.conf

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
CONFIG_ETH_NATIVE_POSIX=y
22
CONFIG_TEST_RANDOM_GENERATOR=y
3-
CONFIG_SYS_CLOCK_TICKS_PER_SEC=200
3+
CONFIG_SYS_CLOCK_TICKS_PER_SEC=1000
44
CONFIG_NATIVE_POSIX_SLOWDOWN_TO_REAL_TIME=n
55

66
CONFIG_NO_OPTIMIZATIONS=y

app/rdd2/prj.conf

+4-2
Original file line numberDiff line numberDiff line change
@@ -8,9 +8,10 @@ CONFIG_CEREBRI_DREAM_SIL=n
88
CONFIG_CEREBRI_RDD2_ESTIMATE=y
99
CONFIG_CEREBRI_RDD2_FSM=y
1010
CONFIG_CEREBRI_RDD2_MANUAL=y
11-
CONFIG_CEREBRI_RDD2_MIXING=y
11+
CONFIG_CEREBRI_RDD2_ALLOCATION=y
1212
CONFIG_CEREBRI_RDD2_POSITION=y
13-
CONFIG_CEREBRI_RDD2_VELOCITY=y
13+
CONFIG_CEREBRI_RDD2_ANGULAR_VELOCITY=y
14+
CONFIG_CEREBRI_RDD2_ATTITUDE=y
1415
CONFIG_CEREBRI_RDD2_LIGHTING=y
1516
CONFIG_CEREBRI_RDD2_CASADI=y
1617

@@ -55,6 +56,7 @@ CONFIG_NANOPB=y
5556
CONFIG_UBXLIB=y
5657

5758
# Debug
59+
CONFIG_CEREBRI_RDD2_LOG_LEVEL_DBG=n
5860
CONFIG_CEREBRI_SENSE_WHEEL_ODOMETRY_LOG_LEVEL_DBG=n
5961
CONFIG_CEREBRI_SENSE_IMU_LOG_LEVEL_DBG=n
6062
CONFIG_CEREBRI_SENSE_SAFETY_LOG_LEVEL_DBG=n

app/rdd2/src/allocation.c

+214
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,214 @@
1+
/*
2+
* Copyright CogniPilot Foundation 2023
3+
* SPDX-License-Identifier: Apache-2.0
4+
*/
5+
6+
#include "casadi/gen/rdd2.h"
7+
#include "math.h"
8+
#include <assert.h>
9+
10+
#include <zephyr/logging/log.h>
11+
#include <zephyr/shell/shell.h>
12+
13+
#include <zros/private/zros_node_struct.h>
14+
#include <zros/private/zros_pub_struct.h>
15+
#include <zros/private/zros_sub_struct.h>
16+
#include <zros/zros_node.h>
17+
#include <zros/zros_pub.h>
18+
#include <zros/zros_sub.h>
19+
20+
#include <synapse_topic_list.h>
21+
22+
#include <cerebri/core/casadi.h>
23+
24+
#define MY_STACK_SIZE 3072
25+
#define MY_PRIORITY 4
26+
27+
#ifndef M_PI
28+
#define M_PI 3.14159265358979323846
29+
#endif
30+
31+
LOG_MODULE_REGISTER(rdd2_allocation, CONFIG_CEREBRI_RDD2_LOG_LEVEL);
32+
33+
static K_THREAD_STACK_DEFINE(g_my_stack_area, MY_STACK_SIZE);
34+
35+
struct context {
36+
struct zros_node node;
37+
synapse_msgs_Status status;
38+
synapse_msgs_Actuators actuators;
39+
synapse_msgs_Vector3 force_sp, moment_sp;
40+
struct zros_sub sub_status, sub_force_sp, sub_moment_sp;
41+
struct zros_pub pub_actuators;
42+
atomic_t running;
43+
size_t stack_size;
44+
k_thread_stack_t* stack_area;
45+
struct k_thread thread_data;
46+
};
47+
48+
static struct context g_ctx = {
49+
.node = {},
50+
.status = synapse_msgs_Status_init_default,
51+
.actuators = {
52+
.has_header = true,
53+
.header = {
54+
.seq = 0,
55+
.frame_id = "odom",
56+
.has_stamp = true },
57+
.velocity_count = 4,
58+
},
59+
.sub_status = {},
60+
.sub_force_sp = {},
61+
.sub_moment_sp = {},
62+
.pub_actuators = {},
63+
.running = ATOMIC_INIT(0),
64+
.stack_size = MY_STACK_SIZE,
65+
.stack_area = g_my_stack_area,
66+
.thread_data = {},
67+
};
68+
69+
static void rdd2_allocation_init(struct context* ctx)
70+
{
71+
LOG_INF("init");
72+
zros_node_init(&ctx->node, "rdd2_allocation");
73+
zros_sub_init(&ctx->sub_status, &ctx->node, &topic_status, &ctx->status, 10);
74+
zros_sub_init(&ctx->sub_force_sp, &ctx->node, &topic_force_sp, &ctx->force_sp, 10);
75+
zros_sub_init(&ctx->sub_moment_sp, &ctx->node, &topic_moment_sp, &ctx->moment_sp, 100);
76+
zros_pub_init(&ctx->pub_actuators, &ctx->node, &topic_actuators, &ctx->actuators);
77+
atomic_set(&ctx->running, 1);
78+
}
79+
80+
static void rdd2_allocation_fini(struct context* ctx)
81+
{
82+
LOG_INF("fini");
83+
zros_node_fini(&ctx->node);
84+
zros_sub_fini(&ctx->sub_status);
85+
zros_sub_fini(&ctx->sub_force_sp);
86+
zros_sub_fini(&ctx->sub_moment_sp);
87+
zros_pub_fini(&ctx->pub_actuators);
88+
atomic_set(&ctx->running, 0);
89+
}
90+
91+
static void stop(struct context* ctx)
92+
{
93+
for (int i = 0; i < 4; i++) {
94+
ctx->actuators.velocity[i] = 0;
95+
}
96+
}
97+
98+
static void rdd2_allocation_run(void* p0, void* p1, void* p2)
99+
{
100+
struct context* ctx = p0;
101+
ARG_UNUSED(p1);
102+
ARG_UNUSED(p2);
103+
104+
rdd2_allocation_init(ctx);
105+
106+
struct k_poll_event events[] = {
107+
*zros_sub_get_event(&ctx->sub_moment_sp),
108+
};
109+
110+
while (atomic_get(&ctx->running)) {
111+
int rc = 0;
112+
rc = k_poll(events, ARRAY_SIZE(events), K_MSEC(100));
113+
if (rc != 0) {
114+
LOG_DBG("not receiving moment_sp");
115+
}
116+
117+
if (zros_sub_update_available(&ctx->sub_status)) {
118+
zros_sub_update(&ctx->sub_status);
119+
}
120+
121+
if (zros_sub_update_available(&ctx->sub_force_sp)) {
122+
zros_sub_update(&ctx->sub_force_sp);
123+
}
124+
125+
if (zros_sub_update_available(&ctx->sub_moment_sp)) {
126+
zros_sub_update(&ctx->sub_moment_sp);
127+
}
128+
129+
if (rc < 0) {
130+
stop(ctx);
131+
LOG_DBG("no data, stopped");
132+
} else if (ctx->status.arming != synapse_msgs_Status_Arming_ARMING_ARMED) {
133+
stop(ctx);
134+
LOG_DBG("not armed, stopped");
135+
} else {
136+
double thrust = ctx->force_sp.z;
137+
double roll = ctx->moment_sp.x;
138+
double pitch = -ctx->moment_sp.y;
139+
double yaw = -ctx->moment_sp.z;
140+
141+
if (thrust + fabs(pitch) + fabs(roll) + fabs(yaw) > 1) {
142+
thrust = 1 - fabs(pitch) - fabs(roll) - fabs(yaw);
143+
LOG_WRN("Thrust Saturation: %10.4f", thrust);
144+
}
145+
146+
const float k = 1600;
147+
synapse_msgs_Actuators* msg = &ctx->actuators;
148+
msg->velocity[0] = k * (thrust + pitch - roll + yaw);
149+
msg->velocity[1] = k * (thrust - pitch + roll + yaw);
150+
msg->velocity[2] = k * (thrust + pitch + roll - yaw);
151+
msg->velocity[3] = k * (thrust - pitch - roll - yaw);
152+
}
153+
154+
stamp_header(&ctx->actuators.header, k_uptime_ticks());
155+
ctx->actuators.header.seq++;
156+
157+
// publish
158+
zros_pub_update(&ctx->pub_actuators);
159+
}
160+
161+
rdd2_allocation_fini(ctx);
162+
}
163+
164+
static int start(struct context* ctx)
165+
{
166+
k_tid_t tid = k_thread_create(&ctx->thread_data, ctx->stack_area,
167+
ctx->stack_size,
168+
rdd2_allocation_run,
169+
ctx, NULL, NULL,
170+
MY_PRIORITY, 0, K_FOREVER);
171+
k_thread_name_set(tid, "rdd2_allocation");
172+
k_thread_start(tid);
173+
return 0;
174+
}
175+
176+
static int rdd2_allocation_cmd_handler(const struct shell* sh,
177+
size_t argc, char** argv, void* data)
178+
{
179+
struct context* ctx = data;
180+
assert(argc == 1);
181+
182+
if (strcmp(argv[0], "start") == 0) {
183+
if (atomic_get(&ctx->running)) {
184+
shell_print(sh, "already running");
185+
} else {
186+
start(ctx);
187+
}
188+
} else if (strcmp(argv[0], "stop") == 0) {
189+
if (atomic_get(&ctx->running)) {
190+
atomic_set(&ctx->running, 0);
191+
} else {
192+
shell_print(sh, "not running");
193+
}
194+
} else if (strcmp(argv[0], "status") == 0) {
195+
shell_print(sh, "running: %d", (int)atomic_get(&ctx->running));
196+
}
197+
return 0;
198+
}
199+
200+
SHELL_SUBCMD_DICT_SET_CREATE(sub_rdd2_allocation, rdd2_allocation_cmd_handler,
201+
(start, &g_ctx, "start"),
202+
(stop, &g_ctx, "stop"),
203+
(status, &g_ctx, "status"));
204+
205+
SHELL_CMD_REGISTER(rdd2_allocation, &sub_rdd2_allocation, "rdd2 allocation commands", NULL);
206+
207+
static int rdd2_allocation_sys_init(void)
208+
{
209+
return start(&g_ctx);
210+
};
211+
212+
SYS_INIT(rdd2_allocation_sys_init, APPLICATION, 1);
213+
214+
// vi: ts=4 sw=4 et

0 commit comments

Comments
 (0)