Skip to content

Commit

Permalink
Merge pull request #4282 from shellixyz/temp_sensors_improvements
Browse files Browse the repository at this point in the history
Temp sensors improvements
  • Loading branch information
digitalentity authored Feb 8, 2019
2 parents 939b206 + e9b65b1 commit 1dbc1a4
Show file tree
Hide file tree
Showing 35 changed files with 1,589 additions and 104 deletions.
4 changes: 4 additions & 0 deletions make/source.mk
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,10 @@ COMMON_SRC = \
drivers/system.c \
drivers/timer.c \
drivers/lights_io.c \
drivers/1-wire.c \
drivers/1-wire/ds_crc.c \
drivers/1-wire/ds2482.c \
drivers/temperature/ds18b20.c \
drivers/temperature/lm75.c \
drivers/pitotmeter_ms4525.c \
fc/cli.c \
Expand Down
53 changes: 45 additions & 8 deletions src/main/blackbox/blackbox.c
Original file line number Diff line number Diff line change
Expand Up @@ -353,8 +353,20 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
{"wind", 0, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"wind", 1, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"wind", 2, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"temperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"temperatureSource", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"IMUTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
#ifdef USE_BARO
{"baroTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
#endif
#ifdef USE_TEMPERATURE_SENSOR
{"sens0Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"sens1Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"sens2Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"sens3Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"sens4Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"sens5Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"sens6Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"sens7Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
#endif
};

typedef enum BlackboxState {
Expand Down Expand Up @@ -453,8 +465,13 @@ typedef struct blackboxSlowState_s {
uint16_t powerSupplyImpedance;
uint16_t sagCompensatedVBat;
int16_t wind[XYZ_AXIS_COUNT];
int16_t temperature;
uint8_t temperatureSource;
int16_t imuTemperature;
#ifdef USE_BARO
int16_t baroTemperature;
#endif
#ifdef USE_TEMPERATURE_SENSOR
int16_t tempSensorTemperature[MAX_TEMP_SENSORS];
#endif
} __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()

//From rc_controls.c
Expand Down Expand Up @@ -1042,8 +1059,15 @@ static void writeSlowFrame(void)

blackboxWriteSigned16VBArray(slowHistory.wind, XYZ_AXIS_COUNT);

blackboxWriteSignedVB(slowHistory.temperature);
blackboxWriteUnsignedVB(slowHistory.temperatureSource);
blackboxWriteSignedVB(slowHistory.imuTemperature);

#ifdef USE_BARO
blackboxWriteSignedVB(slowHistory.baroTemperature);
#endif

#ifdef USE_TEMPERATURE_SENSOR
blackboxWriteSigned16VBArray(slowHistory.tempSensorTemperature, MAX_TEMP_SENSORS);
#endif

blackboxSlowFrameIterationTimer = 0;
}
Expand Down Expand Up @@ -1075,10 +1099,23 @@ static void loadSlowState(blackboxSlowState_t *slow)
#else
slow->wind[i] = 0;
#endif
}

bool valid_temp;
valid_temp = getIMUTemperature(&slow->imuTemperature);
if (!valid_temp) slow->imuTemperature = 0xFFFF;

#ifdef USE_BARO
valid_temp = getBaroTemperature(&slow->baroTemperature);
if (!valid_temp) slow->baroTemperature = 0xFFFF;
#endif

slow->temperature = (int) getCurrentTemperature() * 10;
slow->temperatureSource = getCurrentTemperatureSensorUsed();
#ifdef USE_TEMPERATURE_SENSOR
for (uint8_t index; index < MAX_TEMP_SENSORS; ++index) {
valid_temp = getSensorTemperature(index, slow->tempSensorTemperature + index);
if (!valid_temp) slow->tempSensorTemperature[index] = 0xFFFF;
}
#endif

}

Expand Down
18 changes: 16 additions & 2 deletions src/main/cms/cms_menu_osd.c
Original file line number Diff line number Diff line change
Expand Up @@ -245,13 +245,27 @@ static const OSD_Entry menuOsdElemsEntries[] =
OSD_ELEMENT_ENTRY("WIND HOR", OSD_WIND_SPEED_HORIZONTAL),
OSD_ELEMENT_ENTRY("WIND VERT", OSD_WIND_SPEED_VERTICAL),

OSD_ELEMENT_ENTRY("TEMPERATURE", OSD_TEMPERATURE),
OSD_ELEMENT_ENTRY("IMU TEMP", OSD_IMU_TEMPERATURE),
#ifdef USE_BARO
OSD_ELEMENT_ENTRY("BARO TEMP", OSD_BARO_TEMPERATURE),
#endif

#ifdef USE_TEMPERATURE_SENSOR
OSD_ELEMENT_ENTRY("SENSOR 0 TEMP", OSD_TEMP_SENSOR_0_TEMPERATURE),
OSD_ELEMENT_ENTRY("SENSOR 1 TEMP", OSD_TEMP_SENSOR_1_TEMPERATURE),
OSD_ELEMENT_ENTRY("SENSOR 2 TEMP", OSD_TEMP_SENSOR_2_TEMPERATURE),
OSD_ELEMENT_ENTRY("SENSOR 3 TEMP", OSD_TEMP_SENSOR_3_TEMPERATURE),
OSD_ELEMENT_ENTRY("SENSOR 4 TEMP", OSD_TEMP_SENSOR_4_TEMPERATURE),
OSD_ELEMENT_ENTRY("SENSOR 5 TEMP", OSD_TEMP_SENSOR_5_TEMPERATURE),
OSD_ELEMENT_ENTRY("SENSOR 6 TEMP", OSD_TEMP_SENSOR_6_TEMPERATURE),
OSD_ELEMENT_ENTRY("SENSOR 7 TEMP", OSD_TEMP_SENSOR_7_TEMPERATURE),
#endif

OSD_BACK_ENTRY,
OSD_END_ENTRY,
};

#if defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT)
#if defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) && defined(USE_TEMPERATURE_SENSOR)
// All CMS OSD elements should be enabled in this case. The menu has 3 extra
// elements (label, back and end), but there's an OSD element that we intentionally
// don't show here (OSD_DEBUG).
Expand Down
3 changes: 2 additions & 1 deletion src/main/config/parameter_group_ids.h
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,8 @@
//#define PG_IBUS_TELEMETRY_CONFIG 53
#define PG_VTX_CONFIG 54
#define PG_ELERES_CONFIG 55
#define PG_CF_END 56
#define PG_TEMP_SENSOR_CONFIG 56
#define PG_CF_END 57

// Driver configuration
//#define PG_DRIVER_PWM_RX_CONFIG 100
Expand Down
31 changes: 31 additions & 0 deletions src/main/drivers/1-wire.c
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@

#include <string.h>

#include "drivers/1-wire.h"
#include "drivers/1-wire/ds2482.h"

#include "drivers/logging.h"


#ifdef USE_1WIRE

#ifdef USE_1WIRE_DS2482
static bool ds2482Detected = false;
static owDev_t ds2482Dev;
#endif

void owInit(void)
{
memset(&ds2482Dev, 0, sizeof(ds2482Dev));
addBootlogEvent2(BOOT_EVENT_TEMP_SENSOR_DETECTION, BOOT_EVENT_FLAGS_NONE);
#ifdef USE_1WIRE_DS2482
if (ds2482Detect(&ds2482Dev)) ds2482Detected = true;
#endif
}

owDev_t *getOwDev(void)
{
return ds2482Detected ? &ds2482Dev : NULL;
}

#endif /* USE_1WIRE */
75 changes: 75 additions & 0 deletions src/main/drivers/1-wire.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,75 @@

#pragma once

#include "drivers/bus.h"

#ifdef USE_1WIRE

#define OW_STATUS_1WB_POS 0 // 1-Wire busy
#define OW_STATUS_PPD_POS 1 // Presense-pulse detect
#define OW_STATUS_SD_POS 2 // Short detected
#define OW_STATUS_LL_POS 3 // Logic level
#define OW_STATUS_RST_POS 4 // Device reset
#define OW_STATUS_SBR_POS 5 // Single bit result
#define OW_STATUS_TSB_POS 6 // Triplet second bit
#define OW_STATUS_DIR_POS 7 // Branch direction taken

#define OW_BUS_BUSY(status) (status & (1 << OW_STATUS_1WB_POS))
#define OW_DEVICE_PRESENT(status) (status & (1 << OW_STATUS_PPD_POS)) // True if a device have been detected on the bus after a bus reset
#define OW_RESET(status) (status & (1 << OW_STATUS_RST_POS))
#define OW_LOGIC_LEVEL(status) (status & (1 << OW_STATUS_LL_POS))
#define OW_SHORT_DETECTED(status) (status & (1 << OW_STATUS_SD_POS))
#define OW_SBR_VALUE(status) ((status >> OW_STATUS_SBR_POS) & 1) // Extract single bit read value or triplet first bit from status register value
#define OW_TSB_VALUE(status) ((status >> OW_STATUS_TSB_POS) & 1) // Extract triplet second bit value from status register value
#define OW_DIR_VALUE(status) ((status >> OW_STATUS_DIR_POS) & 1) // Extract triplet chosen direction bit value from status register value

#define OW_TRIPLET_FIRST_BIT(tripletResult) (tripletResult & (1 << 0))
#define OW_TRIPLET_SECOND_BIT(tripletResult) (tripletResult & (1 << 1))
#define OW_TRIPLET_DIRECTION_BIT(tripletResult) (tripletResult & (1 << 2))

#define OW_SINGLE_BIT_WRITE0 0
#define OW_SINGLE_BIT_WRITE1_READ (1<<7)

typedef struct owDev_s {
busDevice_t *busDev;

uint8_t status;

bool (*reset)(struct owDev_s *owDev);
bool (*owResetCommand)(struct owDev_s *owDev);
bool (*owReset)(struct owDev_s *owDev);
bool (*waitForBus)(struct owDev_s *owDev);
bool (*readConfig)(struct owDev_s *owDev, uint8_t *config);
bool (*writeConfig)(struct owDev_s *owDev, uint8_t config);
bool (*readStatus)(struct owDev_s *owDev, uint8_t *status);
uint8_t (*getStatus)(struct owDev_s *owDev);
bool (*poll)(struct owDev_s *owDev, bool waitForBus, uint8_t *status);
bool (*owBusReady)(struct owDev_s *owDev);

// 1-Wire ROM
bool (*owSearchRom)(struct owDev_s *owDev, uint8_t familyCode, uint64_t *romTable, uint8_t *romTableLen);
bool (*owMatchRomCommand)(struct owDev_s *owDev);
bool (*owMatchRom)(struct owDev_s *owDev, uint64_t rom);
bool (*owSkipRomCommand)(struct owDev_s *owDev);
bool (*owSkipRom)(struct owDev_s *owDev);

// 1-Wire read/write
bool (*owWriteByteCommand)(struct owDev_s *owDev, uint8_t byte);
bool (*owWriteByte)(struct owDev_s *owDev, uint8_t byte);
bool (*owWriteBuf)(struct owDev_s *owDev, const uint8_t *buf, uint8_t len);
bool (*owReadByteCommand)(struct owDev_s *owDev);
bool (*owReadByteResult)(struct owDev_s *owDev, uint8_t *result);
bool (*owReadByte)(struct owDev_s *owDev, uint8_t *result);
bool (*owReadBuf)(struct owDev_s *owDev, uint8_t *buf, uint8_t len);
bool (*owSingleBitCommand)(struct owDev_s *owDev, uint8_t type);
bool (*owSingleBitResult)(struct owDev_s *owDev);
bool (*owSingleBit)(struct owDev_s *owDev, uint8_t type, bool *result);
bool (*owTripletCommand)(struct owDev_s *owDev, uint8_t direction);
uint8_t (*owTripletResult)(struct owDev_s *owDev);
bool (*owTriplet)(struct owDev_s *owDev, uint8_t direction, uint8_t *result);
} owDev_t;

void owInit(void);
owDev_t *getOwDev(void);

#endif /* defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) */
Loading

0 comments on commit 1dbc1a4

Please sign in to comment.