|
| 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