cook/openrcore
Watch
1
0
Fork
You've already forked openrcore
 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Routine infrastructure

Browse source
This commit is contained in:
Aaron Cook 2025-08-31 22:03:41 -05:00
commit 9f005b547b
11 changed files with 527 additions and 30 deletions
Download patch file Download diff file Expand all files Collapse all files

195
core/ble.c
View file

@ -9,7 +9,8 @@
#include <zephyr/sys/util_macro.h>
#include <zephyr/types.h>
#include "ble.h"
#include "core/ble.h"
#include "core/routine.h"
/**
* @brief Begin BLE advertising with the default parameters
@ -35,7 +36,7 @@ static struct bt_conn_cb connection_callbacks = {
/** Advertising data **/
/** Our name to advertise */
#define BLE_NAME_DEFAULT ("FM25 Kit 1")
#define BLE_NAME_DEFAULT ("FM25 Kit x")
/** Advertising/Scanning parameters */
static struct bt_le_adv_param advertise_params = {
@ -107,6 +108,11 @@ static struct bt_uuid_16 characteristic_description_uuid = {
.val = BT_UUID_GATT_CUD_VAL,
};
static struct bt_uuid_16 characteristic_format_uuid = {
.uuid = { .type = BT_UUID_TYPE_16 },
.val = BT_UUID_GATT_CPF_VAL,
};
static struct bt_uuid_128 service_uuid = {
.uuid = { .type = BT_UUID_TYPE_128 },
.val = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
@ -122,7 +128,25 @@ static struct bt_uuid_128 telemetry_characteristic_uuid = {
.val = { 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3 },
};
static struct bt_uuid_128 routine_list_characteristic_uuid = {
.uuid = { .type = BT_UUID_TYPE_128 },
.val = { 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4 },
};
static struct bt_uuid_128 current_routine_characteristic_uuid = {
.uuid = { .type = BT_UUID_TYPE_128 },
.val = { 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5 },
};
/*** Characteristic Data ***/
static struct bt_gatt_cpf string_format = {
.format = 0x19, /* UTF8 string */
.exponent = 0,
.unit = 0,
.name_space = 0x01, /* Bluetooth SIG */
.description = 0,
};
typedef struct ble_attribute_userdata_struct {
/** Buffer allocated for this attribute's data */
void *buffer;
@ -155,7 +179,7 @@ char input_characteristic_description[] = "Input Data";
/**** Telemetry Characteristic ****/
struct bt_gatt_chrc telemetry_characteristic = {
.uuid = (void *)&telemetry_characteristic_uuid,
.uuid = (struct bt_uuid *)&telemetry_characteristic_uuid,
.properties = BT_GATT_CHRC_READ,
};
/** Buffer for the contents of the Telemetry characteristic */
@ -171,6 +195,45 @@ ble_attribute_userdata_t telemetry_characteristic_userdata = {
char telemetry_characteristic_description[] = "Telemetry Data";
/**** Routine List Characteristic ****/
extern const routine_listing_t *routine_list;
struct bt_gatt_chrc routine_list_characteristic = {
.uuid = (struct bt_uuid *)&routine_list_characteristic_uuid,
.properties = BT_GATT_CHRC_READ,
};
char routine_list_characteristic_buffer[512];
ble_attribute_userdata_t routine_list_characterist_userdata = {
.buffer = routine_list_characteristic_buffer,
.buffer_size = sizeof(routine_list_characteristic_buffer),
.value_size = 0,
};
char routine_list_characteristic_description[] = "Routine List";
/**** Current Routine Characteristic ****/
struct bt_gatt_chrc current_routine_characteristic = {
.uuid = (struct bt_uuid *)&current_routine_characteristic_uuid,
.properties = BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE,
};
static ssize_t _read_current_routine(
struct bt_conn *connection,
const struct bt_gatt_attr *attribute,
void *buffer,
uint16_t buffer_len,
uint16_t offset
);
static ssize_t _write_current_routine(
struct bt_conn *connection,
const struct bt_gatt_attr *attribute,
const void *buffer,
uint16_t buffer_len,
uint16_t offset,
uint8_t flags
);
char current_routine_characteristic_description[] = "Current Routine";
/*** Service Layout ***/
static struct bt_gatt_attr primary_service_attributes[] = {
/* Service declaration */
@ -200,6 +263,12 @@ static struct bt_gatt_attr primary_service_attributes[] = {
.user_data = (void *)input_characteristic_description,
.perm = BT_GATT_PERM_READ,
},
{
.uuid = (struct bt_uuid *)&characteristic_format_uuid,
.read = bt_gatt_attr_read_cpf,
.user_data = (void *)&string_format,
.perm = BT_GATT_PERM_READ,
},
/** Telemetry Data Characteristic **/
{
.uuid = (struct bt_uuid *)&characteristic_declaration_uuid,
@ -218,7 +287,57 @@ static struct bt_gatt_attr primary_service_attributes[] = {
.read = bt_gatt_attr_read_cud,
.user_data = (void *)telemetry_characteristic_description,
.perm = BT_GATT_PERM_READ,
}
},
{
.uuid = (struct bt_uuid *)&characteristic_format_uuid,
.read = bt_gatt_attr_read_cpf,
.user_data = (void *)&string_format,
.perm = BT_GATT_PERM_READ,
},
/** Routine List Characteristic **/
{
.uuid = (struct bt_uuid *)&characteristic_declaration_uuid,
.read = bt_gatt_attr_read_chrc,
.user_data = (void *)&routine_list_characteristic,
.perm = BT_GATT_PERM_READ,
},
{
.uuid = (struct bt_uuid *)&routine_list_characteristic_uuid,
.read = _generic_attribute_read,
.user_data = (void *)&routine_list_characterist_userdata,
.perm = BT_GATT_PERM_READ,
},
{
.uuid = (struct bt_uuid *)&characteristic_description_uuid,
.read = bt_gatt_attr_read_cud,
.user_data = (void *)routine_list_characteristic_description,
.perm = BT_GATT_PERM_READ,
},
{
.uuid = (struct bt_uuid *)&characteristic_format_uuid,
.read = bt_gatt_attr_read_cpf,
.user_data = (void *)&string_format,
.perm = BT_GATT_PERM_READ,
},
/** Current Routine Characteristic */
{
.uuid = (struct bt_uuid *)&characteristic_declaration_uuid,
.read = bt_gatt_attr_read_chrc,
.user_data = (void *)&current_routine_characteristic,
.perm = BT_GATT_PERM_READ,
},
{
.uuid = (struct bt_uuid *)&current_routine_characteristic_uuid,
.read = _read_current_routine,
.write = _write_current_routine,
.perm = BT_GATT_PERM_READ | BT_GATT_PERM_WRITE
},
{
.uuid = (struct bt_uuid *)&characteristic_description_uuid,
.read = bt_gatt_attr_read_cud,
.user_data = (void *)current_routine_characteristic_description,
.perm = BT_GATT_PERM_READ,
},
};
static struct bt_gatt_service primary_service = {
@ -231,6 +350,8 @@ static struct bt_gatt_service primary_service = {
typedef struct ble_state_struct {
/** Current callback for a complete input write */
void (*oninput_callback)(const void *buffer, uint16_t buffer_len);
#define BLE_FLAG_DISCONNECTED (0x01)
uint32_t flags;
/** Lock for data in the BLE state */
struct k_mutex lock;
} ble_state_t;
@ -257,6 +378,8 @@ ble_init()
/* Initialize the service data */
k_mutex_init(&input_characteristic_userdata.lock);
k_mutex_init(&telemetry_characteristic_userdata.lock);
routine_get_listing(routine_list_characteristic_buffer, sizeof(routine_list_characteristic_buffer));
routine_list_characterist_userdata.value_size = strlen(routine_list_characteristic_buffer);
ret = bt_gatt_service_register(&primary_service);
if (ret) {
@ -268,6 +391,17 @@ ble_init()
return 0;
}
void
ble_tick()
{
k_mutex_lock(&ble.lock, K_FOREVER);
if (ble.flags & BLE_FLAG_DISCONNECTED) {
_start_advertising();
ble.flags &= ~BLE_FLAG_DISCONNECTED;
}
k_mutex_unlock(&ble.lock);
}
void
ble_register_oninput_callback(void (*callback)(const void *buffer, uint16_t buffer_len))
{
@ -276,6 +410,14 @@ ble_register_oninput_callback(void (*callback)(const void *buffer, uint16_t buff
k_mutex_unlock(&ble.lock);
}
void
ble_clear_oninput_callback()
{
k_mutex_lock(&ble.lock, K_FOREVER);
ble.oninput_callback = NULL;
k_mutex_unlock(&ble.lock);
}
void
ble_set_telemetry_data(const void *buffer, uint16_t buffer_len)
{
@ -314,7 +456,9 @@ static void
_disconnected_callback(struct bt_conn *connection, uint8_t reason)
{
/* Resume advertising so we can be found again */
_start_advertising();
k_mutex_lock(&ble.lock, K_FOREVER);
ble.flags |= BLE_FLAG_DISCONNECTED;
k_mutex_unlock(&ble.lock);
}
static ssize_t
@ -346,7 +490,7 @@ _generic_attribute_read(
return read_len;
}
static int _generic_attribute_write(
static ssize_t _generic_attribute_write(
struct bt_conn *connection,
const struct bt_gatt_attr *attribute,
const void *buffer,
@ -381,3 +525,42 @@ static int _generic_attribute_write(
return write_len;
}
static ssize_t _read_current_routine(
struct bt_conn *connection,
const struct bt_gatt_attr *attribute,
void *buffer,
uint16_t buffer_len,
uint16_t offset
) {
if ((offset != 0) || (buffer_len < sizeof(int8_t))) {
return (0);
}
((int8_t *)buffer)[0] = routine_get_current();
return (sizeof(int8_t));
}
static ssize_t _write_current_routine(
struct bt_conn *connection,
const struct bt_gatt_attr *attribute,
const void *buffer,
uint16_t buffer_len,
uint16_t offset,
uint8_t flags
) {
uint8_t index;
if ((offset != 0) || (buffer_len < sizeof(uint8_t))) {
return (0);
}
index = ((uint8_t *)buffer)[0];
if (routine_select(index)) {
return (0);
} else {
return (sizeof(uint8_t));
}
}

14
core/ble.h
View file

@ -1,19 +1,15 @@
#ifndef _BLE_H_
#define _BLE_H_
#include <stdint.h>
#include "includes/ble.h"
/**
* @brief Enable Bluetooth, setup default services, and begin advertising
*/
int ble_init(void);
/**
* @brief Register a callback to occur when a new complete input is sent
*/
void ble_register_oninput_callback(void (*callback)(const void *buffer, uint16_t buffer_len));
/**
* @brief Set a new value for telemetry data
*/
void ble_set_telemetry_data(const void *buffer, uint16_t buffer_len);
void ble_tick(void);
#endif /* _BLE_H_ */

17
core/main.c
View file

@ -3,23 +3,14 @@
#include <zephyr/kernel.h>
#include "ble.h"
#include "core/ble.h"
#include "core/routine.h"
static void oninput(const void *buffer, uint16_t buffer_len);
int main(void)
{
routine_init();
ble_init();
ble_register_oninput_callback(oninput);
while (1) {
k_sleep(K_MSEC(1000));
k_yield();
}
}
static void
oninput(const void *buffer, uint16_t buffer_len)
{
ble_set_telemetry_data(buffer, buffer_len);
routine_exec();
}

154
core/routine.c Normal file
View file

@ -0,0 +1,154 @@
#include <stdint.h>
#include <stdbool.h>
#include <zephyr/kernel.h>
#include "core/ble.h"
#include "core/routine.h"
extern const routine_listing_t *routine_list;
typedef struct routine_state_struct {
uint8_t routine_count;
/** Index of the current routine */
int8_t current_routine;
/** Index of the next routine */
int8_t next_routine;
/** Return status of the last routine to complete execution */
routine_return_e last_routine_ret;
/** Routine status */
routine_status_e status;
/** Lock for the */
struct k_mutex lock;
} routine_state_t;
static routine_state_t routine_state = {
.routine_count = 0,
.current_routine = ROUTINE_INDEX_NONE,
.next_routine = ROUTINE_INDEX_NONE,
};
void
routine_init()
{
k_mutex_init(&routine_state.lock);
while (1) {
const routine_listing_t *listing = &routine_list[routine_state.routine_count];
if (listing->entry == NULL) {
break;
}
routine_state.routine_count++;
}
}
void
routine_exec()
{
while (1) {
routine_return_e ret;
ble_tick();
k_mutex_lock(&routine_state.lock, K_FOREVER);
if (routine_state.next_routine == ROUTINE_INDEX_NONE) {
/* nothing to do */
k_mutex_unlock(&routine_state.lock);
k_yield();
continue;
}
/* something to do! */
routine_state.current_routine = routine_state.next_routine;
routine_state.next_routine = ROUTINE_INDEX_NONE;
routine_state.status = ROUTINE_STATUS_RUNNING;
k_mutex_unlock(&routine_state.lock);
ret = routine_list[routine_state.current_routine].entry();
k_mutex_lock(&routine_state.lock, K_FOREVER);
routine_state.last_routine_ret = ret;
routine_state.current_routine = ROUTINE_INDEX_NONE;
routine_state.status = ROUTINE_STATUS_STOPPED;
k_mutex_unlock(&routine_state.lock);
}
}
int
routine_select(uint8_t index)
{
if (index >= routine_state.routine_count) {
return (-1);
}
k_mutex_lock(&routine_state.lock, K_FOREVER);
routine_state.next_routine = index;
routine_state.status = ROUTINE_STATUS_STOPPING;
k_mutex_unlock(&routine_state.lock);
return (0);
}
void
routine_get_listing(char *buf, uint16_t buf_len)
{
int written;
uint16_t pos = 0;
written = snprintf(&buf[pos], buf_len - pos, "Routines (%d):\r\n", routine_state.routine_count);
pos += written;
for (int i = 0; i < routine_state.routine_count; i++) {
written = snprintf(
&buf[pos], buf_len - pos,
"%d: %s\r\n",
i, routine_list[i].name
);
pos += written;
}
/* walk back trailing \r\n */
buf[pos - 2] = '\0';
}
int8_t
routine_get_current()
{
int8_t ret;
k_mutex_lock(&routine_state.lock, K_FOREVER);
ret = routine_state.current_routine;
k_mutex_unlock(&routine_state.lock);
return (ret);
}
routine_status_e
routine_status()
{
routine_status_e ret;
k_mutex_lock(&routine_state.lock, K_FOREVER);
ret = routine_state.status;
k_mutex_unlock(&routine_state.lock);
return (ret);
}
void
routine_delay(uint32_t milliseconds)
{
k_sleep(K_MSEC(milliseconds));
}
void
routine_yield()
{
ble_tick();
k_yield();
}

36
core/routine.h Normal file
View file

@ -0,0 +1,36 @@
#ifndef _ROUTINE_H_
#define _ROUTINE_H_
#include <stdbool.h>
#include <stdint.h>
#include "includes/routine.h"
#define ROUTINE_INDEX_NONE (-1)
/**
* @brief Initialize the routine state
*/
void routine_init(void);
/**
* @brief Execute the routine, intended to be called from main. Does not return.
*/
void routine_exec(void);
/**
* @brief Select a new routine to be active
*/
int routine_select(uint8_t index);
/**
* @brief Fill out a buffer with a human readable list of routines
*/
void routine_get_listing(char *buf, uint16_t buf_len);
/**
* @brief Get the index of the current running routine
*/
int8_t routine_get_current(void);
#endif /* _ROUTINE_H_ */