Thank you all for your responses.
I have managed to make the code compile but the program is not working as expected. Or rather, at all.
I think tasks are not getting added to the queue because queueIsEmpty always returns True -- I think I have some pointers and addresses that are being passed to functions mixed up, but my programming is not so strong.
Bill, could you recommend any debug tools? I have Linux at home and Windows at work. No Mac though.
From what I understand from your post, I will need to consider a trade-off between using a handful of big functions (slower execution), or to use lots of small functions (lots of memory).
My intention is to use this as a basis for UAV controller. Therefore, the main tasks in the queue would be:
Read data --> Filter & Processing --> Estimator --> PID --> Output
The sensors I have are on I2C which are pretty slow to update (e.g. 100 Hz). My assumption (hope) is that the Estimator-->PID-->Output loop will execute much faster so I can run this several times before data is available on I2C. The 'Read data' task could be scheduled to run every nth cycle or else be scheduled by an interrupt.
If my assumption is incorrect and the data update rate is not a limiting factor, it would make the task vastly simpler because the loop would reduce to:
Read data --> Filter & Processing --> PID --> Output
After Output, the loop can then just wait until data is available.
Below is the datastructures.h file:
typedef struct
{
int size;
int head;
int count;
char *tasks;
} queue;
Here is the main program, including the (currently non-functional) queue functions:
//#include <malloc.h>
#include "datastructures.h"
queue testqueue; //create queue
char currenttask;
char addtask;
char resettask = 1;
void setup() {
queueInit(&testqueue, 8);
addtask = 'a';
queueAdd(&testqueue, &addtask);
Serial.begin(9600);
}
void loop() {
if (!queueIsEmpty){
queueGetTask(&testqueue, ¤ttask);
queueExecuteTask(&testqueue, ¤ttask);
}
addtask = 'b';
if (queueIsFull) queueFree(&testqueue);
else queueAddFront(&testqueue, &addtask);
if (!queueIsEmpty){
queueGetTask(&testqueue, ¤ttask);
queueExecuteTask(&testqueue, ¤ttask);
}
// As an alternative to checking !queueIsEmpty before get & execute next task,
// we can assume something went wrong if the queue is empty at the end of the
// main loop, so we can add a 'reset' task to the queue to be executed next.
// This implies that tasks must add themselves or another task upon completion.
if (queueIsEmpty) queueAdd(&testqueue, &resettask);
}
/******************************************************************/
/* Task Queue functions */
/******************************************************************/
void queueInit(queue *q, int size) {
q->size = size;
q->head = 0;
q->count = 0;
q->tasks = (char *)calloc(q->size, sizeof(char)); }
void queueFree(queue *q) {
free(q->tasks); }
int queueIsFull(queue *q) {
return q->count == q->size; }
int queueIsEmpty(queue *q) {
return q->count == 0; }
int queueDuplicateExists(queue *q, char *task) {
for (int i=0; i < q->count; i++) { //loop is skipped if queue is empty
if (*task == q->tasks[i]) return 1; } //true
return 0; } //false
// Since nothing is returned, app must check if !queueIsFull() before attempting to add task
// otherwise it cannot know if Add was successful
void queueAdd(queue *q, char *task) {
if (queueDuplicateExists) return;
if (queueIsFull) return;
else {
int tail = (q->head + q->count) % q->size;
q->tasks[tail] = *task;
++ q->count; } }
void queueAddFront(queue *q, char *task) {
if (queueDuplicateExists) return;
if (queueIsFull) return;
else {
q->tasks[(q->head - 1) % q->size] = *task;
q->head -- % q->size;
++ q->count; } }
// App must ensure !queueIsEmpty() first!!
void queueGetTask(queue *q, char *task) {
*task = q->tasks[q->head];
q->head = (q->head + 1) % q->size;
-- q->count; }
void queueExecuteTask(queue *q, char *task) {
switch (*task) {
case 'a':
Serial.println("function 'a' was called");
case 'b':
Serial.println("fucntion 'b' was called");
case 127:
queueAdd(q, &resettask); // special reset task to be added in case queue is unexpectedly empty at end of main loop
// e.g. set char resettask = 'a' to start from that function again
}
}
If anybody can spot my mistake(s) or suggest any good debugging tools on Win or Linux that will allow me to debug this application better, please let me know.