Important changes to forums and questions

All forums and questions are now archived. To start a new conversation or read the latest updates go to forums.mbed.com.

K64F RTOS Serial port

Topic last updated 13 Jun 2014, by A W. 0 replies
A W
13 Jun 2014

Hardware: K64F, onboard compass/accel, venus GPS (115200, 10Hz)

Currently I am trying to access the serial port and compass using the mbed RTOS with mixed results. The program will run, but it will freeze at unknown times. I have seen this problem solved with the MODSERIAL and buffered serial libraries but, at this point I have not been able to implement them on the K64F. Without the compass code the program will run indefinately, but the shared resources have become a problem.

Is there a way to safely access the serial port with RTOS and onboard senors?

FYI, my background is mechanical engineering and I am stretching my capabilities with the code...

#include "mbed.h"
#include "rtos.h"
#include "nmea.h"
#include "ff.h"

#include "FXOS8700Q.h"
#include "eCompass_Lib.h"


//DigitalOut blue(LED3); // BLUE LED on K64F Freedom board

//______________________________ DEFINE COMPASS_____________________
FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
eCompass compass;

extern axis6_t axis6;
extern uint32_t seconds;
extern uint32_t compass_type; // optional, NED compass is default
extern int32_t tcount;
extern uint8_t cdebug;
int  l = 0;
volatile int sflag = 0;

MotionSensorDataCounts mag_raw;
MotionSensorDataCounts acc_raw;
Thread *(calibrate);

// This HAL map is for the FXOS8700Q on the K64F Freedom board.
void hal_map( MotionSensorDataCounts * acc_raw, MotionSensorDataCounts * mag_raw)
{
    int16_t t;
// swap and negate accelerometer X & Y axis
    t = acc_raw->x;
    acc_raw->x = acc_raw->y * -1;
    acc_raw->y = t * -1;
// swap magnetometer X & Y axis
    t = mag_raw->x;
    mag_raw->x = mag_raw->y;
    mag_raw->y = t;
// negate magnetometer Z axis
    mag_raw->z *= -1;
}

void calibrate_thread(void const *argument)
{
    while(true) {
        Thread::signal_wait(0x2);
        compass.calibrate();
    }
}

void compass_thread(void const *argument)
{
    // get raw data from the sensors
    acc.getAxis( acc_raw);
    mag.getAxis( mag_raw);
    if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
    if(l++ >= 49) { // take car of business once a second
        seconds++;
        calibrate->signal_set(0x2);
        l = 0;
        sflag = 1;
    }
    tcount++;
}


//______________________________ END DEFINE COMPASS_____________________

RawSerial pc(USBTX, USBRX);
RawSerial gps(PTC17, PTC16);

Thread *RX_THREAD_POINTER;

int get_filename(char *fn);

NMEA nmea;

Semaphore sd_sem(0);

typedef struct {
    char line[80];
} sensor_line;
MemoryPool<sensor_line, 16> mpool_sensor_line;
Queue<sensor_line, 16> queue_sensor_line;


// Rx Interupt routine
void Rx_interrupt(void)
{
    gps.attach(NULL, gps.RxIrq); // Disable Rx interrupt
    (*RX_THREAD_POINTER).signal_set(0x1);
}


void gps_thread(void const *args)
{

    gps.baud(115200);

    while(1) {

        Thread::signal_wait(0x1);
        while(gps.readable()) {
            volatile char c = gps.getc();
            nmea.fusedata(c);
        }
        gps.attach(&Rx_interrupt, gps.RxIrq);
    }
}

void log_thread(const void *args)
{

    DigitalOut log_led(LED3);

    FATFS fs;
    FIL fp_sensor;

    f_mount(0, &fs);
    f_open(&fp_sensor, "0:sensors.csv", FA_OPEN_EXISTING | FA_WRITE);
    f_lseek(&fp_sensor, f_size(&fp_sensor));


    sd_sem.release(); // sd card initialized... start sensor thread

    Timer t;
    t.start();

    while(1) {


        osEvent evt = queue_sensor_line.get(1);
        if(evt.status == osEventMessage) {

            log_led = !log_led;
            sensor_line *message = (sensor_line*)evt.value.p;
            f_puts(message->line, &fp_sensor);
            f_sync(&fp_sensor);

            pc.puts(message->line);

            mpool_sensor_line.free(message);
        }

    }
}

void sensor_thread(const void* args)
{
    DigitalOut sensor_led(LED2);

    while( sd_sem.wait(50) <= 0) {} // wait for the sd card to initialize and open files

    while(!nmea.isdataready()) {
        Thread::wait(50); // wait for lock
    }

    sensor_line *message = mpool_sensor_line.alloc();
    sprintf(message->line, "Lattitude");
    queue_sensor_line.put(message);

    while(true) {
        //get sensor line memory
        sensor_led = !sensor_led;

        sensor_line *message = mpool_sensor_line.alloc();
        sprintf(message->line, "Lat: %f Heading: %i \n\r", nmea.getLatitude(), axis6.yaw);
        queue_sensor_line.put(message);

        Thread::wait(200);
    }

}
int main()
{
    pc.baud(115200);
//eCOMPASS START

    acc.enable();
    mag.enable();

    acc.getAxis( acc_raw);
    mag.getAxis( mag_raw);
    
    RtosTimer compass_timer(compass_thread, osTimerPeriodic);

//eCOMPASS END

    Thread t2(calibrate_thread);
    calibrate = &t2;


    Thread t_rx(gps_thread);
    RX_THREAD_POINTER = &t_rx;
    t_rx.set_priority(osPriorityHigh);
    gps.attach(&Rx_interrupt, gps.RxIrq);

    Thread thread2(log_thread, NULL, osPriorityNormal);
    Thread thread3(sensor_thread, NULL, osPriorityNormal);


    calibrate->set_priority(osPriorityBelowNormal);
    compass_timer.start(20);

    while(true) {
        Thread::wait(osWaitForever);
    }
}