AMiRo-OS

/*

AMiRo-OS is an operating system designed for the Autonomous Mini Robot (AMiRo) platform.

Copyright (C) 2016..2018  Thomas Schöpping et al.

This program is free software: you can redistribute it and/or modify

it under the terms of the GNU General Public License as published by

the Free Software Foundation, either version 3 of the License, or

(at your option) any later version.

This program is distributed in the hope that it will be useful,

but WITHOUT ANY WARRANTY; without even the implied warranty of

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

GNU General Public License for more details.

You should have received a copy of the GNU General Public License

along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#include <hal.h>

#include <hal_can.h>

#include <hal_can_lld.h>

#include <aos_rtcan.h>

#include <aos_thread.h>

#include <chthreads.h>

#include <chtime.h>

#include <ch.h>

#include <stdbool.h>

#define True 1

#define False 0

#define HRTMSG 00

#define SRTMSG 01

#define NRTMSG 11

static THD_WORKING_AREA(myThreadWorkingArea, 128);

msgqueue_t hrtQueue;

msgqueue_t srtQueue;

msgqueue_t nrtQueue;

/*

 * @brief transmit Message over CAN

 */

msg_t rtcan_transmit(CANDriver *canp, canmbx_t mailbox, const CANTxFrame *ctfp){

    // call cantransmit, needed params driver, mailbox and tx frame

    msg_t transmit = canTransmitTimeout(canp,mailbox,ctfp,TIME_IMMEDIATE);

    return transmit;

}

/*

 * @brief receive Message over CAN

 */

msg_t rtcan_receive(CANDriver *canp, canmbx_t mailbox,CANRxFrame *crfp){

    // call lld cantransmit, needed params driver, mailbox and rx frame

    return canReceiveTimeout(canp,mailbox,crfp,TIME_IMMEDIATE);

}

bool pushRTCanMsgSorted(rtcan_msg_t *msg,msgqueue_t* msgqueue){

    rtcan_msg_t * prevElem = msgqueue->head;

    rtcan_msg_t * compareElem = prevElem->next;

    while(ST2MS(msg->deadline) >= ST2MS(compareElem->deadline)){

        if(compareElem->next != NULL){

            prevElem = compareElem;

            compareElem = compareElem->next;

        }else{

            prevElem->next = msg;

            msgqueue->tail = msg;

            return true;

        }

    }

    if(compareElem!= NULL){

      msg->next = compareElem;

    }

    return false;

}

/*

 * @brief push Message to its Queue, based on type of Message

 */

bool pushRTCanMsg(rtcan_msg_t *msg,msgqueue_t* msgqueue){

  if(msg != NULL){

    if(msgqueueEmpty(msgqueue)){

        queueInit(msg,msgqueue);

        return true;

    }

      msgqueue->tail->next = msg;

      msg->next = NULL;

      msgqueue->tail = msg;

      msgqueue->size++;

      return true;

    }

  return false;

}

/*

 * @brief dequeue Message from Queue

 */

rtcan_msg_t* popRTCanMsg(msgqueue_t* msgqueue){

    //todo special case head = tail

  if(!msgqueueEmpty(msgqueue)){

      rtcan_msg_t *elem = msgqueue->head;

      msgqueue->head = elem->next;

      return elem;

  }

  return NULL;

}

/*

 * @brief init Message Queue

 */

void queueInit(rtcan_msg_t *head,msgqueue_t* msgqueue){

  msgqueue->size = 0;

  msgqueue->head = head;

  msgqueue->tail = head;

  head->next = NULL;

  msgqueue->size++;

}

// Handling Message Queues

bool enqueueMsg(uint8_t *payload, uint8_t msgType, systime_t deadline){

  static rtcan_msg_t msg;

  msg.payload = payload;

  msg.msgType = msgType;

  msg.deadline = deadline;

  if(msgType == HRTMSG){

    return pushRTCanMsg(&msg,&hrtQueue);

  }else if(msgType == SRTMSG){

    return pushRTCanMsgSorted(&msg,&srtQueue);

  }else if(msgType == NRTMSG){

    return pushRTCanMsg(&msg,&nrtQueue);

  }

  return false;

}

/**

 * @brief schedule rtcan messages into mailboxes

 */

static msg_t Thread(void *arg){

  (void)arg;

  chRegSetThreadName("RTCAN Timer");

  while (true){

    chThdSleepMilliseconds( 20 );

    // Take next element from queue

    // place it in buffer to be sent in next time slot

    chThdSleepMilliseconds( 20 );

  }

  return 0;

}

/** empty or clear?*/

int msgqueueEmpty(msgqueue_t* mqueue){

    if(mqueue == NULL){

        return true;

    }

    return false;

}

//TODO: ISR

//TODO: Params