bsproxy/bpfoot.c File Reference

Detailed Description

Built-in client device support for the RoadNarrows BrainPack foot.

Version:
$LastChangedDate$ $Rev$
Author:
Robin Knight (robin.knight@roadnarrows.com)
Copyright:
(C) 2007. RoadNarrows LLC. (http://www.roadnarrows.com)
All Rights Reserved

Definition in file bpfoot.c.

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rnr/rnrconfig.h"
#include "rnr/log.h"
#include "botsense/bsproxy_if.h"
#include "bsproxy.h"

Go to the source code of this file.

Defines

#define BPFOOT_NUM_SENSORS   8
 number of pressure sensors
#define TWI_CMD_SET_CALIBRATION   0x10
 set calibration
#define TWI_CMD_GET_RAWPRESSUREDATA   0x20
 get sensor raw pressure data
#define TWI_CMD_GET_PRESSUREDATA   0x30
 get sensor calib. pressure data
#define TWI_CMD_GET_PRESSUREDIR   0x40
 get sensor vector direction
#define TWI_CMD_GET_PRESSUREMAG   0x41
 get sensor vector magnitude
#define TWI_CMD_GET_VERSION   0x50
 get firmware version
#define TWI_CMD_GET_DEVICEID   0x60
 get BrainPack device id
#define TWI_CMD_MAX_LEN   2
 max. I2C command length
#define TWI_RSP_MAX_LEN   4
 max. I2C response length

Functions

static int BPFootCmdGetIds (BsProxyServer_T *pServer, BsProxyClient_T *pClient, BsProxyClientDev_T *pDev, byte_t bufCmd[], size_t lenCmd)
 BrainPack Foot command to get foot device id and firmware version.
static int BPFootCmdGetRaw (BsProxyServer_T *pServer, BsProxyClient_T *pClient, BsProxyClientDev_T *pDev, byte_t bufCmd[], size_t lenCmd)
 BrainPack Foot command to get raw sensor data.
static int BPFootCmdGetCooked (BsProxyServer_T *pServer, BsProxyClient_T *pClient, BsProxyClientDev_T *pDev, byte_t bufCmd[], size_t lenCmd)
 BrainPack Foot command to get calibrated sensor data.
static int BPFootCmdSetCal (BsProxyServer_T *pServer, BsProxyClient_T *pClient, BsProxyClientDev_T *pDev, byte_t bufCmd[], size_t lenCmd)
 BrainPack Foot command to set pressure sensor calibration.
int BPFootDispatch (BsProxyServer_T *pServer, BsProxyClient_T *pClient, BsProxyClientDev_T *pDev, byte_t bufCmd[], size_t lenCmd)
 BrainPack Foot command dispatcher.

Define Documentation

#define BPFOOT_NUM_SENSORS   8
 

number of pressure sensors

BrainPack Characteristics

Definition at line 60 of file bpfoot.c.

Referenced by BPFootCmdGetCooked(), and BPFootCmdGetRaw().

#define TWI_CMD_SET_CALIBRATION   0x10
 

set calibration

Two Wire Interface (I2C) BrainPack Foot Messaging

Todo:
need shared header file

Definition at line 67 of file bpfoot.c.

Referenced by BPFootCmdSetCal(), and BPIMUCmdSetCal().

Function Documentation

static int BPFootCmdGetCooked (  BsProxyServer_T *  pServer,
BsProxyClient_T *  pClient,
BsProxyClientDev_T *  pDev,
byte_t  bufCmd[],
size_t  lenCmd
)  [static]
 

BrainPack Foot command to get calibrated sensor data.

There a 8 pressure sensors, with each sensor providing 2 bytes of data.

Command Format:
cmdhdr handle cmd_id
Response Format:
rsphdr press_cal0 ... press_cal7
rsphdr errmsg
Parameters:
pServer BotSense server.
pClient BotSense client.
pDev Client device.
bufCmd Received command buffer.
lenCmd Received command length (number of bytes).
Returns:
Returns OK on success, RC_ERROR on failure.

Definition at line 239 of file bpfoot.c.

References BPFOOT_NUM_SENSORS, BSPROXY_LOG_DEV_CMD, BSPROXY_MSG_MAX_LEN, BSUNPACK16(), ClientSendFailRsp(), BsProxyClientDev_T::m_i2cAddr, BsProxyClientDev_T::m_pProxDev, ProxDevI2CTrans(), TWI_CMD_GET_PRESSUREDATA, TWI_CMD_MAX_LEN, and TWI_RSP_MAX_LEN.

Referenced by BPFootDispatch().

00243 {
00244   byte_t        bufRsp[BSPROXY_MSG_MAX_LEN];
00245   size_t        lenRsp;
00246   byte_t        bufI2CWrite[TWI_CMD_MAX_LEN];
00247   byte_t        bufI2CRead[TWI_RSP_MAX_LEN];
00248   size_t        n;
00249   int           rc;
00250   int           iSensor;
00251   uint_t        uCooked[BPFOOT_NUM_SENSORS];
00252   //uint_t        uVecDir;
00253   //uint_t        uVecMag;
00254 
00255   BSPROXY_LOG_DEV_CMD(pClient, bufCmd, lenCmd);
00256 
00257   //
00258   // Get calibrated sensor pressure data 
00259   //
00260   for(iSensor=1; iSensor<=BPFOOT_NUM_SENSORS; ++iSensor)
00261   {
00262     n                 = 0;
00263     bufI2CWrite[n++]  = TWI_CMD_GET_PRESSUREDATA;
00264     bufI2CWrite[n++]  = (byte_t)iSensor;
00265 
00266     rc = ProxDevI2CTrans(pDev->m_pProxDev, pDev->m_i2cAddr,
00267                         bufI2CWrite, n,
00268                         bufI2CRead, (size_t)2);
00269 
00270     if( rc <= 0 )
00271     {
00272       return ClientSendFailRsp(pClient, bufCmd, "cannot get sensor data");
00273     }
00274 
00275     BSUNPACK16(&bufI2CRead[0], &uCooked[iSensor-1]);
00276   }
00277 
00278 #if 0
00279   //
00280   // Get calculated pressure vector direction
00281   //
00282   n                 = 0;
00283   bufI2CWrite[n++]  = TWI_CMD_GET_PRESSUREDIR;
00284 
00285   rc = ProxDevI2CTrans(pDev->m_pProxDev, pDev->m_i2cAddr,
00286                         bufI2CWrite, n,
00287                         bufI2CRead, (size_t)4);
00288 
00289   if( rc <= 0 )
00290   {
00291     return ClientSendFailRsp(pClient, bufCmd, "cannot get vector direction");
00292   }
00293 
00294   BSUNPACK32(&bufI2CRead[0], &uVecDir);
00295 
00296   //
00297   // Get calculated pressure vector magnitude
00298   //
00299   n                 = 0;
00300   bufI2CWrite[n++]  = TWI_CMD_GET_PRESSUREMAG;
00301 
00302   rc = ProxDevI2CTrans(pDev->m_pProxDev, pDev->m_i2cAddr,
00303                       bufI2CWrite, n,
00304                       bufI2CRead, (size_t)4);
00305 
00306   if( rc <= 0 )
00307   {
00308     return ClientSendFailRsp(pClient, bufCmd, "cannot get vector magnitude");
00309   }
00310 
00311   BSUNPACK32(&bufI2CRead[0], &uVecMag);
00312 #endif // 0
00313 
00314   lenRsp            = BSPROXY_RSP_HDR_LEN;
00315 
00316   for(iSensor=0; iSensor<BPFOOT_NUM_SENSORS; ++iSensor)
00317   {
00318     lenRsp += BSPACK16(uCooked[iSensor], &bufRsp[lenRsp]);
00319   }
00320   //lenRsp += BSPACK32(uVecDir, &bufRsp[lenRsp]);
00321   //lenRsp += BSPACK32(uVecMag, &bufRsp[lenRsp]);
00322 
00323   return ClientSendPassRsp(pClient, bufCmd, bufRsp, lenRsp);
00324 }

static int BPFootCmdGetIds (  BsProxyServer_T *  pServer,
BsProxyClient_T *  pClient,
BsProxyClientDev_T *  pDev,
byte_t  bufCmd[],
size_t  lenCmd
)  [static]
 

BrainPack Foot command to get foot device id and firmware version.

Command Format:
cmdhdr handle cmd_id
Response Format:
rsphdr dev_id version
rsphdr errmsg
Parameters:
pServer BotSense server.
pClient BotSense client.
pDev Client device.
bufCmd Received command buffer.
lenCmd Received command length (number of bytes).
Returns:
Returns OK on success, RC_ERROR on failure.

Definition at line 100 of file bpfoot.c.

References BSPACK16(), BSPROXY_LOG_DEV_CMD, BSPROXY_MSG_MAX_LEN, BSPROXY_RSP_HDR_LEN, BSUNPACK16(), ClientSendFailRsp(), ClientSendPassRsp(), BsProxyClientDev_T::m_i2cAddr, BsProxyClientDev_T::m_pProxDev, ProxDevI2CTrans(), TWI_CMD_GET_DEVICEID, TWI_CMD_GET_VERSION, TWI_CMD_MAX_LEN, and TWI_RSP_MAX_LEN.

Referenced by BPFootDispatch().

00104 {
00105   byte_t        bufRsp[BSPROXY_MSG_MAX_LEN];
00106   size_t        lenRsp;
00107   byte_t        bufI2CWrite[TWI_CMD_MAX_LEN];
00108   byte_t        bufI2CRead[TWI_RSP_MAX_LEN];
00109   size_t        n;
00110   int           rc;
00111   uint_t        uDevId;
00112   byte_t        byVersion;
00113 
00114   BSPROXY_LOG_DEV_CMD(pClient, bufCmd, lenCmd);
00115 
00116   n                 = 0;
00117   bufI2CWrite[n++]  = TWI_CMD_GET_DEVICEID;
00118 
00119   rc = ProxDevI2CTrans(pDev->m_pProxDev, pDev->m_i2cAddr,
00120                       bufI2CWrite, n,
00121                       bufI2CRead, (size_t)2);
00122 
00123   if( rc <= 0 )
00124   {
00125     return ClientSendFailRsp(pClient, bufCmd, "cannot get device id");
00126   }
00127 
00128   BSUNPACK16(&bufI2CRead[0], &uDevId);
00129 
00130   LOGDIAG3("%u, %02x %02x", uDevId, bufI2CRead[0], bufI2CRead[1]);
00131 
00132   n                 = 0;
00133   bufI2CWrite[n++]  = TWI_CMD_GET_VERSION;
00134 
00135   rc = ProxDevI2CTrans(pDev->m_pProxDev, pDev->m_i2cAddr,
00136                       bufI2CWrite, n,
00137                       bufI2CRead, (size_t)1);
00138 
00139   if( rc <= 0 )
00140   {
00141     return ClientSendFailRsp(pClient, bufCmd, "cannot get firmware version");
00142   }
00143 
00144   byVersion = bufI2CRead[0];
00145 
00146   LOGDIAG3("%u", (uint_t)byVersion);
00147 
00148   lenRsp            = BSPROXY_RSP_HDR_LEN;
00149   lenRsp           += BSPACK16(uDevId, &bufRsp[lenRsp]);
00150   bufRsp[lenRsp++]  = byVersion;
00151 
00152   return ClientSendPassRsp(pClient, bufCmd, bufRsp, lenRsp);
00153 }

static int BPFootCmdGetRaw (  BsProxyServer_T *  pServer,
BsProxyClient_T *  pClient,
BsProxyClientDev_T *  pDev,
byte_t  bufCmd[],
size_t  lenCmd
)  [static]
 

BrainPack Foot command to get raw sensor data.

There a 8 pressure sensors, with each sensor providing 2 bytes of data.

Command Format:
cmdhdr handle cmd_id
Response Format:
rsphdr press_raw0 ... press_raw7
rsphdr errmsg
Parameters:
pServer BotSense server.
pClient BotSense client.
pDev Client device.
bufCmd Received command buffer.
lenCmd Received command length (number of bytes).
Returns:
Returns OK on success, RC_ERROR on failure.

Definition at line 175 of file bpfoot.c.

References BPFOOT_NUM_SENSORS, BSPROXY_LOG_DEV_CMD, BSPROXY_MSG_MAX_LEN, BSUNPACK16(), ClientSendFailRsp(), BsProxyClientDev_T::m_i2cAddr, BsProxyClientDev_T::m_pProxDev, ProxDevI2CTrans(), TWI_CMD_GET_RAWPRESSUREDATA, TWI_CMD_MAX_LEN, and TWI_RSP_MAX_LEN.

Referenced by BPFootDispatch().

00179 {
00180   byte_t        bufRsp[BSPROXY_MSG_MAX_LEN];
00181   size_t        lenRsp;
00182   byte_t        bufI2CWrite[TWI_CMD_MAX_LEN];
00183   byte_t        bufI2CRead[TWI_RSP_MAX_LEN];
00184   size_t        n;
00185   int           rc;
00186   int           iSensor;
00187   uint_t        uRaw[BPFOOT_NUM_SENSORS];
00188 
00189   BSPROXY_LOG_DEV_CMD(pClient, bufCmd, lenCmd);
00190 
00191   for(iSensor=1; iSensor<=BPFOOT_NUM_SENSORS; ++iSensor)
00192   {
00193     n                 = 0;
00194     bufI2CWrite[n++]  = TWI_CMD_GET_RAWPRESSUREDATA;
00195     bufI2CWrite[n++]  = (byte_t)iSensor;
00196 
00197     rc = ProxDevI2CTrans(pDev->m_pProxDev, pDev->m_i2cAddr,
00198                         bufI2CWrite, n,
00199                         bufI2CRead, (size_t)2);
00200 
00201     if( rc <= 0 )
00202     {
00203       return ClientSendFailRsp(pClient, bufCmd, "cannot get raw data");
00204     }
00205 
00206     BSUNPACK16(&bufI2CRead[0], &uRaw[iSensor-1]);
00207   }
00208 
00209   lenRsp            = BSPROXY_RSP_HDR_LEN;
00210 
00211   for(iSensor=0; iSensor<BPFOOT_NUM_SENSORS; ++iSensor)
00212   {
00213     lenRsp += BSPACK16(uRaw[iSensor], &bufRsp[lenRsp]);
00214   }
00215 
00216   return ClientSendPassRsp(pClient, bufCmd, bufRsp, lenRsp);
00217 }

static int BPFootCmdSetCal (  BsProxyServer_T *  pServer,
BsProxyClient_T *  pClient,
BsProxyClientDev_T *  pDev,
byte_t  bufCmd[],
size_t  lenCmd
)  [static]
 

BrainPack Foot command to set pressure sensor calibration.

Command Format:
cmdhdr handle cmd_id
Response Format:
rsphdr
rsphdr errmsg
Parameters:
pServer BotSense server.
pClient BotSense client.
pDev Client device.
bufCmd Received command buffer.
lenCmd Received command length (number of bytes).
Returns:
Returns OK on success, RC_ERROR on failure.

Definition at line 344 of file bpfoot.c.

References BSPROXY_LOG_DEV_CMD, BSPROXY_MSG_MAX_LEN, BSPROXY_RSP_HDR_LEN, ClientSendFailRsp(), ClientSendPassRsp(), BsProxyClientDev_T::m_i2cAddr, BsProxyClientDev_T::m_pProxDev, ProxDevI2CWrite(), TWI_CMD_MAX_LEN, and TWI_CMD_SET_CALIBRATION.

Referenced by BPFootDispatch().

00348 {
00349   byte_t        bufRsp[BSPROXY_MSG_MAX_LEN];
00350   byte_t        bufI2CWrite[TWI_CMD_MAX_LEN];
00351   size_t        n;
00352   int           rc;
00353 
00354   BSPROXY_LOG_DEV_CMD(pClient, bufCmd, lenCmd);
00355 
00356   n                 = 0;
00357   bufI2CWrite[n++]  = TWI_CMD_SET_CALIBRATION;
00358 
00359   rc = ProxDevI2CWrite(pDev->m_pProxDev, pDev->m_i2cAddr,
00360                       bufI2CWrite, n);
00361 
00362   if( rc <= 0 )
00363   {
00364     return ClientSendFailRsp(pClient, bufCmd, "cannot set calibration data");
00365   }
00366 
00367   return ClientSendPassRsp(pClient, bufCmd, bufRsp,
00368       (size_t)BSPROXY_RSP_HDR_LEN);
00369 }

int BPFootDispatch (  BsProxyServer_T *  pServer,
BsProxyClient_T *  pClient,
BsProxyClientDev_T *  pDev,
byte_t  bufCmd[],
size_t  lenCmd
) 
 

BrainPack Foot command dispatcher.

Parameters:
pServer BotSense server.
pClient BotSense client.
pDev Client device.
bufCmd Received command buffer.
lenCmd Received command length (number of bytes).
Returns:
Returns OK on success, RC_ERROR on failure.

Definition at line 387 of file bpfoot.c.

References BPFootCmdGetCooked(), BPFootCmdGetIds(), BPFootCmdGetRaw(), BPFootCmdSetCal(), BSPROXY_BPFOOT_CMDID_CAL, BSPROXY_BPFOOT_CMDID_GET_COOKED, BSPROXY_BPFOOT_CMDID_GET_IDS, BSPROXY_BPFOOT_CMDID_GET_RAW, BSPROXY_CMD_HDR_LEN, ClientSendFailRsp(), and BsProxyClient_T::m_sClientName.

Referenced by CmdDevCmd().

00391 {
00392   byte_t  byDevCmdId;
00393 
00394   byDevCmdId = bufCmd[BSPROXY_CMD_HDR_LEN+1];
00395 
00396   //
00397   // Dispatch BrainPack Foot command.
00398   //
00399   switch( byDevCmdId )
00400   {
00401     // get device identifiers
00402     case BSPROXY_BPFOOT_CMDID_GET_IDS:
00403       return BPFootCmdGetIds(pServer, pClient, pDev, bufCmd, lenCmd);
00404 
00405     // get raw sensor data 
00406     case BSPROXY_BPFOOT_CMDID_GET_RAW:
00407       return BPFootCmdGetRaw(pServer, pClient, pDev, bufCmd, lenCmd);
00408 
00409     // get cooked sensor data 
00410     case BSPROXY_BPFOOT_CMDID_GET_COOKED:
00411       return BPFootCmdGetCooked(pServer, pClient, pDev, bufCmd, lenCmd);
00412 
00413     // calibrate sensors
00414     case BSPROXY_BPFOOT_CMDID_CAL:
00415       return BPFootCmdSetCal(pServer, pClient, pDev, bufCmd, lenCmd);
00416 
00417     default:
00418       LOGERROR("%s: BrainPack Foot: 0x%02x: unsupported device command",
00419           pClient->m_sClientName, byDevCmdId);
00420       return ClientSendFailRsp(pClient, bufCmd, "bad device command");
00421   }
00422 
00423   LOGERROR("%s: BrainPack Foot: here?", pClient->m_sClientName);
00424   return ClientSendFailRsp(pClient, bufCmd, "bad message");
00425 }

Generated on Wed Nov 28 11:01:08 2007 for botsense by doxygen 1.4.6