ChibiOS Free Embedded RTOS

Code: Select all

#include "ch.h"
#include "hal.h"
#include "motor.h"
#include "gyro.h"
#include "XBee.h"
#include "alti.h"

#define MAILBOX_SIZE   1

static Mailbox mb_Comm;
static msg_t b_Comm[MAILBOX_SIZE];

MemoryPool comPool;

static WORKING_AREA(waMotor, 128);
static msg_t ThreadMotor( void *arg )
{
   DATA_COMM *data;
   
   initMotor();

   while(TRUE)
   {
      chMBFetch(&mb_Comm, (msg_t*)&data, TIME_INFINITE);
      data = (chPoolAlloc(&comPool));
      setSpeed(data->battery, MOTOR_1);
      setSpeed(data->battery, MOTOR_2);
      setSpeed(data->battery, MOTOR_3);
      setSpeed(data->battery, MOTOR_4);
   }
}

static WORKING_AREA(waComSnd, 128);
static msg_t ThreadComSnd( void *arg )
{
   DATA_COMM data;
   
   while(TRUE)
   {
      data.tangage = 15;
      data.roulis = 46;
      data.lacet = 13;
      data.altitude = 1245;
      data.battery = 95;
      data.signal = 51;

      sendData ( &data );
      chThdSleepMilliseconds( 100 );
   }
}

static WORKING_AREA(waComRcv, 512);
static msg_t ThreadComRcv( void *arg )
{
   while(TRUE)
   {
      DATA_COMM data __attribute__((aligned(sizeof(stkalign_t))));
      
      data = rcvData();
      
      chPoolFree(&comPool, &data);
      chMBPost(&mb_Comm, (msg_t)&data, TIME_INFINITE);
      
      chThdSleepMilliseconds( 100 );
   }
}

/*
 * Application entry point.
 */
int main(void) {
   
   msg_t msg;
   
   
   halInit();
   chSysInit();
   
   chPoolInit(&comPool, sizeof(DATA_COMM), NULL);
   chMBInit(&mb_Comm, (msg_t*)msg, sizeof(msg));
   
   
   chThdCreateStatic(waMotor, sizeof(waMotor), NORMALPRIO, ThreadMotor, NULL);
   chThdCreateStatic(waComSnd, sizeof(waComSnd), NORMALPRIO, ThreadComSnd, NULL);
   chThdCreateStatic(waComRcv, sizeof(waComRcv), NORMALPRIO, ThreadComRcv, NULL);

   while(1)
   {
      chThdSleepMilliseconds( 500 );
   }
}

Code: Select all

#include "XBee.h"

 SerialConfig uartCfg=
{
    BAUDRATE,                           /* Baudrate                                                       */
    0,                                        /* cr1 register values                                              */
    0,                                        /* cr2 register values                                              */
    0                                       /* cr3 register values                                              */
};

void initXBee()
{
      //SD2   
   palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
   palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));

   sdStart(&SD2, &uartCfg);
}

// Communication sans fils
void sendData ( DATA_COMM *data )
{
   char bufSend[40]={0};

   sprintf(bufSend, "T%03dR%03dL%03dA%05dB%03dS%03d", data->tangage, data->roulis, data->lacet, data->altitude, data->battery, data->signal);
   sdWrite(&SD2, (uint8_t*)bufSend, strlen(bufSend));
}

DATA_COMM rcvData()
{   
   int i = 0;
   int j = 0;
   DATA_COMM data;
   int retSscanf = 0;
   char bufRead[] = {'T','0','1','5','R','0','4','6','L','0','1','3','A','0','1','2','4','5','B','0','9','5','S','0','5','1','\0'};
   char bufFinalString[] = {'T','0','1','5','R','0','4','6','L','0','1','3','A','0','1','2','4','5','B','0','9','5','S','0','5','1','\0'};

   sdReadTimeout(&SD2, (uint8_t*)bufRead, (size_t)strlen(bufRead), (systime_t)TIME_INFINITE); 
   i = 0;
   while(bufRead[i] != 'T')
   {
      i++;
   }
   
   for ( j = 0; j<strlen(bufRead); j++)
   {
      bufFinalString[j] = bufRead[(j+i)%strlen(bufRead)];
   }
   
   sscanf(bufFinalString, "T%dR%dL%dA%dB%dS%d", &data.tangage, &data.roulis, &data.lacet, &data.altitude, &data.battery, &data.signal);

   
   return data;
}