www.pudn.com > MPL.zip > log_stm32.c, change:2016-11-26,size:6100b


/* 
 $License: 
    Copyright (C) 2010 InvenSense Corporation, All Rights Reserved. 
 $ 
 */ 
/******************************************************************************* 
 * $Id: $ 
 *******************************************************************************/ 
 
/** 
 *  @defgroup MSP430_System_Layer MSP430 System Layer 
 *  @brief  MSP430 System Layer APIs. 
 *          To interface with any platform, eMPL needs access to various 
 *          system layer functions. 
 * 
 *  @{ 
 *      @file   log_msp430.c 
 *      @brief  Logging facility for the TI MSP430. 
 */ 
 
#include <stdio.h> 
#include <stdint.h> 
#include <stdlib.h> 
#include <string.h> 
#include <stdarg.h> 
 
#include "packet.h" 
#include "log.h" 
#include "usart.h" 
 
#define BUF_SIZE        (256) 
#define PACKET_LENGTH   (23) 
 
#define PACKET_DEBUG    (1) 
#define PACKET_QUAT     (2) 
#define PACKET_DATA     (3) 
 
/** 
 *  @brief      Prints a variable argument log message. 
 *  USB output will be formatted as follows:\n 
 *  packet[0]       = $\n 
 *  packet[1]       = packet type (1: debug, 2: quat, 3: data)\n 
 *  packet[2]       = \n for debug packets: log priority\n 
 *                    for quaternion packets: unused\n 
 *                    for data packets: packet content (accel, gyro, etc)\n 
 *  packet[3-20]    = data\n 
 *  packet[21]      = \\r\n 
 *  packet[22]      = \\n 
 *  @param[in]  priority    Log priority (based on Android). 
 *  @param[in]  tag         File specific string. 
 *  @param[in]  fmt         String of text with optional format tags. 
 * 
 *  @return     0 if successful. 
 */ 
int _MLPrintLog (int priority, const char* tag, const char* fmt, ...) 
{ 
    va_list args; 
    int length, ii, i; 
    char buf[BUF_SIZE], out[PACKET_LENGTH], this_length; 
    FILE * noUse; 
    /* This can be modified to exit for unsupported priorities. */ 
    switch (priority) { 
    case MPL_LOG_UNKNOWN: 
    case MPL_LOG_DEFAULT: 
    case MPL_LOG_VERBOSE: 
    case MPL_LOG_DEBUG: 
    case MPL_LOG_INFO: 
    case MPL_LOG_WARN: 
    case MPL_LOG_ERROR: 
    case MPL_LOG_SILENT: 
        break; 
    default: 
        return 0; 
    } 
 
    va_start(args, fmt); 
 
    length = vsprintf(buf, fmt, args); 
    if (length <= 0) { 
        va_end(args); 
        return length; 
    } 
 
    memset(out, 0, PACKET_LENGTH); 
    out[0] = '$'; 
    out[1] = PACKET_DEBUG; 
    out[2] = priority; 
    out[21] = '\r'; 
    out[22] = '\n'; 
    for (ii = 0; ii < length; ii += (PACKET_LENGTH-5)) { 
#define min(a,b) ((a < b) ? a : b) 
        this_length = min(length-ii, PACKET_LENGTH-5); 
        memset(out+3, 0, 18); 
        memcpy(out+3, buf+ii, this_length); 
        for (i=0; i<PACKET_LENGTH; i++) { 
          fputc(out[i],noUse); 
        } 
    } 
     
             
    va_end(args); 
 
    return 0; 
} 
 
void eMPL_send_quat(long *quat) 
{ 
    char out[PACKET_LENGTH]; 
    int i; 
    FILE *noUse; 
    if (!quat) 
        return; 
    memset(out, 0, PACKET_LENGTH); 
    out[0] = '$'; 
    out[1] = PACKET_QUAT; 
    out[3] = (char)(quat[0] >> 24); 
    out[4] = (char)(quat[0] >> 16); 
    out[5] = (char)(quat[0] >> 8); 
    out[6] = (char)quat[0]; 
    out[7] = (char)(quat[1] >> 24); 
    out[8] = (char)(quat[1] >> 16); 
    out[9] = (char)(quat[1] >> 8); 
    out[10] = (char)quat[1]; 
    out[11] = (char)(quat[2] >> 24); 
    out[12] = (char)(quat[2] >> 16); 
    out[13] = (char)(quat[2] >> 8); 
    out[14] = (char)quat[2]; 
    out[15] = (char)(quat[3] >> 24); 
    out[16] = (char)(quat[3] >> 16); 
    out[17] = (char)(quat[3] >> 8); 
    out[18] = (char)quat[3]; 
    out[21] = '\r'; 
    out[22] = '\n'; 
     
    for (i=0; i<PACKET_LENGTH; i++) { 
      fputc(out[i],noUse); 
    } 
} 
 
void eMPL_send_data(unsigned char type, long *data) 
{ 
    char out[PACKET_LENGTH]; 
    int i; 
    FILE *noUse; 
    if (!data) 
        return; 
    memset(out, 0, PACKET_LENGTH); 
    out[0] = '$'; 
    out[1] = PACKET_DATA; 
    out[2] = type; 
    out[21] = '\r'; 
    out[22] = '\n'; 
    switch (type) { 
    /* Two bytes per-element. */ 
    case PACKET_DATA_ROT: 
        out[3] = (char)(data[0] >> 24); 
        out[4] = (char)(data[0] >> 16); 
        out[5] = (char)(data[1] >> 24); 
        out[6] = (char)(data[1] >> 16); 
        out[7] = (char)(data[2] >> 24); 
        out[8] = (char)(data[2] >> 16); 
        out[9] = (char)(data[3] >> 24); 
        out[10] = (char)(data[3] >> 16); 
        out[11] = (char)(data[4] >> 24); 
        out[12] = (char)(data[4] >> 16); 
        out[13] = (char)(data[5] >> 24); 
        out[14] = (char)(data[5] >> 16); 
        out[15] = (char)(data[6] >> 24); 
        out[16] = (char)(data[6] >> 16); 
        out[17] = (char)(data[7] >> 24); 
        out[18] = (char)(data[7] >> 16); 
        out[19] = (char)(data[8] >> 24); 
        out[20] = (char)(data[8] >> 16); 
        break; 
    /* Four bytes per-element. */ 
    /* Four elements. */ 
    case PACKET_DATA_QUAT: 
        out[15] = (char)(data[3] >> 24); 
        out[16] = (char)(data[3] >> 16); 
        out[17] = (char)(data[3] >> 8); 
        out[18] = (char)data[3]; 
    /* Three elements. */ 
    case PACKET_DATA_ACCEL: 
    case PACKET_DATA_GYRO: 
    case PACKET_DATA_COMPASS: 
    case PACKET_DATA_EULER: 
        out[3] = (char)(data[0] >> 24); 
        out[4] = (char)(data[0] >> 16); 
        out[5] = (char)(data[0] >> 8); 
        out[6] = (char)data[0]; 
        out[7] = (char)(data[1] >> 24); 
        out[8] = (char)(data[1] >> 16); 
        out[9] = (char)(data[1] >> 8); 
        out[10] = (char)data[1]; 
        out[11] = (char)(data[2] >> 24); 
        out[12] = (char)(data[2] >> 16); 
        out[13] = (char)(data[2] >> 8); 
        out[14] = (char)data[2]; 
        break; 
    case PACKET_DATA_HEADING: 
        out[3] = (char)(data[0] >> 24); 
        out[4] = (char)(data[0] >> 16); 
        out[5] = (char)(data[0] >> 8); 
        out[6] = (char)data[0]; 
        break; 
    default: 
        return; 
    } 
    for (i=0; i<PACKET_LENGTH; i++) { 
      fputc(out[i],noUse); 
    } 
} 
 
/** 
 * @} 
**/