status_t SensorService::dump(int fd, const Vector<String16>& args)
{
const size_t SIZE = 1024;
char buffer[SIZE];
String8 result;
if (!PermissionCache::checkCallingPermission(sDump)) {
snprintf(buffer, SIZE, "Permission Denial: "
"can't dump SurfaceFlinger from pid=%d, uid=%d\n",
IPCThreadState::self()->getCallingPid(),
IPCThreadState::self()->getCallingUid());
result.append(buffer);
} else {
Mutex::Autolock _l(mLock);
snprintf(buffer, SIZE, "Sensor List:\n");
result.append(buffer);
for (size_t i=0 ; i<mSensorList.size() ; i++) {
const Sensor& s(mSensorList[i]);
const sensors_event_t& e(mLastEventSeen.valueFor(s.getHandle()));
snprintf(buffer, SIZE,
"%-48s| %-32s | 0x%08x | maxRate=%7.2fHz | "
"last=<%5.1f,%5.1f,%5.1f>\n",
s.getName().string(),
s.getVendor().string(),
s.getHandle(),
s.getMinDelay() ? (1000000.0f / s.getMinDelay()) : 0.0f,
e.data[0], e.data[1], e.data[2]);
result.append(buffer);
}
SensorFusion::getInstance().dump(result, buffer, SIZE);
SensorDevice::getInstance().dump(result, buffer, SIZE);
snprintf(buffer, SIZE, "%d active connections\n",
mActiveConnections.size());
result.append(buffer);
snprintf(buffer, SIZE, "Active sensors:\n");
result.append(buffer);
for (size_t i=0 ; i<mActiveSensors.size() ; i++) {
int handle = mActiveSensors.keyAt(i);
snprintf(buffer, SIZE, "%s (handle=0x%08x, connections=%d)\n",
getSensorName(handle).string(),
handle,
mActiveSensors.valueAt(i)->getNumConnections());
result.append(buffer);
}
}
write(fd, result.string(), result.size());
return NO_ERROR;
}
//responsible for displaying all the necessary information to the screen many times per second.
//runs at fairly low priority by design.
//output includes:
// * switch states
// * sensor triggering timestamps
// * current positions of up to 3 trains relative to parts of the track
// almost never blocks, just keeps spinning when nothing higher-priority needs to run
void display_server(void){
TrainState trains[MAX_TRAINS];
int train_count = 0;
SensorState sensors[SENSOR_HISTORY_LEN];
int sensor_start = 0;
int sensor_len = 0;
SwitchState switches[18+4];
int active = 0;
int i;
// initialze switches 1-18
for(i=0; i < 18; i++){
switches[i].number = i+1;
switches[i].state = '?';
}
// and switches 153-156
for(i=0; i < 4; i++){
switches[i+18].number = i+153;
switches[i+18].state = '?';
}
RegisterAs("displayserver");
int clockserverTid = WhoIs("clockserver");
if(clockserverTid < 0){
bwprintf(COM2, "Display server got tid %d for clocckserver\n\r", clockserverTid);
Halt();
}
Create(PRIORITY_MID, &display_notifier);
Create(PRIORITY_MID, &display_sensor_notifier);
int tid, len;
MsgPrintf msg;
int printf_col = 0;
// now we can start drawing and receiving messages in a loop.
FOREVER {
len = Receive(&tid, (char*) &msg, sizeof(msg));
if(len > 0){
// I never have a useful Reply, so Reply immediately
Reply(tid, NULL, 0);
switch(msg.type){
case MSG_DISPLAY_REGISTER: ;
// add a train with the provided number and speed 0
// if this is the first train, initialize the display
if(train_count == 0){
clear_screen();
initialize_scrolling();
refresh_switches(switches);
refresh_sensors(sensors, sensor_start, sensor_len);
active = 1;
}
MsgRegister *msgReg = (MsgRegister*) &msg;
trains[train_count].number = msgReg->train;
trains[train_count].speed = 0;
trains[train_count].route_len = 0;
train_count++;
break;
case MSG_DISPLAY_DEREGISTER: ;
// look up the train in the trains array, and pack the ones after it
// special case for the last train
msgReg = (MsgRegister*) &msg;
if(train_count == 1){
//clear_screen();
restore_scrolling();
move_cursor(SCREEN_BOTTOM, 1);
active = 0;
train_count = 0;
} else if(train_count == 0){
// do nothing
} else {
int i = 0;
for(i=0; i < train_count; i++){
if(trains[i].number == msgReg->train){
break;
}
}
// if we find the train, shift the rest of the array down
if(i < train_count){
for(i=i+1; i < train_count; i++){
//copy to the previous one
trains[i-1] = trains[i];
}
train_count--;
}
}
break;
case MSG_DISPLAY_SPEED: ;
// update the given train's speed
MsgSpeed *msgSpeed = (MsgSpeed*) &msg;
// flip through my trains for this number
for(i = 0; i < train_count; i++){
if(trains[i].number == msgSpeed->train){
trains[i].speed = msgSpeed->speed;
break;
}
}
// will be redisplayed below
break;
case MSG_DISPLAY_SWITCH: ;
MsgSwitch *msgSwitch = (MsgSwitch*) &msg;
if(msgSwitch->number <= 18){
switches[msgSwitch->number-1].state = msgSwitch->state;
} else {
switches[msgSwitch->number-153+18].state = msgSwitch->state;
}
if(active){
refresh_switches(switches);
}
break;
case MSG_DISPLAY_ROUTE: ;
// somewhat more complicated. copy the route for the given train
// first find the train index
MsgRouteSegment *msgRoute = (MsgRouteSegment*) &msg;
for(i = 0; i < train_count; i++){
if(trains[i].number == msgRoute->train){
trains[i].route_len = msgRoute->route_len;
int j;
for(j = 0; j < msgRoute->route_len; j++){
trains[i].route[j] = msgRoute->route[j];
}
}
}
break;
case MSG_SENSOR_CLIENT: ; // message from the sensor server
// work through the array from the sensor server, and update my list of sensors
// remember that the sensors array starts at start and start gets decremented
MsgSensorResponse *msgResp = (MsgSensorResponse*) &msg;
int timestamp = Time(clockserverTid);
for(i = 0; i < msgResp->length; i++){
sensor_start--;
if(sensor_start < 0) sensor_start = SENSOR_HISTORY_LEN-1;
if(sensor_len < SENSOR_HISTORY_LEN) sensor_len++;
getSensorName(msgResp->triggered[i], &(sensors[sensor_start]));
sensors[sensor_start].time = timestamp;
}
if(active && msgResp->length > 0){
refresh_sensors(sensors, sensor_start, sensor_len);
}
break;
case MSG_DISPLAY_PRINTF: ;
move_cursor(SCREEN_BOTTOM, printf_col);
switch(msg.argc){
case 0: aprintf(COM2, msg.format); break;
case 1: aprintf(COM2, msg.format, msg.argv[0]); break;
case 2: aprintf(COM2, msg.format, msg.argv[0], msg.argv[1]); break;
case 3: aprintf(COM2, msg.format, msg.argv[0], msg.argv[1], msg.argv[2]); break;
case 4: aprintf(COM2, msg.format, msg.argv[0], msg.argv[1], msg.argv[2], msg.argv[3]); break;
case 5: aprintf(COM2, msg.format, msg.argv[0], msg.argv[1], msg.argv[2], msg.argv[3], msg.argv[4]); break;
default: aprintf(COM2, "Bad argc: %d\n\r", msg.argc); break;
}
int len = strlen(msg.format);
if(msg.format[len-1] == '\n' || msg.format[len-1] == '\r'){
printf_col = 0;
} else {
printf_col += len;
}
break;
case MSG_TICK: ;
// display the train position
// very crude for now. also only handling one train
if(active){
int i;
int base_row;
for(i = 0; i < train_count; i++){
base_row = TRAIN_TOP + TRAIN_SIZE*i;
move_and_clear(base_row, TRAIN_LEFT);
aprintf(COM2, "Train %2d: ", trains[i].number);
move_and_clear(base_row+1, TRAIN_LEFT);
aprintf(COM2, "Speed: %2d ", trains[i].speed);
if(trains[i].route_len > 0){
SensorState ss;
Sensor *s = (Sensor*) (trains[i].route[0]);
getSensorName(s->alignment == 'F' ? s->forward_num : s->backward_num, &ss);
move_and_clear(base_row+2, TRAIN_LEFT);
aprintf(COM2, "Current sensor:\t%c%2d ", ss.module, ss.number);
s = (Sensor*) (trains[i].route[ trains[i].route_len-1 ]);
getSensorName(s->alignment == 'F' ? s->forward_num : s->backward_num, &ss);
move_and_clear(base_row+3, TRAIN_LEFT);
aprintf(COM2, "Next sensor:\t%c%2d ", ss.module, ss.number);
} else {
move_and_clear(base_row+2, TRAIN_LEFT);
aprintf(COM2, "No route supplied.");
}
}
}
break;
default: ;
aprintf(COM2, "Display server: bogus type %d\n\r", msg.type);
}
}
}
}
void paintEvent(QPaintEvent *event)
{
static const ViewInfo viewInfoNao[] = {
{SensorData::gyroX, angularSpeed, false},
{SensorData::gyroY, angularSpeed, true},
{SensorData::accX, acceleration, false},
{SensorData::accY, acceleration, false},
{SensorData::accZ, acceleration, true},
{SensorData::batteryLevel, ratio, true},
{SensorData::fsrLFL, pressure, false},
{SensorData::fsrLFR, pressure, false},
{SensorData::fsrLBL, pressure, false},
{SensorData::fsrLBR, pressure, false},
{SensorData::fsrRFL, pressure, false},
{SensorData::fsrRFR, pressure, false},
{SensorData::fsrRBL, pressure, false},
{SensorData::fsrRBR, pressure, true},
{SensorData::us, usLDistance, false},
{SensorData::us, usLtRDistance, false},
{SensorData::us, usRtLDistance, false},
{SensorData::us, usRDistance, true},
{SensorData::angleX, angle2, false},
{SensorData::angleY, angle2, false},
{SensorData::numOfSensors, end, true}
};
painter.begin(this);
painter.setFont(font);
painter.setBrush(altBrush);
painter.setPen(fontPen);
fillBackground = false;
paintRect = painter.window();
paintRectField0 = QRect(headerView->sectionViewportPosition(0) + textOffset, 0, headerView->sectionSize(0) - textOffset * 2, lineSpacing);
paintRectField1 = QRect(headerView->sectionViewportPosition(1) + textOffset, 0, headerView->sectionSize(1) - textOffset * 2, lineSpacing);
paintRectField2 = QRect(headerView->sectionViewportPosition(2) + textOffset, 0, headerView->sectionSize(2) - textOffset * 2, lineSpacing);
{
char value[32];
char filtered[32];
SYNC_WITH(console);
const SensorData& sensorData(sensorView.sensorData);
const SensorData& filteredSensorData(sensorView.filteredSensorData);
if(sensorData.data[SensorData::us] != SensorData::off)
usDistances[0][sensorData.usSensorType] = sensorData.data[SensorData::us];
if(filteredSensorData.data[SensorData::us] != SensorData::off)
usDistances[1][sensorData.usSensorType] = filteredSensorData.data[SensorData::us];
for(int i = 0;; ++i)
{
const ViewInfo &vi = viewInfoNao[i];
if(vi.type == SensorWidget::end)
break;
formatSensor(vi.type, sensorData.data[vi.sensor], false, value);
formatSensor(vi.type, filteredSensorData.data[vi.sensor], true, filtered);
print(getSensorName(vi), value, filtered);
newBlock();
if(vi.endOfSection)
newSection();
}
}
painter.end();
setMinimumHeight(paintRectField1.top());
}
int main(int argc, char** argv)
{
int err;
struct sensors_poll_device_t* device;
struct sensors_module_t* module;
err = hw_get_module(SENSORS_HARDWARE_MODULE_ID, (hw_module_t const**)&module);
if (err != 0) {
printf("hw_get_module() failed (%s)\n", err);
return 0;
}
err = sensors_open(&module->common, &device);
if (err != 0) {
printf("sensors_open() failed (%s)\n", err);
return 0;
}
printf ("Opened sensors\n");
struct sensor_t const* list;
int count = module->get_sensors_list(module, &list);
printf("%d sensors found:\n", count);
int i =0;
for (i ; i<count ; i++) {
#if 0
printf("%s\n"
"\tvendor: %s\n"
"\tversion: %d\n"
"\thandle: %d\n"
"\ttype: %d\n"
"\tmaxRange: %f\n"
"\tresolution: %f\n"
"\tpower: %f mA\n",
list[i].name,
list[i].vendor,
list[i].version,
list[i].handle,
list[i].type,
list[i].maxRange,
list[i].resolution,
list[i].power);
#endif
printf("%s\n"
"\tvendor: %s\n"
"\tversion: %d\n"
"\thandle: %d\n"
"\ttype: %d\n",
list[i].name,
list[i].vendor,
list[i].version,
list[i].handle,
list[i].type);
}
static const size_t numEvents = 16;
sensors_event_t buffer[numEvents];
i = 0;
for (i ; i<count ; i++) {
printf("De-activate \n");
err = device->activate(device, list[i].handle, 0);
if (err != 0) {
printf("deactivate() for '%s'failed (%s)\n",
list[i].name, err);
return 0;
}
}
i = 0;
for (i ; i<count ; i++) {
err = device->activate(device, list[i].handle, 1);
if (err != 0) {
printf("activate() for '%s'failed (%s)\n",
list[i].name, err);
return 0;
}
device->setDelay(device, list[i].handle, 1);
}
do {
int n = device->poll(device, buffer, numEvents);
if (n < 0) {
printf("poll() failed (%s)\n", err);
break;
}
i = 0;
//printf("read %d events:\n", n);
for ( i ; i<n ; i++) {
const sensors_event_t data = buffer[i];
if (data.version != sizeof(sensors_event_t)) {
printf("incorrect event version (version=%d, expected=%d",
data.version, sizeof(sensors_event_t));
break;
}
switch(data.type) {
case SENSOR_TYPE_ACCELEROMETER:
case SENSOR_TYPE_MAGNETIC_FIELD:
case SENSOR_TYPE_ORIENTATION:
case SENSOR_TYPE_GYROSCOPE:
case SENSOR_TYPE_GRAVITY:
case SENSOR_TYPE_LINEAR_ACCELERATION:
case SENSOR_TYPE_ROTATION_VECTOR:
printf("sensor=%s, time=%lld, value=<%5.1f,%5.1f,%5.1f>\n",
getSensorName(data.type),
data.timestamp,
data.data[0],
data.data[1],
data.data[2]);
break;
case SENSOR_TYPE_LIGHT:
case SENSOR_TYPE_PRESSURE:
case SENSOR_TYPE_TEMPERATURE:
case SENSOR_TYPE_PROXIMITY:
case SENSOR_TYPE_RELATIVE_HUMIDITY:
case SENSOR_TYPE_AMBIENT_TEMPERATURE:
/*printf("sensor=%s, time=%lld, value=%f\n",
getSensorName(data.type),
data.timestamp,
data.data[0]);
*/
break;
default:
printf("sensor=%d, time=%lld, value=<%f,%f,%f, ...>\n",
data.type,
data.timestamp,
data.data[0],
data.data[1],
data.data[2]);
break;
}
}
} while (1); // fix that
i = 0;
for (i ; i<count ; i++) {
err = device->activate(device, list[i].handle, 0);
if (err != 0) {
printf("deactivate() for '%s'failed (%s)\n",
list[i].name, err);
return 0;
}
}
err = sensors_close(device);
if (err != 0) {
printf("sensors_close() failed (%s)\n", err);
}
return 0;
}
