Index: firmware/App/Drivers/ConductivityTeensy.c =================================================================== diff -u -r290cac3c0557c978647cef2972800dfe137dd662 -rbc7dad193df69c64cf213db463e5115b1ba8a5f0 --- firmware/App/Drivers/ConductivityTeensy.c (.../ConductivityTeensy.c) (revision 290cac3c0557c978647cef2972800dfe137dd662) +++ firmware/App/Drivers/ConductivityTeensy.c (.../ConductivityTeensy.c) (revision bc7dad193df69c64cf213db463e5115b1ba8a5f0) @@ -8,14 +8,14 @@ * @file ConductivityTeensy.c * * @author (last) Michael Garthwaite -* @date (last) 10-Feb-2026 +* @date (last) 06-Mar-2026 * * @author (original) Arpita Srivastava * @date (original) 03-Nov-2025 * ***************************************************************************/ -#include // Used for calculating the polynomial calibration equation and INF +#include // Used for log() and INF #include // For memcpy #include "sci.h" @@ -52,25 +52,23 @@ #define RX_SIZE_SELECT_SENSOR 1 ///< Expected response bytes length of select sensor cmd. #define RX_SIZE_GET_SINGLE_MEASUREMENT ( sizeof( COND_SENSOR_DATA_T ) ) ///< Expected response bytes length of get single sensor measurement cmd. -#define COND_INIT_TIMEOUT_MS ( 3 * MS_PER_SECOND ) ///< Timeout before which we should transition to idle state. +#define COND_INIT_TIMEOUT_MS ( 5 * MS_PER_SECOND ) ///< Timeout before which we should transition to idle state. #define COND_STATUS_TIMEOUT_MS ( 10 * MS_PER_SECOND ) ///< Timeout before which we should receive acknowledgment from teensy #define COND_DATA_TIMEOUT_MS ( 10 * MS_PER_SECOND ) ///< Timeout before which we should receive data from teensy #define COND_TEMP_OFFSET 25 ///< Temperature offset constant used in RTD calculations. -#define COND_CONVERSION_SM_TO_USCM 100000 ///< Conductivity conversion from siemens per meter to micro siemens per centimeter. +#define COND_CONVERSION_SM_TO_USCM 1000000.0F ///< Conductivity conversion from siemens per meter to micro siemens per centimeter. #define COND_INFINITE_R_VALUE 800000 ///< Resistance value for when the driver detects INF from the sensor. // ********** private data ********** -static COND_COMM_STATE_T condCommState; ///< current Conductivity driver comm state. +static COND_COMM_STATE_T condCommState; ///< Current Conductivity driver comm state. static U32 condInitTime; ///< Tracks duration of init state. static U32 condResponseTime; ///< Tracks duration between cmd sent and its response received. static U32 condReceiptCounter; ///< Conductivity response receipt counter. static U32 condTransmitCounter; ///< Conductivity command transmit counter. static BOOL condAutomatedDataPolling; ///< Flag indicating automated conductivity measurements polling has started. static BOOL condWriteCommandInProgress; ///< Flag indicating an Conductivity write command is in progress. -static BOOL condReadCommandInProgress; ///< Flag indicating an Conductivity read command is in progress. -static BOOL condBulkWriteAndReadInProgress; ///< Flag indicating an Conductivity bulk write and read command are in progress. static BOOL condWriteCmdRspnsRcvd; ///< Flag indicating a response to an Conductivity write command has been received. // Conductivity comm buffers @@ -108,7 +106,7 @@ static COND_MODELS_T currentConductivityModel; ///< Current Conductivity Model selected. static BOOL eepromInit; ///< Boolean to determine if we are loading eeprom data from sensors. -// Command Map +// Teensy Command Map static const COND_CMD_DATA_T teensyCmdMap[] = { // Command Index / Sub state Command Length of expected response data { TEENSY_CMD_INIT_SENSOR , ( U08* )"a" , RX_SIZE_INIT_SENSOR }, @@ -127,14 +125,11 @@ // ********** private function prototypes ********** static void initCondDMAchannels( void ); -static void initEEPROMdata( void ); -static void initMeasurementSettings( void ); static COND_COMM_STATE_T handleConductivityInit( void ); static COND_COMM_STATE_T handleConductivityIdle( void ); static COND_COMM_STATE_T handleConductivityTX( void ); static COND_COMM_STATE_T handleConductivityRX( void ); -static COND_COMM_STATE_T handleFailedState( void ); static void consumeUnexpectedConductivityData( void ); static void setupConductivityDMAForWriteCmd( U32 bytes2Transmit ); @@ -183,17 +178,16 @@ static COND_COMM_STATE_T txSelectSensor( void ); static COND_COMM_STATE_T rxSelectSensor( void ); -static COND_PARSE_STATUS parseMeasurementSettings( const U08 *buffer, U32 len ); static COND_PARSE_STATUS parseEEPROMdata( const U08 *buffer, U32 len ); -static COND_PARSE_STATUS parseConductivityMeasurements( const U08 *buffer, U32 len ); +static COND_PARSE_STATUS parseConductivityMeasurements( const U08 *buffer ); -static U32 getTeensyCondId( CONDUCTIVITY_SENSORS_T sensorId ); -static void calculateConductivity( TEENSY_SENSOR_INDEX_T sensorNum ); -static void calculateConductivityAly( TEENSY_SENSOR_INDEX_T sensorNum, BOOL isFPSensor ); -static void calculateConductivityUpdatedStandard( TEENSY_SENSOR_INDEX_T sensorNum, BOOL isFPSensor ); -static void calculateConductivityStandard( TEENSY_SENSOR_INDEX_T sensorNum, BOOL isFPSensor ); -static void calculateTemperature( TEENSY_SENSOR_INDEX_T sensorNum ); -static void calculateResistance( TEENSY_SENSOR_INDEX_T sensorNum, BOOL isFPSensor ); +static U32 getTeensyCondId( U32 sensorId ); +static void calculateConductivity( U32 sensorNum ); +static void calculateConductivityAly( U32 sensorNum, BOOL isFPSensor ); +static void calculateConductivityUpdatedStandard( U32 sensorNum, BOOL isFPSensor ); +static void calculateConductivityStandard( U32 sensorNum, BOOL isFPSensor ); +static void calculateTemperature( U32 sensorNum ); +static void calculateResistance( U32 sensorNum, BOOL isFPSensor ); /*********************************************************************//** * @brief @@ -211,8 +205,6 @@ condTransmitCounter = 0; condAutomatedDataPolling = FALSE; condWriteCommandInProgress = FALSE; - condReadCommandInProgress = FALSE; - condBulkWriteAndReadInProgress = FALSE; condWriteCmdRspnsRcvd = FALSE; memset( &condWriteCmdBuffer, 0, COND_WRITE_CMD_BUFFER_LEN ); @@ -312,68 +304,6 @@ /*********************************************************************//** * @brief - * The initEEPROMdata function populates eePromDataTX structure and enqueues - * update EEPROM data command in the the command queue. - * @details \b Inputs: none - * @details \b Outputs: eePromDataTX : EEPROM data to be updated on Teensy. - * @return none - *************************************************************************/ -static void initEEPROMdata( void ) -{ - // TODO Update real values in eePromDataTX - eePromDataTX.doubleValue[ 0 ] = 0.0; - eePromDataTX.doubleValue[ 1 ] = 0.0; - eePromDataTX.doubleValue[ 2 ] = 0.0; - eePromDataTX.doubleValue[ 3 ] = 0.0; - eePromDataTX.doubleValue[ 4 ] = 0.0; - eePromDataTX.doubleValue[ 5 ] = 0.0; - eePromDataTX.doubleValue[ 6 ] = 0.0; - eePromDataTX.doubleValue[ 7 ] = 0.0; - - eePromDataTX.floatValue[ 0 ] = 0.0; - eePromDataTX.floatValue[ 1 ] = 0.0; - eePromDataTX.floatValue[ 2 ] = 0.0; - eePromDataTX.floatValue[ 3 ] = 0.0; - eePromDataTX.floatValue[ 4 ] = 0.0; - eePromDataTX.floatValue[ 5 ] = 0.0; - eePromDataTX.floatValue[ 6 ] = 0.0; - eePromDataTX.floatValue[ 7 ] = 0.0; - eePromDataTX.floatValue[ 8 ] = 0.0; - eePromDataTX.floatValue[ 8 ] = 0.0; - eePromDataTX.floatValue[ 10 ] = 0.0; - eePromDataTX.floatValue[ 11 ] = 0.0; - eePromDataTX.floatValue[ 12 ] = 0.0; - eePromDataTX.floatValue[ 13 ] = 0.0; - eePromDataTX.floatValue[ 14 ] = 0.0; - eePromDataTX.floatValue[ 15 ] = 0.0; - - enqueue( TEENSY_CMD_UPDATE_EEPROM_DATA ); -} - -/*********************************************************************//** - * @brief - * The initMeasurementSettings function populates measurementSettingsTX structure and enqueues - * update measurement settings command in the the command queue. - * @details \b Inputs:none - * @details \b Outputs:measurementSettingsTX - Measurement settings data to updated on Teensy. - * @return none - *************************************************************************/ -static void initMeasurementSettings( void ) -{ - // TODO Update real values in measurementSettingsTX - measurementSettingsTX.SinFreq = 11000.0; - measurementSettingsTX.DacVoltPP = 400.0; - measurementSettingsTX.BiasVolt = 200.0; - measurementSettingsTX.HstiaRtiaSel = 7; - measurementSettingsTX.AdcPgaGain = 2; - measurementSettingsTX.DftNum = 256; - measurementSettingsTX.ADCAvgNum = 16; - - enqueue( TEENSY_CMD_UPDATE_MEASUREMENT_SETTINGS ); -} - -/*********************************************************************//** - * @brief * The execConductivityTeensy function manages incoming data exchanges with * the Teensy board over UART. * @details \b Inputs: condCommState - Current state. @@ -400,10 +330,6 @@ condCommState = handleConductivityRX(); break; - case COND_COMM_STATE_FAILED: - condCommState = handleFailedState(); - break; - default: SET_ALARM_WITH_1_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, ( U32 )condCommState ) break; @@ -425,6 +351,7 @@ if ( TRUE == didTimeout( condInitTime, COND_INIT_TIMEOUT_MS ) ) { state = COND_COMM_STATE_IDLE; + consumeUnexpectedConductivityData(); } return state; } @@ -590,50 +517,6 @@ /*********************************************************************//** * @brief - * The handleFailedState function handles failure or errors of the states. - * @details \b Inputs: currentCmd - Current command being executed. - * The error occurred during TX or RX phase of this command. - * @details \b Outputs: none - * @return state - Next state. - *************************************************************************/ -static COND_COMM_STATE_T handleFailedState( void ) -{ - COND_COMM_STATE_T state = COND_COMM_STATE_FAILED; - - switch ( currentCmd ) - { - case TEENSY_CMD_INIT_SENSOR: - break; - case TEENSY_CMD_GET_INIT_STATUS: - break; - case TEENSY_CMD_UPDATE_EEPROM_DATA: - break; - case TEENSY_CMD_GET_EEPROM_DATA: - break; - case TEENSY_CMD_UPDATE_MEASUREMENT_SETTINGS: - break; - case TEENSY_CMD_GET_MEASUREMENT_SETTINGS: - break; - case TEENSY_CMD_START_MEASUREMENT: - break; - case TEENSY_CMD_STOP_MEASUREMENT: - break; - case TEENSY_CMD_GET_ALL_MEASUREMENTS: - break; - case TEENSY_CMD_GET_SINGLE_MEASUREMENT: - break; - case TEENSY_CMD_SELECT_SENSOR: - break; - default: - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_FP_SOFTWARE_FAULT, SW_FAULT_ID_CONDUCTIVITY_SENOR_INVALID_STATE, currentCmd ) - break; - } - - return state; -} - -/*********************************************************************//** - * @brief * The consumeUnexpectedConductivityData function checks to see if a byte is sitting in * the SCI received data register and consumes the byte if found. * @details \b Inputs: SCI FLR register @@ -911,10 +794,6 @@ condWriteCmdRspnsRcvd = FALSE; success = TRUE; } - - // Should not be any data received at this time - //consumeUnexpectedConductivityData(); - return success; } @@ -958,16 +837,15 @@ static COND_COMM_STATE_T rxInitSensor( void ) { COND_COMM_STATE_T state = COND_COMM_STATE_RX; - COND_INIT_STATUS_T initStatusInProgress = COND_INIT_STATUS_FAILED; // set to fail for testing COND_INIT_STATUS_T initStatusInitialized = COND_INIT_STATUS_FAILED; // if data populates BOOL recvComplete = rxTeensyReadRsp( TEENSY_CMD_INIT_SENSOR ); if ( TRUE == recvComplete ) { // Reset the timer for next use. condResponseTime = 0; - // Read the data from the receive buffer - initStatusInProgress = ( COND_INIT_STATUS_T )condRxBuffer[ 0 ]; + // Read the data from the receive buffer. Pull 2nd byte as first byte is + // discarded. initStatusInitialized = ( COND_INIT_STATUS_T )condRxBuffer[ 1 ]; // Store the init status @@ -1502,7 +1380,7 @@ condResponseTime = getMSTimerCount(); // Read the data from the receive buffer - parseStatus = parseConductivityMeasurements( condRxBuffer, COND_RX_BUFFER_LEN ); + parseStatus = parseConductivityMeasurements( condRxBuffer ); // Check if parsing was done successfully if ( COND_PARSE_SUCCESS == parseStatus ) @@ -1665,7 +1543,7 @@ condResponseTime = 0; // Read the data from the receive buffer - parseStatus = parseConductivityMeasurements( condRxBuffer, COND_RX_BUFFER_LEN ); + parseStatus = parseConductivityMeasurements( condRxBuffer ); // Check if parsing was done successfully if ( COND_PARSE_SUCCESS == parseStatus ) @@ -1740,7 +1618,7 @@ condResponseTime = 0; // Read the data from the receive buffer - COND_PARSE_STATUS parseStatus = parseConductivityMeasurements( condRxBuffer, COND_RX_BUFFER_LEN ); + COND_PARSE_STATUS parseStatus = parseConductivityMeasurements( condRxBuffer ); // Check if parsing was done successfully if ( COND_PARSE_SUCCESS == parseStatus ) @@ -1841,40 +1719,6 @@ /*********************************************************************//** * @brief - * The parseMeasurementSettings Reads measurement settings from buffer and - * stores in measurementSettingsRX. - * @details \b Inputs : none - * @details \b Outputs: measurementSettingsRX - Received Measurement Settings - * @param buffer - Data to be parsed and stored. - * len - Length of the input data. - * @return COND_PARSE_STATUS to tell if parsing was successful or not. - *************************************************************************/ -static COND_PARSE_STATUS parseMeasurementSettings( const U08 *buffer, U32 len ) -{ - COND_PARSE_STATUS parseStatus = COND_PARSE_NONE; - U32 expectedDataLength = sizeof( COND_MEASUREMENT_SETTINGS_T ); - - // Validate buffer - if ( buffer == NULL ) - { - parseStatus = COND_PARSE_ERROR_NULL_BUFFER; - } - else if ( len != expectedDataLength ) - { - parseStatus = COND_PARSE_ERROR_INVALID_LENGTH; - } - else - { - // Parse and store the data - memcpy(&measurementSettingsRX, buffer, expectedDataLength ); - parseStatus = COND_PARSE_SUCCESS; - } - - return parseStatus; -} - -/*********************************************************************//** - * @brief * The parseEEPROMdata Reads EEPROM data from buffer and * stores in eePromDataRX. * @details \b Inputs : none @@ -1920,7 +1764,7 @@ * len - Length of the input data. * @return COND_PARSE_STATUS to tell if parsing was successful or not. *************************************************************************/ -static COND_PARSE_STATUS parseConductivityMeasurements( const U08 *buffer, U32 len ) +static COND_PARSE_STATUS parseConductivityMeasurements( const U08 *buffer ) { COND_PARSE_STATUS parseStatus = COND_PARSE_NONE; COND_SENSOR_DATA_T tempSensor; @@ -1943,14 +1787,14 @@ else { // Store Raw value in array index for position ( sensorNum - 1 ) - condRawMeasurement[ tempSensor.sensorNum - 1 ] = tempSensor; + memcpy( &condRawMeasurement[ tempSensor.sensorNum - 1 ], &tempSensor, sizeof( COND_SENSOR_DATA_T ) ); - // Calculate and store Conductivity from raw values - calculateConductivity( tempSensor.sensorNum - 1 ); - // Calculate and store Temperature from raw values. calculateTemperature( tempSensor.sensorNum - 1 ); + // Calculate and store Conductivity from raw values + calculateConductivity( tempSensor.sensorNum - 1 ); + parseStatus = COND_PARSE_SUCCESS; } } @@ -1966,7 +1810,7 @@ * @param sensorId - Id of DD Conductivity Sensors * @return sensorNum - Sensor Index. *************************************************************************/ -static U32 getTeensyCondId( CONDUCTIVITY_SENSORS_T sensorId ) +static U32 getTeensyCondId( U32 sensorId ) { U32 sensorNum = 0; @@ -1990,6 +1834,10 @@ case D43_COND: sensorNum = TEENSY_SENSOR_5; break; + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_INVALID_CONDUCTIVITY_SENSOR_ID, sensorId ) + break; + } return sensorNum; @@ -2001,21 +1849,18 @@ impedance value. * @details \b Inputs: condCalculatedMeasurement * @details \b Outputs: none - * @param sensor ID of conductivity sensor to get conductivity. + * @param sensorId ID of conductivity sensor to get conductivity. * @return The current conductivity of a given conductivity sensor. *************************************************************************/ -F32 getTeensyConductivityValue( CONDUCTIVITY_SENSORS_T sensorId ) +F32 getTeensyConductivityValue( U32 sensorId ) { U32 sensorNum = getTeensyCondId( sensorId ); F32 result = 0.0F; - if ( D74_COND != sensorId ) + result = condCalculatedMeasurement[ sensorNum ].Conductivity.data; + if ( OVERRIDE_KEY == condCalculatedMeasurement[ sensorNum ].Conductivity.override ) { - result = condCalculatedMeasurement[ sensorNum ].Conductivity.data; - if ( OVERRIDE_KEY == condCalculatedMeasurement[ sensorNum ].Conductivity.override ) - { - result = condCalculatedMeasurement[ sensorNum ].Conductivity.ovData; - } + result = condCalculatedMeasurement[ sensorNum ].Conductivity.ovData; } return result; @@ -2030,18 +1875,15 @@ * @param sensorId ID of conductivity sensor to get temperature. * @return The current temperature of a given conductivity sensor. *************************************************************************/ -F32 getTeensyConductivityTemperatureValue( CONDUCTIVITY_SENSORS_T sensorId ) +F32 getTeensyConductivityTemperatureValue( U32 sensorId ) { U32 sensorNum = getTeensyCondId( sensorId ); F32 result = 0.0F; - if ( D74_COND != sensorId ) + result = condCalculatedMeasurement[ sensorNum ].Temperature.data; + if ( OVERRIDE_KEY == condCalculatedMeasurement[ sensorNum ].Temperature.override ) { - result = condCalculatedMeasurement[ sensorNum ].Temperature.data; - if ( OVERRIDE_KEY == condCalculatedMeasurement[ sensorNum ].Temperature.override ) - { - result = condCalculatedMeasurement[ sensorNum ].Temperature.ovData; - } + result = condCalculatedMeasurement[ sensorNum ].Temperature.ovData; } return result; @@ -2056,18 +1898,15 @@ * @param sensorId ID of conductivity sensor to get temperature. * @return The current temperature of a given conductivity sensor. *************************************************************************/ -F32 getTeensyConductivityResistanceValue( CONDUCTIVITY_SENSORS_T sensorId ) +F32 getTeensyConductivityResistanceValue( U32 sensorId ) { U32 sensorNum = getTeensyCondId( sensorId ); F32 result = 0.0F; - if ( D74_COND != sensorId ) + result = condCalculatedMeasurement[ sensorNum ].Resistance.data; + if ( OVERRIDE_KEY == condCalculatedMeasurement[ sensorNum ].Resistance.override ) { - result = condCalculatedMeasurement[ sensorNum ].Resistance.data; - if ( OVERRIDE_KEY == condCalculatedMeasurement[ sensorNum ].Resistance.override ) - { - result = condCalculatedMeasurement[ sensorNum ].Resistance.ovData; - } + result = condCalculatedMeasurement[ sensorNum ].Resistance.ovData; } return result; @@ -2082,7 +1921,7 @@ * @param isFPSensor - T/F if sensor is on FP hardware. * @return *************************************************************************/ -static void calculateConductivity( TEENSY_SENSOR_INDEX_T sensorNum ) +static void calculateConductivity( U32 sensorNum ) { BOOL isFPSensor = FALSE; @@ -2115,15 +1954,15 @@ * @param isFPSensor - T/F if sensor is on FP hardware. * @return *************************************************************************/ -static void calculateConductivityAly( TEENSY_SENSOR_INDEX_T sensorNum, BOOL isFPSensor ) +static void calculateConductivityAly( U32 sensorNum, BOOL isFPSensor ) { - F64 calculatedConductivity = 0.0; + F32 calculatedConductivity = 0.0; F32 B3 = 0.0; F32 B2 = 0.0; F32 B1 = 0.0; F32 B0 = 0.0; - F64 R = condRawMeasurement[ sensorNum ].impRzMag; - F64 Z = condRawMeasurement[ sensorNum ].rtdRzMag; + F32 R = condRawMeasurement[ sensorNum ].impRzMag; + F32 Z = condRawMeasurement[ sensorNum ].rtdRzMag; // Aly sensors are known to send the driver INF in unfavorable conditions for impedance. e.g: open air // Change it to some high value that can be determined that we are reading INF. @@ -2132,6 +1971,11 @@ { R = COND_INFINITE_R_VALUE; } + // To prevent nan's being thrown into our rolling average by dividing by 0, we set to 1. + else if ( R == 0 ) + { + R = 1; + } if ( TRUE == isFPSensor ) { @@ -2148,13 +1992,14 @@ B0 = condCoeff[ sensorNum ].beta0DD; } + // B3( 1000 / impRz ) + B2( rtdRz / impRz ) + B1( 100ln(rtdRz) / impRz ) + B0 calculatedConductivity = ( ( B3 * ( 1000.0 / R ) ) + ( B2 * ( Z / R ) ) + ( B1 * ( ( 100 * log( Z ) ) / R ) ) + B0 ); - - condCalculatedMeasurement[ sensorNum ].Conductivity.data = (F32)calculatedConductivity; + calculatedConductivity = calculatedConductivity * COND_CONVERSION_SM_TO_USCM; + condCalculatedMeasurement[ sensorNum ].Conductivity.data = calculatedConductivity; } /*********************************************************************//** @@ -2167,9 +2012,9 @@ * @param isFPSensor - T/F if sensor is on FP hardware. * @return *************************************************************************/ -static void calculateConductivityUpdatedStandard( TEENSY_SENSOR_INDEX_T sensorNum, BOOL isFPSensor ) +static void calculateConductivityUpdatedStandard( U32 sensorNum, BOOL isFPSensor ) { - F64 calculatedConductivity = 0.0; + F32 calculatedConductivity = 0.0; F64 alpha = 0.0; F64 k = 0.0; @@ -2185,10 +2030,10 @@ } calculateResistance( sensorNum, isFPSensor ); calculateTemperature( sensorNum ); - - calculatedConductivity = ( ( k / getTeensyConductivityResistanceValue( sensorNum ) ) * - ( 1 + alpha * ( COND_TEMP_OFFSET - getTeensyConductivityTemperatureValue( sensorNum ) ) ) ); - condCalculatedMeasurement[ sensorNum ].Conductivity.data = (F32)calculatedConductivity; + // ( k / calculatedReistance ) * (1 + alpha*( 25 - calculatedTemperature ) ) + calculatedConductivity = ( ( k / condCalculatedMeasurement[ sensorNum ].Resistance.data ) * + ( 1 + ( alpha * ( COND_TEMP_OFFSET - condCalculatedMeasurement[ sensorNum ].Temperature.data ) ) ) ); + calculatedConductivity = calculatedConductivity * COND_CONVERSION_SM_TO_USCM; } /*********************************************************************//** @@ -2201,20 +2046,25 @@ * @param isFPSensor - T/F if sensor is on FP hardware. * @return *************************************************************************/ -static void calculateConductivityStandard( TEENSY_SENSOR_INDEX_T sensorNum, BOOL isFPSensor ) +static void calculateConductivityStandard( U32 sensorNum, BOOL isFPSensor ) { - F64 calculatedConductivity = 0.0; + F32 calculatedConductivity = 0.0; F64 alpha = 0.0; F64 k = 0.0; - F64 R = condRawMeasurement[ sensorNum ].impRzMag; + F32 R = condRawMeasurement[ sensorNum ].impRzMag; - // Aly sensors are known to send the driver INF in unfavorable conditions for impedance. e.g: open air + // Sensors are known to send the driver INF in unfavorable conditions for impedance. e.g: open air // Change it to some high value that can be determined that we are reading INF. // Otherwise we will publish no updated value or 0. if ( R == INFINITY ) { R = COND_INFINITE_R_VALUE; } + // To prevent nan's being thrown into our rolling average by dividing by 0, we set to 1. + else if ( R == 0 ) + { + R = 1; + } if ( TRUE == isFPSensor ) { @@ -2227,12 +2077,12 @@ k = condCoeff[ sensorNum ].kHighDDS; } calculateTemperature( sensorNum ); - + // ( k / impRz ) * (1 + alpha*( 25 - calculatedTemperature ) ) calculatedConductivity = ( ( k / R ) * - ( 1 + alpha * ( COND_TEMP_OFFSET - getTeensyConductivityTemperatureValue( sensorNum ) ) ) ); + ( 1 + ( alpha * ( COND_TEMP_OFFSET - condCalculatedMeasurement[ sensorNum ].Temperature.data ) ) ) ); calculatedConductivity = calculatedConductivity * COND_CONVERSION_SM_TO_USCM; - condCalculatedMeasurement[ sensorNum ].Conductivity.data = (F32)calculatedConductivity; + condCalculatedMeasurement[ sensorNum ].Conductivity.data = calculatedConductivity; } /*********************************************************************//** @@ -2245,13 +2095,13 @@ * @param isFPSensor - T/F if sensor is on FP hardware. * @return *************************************************************************/ -static void calculateResistance( TEENSY_SENSOR_INDEX_T sensorNum, BOOL isFPSensor ) +static void calculateResistance( U32 sensorNum, BOOL isFPSensor ) { - F64 calculatedResistance = 0.0; + F32 calculatedResistance = 0.0; F64 eta = 0.0; F64 zeta = 0.0; - F64 R = condRawMeasurement[ sensorNum ].impRzMag; + F32 R = condRawMeasurement[ sensorNum ].impRzMag; // Aly sensors are known to send the driver INF in unfavorable conditions. e.g: open air // Change it to some high value that can be determined that we are reading INF. @@ -2272,7 +2122,7 @@ zeta = condCoeff[ sensorNum ].zetaHighDDUS; } calculatedResistance = ( ( eta * R ) + zeta ); - condCalculatedMeasurement[ sensorNum ].Resistance.data = (F32)calculatedResistance; + condCalculatedMeasurement[ sensorNum ].Resistance.data = calculatedResistance; } /*********************************************************************//** @@ -2284,16 +2134,16 @@ * @param sensorNum - Teensy sensor index value. * @return *************************************************************************/ -static void calculateTemperature( TEENSY_SENSOR_INDEX_T sensorNum ) +static void calculateTemperature( U32 sensorNum ) { - F64 calculatedTemperature = 0.0; + F32 calculatedTemperature = 0.0; F64 beta = condCoeff[ sensorNum ].beta; F64 gamma = condCoeff[ sensorNum ].gamma; - F64 Z = condRawMeasurement[ sensorNum ].rtdRzMag; + F32 Z = condRawMeasurement[ sensorNum ].rtdRzMag; calculatedTemperature = ( ( beta * Z ) + gamma ); - condCalculatedMeasurement[ sensorNum ].Temperature.data = (F32)calculatedTemperature; + condCalculatedMeasurement[ sensorNum ].Temperature.data = calculatedTemperature; } @@ -2318,7 +2168,7 @@ TEST_OVERRIDE_ARRAY_PAYLOAD_T payload; OVERRIDE_TYPE_T ovType = getOverrideArrayPayloadFromMessage( message, &payload ); - if ( ( payload.index >= FIRST_DD_COND_SENSOR ) && ( payload.index <= LAST_FP_COND_SENSOR ) ) + if ( payload.index < NUM_OF_CONDUCTIVITY_SENSORS ) { // D74 is not connected to Teensy and does not have a data structure to override. if ( LAST_DD_COND_SENSOR != payload.index ) @@ -2328,13 +2178,13 @@ { F32 value = payload.state.f32; - condCalculatedMeasurement[ payload.index ].Conductivity.ovData = value; - condCalculatedMeasurement[ payload.index ].Conductivity.override = OVERRIDE_KEY; + condCalculatedMeasurement[ getTeensyCondId( payload.index ) ].Conductivity.ovData = value; + condCalculatedMeasurement[ getTeensyCondId( payload.index ) ].Conductivity.override = OVERRIDE_KEY; } else { - condCalculatedMeasurement[ payload.index ].Conductivity.override = OVERRIDE_RESET; - condCalculatedMeasurement[ payload.index ].Conductivity.ovData = condCalculatedMeasurement[ payload.index ].Conductivity.ovInitData; + condCalculatedMeasurement[ getTeensyCondId( payload.index ) ].Conductivity.override = OVERRIDE_RESET; + condCalculatedMeasurement[ getTeensyCondId( payload.index ) ].Conductivity.ovData = condCalculatedMeasurement[ payload.index ].Conductivity.ovInitData; } } } @@ -2357,7 +2207,7 @@ TEST_OVERRIDE_ARRAY_PAYLOAD_T payload; OVERRIDE_TYPE_T ovType = getOverrideArrayPayloadFromMessage( message, &payload ); - if ( ( payload.index >= FIRST_DD_COND_SENSOR ) && ( payload.index <= LAST_FP_COND_SENSOR ) ) + if ( payload.index < NUM_OF_CONDUCTIVITY_SENSORS ) { // D74 is not connected to Teensy and does not have a data structure to override. if ( LAST_DD_COND_SENSOR != payload.index ) @@ -2397,7 +2247,7 @@ TEST_OVERRIDE_ARRAY_PAYLOAD_T payload; OVERRIDE_TYPE_T ovType = getOverrideArrayPayloadFromMessage( message, &payload ); - if ( ( payload.index >= FIRST_DD_COND_SENSOR ) && ( payload.index <= LAST_FP_COND_SENSOR ) ) + if ( payload.index < NUM_OF_CONDUCTIVITY_SENSORS ) { // D74 is not connected to Teensy and does not have a data structure to override. if ( LAST_DD_COND_SENSOR != payload.index ) @@ -2437,12 +2287,12 @@ { if ( sizeof( U32 ) == message->hdr.payloadLen ) { - COND_MODELS_T newCondModel; + U32 newCondModel; memcpy( &newCondModel, &message->payload[0], sizeof( U32 ) ); if ( newCondModel < NUM_OF_MODELS ) { - currentConductivityModel = newCondModel; + currentConductivityModel = (COND_MODELS_T)newCondModel; result = TRUE; } }