/* -*- Mode: Text -*- */

/********************************************************************\
* File: speed.hs                                                     *
* Date: 05/25/1997                                                   *
* Author: Cem Unsal                                                  *
*         Robotics Institute, Carnegie-Mellon University             *
*         unsal@ri.cmu.edu                                           *
*                                                                    *
* Description:                                                       *
* Simple speed sensor                                                *
*                                                                    *
* Gaussian measurement noise is added to actual value of the speed   *
* obtained from the vehicle model (or alternatively VREP).           *    
*                                                                    *
* Sensor parameters can be set in this file, or the type 'creating'  *
* the sensor.                                                        *
*                                                                    *
* External C functions: -                                            *
*                                                                    *
* This file is distributed under the conditions described in the     *
* file 'CONDITIONS' which should accompany this file.                *
\********************************************************************/

#ifndef SAHS_SENSOR_HS
#define SAHS_SENSOR_HS

#include <cmu/sensors/noise.hs>

type SpeedSensor
{
  output continuous number spd_reading;  // Sensor reading 

  input  continuous number xDot;         // Actual speed and
         continuous number precip;       // Precipitation
                                         // from associated VREP

  state  Vehicle vehicle;                // Vehicle associated with sensor
         continuous number mean, var;    // Measurement noise parameters
         Gaussian error_signal;          // Noise generator
  
  discrete normal, 
           problem;        // Discrete states for operation modes

  setup
    define
    {
      Gaussian tnoise := create(Gaussian);
    }
    do
    {
      error_signal := tnoise;
      mean := 0;
      var := 1;
    };

  flow default
     {
       spd_reading = xDot + sqrt(var) * sg1(error_signal) + mean;
       // Gaussian noise injected: N(mean,var)
     };

  transition
    normal -> problem {}
    when precip >= 10
      do
      {
        var := 2;          
      },
                  
    problem -> normal {}
    when precip < 10
      do
      {
        var := 0.5;
      };
}

#endif // SAHS_SPEED_HS