forked from ROMEO/obsw
implemented queues, broke threads on the way :/
This commit is contained in:
parent
1c3ba9e20f
commit
63b0a25030
@ -2,9 +2,11 @@
|
||||
#include "semphr.h"
|
||||
#include "task.h"
|
||||
|
||||
//TODO namespace the names
|
||||
|
||||
/**
|
||||
* Task descriptors
|
||||
*/
|
||||
*/
|
||||
#define NUMBER_OF_TASKS 100
|
||||
SemaphoreHandle_t taskMutex = NULL;
|
||||
StaticSemaphore_t taskMutexDescriptor;
|
||||
@ -13,7 +15,7 @@ StaticTask_t taskDescriptors[NUMBER_OF_TASKS];
|
||||
|
||||
/**
|
||||
* Queue descriptors
|
||||
*/
|
||||
*/
|
||||
#define NUMBER_OF_QUEUES 300
|
||||
SemaphoreHandle_t queueMutex = NULL;
|
||||
StaticSemaphore_t queueMutexDescriptor;
|
||||
@ -26,17 +28,13 @@ void initFreeRTOSHelper() {
|
||||
queueMutex = xSemaphoreCreateRecursiveMutexStatic(&queueMutexDescriptor);
|
||||
}
|
||||
|
||||
const char * get_task_name() {
|
||||
return pcTaskGetName( NULL );
|
||||
}
|
||||
const char *get_task_name() { return pcTaskGetName(NULL); }
|
||||
|
||||
void stop_it() {
|
||||
taskENTER_CRITICAL();
|
||||
}
|
||||
void stop_it() { taskENTER_CRITICAL(); }
|
||||
|
||||
// TODO return some error code?
|
||||
void *create_task(TaskFunction_t taskFunction, void *parameter, void *buffer,
|
||||
size_t buffersize) {
|
||||
size_t buffersize) {
|
||||
if (xSemaphoreTakeRecursive(taskMutex, portMAX_DELAY) != pdTRUE) {
|
||||
return NULL;
|
||||
}
|
||||
@ -60,8 +58,8 @@ void task_delay(uint32_t milliseconds) {
|
||||
vTaskDelay(pdMS_TO_TICKS(milliseconds));
|
||||
}
|
||||
|
||||
void * create_queue(size_t length, size_t element_size, uint8_t * buffer) {
|
||||
if (xSemaphoreTakeRecursive(queueMutex, portMAX_DELAY) != pdTRUE) {
|
||||
void *create_queue(size_t length, size_t element_size, uint8_t *buffer) {
|
||||
if (xSemaphoreTakeRecursive(queueMutex, portMAX_DELAY) != pdTRUE) {
|
||||
return NULL;
|
||||
}
|
||||
// we hold the queue mutex and are now allowed to access the taskDescriptors
|
||||
@ -69,8 +67,25 @@ void * create_queue(size_t length, size_t element_size, uint8_t * buffer) {
|
||||
sizeof(queueDescriptors) / sizeof(*queueDescriptors)) {
|
||||
return NULL;
|
||||
}
|
||||
QueueHandle_t newQueue = xQueueCreateStatic(length, element_size, buffer, queueDescriptors + nextFreeQueueDescriptor);
|
||||
nextFreeQueueDescriptor++;
|
||||
xSemaphoreGiveRecursive(queueMutex);
|
||||
QueueHandle_t newQueue = xQueueCreateStatic(
|
||||
length, element_size, buffer, queueDescriptors + nextFreeQueueDescriptor);
|
||||
nextFreeQueueDescriptor++;
|
||||
xSemaphoreGiveRecursive(queueMutex);
|
||||
return newQueue;
|
||||
}
|
||||
|
||||
uint8_t queue_receive(void *queue, void *message) {
|
||||
if (xQueueReceive(queue, message, portMAX_DELAY) == pdPASS) {
|
||||
return 1;
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t queue_send(void *queue, void *message) {
|
||||
if (xQueueSend(queue, message, 0) != pdPASS) {
|
||||
return 1;
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
@ -175,7 +175,7 @@ void mission(void *) {
|
||||
|
||||
// printf("Starting Mission\n");
|
||||
|
||||
testIp();
|
||||
//testIp();
|
||||
|
||||
rust_main();
|
||||
|
||||
|
||||
@ -1,6 +1,7 @@
|
||||
#![no_std]
|
||||
|
||||
//TODO look into using core::ffi (some types do not seem to work)
|
||||
//TODO os errors in API calls
|
||||
|
||||
#[macro_export]
|
||||
macro_rules! sifln {
|
||||
@ -71,6 +72,9 @@ extern "C" {
|
||||
element_size: cty::size_t,
|
||||
buffer: *mut cty::c_void,
|
||||
) -> *const cty::c_void;
|
||||
|
||||
fn queue_receive(queue: *const cty::c_void, message: *const cty::c_void) -> cty::uint8_t;
|
||||
fn queue_send(queue: *const cty::c_void, message: *const cty::c_void) -> cty::uint8_t;
|
||||
}
|
||||
|
||||
#[no_mangle]
|
||||
@ -83,6 +87,7 @@ extern "C" fn rust_main() {
|
||||
|
||||
#[no_mangle]
|
||||
extern "C" fn task_entry(task_object: *mut cty::c_void) {
|
||||
sifln!("Task Entry");
|
||||
let task: &mut PeriodicTaskRunner;
|
||||
unsafe {
|
||||
let pointer = task_object as *mut PeriodicTaskRunner;
|
||||
@ -107,6 +112,7 @@ struct PeriodicTask<'a, const STACKSIZE: usize> {
|
||||
|
||||
impl<'a> PeriodicTaskRunner<'a> {
|
||||
fn execute(&mut self) {
|
||||
sifln!("Task Runner {}", self.period);
|
||||
loop {
|
||||
self.task_object.perform();
|
||||
unsafe {
|
||||
@ -131,7 +137,6 @@ impl<'a, const STACKSIZE: usize> PeriodicTask<'a, STACKSIZE> {
|
||||
unsafe {
|
||||
instance.task = create_task(task_entry, runner_pointer, stack_pointer, STACKSIZE);
|
||||
}
|
||||
let a: core::ffi::c_char;
|
||||
if instance.task == 0 as *mut cty::c_void {
|
||||
panic!("could not create Task");
|
||||
}
|
||||
@ -149,51 +154,137 @@ impl<'a, const STACKSIZE: usize> Drop for PeriodicTask<'a, STACKSIZE> {
|
||||
|
||||
struct Handler {
|
||||
id: u32,
|
||||
//command_queue: MessageQueue<Message, 10>,
|
||||
}
|
||||
|
||||
struct HandlerSender {
|
||||
id: u32,
|
||||
cycle: u8,
|
||||
other_handler: MessageQueueSender<Message>,
|
||||
}
|
||||
|
||||
impl Handler {
|
||||
fn handle_message(&self, message: Message) {
|
||||
match message {
|
||||
Message::OK=> {sifln!("OK");},
|
||||
Message::FAILED => {sifln!("FAILED");},
|
||||
Message::DATA(data) => {sifln!("p1: {}, p2 {}", data.p1, data.p2);}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
impl ExecutableObjectIF for Handler {
|
||||
fn perform(&mut self) {
|
||||
sifln!("Handler {} performs", self.id);
|
||||
// let result = self.command_queue.receive();
|
||||
// match result {
|
||||
// Ok(message) => self.handle_message(message),
|
||||
// Err(_) => {sifln!("Handler {} got nothing", self.id);}
|
||||
// }
|
||||
}
|
||||
}
|
||||
|
||||
impl ExecutableObjectIF for HandlerSender {
|
||||
fn perform(&mut self) {
|
||||
sifln!("HandlerSender {} performs step {}", self.id, self.cycle);
|
||||
// match self.cycle {
|
||||
// 0 => {let _ = self.other_handler.send(Message::OK);},
|
||||
// 1 => {let _ = self.other_handler.send(Message::FAILED);},
|
||||
// 2 => {let _ = self.other_handler.send(Message::DATA(GenericMessageData { p1: 1, p2: 2 }));},
|
||||
// _ => (),
|
||||
// }
|
||||
// self.cycle += 1;
|
||||
}
|
||||
}
|
||||
|
||||
struct MessageQueue<T, const LENGTH: usize> {
|
||||
queueId: *const cty::c_void,
|
||||
queue_id: *const cty::c_void,
|
||||
buffer: [T; LENGTH],
|
||||
}
|
||||
|
||||
struct MessageQueueSender<T> {
|
||||
queueId: *const cty::c_void,
|
||||
queue_id: Option<*const cty::c_void>,
|
||||
_unused: Option<T> //need to constrain the sender to one message type for safety, but compiler needs that to be used
|
||||
}
|
||||
|
||||
impl<T: Copy, const LENGTH: usize> MessageQueue<T, LENGTH> {
|
||||
fn new() -> Self {
|
||||
let mut instance: Self;
|
||||
unsafe {
|
||||
let mut instance = Self {
|
||||
queueId: 0 as *const cty::c_void,
|
||||
buffer: [core::mem::MaybeUninit::zeroed().assume_init();LENGTH]
|
||||
};
|
||||
instance = Self {
|
||||
queue_id: 0 as *const cty::c_void,
|
||||
buffer: [core::mem::MaybeUninit::zeroed().assume_init(); LENGTH], //Gets passed to C/FreeRTOS, so we never ever touch it ever again
|
||||
};
|
||||
let buffer_pointer: *mut cty::c_void =
|
||||
&mut instance.buffer as *mut _ as *mut cty::c_void;
|
||||
instance.queueId = create_queue(LENGTH, core::mem::size_of::<T>(), buffer_pointer);
|
||||
|
||||
if instance.queueId == 0 as *mut cty::c_void {
|
||||
panic!("could not create Queue");
|
||||
instance.queue_id = create_queue(LENGTH, core::mem::size_of::<T>(), buffer_pointer);
|
||||
|
||||
if instance.queue_id == 0 as *mut cty::c_void {
|
||||
panic!("could not create Queue");
|
||||
}
|
||||
instance
|
||||
}
|
||||
instance
|
||||
}
|
||||
}
|
||||
|
||||
fn getSender(self) -> MessageQueueSender<T> {
|
||||
let instance:MessageQueueSender<T> = MessageQueueSender::<T>{queueId: self.queueId};
|
||||
fn get_sender(&self) -> MessageQueueSender<T> {
|
||||
let instance: MessageQueueSender<T> = MessageQueueSender::<T> {
|
||||
queue_id: Some(self.queue_id),
|
||||
_unused: None
|
||||
};
|
||||
instance
|
||||
}
|
||||
|
||||
fn receive(&self) -> Result<T, ()> {
|
||||
let mut message: T;
|
||||
let res: cty::uint8_t;
|
||||
unsafe {
|
||||
message = core::mem::MaybeUninit::zeroed().assume_init(); // We only return it if the queue received something
|
||||
let message_pointer: *mut cty::c_void = &mut message as *mut _ as *mut cty::c_void;
|
||||
res = queue_receive(self.queue_id, message_pointer);
|
||||
}
|
||||
if res == 1 {
|
||||
Ok(message)
|
||||
} else {
|
||||
Err(())
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<T> MessageQueueSender<T> {
|
||||
fn send(message: T) {
|
||||
|
||||
fn new() -> Self {
|
||||
Self {
|
||||
queue_id: None,
|
||||
_unused: None
|
||||
}
|
||||
}
|
||||
|
||||
fn send(&self, message: T) -> Result<(), ()> {
|
||||
let queue_id = self.queue_id.expect("unitialized Message Queue");
|
||||
let res: cty::uint8_t;
|
||||
unsafe {
|
||||
let message_pointer: *const cty::c_void = &message as *const _ as *const cty::c_void;
|
||||
res = queue_send(queue_id, message_pointer);
|
||||
}
|
||||
if res == 1 {
|
||||
Ok(())
|
||||
} else {
|
||||
Err(())
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy)]
|
||||
struct GenericMessageData {
|
||||
p1: u32,
|
||||
p2: u32,
|
||||
}
|
||||
|
||||
#[derive(Copy, Clone)]
|
||||
enum Message {
|
||||
OK,
|
||||
FAILED,
|
||||
DATA(GenericMessageData),
|
||||
}
|
||||
|
||||
struct Outbytes {}
|
||||
@ -211,22 +302,29 @@ impl Write for Outbytes {
|
||||
}
|
||||
}
|
||||
|
||||
fn panics(a: &mut [i32]) {
|
||||
a[4] = 3;
|
||||
}
|
||||
|
||||
fn mission() {
|
||||
sifln!("Mission enter");
|
||||
|
||||
let mut h1 = Handler { id: 1 };
|
||||
let mut h2 = Handler { id: 2 };
|
||||
let mut h1 = Handler {
|
||||
id: 1,
|
||||
//command_queue: MessageQueue::new(),
|
||||
};
|
||||
// let mut h2 = HandlerSender {
|
||||
// id: 2,
|
||||
// cycle: 0,
|
||||
// other_handler: MessageQueueSender::<Message>::new(),
|
||||
// };
|
||||
|
||||
// h2.other_handler = h1.command_queue.get_sender();
|
||||
|
||||
let _t1: PeriodicTask<'_, 512> = PeriodicTask::new(&mut h1);
|
||||
|
||||
let _t2: PeriodicTask<'_, 512> = PeriodicTask::new(&mut h2);
|
||||
//let _t2: PeriodicTask<'_, 512> = PeriodicTask::new(&mut h2);
|
||||
|
||||
sifln!("Mission done delay");
|
||||
sifln!("Mission delay");
|
||||
unsafe {
|
||||
task_delay(3000);
|
||||
}
|
||||
sifln!("Mission delay done");
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user