dashboard.cc 9.05 KB
Newer Older
1 2
/*
 * Copyright (c) 2005 University of Utah and the Flux Group.
3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21
 * 
 * {{{EMULAB-LICENSE
 * 
 * This file is part of the Emulab network testbed software.
 * 
 * This file is free software: you can redistribute it and/or modify it
 * under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or (at
 * your option) any later version.
 * 
 * This file is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Affero General Public
 * License for more details.
 * 
 * You should have received a copy of the GNU Affero General Public License
 * along with this file.  If not, see <http://www.gnu.org/licenses/>.
 * 
 * }}}
22 23 24 25 26 27 28 29 30 31 32 33 34 35 36
 */

/**
 * @file dashboard.cc
 *
 * Implementation file for the dashboard class.
 */

#include "config.h"

#include <time.h>
#include <math.h>
#include <string.h>
#include <assert.h>

Timothy Stack's avatar
Timothy Stack committed
37
#include "aGarciaGeom.h"
38 39 40 41 42
#include "aGarciaDefs.tea"

#include "dashboard.hh"

#if !defined(timeradd)
43 44 45 46 47 48 49 50 51
#define timeradd(tvp, uvp, vvp)				\
  do {							\
    (vvp)->tv_sec = (tvp)->tv_sec + (uvp)->tv_sec;	\
    (vvp)->tv_usec = (tvp)->tv_usec + (uvp)->tv_usec;	\
    if ((vvp)->tv_usec >= 1000000) {			\
      (vvp)->tv_sec++;					\
      (vvp)->tv_usec -= 1000000;			\
    }							\
  } while (0)
52 53 54
#endif

#if !defined(timersub)
55 56 57 58 59 60 61 62 63
#define timersub(tvp, uvp, vvp)				\
  do {							\
    (vvp)->tv_sec = (tvp)->tv_sec - (uvp)->tv_sec;	\
    (vvp)->tv_usec = (tvp)->tv_usec - (uvp)->tv_usec;	\
    if ((vvp)->tv_usec < 0) {				\
      (vvp)->tv_sec--;					\
      (vvp)->tv_usec += 1000000;			\
    }							\
  } while (0)
64 65 66
#endif

dashboard::dashboard(acpGarcia &garcia, FILE *battery_log)
Timothy Stack's avatar
Timothy Stack committed
67 68 69 70 71 72 73 74 75 76 77
    : db_garcia(garcia),
      db_battery_log(battery_log),
      db_user_led(garcia, "user-led"),
      db_user_button(garcia, "user-button"),
      db_fault(garcia, this),
      db_telemetry_updated(false),
      db_last_left_odometer(0.0),
      db_last_right_odometer(0.0),
      db_battery_warning(NULL),
      db_next_short_tick(0),
      db_next_long_tick(0)
78
{
Timothy Stack's avatar
Timothy Stack committed
79 80
    memset(&this->db_telemetry, 0, sizeof(struct mtp_garcia_telemetry));
    memset(&this->db_move_start_time, 0, sizeof(struct timeval));
81

Timothy Stack's avatar
Timothy Stack committed
82 83
    this->db_poll_callbacks[DBC_USER_LED] = &this->db_user_led;
    this->db_poll_callbacks[DBC_USER_BUTTON] = &this->db_user_button;
Timothy Stack's avatar
Timothy Stack committed
84
    // XXX this->db_poll_callbacks[DBC_FAULT] = &this->db_fault;
85 86 87 88 89 90
}

dashboard::~dashboard()
{
}

91
void dashboard::startMove()
92
{
Timothy Stack's avatar
Timothy Stack committed
93 94
    assert(this->db_move_start_time.tv_sec == 0);
    assert(this->db_move_start_time.tv_usec == 0);
95
    
Timothy Stack's avatar
Timothy Stack committed
96
    this->db_telemetry.move_count += 1;
97

Timothy Stack's avatar
Timothy Stack committed
98
    gettimeofday(&this->db_move_start_time, NULL);
99 100
}

101
void dashboard::endMove(float &left_odometer, float &right_odometer)
102
{
Timothy Stack's avatar
Timothy Stack committed
103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131
    struct timeval tv, diff, total, grande_total;
    acpValue zvalue(0.0f);

    assert(this->db_move_start_time.tv_sec != 0);
    assert(this->db_move_start_time.tv_usec != 0);

    /* Compute the move time, */
    gettimeofday(&tv, NULL);
    timersub(&tv, &this->db_move_start_time, &diff);
    total.tv_sec = this->db_telemetry.move_time_sec;
    total.tv_usec = this->db_telemetry.move_time_usec;
    timeradd(&total, &diff, &grande_total);
    this->db_telemetry.move_time_sec = grande_total.tv_sec;
    this->db_telemetry.move_time_usec = grande_total.tv_usec;

    /* ... update our global odometers, and */
    left_odometer = this->db_garcia.getNamedValue("distance-left")->
	getFloatVal();
    right_odometer = this->db_garcia.getNamedValue("distance-right")->
	getFloatVal();
    this->db_telemetry.left_odometer += fabs(left_odometer);
    this->db_telemetry.right_odometer += fabs(right_odometer);

    /* ... clear the garcia's per-move odometers. */
    this->db_garcia.setNamedValue("distance-left", &zvalue);
    this->db_garcia.setNamedValue("distance-right", &zvalue);

    /* Clear the start time so we can tell that the move stopped. */
    memset(&this->db_move_start_time, 0, sizeof(struct timeval));
132 133 134 135
}

void dashboard::dump(FILE *file)
{
Timothy Stack's avatar
Timothy Stack committed
136
    time_t now;
137

Timothy Stack's avatar
Timothy Stack committed
138
    assert(file != NULL);
139
    
Timothy Stack's avatar
Timothy Stack committed
140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163
    time(&now);
    fprintf(file,
	    "Current time: %s"
	    "  Left odometer:\t%f meters\n"
	    "  Right odometer:\t%f meters\n"
	    "  Left velocity:\t%f m/s\n"
	    "  Right velocity:\t%f m/s\n"
	    "  Move count:\t\t%u\n"
	    "  Move time:\t\t%d.%d s\n"
	    "  Battery level:\t%f%%\n"
	    "  Battery voltage:\t%f\n"
	    "  Battery misses:\t%d\n",
	    ctime(&now),
	    this->db_telemetry.left_odometer,
	    this->db_telemetry.right_odometer,
	    this->db_telemetry.left_velocity,
	    this->db_telemetry.right_velocity,
	    this->db_telemetry.move_count,
	    this->db_telemetry.move_time_sec,
	    this->db_telemetry.move_time_usec,
	    this->db_telemetry.battery_level,
	    this->db_telemetry.battery_voltage,
	    this->db_telemetry.battery_misses);
    fflush(file);
164 165 166 167
}

bool dashboard::update(unsigned long now)
{
Timothy Stack's avatar
Timothy Stack committed
168 169 170 171
    bool retval = true;
    float lo, ro;
    int lpc;

Timothy Stack's avatar
Timothy Stack committed
172
#if 0
Timothy Stack's avatar
Timothy Stack committed
173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195
    /*
     * Note, these queries need to come first so that we can get an accurate
     * velocity measure.  Otherwise, the time difference from the queries below
     * screw things up.
     */
    this->db_telemetry.left_instant_odometer = lo =
	this->db_garcia.getNamedValue("distance-left")->getFloatVal();
    this->db_telemetry.right_instant_odometer = ro =
	this->db_garcia.getNamedValue("distance-right")->getFloatVal();

    /*
     * The odometers are reset to zero when a new move is executed, so we
     * need to detect that and reset our idea of the last odometer reading.
     */
    if (fabsf(lo) < fabsf(this->db_last_left_odometer))
	this->db_last_left_odometer = 0.0f;
    if (fabsf(ro) < fabsf(this->db_last_right_odometer))
	this->db_last_right_odometer = 0.0f;

    this->db_telemetry.left_velocity = (lo - this->db_last_left_odometer) *
	(1000.0f / (float)(now - this->db_last_tick));
    this->db_telemetry.right_velocity = (ro - this->db_last_right_odometer) *
	(1000.0f / (float)(now - this->db_last_tick));
196

Timothy Stack's avatar
Timothy Stack committed
197 198
    this->db_last_left_odometer = lo;
    this->db_last_right_odometer = ro;
Timothy Stack's avatar
Timothy Stack committed
199
#endif
200
    
Timothy Stack's avatar
Timothy Stack committed
201 202 203 204 205 206 207 208 209
    if (now > this->db_next_long_tick) {
	float battVoltage;

	battVoltage = this->db_garcia.getNamedValue("battery-voltage")->
	    getFloatVal();
	if (battVoltage <= 0.0) {
	    /* XXX Bad read of the battery, try again in a little bit. */
	    this->db_telemetry.battery_misses += 1;
	    this->db_next_long_tick = now + 1000;
210
	}
Timothy Stack's avatar
Timothy Stack committed
211 212 213
	else {
	    this->db_telemetry.battery_voltage = battVoltage;
	    this->db_telemetry.battery_level =
Timothy Stack's avatar
Timothy Stack committed
214
		aGarciaGeom_VoltageToCapacity(battVoltage) * 100.0;
Timothy Stack's avatar
Timothy Stack committed
215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230

	    if (this->db_telemetry.battery_level < BATTERY_LOW_THRESHOLD) {
		if (this->db_battery_warning == NULL) {
		    /*
		     * First time the battery level dropped below the
		     * threshold, turn on the LED.
		     */
		    this->db_battery_warning =
			new ledClient(LED_PRI_BATTERY, LED_PATTERN_BATTERY);
		    this->addUserLEDClient(this->db_battery_warning);
		}
	    }
	    else if ((this->db_telemetry.
		      battery_level > BATTERY_HIGH_THRESHOLD) &&
		     (this->db_battery_warning != NULL)) {
		this->remUserLEDClient(this->db_battery_warning);
231
		
Timothy Stack's avatar
Timothy Stack committed
232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251
		delete this->db_battery_warning;
		this->db_battery_warning = NULL;
	    }

	    this->db_next_long_tick = now + LONG_UPDATE_INTERVAL;

	    fprintf(this->db_battery_log,
		    "%lu %f %f %d %f %f %u %d,%d\n",
		    now,
		    this->db_telemetry.battery_level,
		    this->db_telemetry.battery_voltage,
		    this->db_telemetry.battery_misses,
		    this->db_telemetry.left_odometer,
		    this->db_telemetry.right_odometer,
		    this->db_telemetry.move_count,
		    this->db_telemetry.move_time_sec,
		    this->db_telemetry.move_time_usec);
	    fflush(this->db_battery_log);
	}
    }
252

Timothy Stack's avatar
Timothy Stack committed
253 254
    if ((now > this->db_next_short_tick) &&
	(this->db_move_start_time.tv_sec == 0)) {
255
#if 0
Timothy Stack's avatar
Timothy Stack committed
256 257 258 259
	this->db_telemetry.down_ranger_left =
	    this->db_garcia.getNamedValue("down-ranger-left")->getIntVal();
	this->db_telemetry.down_ranger_right =
	    this->db_garcia.getNamedValue("down-ranger-right")->getIntVal();
260
#endif
261
	
Timothy Stack's avatar
Timothy Stack committed
262 263 264 265
	this->db_telemetry.front_ranger_left =
	    this->db_garcia.getNamedValue("front-ranger-left")->getFloatVal();
	this->db_telemetry.front_ranger_right =
	    this->db_garcia.getNamedValue("front-ranger-right")->getFloatVal();
266
	
Timothy Stack's avatar
Timothy Stack committed
267 268 269 270
	this->db_telemetry.rear_ranger_left =
	    this->db_garcia.getNamedValue("rear-ranger-left")->getFloatVal();
	this->db_telemetry.rear_ranger_right =
	    this->db_garcia.getNamedValue("rear-ranger-right")->getFloatVal();
271
	
Timothy Stack's avatar
Timothy Stack committed
272 273 274 275
	this->db_telemetry.side_ranger_left =
	    this->db_garcia.getNamedValue("side-ranger-left")->getFloatVal();
	this->db_telemetry.side_ranger_right =
	    this->db_garcia.getNamedValue("side-ranger-right")->getFloatVal();
276 277

#if 0
Timothy Stack's avatar
Timothy Stack committed
278 279
	this->db_telemetry.status =
	    this->db_garcia.getNamedValue("status")->getIntVal();
280
	
Timothy Stack's avatar
Timothy Stack committed
281 282
	this->db_telemetry.idle =
	    this->db_garcia.getNamedValue("idle")->getIntVal();
283
#endif
284
	
Timothy Stack's avatar
Timothy Stack committed
285
	this->db_next_short_tick = now + SHORT_UPDATE_INTERVAL;
286

Timothy Stack's avatar
Timothy Stack committed
287 288
	this->db_telemetry_updated = true;
    }
289

Timothy Stack's avatar
Timothy Stack committed
290 291 292
    for (lpc = 0; lpc < DBC_MAX; lpc++) {
	retval = this->db_poll_callbacks[lpc]->update(now) && retval;
    }
293

Timothy Stack's avatar
Timothy Stack committed
294
    this->db_last_tick = now;
295

Timothy Stack's avatar
Timothy Stack committed
296
    return retval;
297
}