irobotSensorStream.cpp

00001 #include "irobotSensor.h"
00002 #include "irobotSensorStream.h"
00003 #include "irobotOpcode.h"
00004 #include "irobotError.h"
00005 #include "irobotUART.h"
00006 
00007 /// Upper-limit for the size of the OP_STREAM packet occurs when including all sensor IDs in a sensor packet
00008 #define OP_SENSOR_STREAM_MAX_CODES  43
00009 /// Maximum number of sensors, plus header and size bytes
00010 #define OP_SENSOR_STREAM_MAX_SIZE   OP_SENSOR_STREAM_MAX_CODES + 2
00011 /// header of a sensor stream packet
00012 #define SENSOR_STREAM_HEADER        19
00013 
00014 int32_t irobotSensorStreamConfigure(
00015     const irobotUARTPort_t          port,
00016     const irobotSensorCode * const  sensorCodes,
00017     uint8_t                         nSensorCodes
00018 ){
00019     uint8_t packet[OP_SENSOR_STREAM_MAX_SIZE];
00020     uint8_t packetIndex = 0;
00021     uint8_t sensorCodeIndex = 0;
00022 
00023     // check for NULL pointers
00024     if(nSensorCodes && !sensorCodes){
00025         return ERROR_INVALID_PARAMETER;
00026     }
00027 
00028     nSensorCodes = nSensorCodes < OP_SENSOR_STREAM_MAX_CODES ? nSensorCodes : OP_SENSOR_STREAM_MAX_CODES;
00029 
00030     packet[packetIndex++] = OP_STREAM;
00031     packet[packetIndex++] = nSensorCodes;
00032     for(sensorCodeIndex = 0; sensorCodeIndex < nSensorCodes; sensorCodeIndex++){
00033         packet[packetIndex++] = sensorCodes[sensorCodeIndex];
00034     }
00035 
00036     return irobotUARTWriteRaw(port, packet, packetIndex);
00037 }
00038 
00039 int32_t irobotSensorStreamPause(
00040     const irobotUARTPort_t port,
00041     const bool pause
00042 ){
00043     uint8_t packet[OP_PAUSE_RESUME_STREAM_SIZE];
00044 
00045     packet[0] = OP_PAUSE_RESUME_STREAM;
00046     packet[1] = !pause;
00047 
00048     return irobotUARTWriteRaw(port, packet, OP_PAUSE_RESUME_STREAM_SIZE);
00049 }
00050 
00051 int32_t irobotSensorStreamStartAll(
00052     const irobotUARTPort_t          port
00053 ){
00054     int32_t status = ERROR_SUCCESS;
00055     const irobotSensorCode allSensors = SENSOR_GROUP6;
00056 
00057     irobot_IfIsNotError(status, irobotSensorStreamConfigure(port, &allSensors, 1));
00058     irobot_IfIsNotError(status, irobotSensorStreamPause(port, false));
00059 
00060     return status;
00061 }
00062 
00063 int32_t irobotSensorStreamProcessAll(
00064     xqueue_t * const queue,
00065     irobotSensorGroup6_t * const sensors,
00066     bool * const packetFound
00067 ){
00068     int32_t status = ERROR_SUCCESS;
00069 
00070     // check for NULL pointers
00071     if(!queue || !sensors || !packetFound){
00072         return ERROR_INVALID_PARAMETER;
00073     }
00074 
00075     *packetFound = false;
00076 
00077     //  Align buffer with iRobot sensor stream, according to format:
00078     //  [Header:19] [Len:27] [Packet ID:0] [SenGroup6 (52 bytes)] [CxSum]
00079     while(!xqueue_empty(queue) && xqueue_front(queue) != SENSOR_STREAM_HEADER){
00080         xqueue_drop(queue);
00081     }
00082 
00083     // Check for properly formatted header;
00084     //  NOTE: iRobot OI spec incorrectly omits the header from the checksum
00085     if( xqueue_count(queue) >= SENSOR_GROUP6_SIZE + 4   // size of entire packet        */
00086      && xqueue_pop(queue) == SENSOR_STREAM_HEADER       // sensor stream packet         */
00087      && xqueue_pop(queue) == SENSOR_GROUP6_SIZE + 1     // size of payload + checksum   */
00088      && xqueue_pop(queue) == SENSOR_GROUP6){            // payload is sensor group 6    */
00089          // Checksum: cxsum = [Header:19] + [n-bytes:Sen6Size+1=53] + [packet ID:6] + [data (Sen6Size)]
00090          uint8_t cxsum = 0;
00091          cxsum += SENSOR_STREAM_HEADER;
00092          cxsum += SENSOR_GROUP6_SIZE + 1;
00093          cxsum += SENSOR_GROUP6;
00094          cxsum += xqueue_checksum(queue, SENSOR_GROUP6_SIZE + 1, 0);    // payload and encoded checksum
00095 
00096          // checksum passed
00097          if(cxsum == 0){
00098              irobot_StatusMerge(&status, irobotReadSensorGroup6(queue, sensors));
00099              xqueue_pop(queue);     // clear checksum
00100              if(!irobot_IsError(status)){
00101                  *packetFound = true;
00102              }
00103          }
00104     }
00105 
00106     return status;
00107 }