smo/MCRLController
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
Simon Oberzier
2026-02-20 20:39:17 +01:00
commit e8b56b4142
12 changed files with 591 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored Normal file
View File

@@ -0,0 +1,3 @@
build/
.DS_Store
.idea/

54
CMakeLists.txt Normal file
View File

@@ -0,0 +1,54 @@
cmake_minimum_required(VERSION 3.12)
set(PICO_BOARD pico2_w)
include(pico_sdk_import.cmake)
project(pico_examples C CXX ASM)
set(CMAKE_C_STANDARD 11)
set(CMAKE_CXX_STANDARD 17)
if (PICO_SDK_VERSION_STRING VERSION_LESS "2.2.0")
message(FATAL_ERROR "Raspberry Pi Pico SDK version 2.2.0 (or later) required. Your version is ${PICO_SDK_VERSION_STRING}")
endif()
pico_sdk_init()
add_compile_options(-Wall
-Wno-format
-Wno-unused-function
)
if (CMAKE_C_COMPILER_ID STREQUAL "GNU")
add_compile_options(-Wno-maybe-uninitialized)
endif()
set(BTSTACK_ROOT ${PICO_SDK_PATH}/lib/btstack)
add_executable(foo
server.c
ble_server.c
gps_handler.c
mpu_handler.c
mpu_handler.h
)
target_include_directories(foo PUBLIC
${CMAKE_CURRENT_LIST_DIR}
)
target_link_libraries(foo
pico_stdlib
pico_btstack_ble
pico_btstack_cyw43
pico_cyw43_arch_none
hardware_adc
hardware_uart
hardware_i2c
)
pico_enable_stdio_usb(foo 1)
pico_enable_stdio_uart(foo 0)
pico_btstack_make_gatt_header(foo PRIVATE "${CMAKE_CURRENT_LIST_DIR}/foo.gatt")
pico_add_extra_outputs(foo)

142
ble_server.c Normal file
View File

@@ -0,0 +1,142 @@
#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <assert.h>
#include "btstack.h"
#include "pico/cyw43_arch.h"
#include "pico/btstack_cyw43.h"
#include "pico/stdlib.h"
#include "foo.h"
#include "ble_server.h"
#define ATT_CHARACTERISTIC_GENERIC_STRING_VALUE_HANDLE 0x0009
#define ATT_CHARACTERISTIC_GENERIC_STRING_CLIENT_CONFIGURATION_HANDLE 0x000a
#define HEARTBEAT_PERIOD_MS 1000
#define APP_AD_FLAGS 0x06
#define GENERIC_MESSAGE_MAX_LEN 64
static uint8_t adv_data[] = {
0x02, BLUETOOTH_DATA_TYPE_FLAGS, APP_AD_FLAGS,
0x17, BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME, 'P', 'i', 'c', 'o', ' ', '0', '0', ':', '0', '0', ':', '0', '0', ':', '0', '0', ':', '0', '0', ':', '0', '0',
0x03, BLUETOOTH_DATA_TYPE_COMPLETE_LIST_OF_16_BIT_SERVICE_CLASS_UUIDS, 0x1a, 0x18,
};
static const uint8_t adv_data_len = sizeof(adv_data);
static int le_generic_string_notification_enabled;
static hci_con_handle_t con_handle;
static char generic_message_buffer[GENERIC_MESSAGE_MAX_LEN];
static btstack_timer_source_t heartbeat;
static btstack_packet_callback_registration_t hci_event_callback_registration;
extern uint8_t const profile_data[];
static void ble_start_advertising(void) {
uint16_t adv_int_min = 800;
uint16_t adv_int_max = 800;
uint8_t adv_type = 0;
bd_addr_t null_addr;
memset(null_addr, 0, 6);
gap_advertisements_set_params(adv_int_min, adv_int_max, adv_type, 0, null_addr, 0x07, 0x00);
assert(adv_data_len <= 31);
bd_addr_t local_addr;
gap_local_bd_addr(local_addr);
sprintf((char*)&adv_data[12], "%02X:%02X:%02X:%02X:%02X:%02X",
local_addr[0], local_addr[1], local_addr[2],
local_addr[3], local_addr[4], local_addr[5]);
gap_advertisements_set_data(adv_data_len, (uint8_t*) adv_data);
gap_advertisements_enable(1);
}
static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) {
UNUSED(size);
UNUSED(channel);
if (packet_type != HCI_EVENT_PACKET) return;
uint8_t event_type = hci_event_packet_get_type(packet);
switch(event_type){
case BTSTACK_EVENT_STATE:
if (btstack_event_state_get_state(packet) != HCI_STATE_WORKING) return;
ble_start_advertising();
break;
case HCI_EVENT_DISCONNECTION_COMPLETE:
le_generic_string_notification_enabled = 0;
break;
case ATT_EVENT_CAN_SEND_NOW:
if (le_generic_string_notification_enabled) {
att_server_notify(con_handle, ATT_CHARACTERISTIC_GENERIC_STRING_VALUE_HANDLE, (uint8_t*)generic_message_buffer, strlen(generic_message_buffer));
}
break;
default:
break;
}
}
static uint16_t att_read_callback(hci_con_handle_t connection_handle, uint16_t att_handle, uint16_t offset, uint8_t * buffer, uint16_t buffer_size) {
UNUSED(connection_handle);
if (att_handle == ATT_CHARACTERISTIC_GENERIC_STRING_VALUE_HANDLE) {
return att_read_callback_handle_blob((const uint8_t *)&generic_message_buffer, strlen(generic_message_buffer), offset, buffer, buffer_size);
}
return 0;
}
static int att_write_callback(hci_con_handle_t connection_handle, uint16_t att_handle, uint16_t transaction_mode, uint16_t offset, uint8_t *buffer, uint16_t buffer_size) {
UNUSED(transaction_mode);
UNUSED(offset);
UNUSED(buffer_size);
if (att_handle == ATT_CHARACTERISTIC_GENERIC_STRING_CLIENT_CONFIGURATION_HANDLE) {
le_generic_string_notification_enabled = little_endian_read_16(buffer, 0) == GATT_CLIENT_CHARACTERISTICS_CONFIGURATION_NOTIFICATION;
con_handle = connection_handle;
if (le_generic_string_notification_enabled) {
att_server_request_can_send_now_event(con_handle);
}
return 0;
}
return 0;
}
static void heartbeat_handler(struct btstack_timer_source *ts) {
btstack_run_loop_set_timer(ts, HEARTBEAT_PERIOD_MS);
btstack_run_loop_add_timer(ts);
}
void start_server(void) {
if (cyw43_arch_init()) {
return;
}
l2cap_init();
sm_init();
att_server_init(profile_data, att_read_callback, att_write_callback);
hci_event_callback_registration.callback = &packet_handler;
hci_add_event_handler(&hci_event_callback_registration);
att_server_register_packet_handler(packet_handler);
heartbeat.process = &heartbeat_handler;
btstack_run_loop_set_timer(&heartbeat, HEARTBEAT_PERIOD_MS);
btstack_run_loop_add_timer(&heartbeat);
hci_power_control(HCI_POWER_ON);
}
void stop_server(void) {
hci_power_control(HCI_POWER_OFF);
btstack_run_loop_remove_timer(&heartbeat);
cyw43_arch_deinit();
}
void send_message(const char* message) {
snprintf(generic_message_buffer, GENERIC_MESSAGE_MAX_LEN, "%s", message);
if (le_generic_string_notification_enabled) {
att_server_request_can_send_now_event(con_handle);
}
}

20
ble_server.h Normal file
View File

@@ -0,0 +1,20 @@
#ifndef BLE_SERVER_H
#define BLE_SERVER_H
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
void start_server(void);
void stop_server(void);
void send_message(const char* message);
#ifdef __cplusplus
}
#endif
#endif // BLE_SERVER_H

58
btstack_config.h Normal file
View File

@@ -0,0 +1,58 @@
#ifndef _PICO_BTSTACK_CONFIG__H
#define _PICO_BTSTACK_CONFIG__H
#ifndef ENABLE_BLE
#error Please link to pico_btstack_ble
#endif
// BTstack features that can be enabled
#define ENABLE_LE_PERIPHERAL
#define ENABLE_LOG_INFO
#define ENABLE_LOG_ERROR
#define ENABLE_PRINTF_HEXDUMP
// for the client
#if RUNNING_AS_CLIENT
#define ENABLE_LE_CENTRAL
#define MAX_NR_GATT_CLIENTS 1
#else
#define MAX_NR_GATT_CLIENTS 0
#endif
// BTstack configuration. buffers, sizes, ...
#define HCI_OUTGOING_PRE_BUFFER_SIZE 4
#define HCI_ACL_PAYLOAD_SIZE (255 + 4)
#define HCI_ACL_CHUNK_SIZE_ALIGNMENT 4
#define MAX_NR_HCI_CONNECTIONS 1
#define MAX_NR_SM_LOOKUP_ENTRIES 3
#define MAX_NR_WHITELIST_ENTRIES 16
#define MAX_NR_LE_DEVICE_DB_ENTRIES 16
// Limit number of ACL/SCO Buffer to use by stack to avoid cyw43 shared bus overrun
#define MAX_NR_CONTROLLER_ACL_BUFFERS 3
#define MAX_NR_CONTROLLER_SCO_PACKETS 3
// Enable and configure HCI Controller to Host Flow Control to avoid cyw43 shared bus overrun
#define ENABLE_HCI_CONTROLLER_TO_HOST_FLOW_CONTROL
#define HCI_HOST_ACL_PACKET_LEN (255+4)
#define HCI_HOST_ACL_PACKET_NUM 3
#define HCI_HOST_SCO_PACKET_LEN 120
#define HCI_HOST_SCO_PACKET_NUM 3
// Link Key DB and LE Device DB using TLV on top of Flash Sector interface
#define NVM_NUM_DEVICE_DB_ENTRIES 16
#define NVM_NUM_LINK_KEYS 16
// We don't give btstack a malloc, so use a fixed-size ATT DB.
#define MAX_ATT_DB_SIZE 512
// BTstack HAL configuration
#define HAVE_EMBEDDED_TIME_MS
// map btstack_assert onto Pico SDK assert()
#define HAVE_ASSERT
// Some USB dongles take longer to respond to HCI reset (e.g. BCM20702A).
#define HCI_RESET_RESEND_TIMEOUT_MS 1000
#define ENABLE_SOFTWARE_AES128
#define ENABLE_MICRO_ECC_FOR_LE_SECURE_CONNECTIONS
#endif // MICROPY_INCLUDED_EXTMOD_BTSTACK_BTSTACK_CONFIG_H

8
foo.gatt Normal file
View File

@@ -0,0 +1,8 @@
PRIMARY_SERVICE, GAP_SERVICE
CHARACTERISTIC, GAP_DEVICE_NAME, READ, "Pico 2 ble example"
PRIMARY_SERVICE, GATT_SERVICE
CHARACTERISTIC, GATT_DATABASE_HASH, READ,
PRIMARY_SERVICE, 0000FF00-0000-1000-8000-00805F9B34FB, GenericStringService
CHARACTERISTIC, 0000FF01-0000-1000-8000-00805F9B34FB, READ | NOTIFY | DYNAMIC, GenericStringCharacteristic

54
gps_handler.c Normal file
View File

@@ -0,0 +1,54 @@
#include "gps_handler.h"
#include <string.h>
#include <stdlib.h>
static char buffer[BUFFER_SIZE];
static int gps_index = 0;
static float convert_to_decimal(char *coord, char dir) {
float raw = atof(coord);
int degrees = (int)(raw / 100);
float minutes = raw - (degrees * 100);
float decimal = degrees + (minutes / 60.0f);
if (dir == 'S' || dir == 'W') decimal *= -1.0f;
return decimal;
}
void gps_init(void) {
uart_init(UART_ID, BAUD_RATE);
gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);
uart_set_hw_flow(UART_ID, false, false);
uart_set_format(UART_ID, 8, 1, UART_PARITY_NONE);
}
void gps_update(GPS_Data_t *data) {
while (uart_is_readable(UART_ID)) {
char c = uart_getc(UART_ID);
if (c == '\n') {
buffer[gps_index] = '\0';
if (strstr(buffer, "$GPRMC")) {
char *token = strtok(buffer, ",");
int field = 0;
char lat[16] = {0}, lon[16] = {0}, lat_dir = 0, lon_dir = 0;
while (token != NULL) {
field++;
if (field == 4) strcpy(lat, token);
if (field == 5) lat_dir = token[0];
if (field == 6) strcpy(lon, token);
if (field == 7) lon_dir = token[0];
token = strtok(NULL, ",");
}
if (lat[0] != 0 && lon[0] != 0) {
data->latitude = convert_to_decimal(lat, lat_dir);
data->longitude = convert_to_decimal(lon, lon_dir);
data->has_fix = true;
}
}
gps_index = 0;
} else if (gps_index < BUFFER_SIZE - 1) {
buffer[gps_index++] = c;
}
}
}

23
gps_handler.h Normal file
View File

@@ -0,0 +1,23 @@
#ifndef GPS_HANDLER_H
#define GPS_HANDLER_H
#include "pico/stdlib.h"
#include "hardware/uart.h"
#define UART_ID uart1
#define BAUD_RATE 9600
#define UART_TX_PIN 8
#define UART_RX_PIN 9
#define BUFFER_SIZE 128
typedef struct {
float latitude;
float longitude;
bool has_fix;
} GPS_Data_t;
void gps_init(void);
void gps_update(GPS_Data_t *data);
#endif //GPS_HANDLER_H

79
mpu_handler.c Normal file
View File

@@ -0,0 +1,79 @@
#include "mpu_handler.h"
#include <stdio.h>
static float offset_ax = 0, offset_ay = 0, offset_az = 0;
static void mpu_write(uint8_t addr, uint8_t reg, uint8_t data) {
uint8_t buf[2] = {reg, data};
i2c_write_blocking(I2C_PORT, addr, buf, 2, false);
}
static void mpu_read(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) {
i2c_write_blocking(I2C_PORT, addr, &reg, 1, true);
i2c_read_timeout_us(I2C_PORT, addr, buf, len, false, 2500);
}
void mpu_init(void) {
i2c_init(I2C_PORT, 400 * 1000);
gpio_set_function(SDA_PIN, GPIO_FUNC_I2C);
gpio_set_function(SCL_PIN, GPIO_FUNC_I2C);
gpio_pull_up(SDA_PIN);
gpio_pull_up(SCL_PIN);
// Wake MPU-92.65
mpu_write(MPU_ADDR, 0x6B, 0x00);
sleep_ms(100);
// Enable Bypass Mode to see the Magnetometer (AK8963)
mpu_write(MPU_ADDR, 0x37, 0x02);
sleep_ms(10);
// Initialize Magnetometer: Power down then Continuous measurement mode 2 (100Hz)
mpu_write(MAG_ADDR, 0x0A, 0x00);
sleep_ms(10);
mpu_write(MAG_ADDR, 0x0A, 0x16);
sleep_ms(10);
}
void mpu_calibrate(void) {
int num_samples = 20;
int32_t sum_ax = 0, sum_ay = 0, sum_az = 0;
uint8_t raw[6];
for (int i = 0; i < num_samples; i++) {
mpu_read(MPU_ADDR, 0x3B, raw, 6);
sum_ax += (int16_t)((raw[0] << 8) | raw[1]);
sum_ay += (int16_t)((raw[2] << 8) | raw[3]);
sum_az += (int16_t)((raw[4] << 8) | raw[5]);
sleep_ms(10);
}
offset_ax = (float)sum_ax / num_samples;
offset_ay = (float)sum_ay / num_samples;
offset_az = ((float)sum_az / num_samples) - 16384.0f;
}
void mpu_read_data(MPU_Data_t *data) {
uint8_t raw_accel_gyro[14];
uint8_t raw_mag[7]; // 6 data bytes + ST2 status byte
// Read Accel (0x3B-0x40) and Gyro (0x43-0x48)
mpu_read(MPU_ADDR, 0x3B, raw_accel_gyro, 14);
// Process Accelerometer
data->ax = ((float)((int16_t)((raw_accel_gyro[0] << 8) | raw_accel_gyro[1])) - offset_ax) / 16384.0f;
data->ay = ((float)((int16_t)((raw_accel_gyro[2] << 8) | raw_accel_gyro[3])) - offset_ay) / 16384.0f;
data->az = ((float)((int16_t)((raw_accel_gyro[4] << 8) | raw_accel_gyro[5])) - offset_az) / 16384.0f;
// Process Gyroscope (Scale 131.0 for +/- 250 deg/s)
data->gx = (float)((int16_t)((raw_accel_gyro[8] << 8) | raw_accel_gyro[9])) / 131.0f;
data->gy = (float)((int16_t)((raw_accel_gyro[10] << 8) | raw_accel_gyro[11])) / 131.0f;
data->gz = (float)((int16_t)((raw_accel_gyro[12] << 8) | raw_accel_gyro[13])) / 131.0f;
// Read Magnetometer
mpu_read(MAG_ADDR, 0x03, raw_mag, 7);
// AK8963 is little-endian
data->mx = (float)((int16_t)((raw_mag[1] << 8) | raw_mag[0]));
data->my = (float)((int16_t)((raw_mag[3] << 8) | raw_mag[2]));
data->mz = (float)((int16_t)((raw_mag[5] << 8) | raw_mag[4]));
}

25
mpu_handler.h Normal file
View File

@@ -0,0 +1,25 @@
#ifndef MPU_HANDLER_H
#define MPU_HANDLER_H
#include "pico/stdlib.h"
#include "hardware/i2c.h"
#define I2C_PORT i2c0
#define SDA_PIN 4
#define SCL_PIN 5
#define MPU_ADDR 0x68
#define MAG_ADDR 0x0C
typedef struct {
float ax, ay, az;
float gx, gy, gz;
float mx, my, mz;
} MPU_Data_t;
void mpu_init(void);
void mpu_calibrate(void);
void mpu_read_data(MPU_Data_t *data);
#endif //MPU_HANDLER_H

73
pico_sdk_import.cmake Normal file
View File

@@ -0,0 +1,73 @@
# This is a copy of <PICO_SDK_PATH>/external/pico_sdk_import.cmake
# This can be dropped into an external project to help locate this SDK
# It should be include()ed prior to project()
if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH))
set(PICO_SDK_PATH $ENV{PICO_SDK_PATH})
message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')")
endif ()
if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT))
set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT})
message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')")
endif ()
if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH))
set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH})
message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')")
endif ()
set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK")
set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable")
set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK")
if (NOT PICO_SDK_PATH)
if (PICO_SDK_FETCH_FROM_GIT)
include(FetchContent)
set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR})
if (PICO_SDK_FETCH_FROM_GIT_PATH)
get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}")
endif ()
# GIT_SUBMODULES_RECURSE was added in 3.17
if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0")
FetchContent_Declare(
pico_sdk
GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
GIT_TAG master
GIT_SUBMODULES_RECURSE FALSE
)
else ()
FetchContent_Declare(
pico_sdk
GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
GIT_TAG master
)
endif ()
if (NOT pico_sdk)
message("Downloading Raspberry Pi Pico SDK")
FetchContent_Populate(pico_sdk)
set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR})
endif ()
set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE})
else ()
message(FATAL_ERROR
"SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git."
)
endif ()
endif ()
get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
if (NOT EXISTS ${PICO_SDK_PATH})
message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found")
endif ()
set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake)
if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE})
message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK")
endif ()
set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE)
include(${PICO_SDK_INIT_CMAKE_FILE})

52
server.c Normal file
View File

@@ -0,0 +1,52 @@
#include <stdio.h>
#include "pico/stdlib.h"
#include "ble_server.h"
#include "mpu_handler.h"
#include "gps_handler.h"
#define GENERIC_MESSAGE_SEND_INTERVAL_MS 100
static repeating_timer_t timer;
static char ble_buffer[64];
static MPU_Data_t mpu_values;
static GPS_Data_t gps_values;
bool periodic_message_timer_callback(repeating_timer_t *rt) {
(void)rt;
// Data Format: S,AccX,AccY,AccZ,Lat,Lon,Fix
snprintf(ble_buffer, sizeof(ble_buffer),
"S,%.2f,%.2f,%.2f,%.4f,%.4f,%d",
mpu_values.ax, mpu_values.ay, mpu_values.az,
gps_values.latitude, gps_values.longitude,
gps_values.has_fix ? 1 : 0);
send_message(ble_buffer);
return true;
}
int main() {
stdio_init_all();
sleep_ms(1000);
start_server();
mpu_init();
mpu_calibrate();
gps_init();
if (!add_repeating_timer_ms(GENERIC_MESSAGE_SEND_INTERVAL_MS, periodic_message_timer_callback, NULL, &timer)) {
printf("Failed to start timer!\n");
}
while (true) {
mpu_read_data(&mpu_values);
gps_update(&gps_values);
sleep_ms(50);
}
cancel_repeating_timer(&timer);
stop_server();
return 0;
}