Skip to content

Commit

Permalink
updateEnsemblesForNewLibraries
Browse files Browse the repository at this point in the history
  • Loading branch information
@MAnn223
MAnn223 committed Jul 16, 2024
1 parent 23cba02 commit e2cbb12
Show file tree
Hide file tree
Showing 2 changed files with 177 additions and 1 deletion.
2 changes: 1 addition & 1 deletion lib/SparkFun_ICM-20948_ParticleLibrary
Submodule SparkFun_ICM-20948_ParticleLibrary updated 0 files
176 changes: 176 additions & 0 deletions src/ensembles.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,185 @@
* @brief Contains efinitions of ensembles
*/
#include "ensembles.hpp"
#include "system.hpp"
#include "util.hpp"
#include "scheduler.hpp"
#include "ensembleTypes.hpp"
#include "imu.hpp"
#ifndef TEST_VERSION
#include "Particle.h"
#else
#include "scheduler_test_system.hpp"
#endif


static void SS_ensemble10Func(DeploymentSchedule_t* pDeployment);
static void SS_ensemble10Init(DeploymentSchedule_t* pDeployment);

static void SS_ensemble07Func(DeploymentSchedule_t* pDeployment);
static void SS_ensemble07Init(DeploymentSchedule_t* pDeployment);

static void SS_ensemble08Func(DeploymentSchedule_t* pDeployment);
static void SS_ensemble08Init(DeploymentSchedule_t* pDeployment);

static void SS_fwVerInit(DeploymentSchedule_t* pDeployment);
static void SS_fwVerFunc(DeploymentSchedule_t* pDeployment);

//define ensemble structs
typedef struct Ensemble10_eventData_
{
double temperature;
int32_t water;
int32_t acc[3];
int32_t ang[3];
int32_t mag[3];
int32_t location[2];
uint8_t hasGPS;
uint32_t accumulateCount;
}Ensemble10_eventData_t;

typedef struct Ensemble07_eventData_
{
float battVoltage;
uint32_t accumulateCount;
}Ensemble07_eventData_t;

typedef struct Ensemble08_eventData_
{
double temperature;
int32_t water;

uint32_t accumulateCount;
}Ensemble08_eventData_t;

static Ensemble10_eventData_t ensemble10Data;
static Ensemble07_eventData_t ensemble07Data;
static Ensemble08_eventData_t ensemble08Data;

static void SS_ensemble10Init(DeploymentSchedule_t* pDeployment)
{
memset(&ensemble10Data, 0, sizeof(Ensemble10_eventData_t));
pDeployment->state.pData = &ensemble10Data;
}

static void SS_ensemble07Init(DeploymentSchedule_t* pDeployment)
{
memset(&ensemble07Data, 0, sizeof(Ensemble07_eventData_t));
pDeployment->state.pData = &ensemble07Data;
}

static void SS_ensemble08Init(DeploymentSchedule_t* pDeployment)
{
memset(&ensemble08Data, 0, sizeof(Ensemble08_eventData_t));
pDeployment->state.pData = &ensemble08Data;
}

static void SS_ensemble10Func(DeploymentSchedule_t* pDeployment)
{
float temp;
uint8_t water;
int32_t lat, lng;
// uint16_t accelRawData[3];
// uint16_t gyroRawData[3];
// uint16_t magRawData[3];
float accelData[3];
float gyroData[3];
float magData[3];
bool hasGPS = false;
Ensemble10_eventData_t* pData = (Ensemble10_eventData_t*)pDeployment->state.pData;

#pragma pack(push, 1)
struct
{
EnsembleHeader_t header;
union{
Ensemble10_data_t ens10;
Ensemble11_data_t ens11;
}data;
}ensData;
#pragma pack(pop)


// Obtain measurements - all <S16 Little Endianess
temp = pSystemDesc->pTempSensor->getTemp();
water = pSystemDesc->pWaterSensor->getCurrentReading();
getAccelerometer(accelData, accelData + 1, accelData + 2);
getGyroscope(gyroData, gyroData + 1, gyroData + 2);
getMagnetometer(magData, magData + 1, magData + 2);
LocationPoint point;
LocationService& instance= LocationService::instance();
lat = point.latitude;
lng = point.longitude;
//TODO: handling gps underwater

// if(pSystemDesc->pGPS->location.isValid() && pSystemDesc->pGPS->location.isUpdated() && (pSystemDesc->pGPS->location.age() < GPS_AGE_VALID_MS))
// {
// hasGPS = true;
// lat = pSystemDesc->pGPS->location.lat_int32();
// lng = pSystemDesc->pGPS->location.lng_int32();
// }
// else
// {
// hasGPS = false;
// lat = pData->location[0];
// lng = pData->location[1];
// }

// Accumulate measurements
pData->temperature += temp;
pData->water += water;
pData->acc[0] += accelData[0];
pData->acc[1] += accelData[1];
pData->acc[2] += accelData[2];
pData->ang[0] += gyroData[0];
pData->ang[1] += gyroData[1];
pData->ang[2] += gyroData[2];
pData->mag[0] += magData[0];
pData->mag[1] += magData[1];
pData->mag[2] += magData[2];
pData->location[0] += lat;
pData->location[1] += lng;
pData->hasGPS += hasGPS ? 1 : 0;
pData->accumulateCount++;


// Report accumulated measurements
if(pData->accumulateCount == pDeployment->measurementsToAccumulate)
{
water = pData->water / pDeployment->measurementsToAccumulate;
temp = pData->temperature / pDeployment->measurementsToAccumulate;
if(water == false)
{
temp -= 100;
}


ensData.header.elapsedTime_ds = Ens_getStartTime(pDeployment->state.deploymentStartTime);
SF_OSAL_printf("Ensemble timestamp: %d\n", ensData.header.elapsedTime_ds);
ensData.data.ens10.rawTemp = N_TO_B_ENDIAN_2(temp / 0.0078125);
ensData.data.ens10.rawAcceleration[0] = N_TO_B_ENDIAN_2(pData->acc[0] / pDeployment->measurementsToAccumulate);
ensData.data.ens10.rawAcceleration[1] = N_TO_B_ENDIAN_2(pData->acc[1] / pDeployment->measurementsToAccumulate);
ensData.data.ens10.rawAcceleration[2] = N_TO_B_ENDIAN_2(pData->acc[2] / pDeployment->measurementsToAccumulate);
ensData.data.ens10.rawAngularVel[0] = N_TO_B_ENDIAN_2(pData->ang[0] / pDeployment->measurementsToAccumulate);
ensData.data.ens10.rawAngularVel[1] = N_TO_B_ENDIAN_2(pData->ang[1] / pDeployment->measurementsToAccumulate);
ensData.data.ens10.rawAngularVel[2] = N_TO_B_ENDIAN_2(pData->ang[2] / pDeployment->measurementsToAccumulate);
ensData.data.ens10.rawMagField[0] = N_TO_B_ENDIAN_2(pData->mag[0] / pDeployment->measurementsToAccumulate);
ensData.data.ens10.rawMagField[1] = N_TO_B_ENDIAN_2(pData->mag[1] / pDeployment->measurementsToAccumulate);
ensData.data.ens10.rawMagField[2] = N_TO_B_ENDIAN_2(pData->mag[2] / pDeployment->measurementsToAccumulate);
ensData.data.ens11.location[0] = N_TO_B_ENDIAN_4(pData->location[0] / pDeployment->measurementsToAccumulate);
ensData.data.ens11.location[1] = N_TO_B_ENDIAN_4(pData->location[1] / pDeployment->measurementsToAccumulate);

if(pData->hasGPS / pDeployment->measurementsToAccumulate)
{
ensData.header.ensembleType = ENS_TEMP_IMU_GPS;
pSystemDesc->pRecorder->putBytes(&ensData, sizeof(EnsembleHeader_t) + sizeof(Ensemble11_data_t));
}
else
{
ensData.header.ensembleType = ENS_TEMP_IMU;
pSystemDesc->pRecorder->putBytes(&ensData, sizeof(EnsembleHeader_t) + sizeof(Ensemble10_data_t));
}

memset(pData, 0, sizeof(Ensemble10_eventData_t));
}
}

0 comments on commit e2cbb12

Please sign in to comment.