RangeSensorWidget::RangeSensorWidget(int& argc, char * argv[]) :
QWidget(),
client_(argc, argv),
sensor_(),
robotName_(client_.namingContextName.c_str()),
sensorName_("Sonar"),
group_(0),
timer_(0),
drawCones_(false)
{
// set widget size
resize(400, 400);
// init menu
menuBar_ = new QMenuBar(this);
menuFile_ = new QPopupMenu(this);
menuView_ = new QPopupMenu(this);
menuView_->setCheckable(true);
menuBar_->insertItem("&Robot", menuFile_);
menuBar_->insertItem("&View", menuView_);
menuFile_->insertItem("Select robot", this, SLOT(selectRobot()));
menuFile_->insertItem("Select sensor", this, SLOT(selectSensor()));
groupIndex_ = menuFile_->insertItem("Select group", this, SLOT(selectGroup()));
menuFile_->setItemEnabled(groupIndex_, false);
menuFile_->insertSeparator();
menuFile_->insertItem("Quit", qApp, SLOT(quit()));
coneIndex_ = menuView_->insertItem("Draw Cone", this, SLOT(toggleCone()));
}
int myBME::initialize(myBMEData &data1, myBMEData &data2){
if (TWCR == 0){ // if Wire library is not already initialized
Wire.begin();
}
selectSensor(INDEX_BALLOON);
if (!bme[INDEX_BALLOON].begin()) {
if(HELIOS_DEBUG) Serial.println("Could not find a valid BME280 sensor inside balloon, check wiring!");
return 0;
}
selectSensor(INDEX_ATMOS);
if (!bme[INDEX_ATMOS].begin()) {
if(HELIOS_DEBUG) Serial.println("Could not find a valid BME280 sensor for atmosphere, check wiring!");
return 0;
}
data1 = {};
data2 = {};
if (HELIOS_DEBUG) Serial.println("Two BME sensors initialized");
return 1;
}
void myBME::read(myBMEData *data, uint8_t index){
if (index != INDEX_BALLOON && index != INDEX_ATMOS) return; //if not a valid array position
selectSensor(index);
data->pressure = bme[index].readPressure();
data->temperature = bme[index].readTemperature();
data->humidity = bme[index].readHumidity();
data->altitude = bme[index].readAltitude(SEA_LEVEL_PRESSURE_HPA);
if(isnan(bme[index].readPressure())){ //if the bme was disconnected, try restarting it
bme[index].begin();
}
}
void updateStatusOfSensor(Sensor sensor)
{
UINT8 byte3;
UINT8 i;
SensorStatus res;
if(!isSensorReadReady)
{
prepareForSensorRead();
}
res.sensor = sensor;
CLOCK_PORT = 1;
SPI_TIME_OFFSET
readTime = ReadTimer1();
selectSensor(sensor);
// Waiting 500 ns for sensor to be ready (Period = 83 ns => about 7 cycles, waiting 20)
SPI_TIME_OFFSET
res.position = 0;
for(i=0 ; i<13 ; i++)
{
res.position <<= 1;
CLOCK_PORT = 1;
SPI_TIME_OFFSET_UP
CLOCK_PORT = 0;
SPI_TIME_OFFSET
res.position += DATA_IN;
}
byte3 = 0;
for(i=0 ; i<6 ; i++)
{
byte3 <<= 1;
CLOCK_PORT = 1;
SPI_TIME_OFFSET_UP
CLOCK_PORT = 0;
SPI_TIME_OFFSET
byte3 += DATA_IN;
}
SPI_TIME_OFFSET
LATA |= 0x3F; // Return to waiting state
SPI_TIME_OFFSET
CLOCK_PORT = 1;
res.error = 0;
if(OCF)
{
res.error += SENSOR_ERROR_OCF_NOT_FINISHED;
}
if(COF)
{
res.error += SENSOR_ERROR_OVERFLOW;
}
if(LIN)
{
res.error += SENSOR_ERROR_LINEARITY;
}
if(MAGDEC)
{
res.error += SENSOR_ERROR_MAG_DEC;
}
if(MAGINC)
{
res.error += SENSOR_ERROR_MAG_INC;
}
if(!checkParity(res.position,byte3))
{
res.error += SENSOR_ERROR_PARITY;
}
if(res.error == 0 || res.error == 48)
{
updateSensorStatus(res,sensor);
}
}
static void read(MY_HONEYWELL *data, uint8_t sensor) {
selectSensor(sensor); //select to talk to the desired sensor
/*uint8_t val[4] = {0}; //four bytes to store sensor data
Wire.requestFrom(SSC_ADDR, (uint8_t) 4); //request sensor data
for (uint8_t i = 0; i <= 3; i++) {
delay(4); // sensor might be missing, do not block by using Wire.available()
val[i] = Wire.read();
}
Serial.println("Data Received");*/
struct cs_raw ps;
char p_str[10], t_str[10];
uint8_t el;
float p, t;
el = ps_get_raw(SSC_ADDR, &ps);
// for some reason my chip triggers a diagnostic fault
// on 50% of powerups without a notable impact
// to the output values.
if ( el == 4 ) {
Serial.println("err sensor missing");
} else {
if ( el == 3 ) {
Serial.print("err diagnostic fault ");
Serial.println(ps.status, BIN);
}
if ( el == 2 ) {
// if data has already been feched since the last
// measurement cycle
Serial.print("warn stale data ");
Serial.println(ps.status, BIN);
}
if ( el == 1 ) {
// chip in command mode
// no clue how to end up here
Serial.print("warn command mode ");
Serial.println(ps.status, BIN);
}
//Serial.print("status ");
//Serial.println(ps.status, BIN);
//Serial.print("bridge_data ");
//Serial.println(ps.bridge_data, DEC);
//Serial.print("temp_data ");
//Serial.println(ps.temperature_data, DEC);
//Serial.println("");
ps_convert(ps, &p, &t, SSC_MIN, SSC_MAX, PRESSURE_MIN,
PRESSURE_MAX);
// floats cannot be easily printed out
dtostrf(p, 2, 2, p_str);
dtostrf(t, 2, 2, t_str);
Serial.print("pressure (Pa) ");
Serial.println(p_str);
Serial.print("temperature (dC) ");
Serial.println(t_str);
//Serial.println("");
data->status=ps.status;
data->rawPressure=ps.bridge_data;
data->rawTemperature=ps.temperature_data;
data->pressure = p;
data->temperature = t;
/*data->status = (val[0] & 0xc0) >> 6; // first 2 bits from first byte are the status
data->rawPressure = ((val[0] & 0x3f) << 8) + val[1];
data->rawTemperature = ((val[2] << 8) + (val[3] & 0xe0)) >> 5;
if (data->rawTemperature == 65535)
data->status = 4;
if (data->status == 4) {
data->pressure = -1;
data->temperature = -1;
}
else {
data->pressure = 1.0 * (data->rawPressure - SSC_MIN) * (PRESSURE_MAX - PRESSURE_MIN) / (SSC_MAX - SSC_MIN) + PRESSURE_MIN;
data->temperature = (data->rawTemperature * 0.0977) - 50;
}*/
}
return;
}
