kopia lustrzana https://github.com/YohanHadji/R2Home
Add files
Mathis et Yohan
rodzic
5a8627b53c
commit
1aa171ff9f
Plik binarny nie jest wyświetlany.
|
|
@ -0,0 +1,546 @@
|
|||
/*!
|
||||
* @file Adafruit_BMP280.cpp
|
||||
*
|
||||
* This is a library for the BMP280 orientation sensor
|
||||
*
|
||||
* Designed specifically to work with the Adafruit BMP280 Sensor.
|
||||
*
|
||||
* Pick one up today in the adafruit shop!
|
||||
* ------> https://www.adafruit.com/product/2651
|
||||
*
|
||||
* These sensors use I2C to communicate, 2 pins are required to interface.
|
||||
*
|
||||
* Adafruit invests time and resources providing this open source code,
|
||||
* please support Adafruit andopen-source hardware by purchasing products
|
||||
* from Adafruit!
|
||||
*
|
||||
* K.Townsend (Adafruit Industries)
|
||||
*
|
||||
* BSD license, all text above must be included in any redistribution
|
||||
*/
|
||||
|
||||
#include "Adafruit_BMP280.h"
|
||||
#include "Arduino.h"
|
||||
#include <Wire.h>
|
||||
|
||||
/*!
|
||||
* @brief BMP280 constructor using i2c
|
||||
* @param *theWire
|
||||
* optional wire
|
||||
*/
|
||||
Adafruit_BMP280::Adafruit_BMP280(TwoWire *theWire)
|
||||
: _cs(-1), _mosi(-1), _miso(-1), _sck(-1) {
|
||||
_wire = theWire;
|
||||
temp_sensor = new Adafruit_BMP280_Temp(this);
|
||||
pressure_sensor = new Adafruit_BMP280_Pressure(this);
|
||||
}
|
||||
|
||||
Adafruit_BMP280::~Adafruit_BMP280(void) {
|
||||
delete temp_sensor;
|
||||
delete pressure_sensor;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief BMP280 constructor using hardware SPI
|
||||
* @param cspin
|
||||
* cs pin number
|
||||
* @param theSPI
|
||||
* optional SPI object
|
||||
*/
|
||||
Adafruit_BMP280::Adafruit_BMP280(int8_t cspin, SPIClass *theSPI)
|
||||
: _cs(cspin), _mosi(-1), _miso(-1), _sck(-1) {
|
||||
_spi = theSPI;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief BMP280 constructor using bitbang SPI
|
||||
* @param cspin
|
||||
* The pin to use for CS/SSEL.
|
||||
* @param mosipin
|
||||
* The pin to use for MOSI.
|
||||
* @param misopin
|
||||
* The pin to use for MISO.
|
||||
* @param sckpin
|
||||
* The pin to use for SCK.
|
||||
*/
|
||||
Adafruit_BMP280::Adafruit_BMP280(int8_t cspin, int8_t mosipin, int8_t misopin,
|
||||
int8_t sckpin)
|
||||
: _cs(cspin), _mosi(mosipin), _miso(misopin), _sck(sckpin) {}
|
||||
|
||||
/*!
|
||||
* Initialises the sensor.
|
||||
* @param addr
|
||||
* The I2C address to use (default = 0x77)
|
||||
* @param chipid
|
||||
* The expected chip ID (used to validate connection).
|
||||
* @return True if the init was successful, otherwise false.
|
||||
*/
|
||||
bool Adafruit_BMP280::begin(uint8_t addr, uint8_t chipid) {
|
||||
_i2caddr = addr;
|
||||
|
||||
if (_cs == -1) {
|
||||
// i2c
|
||||
_wire->begin();
|
||||
} else {
|
||||
digitalWrite(_cs, HIGH);
|
||||
pinMode(_cs, OUTPUT);
|
||||
|
||||
if (_sck == -1) {
|
||||
// hardware SPI
|
||||
_spi->begin();
|
||||
} else {
|
||||
// software SPI
|
||||
pinMode(_sck, OUTPUT);
|
||||
pinMode(_mosi, OUTPUT);
|
||||
pinMode(_miso, INPUT);
|
||||
}
|
||||
}
|
||||
|
||||
if (read8(BMP280_REGISTER_CHIPID) != chipid)
|
||||
return false;
|
||||
|
||||
readCoefficients();
|
||||
// write8(BMP280_REGISTER_CONTROL, 0x3F); /* needed? */
|
||||
setSampling();
|
||||
delay(100);
|
||||
return true;
|
||||
}
|
||||
|
||||
/*!
|
||||
* Sets the sampling config for the device.
|
||||
* @param mode
|
||||
* The operating mode of the sensor.
|
||||
* @param tempSampling
|
||||
* The sampling scheme for temp readings.
|
||||
* @param pressSampling
|
||||
* The sampling scheme for pressure readings.
|
||||
* @param filter
|
||||
* The filtering mode to apply (if any).
|
||||
* @param duration
|
||||
* The sampling duration.
|
||||
*/
|
||||
void Adafruit_BMP280::setSampling(sensor_mode mode,
|
||||
sensor_sampling tempSampling,
|
||||
sensor_sampling pressSampling,
|
||||
sensor_filter filter,
|
||||
standby_duration duration) {
|
||||
_measReg.mode = mode;
|
||||
_measReg.osrs_t = tempSampling;
|
||||
_measReg.osrs_p = pressSampling;
|
||||
|
||||
_configReg.filter = filter;
|
||||
_configReg.t_sb = duration;
|
||||
|
||||
write8(BMP280_REGISTER_CONFIG, _configReg.get());
|
||||
write8(BMP280_REGISTER_CONTROL, _measReg.get());
|
||||
}
|
||||
|
||||
uint8_t Adafruit_BMP280::spixfer(uint8_t x) {
|
||||
if (_sck == -1)
|
||||
return _spi->transfer(x);
|
||||
|
||||
// software spi
|
||||
// Serial.println("Software SPI");
|
||||
uint8_t reply = 0;
|
||||
for (int i = 7; i >= 0; i--) {
|
||||
reply <<= 1;
|
||||
digitalWrite(_sck, LOW);
|
||||
digitalWrite(_mosi, x & (1 << i));
|
||||
digitalWrite(_sck, HIGH);
|
||||
if (digitalRead(_miso))
|
||||
reply |= 1;
|
||||
}
|
||||
return reply;
|
||||
}
|
||||
|
||||
/**************************************************************************/
|
||||
/*!
|
||||
@brief Writes an 8 bit value over I2C/SPI
|
||||
*/
|
||||
/**************************************************************************/
|
||||
void Adafruit_BMP280::write8(byte reg, byte value) {
|
||||
if (_cs == -1) {
|
||||
_wire->beginTransmission((uint8_t)_i2caddr);
|
||||
_wire->write((uint8_t)reg);
|
||||
_wire->write((uint8_t)value);
|
||||
_wire->endTransmission();
|
||||
} else {
|
||||
if (_sck == -1)
|
||||
_spi->beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE0));
|
||||
digitalWrite(_cs, LOW);
|
||||
spixfer(reg & ~0x80); // write, bit 7 low
|
||||
spixfer(value);
|
||||
digitalWrite(_cs, HIGH);
|
||||
if (_sck == -1)
|
||||
_spi->endTransaction(); // release the SPI bus
|
||||
}
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Reads an 8 bit value over I2C/SPI
|
||||
* @param reg
|
||||
* selected register
|
||||
* @return value from selected register
|
||||
*/
|
||||
uint8_t Adafruit_BMP280::read8(byte reg) {
|
||||
uint8_t value;
|
||||
|
||||
if (_cs == -1) {
|
||||
_wire->beginTransmission((uint8_t)_i2caddr);
|
||||
_wire->write((uint8_t)reg);
|
||||
_wire->endTransmission();
|
||||
_wire->requestFrom((uint8_t)_i2caddr, (byte)1);
|
||||
value = _wire->read();
|
||||
|
||||
} else {
|
||||
if (_sck == -1)
|
||||
_spi->beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE0));
|
||||
digitalWrite(_cs, LOW);
|
||||
spixfer(reg | 0x80); // read, bit 7 high
|
||||
value = spixfer(0);
|
||||
digitalWrite(_cs, HIGH);
|
||||
if (_sck == -1)
|
||||
_spi->endTransaction(); // release the SPI bus
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Reads a 16 bit value over I2C/SPI
|
||||
*/
|
||||
uint16_t Adafruit_BMP280::read16(byte reg) {
|
||||
uint16_t value;
|
||||
|
||||
if (_cs == -1) {
|
||||
_wire->beginTransmission((uint8_t)_i2caddr);
|
||||
_wire->write((uint8_t)reg);
|
||||
_wire->endTransmission();
|
||||
_wire->requestFrom((uint8_t)_i2caddr, (byte)2);
|
||||
value = (_wire->read() << 8) | _wire->read();
|
||||
|
||||
} else {
|
||||
if (_sck == -1)
|
||||
_spi->beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE0));
|
||||
digitalWrite(_cs, LOW);
|
||||
spixfer(reg | 0x80); // read, bit 7 high
|
||||
value = (spixfer(0) << 8) | spixfer(0);
|
||||
digitalWrite(_cs, HIGH);
|
||||
if (_sck == -1)
|
||||
_spi->endTransaction(); // release the SPI bus
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
uint16_t Adafruit_BMP280::read16_LE(byte reg) {
|
||||
uint16_t temp = read16(reg);
|
||||
return (temp >> 8) | (temp << 8);
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Reads a signed 16 bit value over I2C/SPI
|
||||
*/
|
||||
int16_t Adafruit_BMP280::readS16(byte reg) { return (int16_t)read16(reg); }
|
||||
|
||||
int16_t Adafruit_BMP280::readS16_LE(byte reg) {
|
||||
return (int16_t)read16_LE(reg);
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Reads a 24 bit value over I2C/SPI
|
||||
*/
|
||||
uint32_t Adafruit_BMP280::read24(byte reg) {
|
||||
uint32_t value;
|
||||
|
||||
if (_cs == -1) {
|
||||
_wire->beginTransmission((uint8_t)_i2caddr);
|
||||
_wire->write((uint8_t)reg);
|
||||
_wire->endTransmission();
|
||||
_wire->requestFrom((uint8_t)_i2caddr, (byte)3);
|
||||
|
||||
value = _wire->read();
|
||||
value <<= 8;
|
||||
value |= _wire->read();
|
||||
value <<= 8;
|
||||
value |= _wire->read();
|
||||
|
||||
} else {
|
||||
if (_sck == -1)
|
||||
_spi->beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE0));
|
||||
digitalWrite(_cs, LOW);
|
||||
spixfer(reg | 0x80); // read, bit 7 high
|
||||
|
||||
value = spixfer(0);
|
||||
value <<= 8;
|
||||
value |= spixfer(0);
|
||||
value <<= 8;
|
||||
value |= spixfer(0);
|
||||
|
||||
digitalWrite(_cs, HIGH);
|
||||
if (_sck == -1)
|
||||
_spi->endTransaction(); // release the SPI bus
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Reads the factory-set coefficients
|
||||
*/
|
||||
void Adafruit_BMP280::readCoefficients() {
|
||||
_bmp280_calib.dig_T1 = read16_LE(BMP280_REGISTER_DIG_T1);
|
||||
_bmp280_calib.dig_T2 = readS16_LE(BMP280_REGISTER_DIG_T2);
|
||||
_bmp280_calib.dig_T3 = readS16_LE(BMP280_REGISTER_DIG_T3);
|
||||
|
||||
_bmp280_calib.dig_P1 = read16_LE(BMP280_REGISTER_DIG_P1);
|
||||
_bmp280_calib.dig_P2 = readS16_LE(BMP280_REGISTER_DIG_P2);
|
||||
_bmp280_calib.dig_P3 = readS16_LE(BMP280_REGISTER_DIG_P3);
|
||||
_bmp280_calib.dig_P4 = readS16_LE(BMP280_REGISTER_DIG_P4);
|
||||
_bmp280_calib.dig_P5 = readS16_LE(BMP280_REGISTER_DIG_P5);
|
||||
_bmp280_calib.dig_P6 = readS16_LE(BMP280_REGISTER_DIG_P6);
|
||||
_bmp280_calib.dig_P7 = readS16_LE(BMP280_REGISTER_DIG_P7);
|
||||
_bmp280_calib.dig_P8 = readS16_LE(BMP280_REGISTER_DIG_P8);
|
||||
_bmp280_calib.dig_P9 = readS16_LE(BMP280_REGISTER_DIG_P9);
|
||||
}
|
||||
|
||||
/*!
|
||||
* Reads the temperature from the device.
|
||||
* @return The temperature in degress celcius.
|
||||
*/
|
||||
float Adafruit_BMP280::readTemperature() {
|
||||
int32_t var1, var2;
|
||||
|
||||
int32_t adc_T = read24(BMP280_REGISTER_TEMPDATA);
|
||||
adc_T >>= 4;
|
||||
|
||||
var1 = ((((adc_T >> 3) - ((int32_t)_bmp280_calib.dig_T1 << 1))) *
|
||||
((int32_t)_bmp280_calib.dig_T2)) >>
|
||||
11;
|
||||
|
||||
var2 = (((((adc_T >> 4) - ((int32_t)_bmp280_calib.dig_T1)) *
|
||||
((adc_T >> 4) - ((int32_t)_bmp280_calib.dig_T1))) >>
|
||||
12) *
|
||||
((int32_t)_bmp280_calib.dig_T3)) >>
|
||||
14;
|
||||
|
||||
t_fine = var1 + var2;
|
||||
|
||||
float T = (t_fine * 5 + 128) >> 8;
|
||||
return T / 100;
|
||||
}
|
||||
|
||||
/*!
|
||||
* Reads the barometric pressure from the device.
|
||||
* @return Barometric pressure in Pa.
|
||||
*/
|
||||
float Adafruit_BMP280::readPressure() {
|
||||
int64_t var1, var2, p;
|
||||
|
||||
// Must be done first to get the t_fine variable set up
|
||||
readTemperature();
|
||||
|
||||
int32_t adc_P = read24(BMP280_REGISTER_PRESSUREDATA);
|
||||
adc_P >>= 4;
|
||||
|
||||
var1 = ((int64_t)t_fine) - 128000;
|
||||
var2 = var1 * var1 * (int64_t)_bmp280_calib.dig_P6;
|
||||
var2 = var2 + ((var1 * (int64_t)_bmp280_calib.dig_P5) << 17);
|
||||
var2 = var2 + (((int64_t)_bmp280_calib.dig_P4) << 35);
|
||||
var1 = ((var1 * var1 * (int64_t)_bmp280_calib.dig_P3) >> 8) +
|
||||
((var1 * (int64_t)_bmp280_calib.dig_P2) << 12);
|
||||
var1 =
|
||||
(((((int64_t)1) << 47) + var1)) * ((int64_t)_bmp280_calib.dig_P1) >> 33;
|
||||
|
||||
if (var1 == 0) {
|
||||
return 0; // avoid exception caused by division by zero
|
||||
}
|
||||
p = 1048576 - adc_P;
|
||||
p = (((p << 31) - var2) * 3125) / var1;
|
||||
var1 = (((int64_t)_bmp280_calib.dig_P9) * (p >> 13) * (p >> 13)) >> 25;
|
||||
var2 = (((int64_t)_bmp280_calib.dig_P8) * p) >> 19;
|
||||
|
||||
p = ((p + var1 + var2) >> 8) + (((int64_t)_bmp280_calib.dig_P7) << 4);
|
||||
return (float)p / 256;
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Calculates the approximate altitude using barometric pressure and the
|
||||
* supplied sea level hPa as a reference.
|
||||
* @param seaLevelhPa
|
||||
* The current hPa at sea level.
|
||||
* @return The approximate altitude above sea level in meters.
|
||||
*/
|
||||
float Adafruit_BMP280::readAltitude(float seaLevelhPa) {
|
||||
float altitude;
|
||||
|
||||
float pressure = readPressure(); // in Si units for Pascal
|
||||
pressure /= 100;
|
||||
|
||||
altitude = 44330 * (1.0 - pow(pressure / seaLevelhPa, 0.1903));
|
||||
|
||||
return altitude;
|
||||
}
|
||||
|
||||
/*!
|
||||
* Calculates the pressure at sea level (QFH) from the specified altitude,
|
||||
* and atmospheric pressure (QFE).
|
||||
* @param altitude Altitude in m
|
||||
* @param atmospheric Atmospheric pressure in hPa
|
||||
* @return The approximate pressure in hPa
|
||||
*/
|
||||
float Adafruit_BMP280::seaLevelForAltitude(float altitude, float atmospheric) {
|
||||
// Equation taken from BMP180 datasheet (page 17):
|
||||
// http://www.adafruit.com/datasheets/BST-BMP180-DS000-09.pdf
|
||||
|
||||
// Note that using the equation from wikipedia can give bad results
|
||||
// at high altitude. See this thread for more information:
|
||||
// http://forums.adafruit.com/viewtopic.php?f=22&t=58064
|
||||
return atmospheric / pow(1.0 - (altitude / 44330.0), 5.255);
|
||||
}
|
||||
|
||||
/*!
|
||||
@brief calculates the boiling point of water by a given pressure
|
||||
@param pressure pressure in hPa
|
||||
@return temperature in °C
|
||||
*/
|
||||
|
||||
float Adafruit_BMP280::waterBoilingPoint(float pressure) {
|
||||
// Magnusformular for calculation of the boiling point of water at a given
|
||||
// pressure
|
||||
return (234.175 * log(pressure / 6.1078)) /
|
||||
(17.08085 - log(pressure / 6.1078));
|
||||
}
|
||||
|
||||
/*!
|
||||
* @brief Take a new measurement (only possible in forced mode)
|
||||
* !!!todo!!!
|
||||
*/
|
||||
/*
|
||||
void Adafruit_BMP280::takeForcedMeasurement()
|
||||
{
|
||||
// If we are in forced mode, the BME sensor goes back to sleep after each
|
||||
// measurement and we need to set it to forced mode once at this point, so
|
||||
// it will take the next measurement and then return to sleep again.
|
||||
// In normal mode simply does new measurements periodically.
|
||||
if (_measReg.mode == MODE_FORCED) {
|
||||
// set to forced mode, i.e. "take next measurement"
|
||||
write8(BMP280_REGISTER_CONTROL, _measReg.get());
|
||||
// wait until measurement has been completed, otherwise we would read
|
||||
// the values from the last measurement
|
||||
while (read8(BMP280_REGISTER_STATUS) & 0x08)
|
||||
delay(1);
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
/*!
|
||||
* @brief Resets the chip via soft reset
|
||||
*/
|
||||
void Adafruit_BMP280::reset(void) {
|
||||
write8(BMP280_REGISTER_SOFTRESET, MODE_SOFT_RESET_CODE);
|
||||
}
|
||||
|
||||
/*!
|
||||
@brief Gets the most recent sensor event from the hardware status register.
|
||||
@return Sensor status as a byte.
|
||||
*/
|
||||
uint8_t Adafruit_BMP280::getStatus(void) {
|
||||
return read8(BMP280_REGISTER_STATUS);
|
||||
}
|
||||
|
||||
/*!
|
||||
@brief Gets an Adafruit Unified Sensor object for the temp sensor component
|
||||
@return Adafruit_Sensor pointer to temperature sensor
|
||||
*/
|
||||
Adafruit_Sensor *Adafruit_BMP280::getTemperatureSensor(void) {
|
||||
return temp_sensor;
|
||||
}
|
||||
|
||||
/*!
|
||||
@brief Gets an Adafruit Unified Sensor object for the pressure sensor
|
||||
component
|
||||
@return Adafruit_Sensor pointer to pressure sensor
|
||||
*/
|
||||
Adafruit_Sensor *Adafruit_BMP280::getPressureSensor(void) {
|
||||
return pressure_sensor;
|
||||
}
|
||||
|
||||
/**************************************************************************/
|
||||
/*!
|
||||
@brief Gets the sensor_t data for the BMP280's temperature sensor
|
||||
*/
|
||||
/**************************************************************************/
|
||||
void Adafruit_BMP280_Temp::getSensor(sensor_t *sensor) {
|
||||
/* Clear the sensor_t object */
|
||||
memset(sensor, 0, sizeof(sensor_t));
|
||||
|
||||
/* Insert the sensor name in the fixed length char array */
|
||||
strncpy(sensor->name, "BMP280", sizeof(sensor->name) - 1);
|
||||
sensor->name[sizeof(sensor->name) - 1] = 0;
|
||||
sensor->version = 1;
|
||||
sensor->sensor_id = _sensorID;
|
||||
sensor->type = SENSOR_TYPE_AMBIENT_TEMPERATURE;
|
||||
sensor->min_delay = 0;
|
||||
sensor->min_value = -40.0; /* Temperature range -40 ~ +85 C */
|
||||
sensor->max_value = +85.0;
|
||||
sensor->resolution = 0.01; /* 0.01 C */
|
||||
}
|
||||
|
||||
/**************************************************************************/
|
||||
/*!
|
||||
@brief Gets the temperature as a standard sensor event
|
||||
@param event Sensor event object that will be populated
|
||||
@returns True
|
||||
*/
|
||||
/**************************************************************************/
|
||||
bool Adafruit_BMP280_Temp::getEvent(sensors_event_t *event) {
|
||||
/* Clear the event */
|
||||
memset(event, 0, sizeof(sensors_event_t));
|
||||
|
||||
event->version = sizeof(sensors_event_t);
|
||||
event->sensor_id = _sensorID;
|
||||
event->type = SENSOR_TYPE_AMBIENT_TEMPERATURE;
|
||||
event->timestamp = millis();
|
||||
event->temperature = _theBMP280->readTemperature();
|
||||
return true;
|
||||
}
|
||||
|
||||
/**************************************************************************/
|
||||
/*!
|
||||
@brief Gets the sensor_t data for the BMP280's pressure sensor
|
||||
*/
|
||||
/**************************************************************************/
|
||||
void Adafruit_BMP280_Pressure::getSensor(sensor_t *sensor) {
|
||||
/* Clear the sensor_t object */
|
||||
memset(sensor, 0, sizeof(sensor_t));
|
||||
|
||||
/* Insert the sensor name in the fixed length char array */
|
||||
strncpy(sensor->name, "BMP280", sizeof(sensor->name) - 1);
|
||||
sensor->name[sizeof(sensor->name) - 1] = 0;
|
||||
sensor->version = 1;
|
||||
sensor->sensor_id = _sensorID;
|
||||
sensor->type = SENSOR_TYPE_PRESSURE;
|
||||
sensor->min_delay = 0;
|
||||
sensor->min_value = 300.0; /* 300 ~ 1100 hPa */
|
||||
sensor->max_value = 1100.0;
|
||||
sensor->resolution = 0.012; /* 0.12 hPa relative */
|
||||
}
|
||||
|
||||
/**************************************************************************/
|
||||
/*!
|
||||
@brief Gets the pressure as a standard sensor event
|
||||
@param event Sensor event object that will be populated
|
||||
@returns True
|
||||
*/
|
||||
/**************************************************************************/
|
||||
bool Adafruit_BMP280_Pressure::getEvent(sensors_event_t *event) {
|
||||
/* Clear the event */
|
||||
memset(event, 0, sizeof(sensors_event_t));
|
||||
|
||||
event->version = sizeof(sensors_event_t);
|
||||
event->sensor_id = _sensorID;
|
||||
event->type = SENSOR_TYPE_PRESSURE;
|
||||
event->timestamp = millis();
|
||||
event->pressure = _theBMP280->readPressure() / 100; // convert Pa to hPa
|
||||
return true;
|
||||
}
|
||||
|
|
@ -0,0 +1,263 @@
|
|||
/*!
|
||||
* @file Adafruit_BMP280.h
|
||||
*
|
||||
* This is a library for the Adafruit BMP280 Breakout.
|
||||
*
|
||||
* Designed specifically to work with the Adafruit BMP280 Breakout.
|
||||
*
|
||||
* Pick one up today in the adafruit shop!
|
||||
* ------> https://www.adafruit.com/product/2651
|
||||
*
|
||||
* These sensors use I2C to communicate, 2 pins are required to interface.
|
||||
*
|
||||
* Adafruit invests time and resources providing this open source code,
|
||||
* please support Adafruit andopen-source hardware by purchasing products
|
||||
* from Adafruit!
|
||||
*
|
||||
* K.Townsend (Adafruit Industries)
|
||||
*
|
||||
* BSD license, all text above must be included in any redistribution
|
||||
*/
|
||||
#ifndef __BMP280_H__
|
||||
#define __BMP280_H__
|
||||
|
||||
#include "Adafruit_Sensor.h"
|
||||
#include "Arduino.h"
|
||||
#include <SPI.h>
|
||||
#include <Wire.h>
|
||||
|
||||
/*!
|
||||
* I2C ADDRESS/BITS/SETTINGS
|
||||
*/
|
||||
#define BMP280_ADDRESS (0x77) /**< The default I2C address for the sensor. */
|
||||
#define BMP280_ADDRESS_ALT \
|
||||
(0x76) /**< Alternative I2C address for the sensor. */
|
||||
#define BMP280_CHIPID (0x58) /**< Default chip ID. */
|
||||
|
||||
// Forward declarations of Wire and SPI for board/variant combinations that
|
||||
// don't have a default 'Wire' or 'SPI'
|
||||
extern TwoWire Wire1; /**< Forward declaration of Wire object */
|
||||
extern SPIClass SPI; /**< Forward declaration of SPI object */
|
||||
|
||||
/*!
|
||||
* Registers available on the sensor.
|
||||
*/
|
||||
enum {
|
||||
BMP280_REGISTER_DIG_T1 = 0x88,
|
||||
BMP280_REGISTER_DIG_T2 = 0x8A,
|
||||
BMP280_REGISTER_DIG_T3 = 0x8C,
|
||||
BMP280_REGISTER_DIG_P1 = 0x8E,
|
||||
BMP280_REGISTER_DIG_P2 = 0x90,
|
||||
BMP280_REGISTER_DIG_P3 = 0x92,
|
||||
BMP280_REGISTER_DIG_P4 = 0x94,
|
||||
BMP280_REGISTER_DIG_P5 = 0x96,
|
||||
BMP280_REGISTER_DIG_P6 = 0x98,
|
||||
BMP280_REGISTER_DIG_P7 = 0x9A,
|
||||
BMP280_REGISTER_DIG_P8 = 0x9C,
|
||||
BMP280_REGISTER_DIG_P9 = 0x9E,
|
||||
BMP280_REGISTER_CHIPID = 0xD0,
|
||||
BMP280_REGISTER_VERSION = 0xD1,
|
||||
BMP280_REGISTER_SOFTRESET = 0xE0,
|
||||
BMP280_REGISTER_CAL26 = 0xE1, /**< R calibration = 0xE1-0xF0 */
|
||||
BMP280_REGISTER_STATUS = 0xF3,
|
||||
BMP280_REGISTER_CONTROL = 0xF4,
|
||||
BMP280_REGISTER_CONFIG = 0xF5,
|
||||
BMP280_REGISTER_PRESSUREDATA = 0xF7,
|
||||
BMP280_REGISTER_TEMPDATA = 0xFA,
|
||||
};
|
||||
|
||||
/*!
|
||||
* Struct to hold calibration data.
|
||||
*/
|
||||
typedef struct {
|
||||
uint16_t dig_T1; /**< dig_T1 cal register. */
|
||||
int16_t dig_T2; /**< dig_T2 cal register. */
|
||||
int16_t dig_T3; /**< dig_T3 cal register. */
|
||||
|
||||
uint16_t dig_P1; /**< dig_P1 cal register. */
|
||||
int16_t dig_P2; /**< dig_P2 cal register. */
|
||||
int16_t dig_P3; /**< dig_P3 cal register. */
|
||||
int16_t dig_P4; /**< dig_P4 cal register. */
|
||||
int16_t dig_P5; /**< dig_P5 cal register. */
|
||||
int16_t dig_P6; /**< dig_P6 cal register. */
|
||||
int16_t dig_P7; /**< dig_P7 cal register. */
|
||||
int16_t dig_P8; /**< dig_P8 cal register. */
|
||||
int16_t dig_P9; /**< dig_P9 cal register. */
|
||||
} bmp280_calib_data;
|
||||
|
||||
class Adafruit_BMP280;
|
||||
|
||||
/** Adafruit Unified Sensor interface for temperature component of BMP280 */
|
||||
class Adafruit_BMP280_Temp : public Adafruit_Sensor {
|
||||
public:
|
||||
/** @brief Create an Adafruit_Sensor compatible object for the temp sensor
|
||||
@param parent A pointer to the BMP280 class */
|
||||
Adafruit_BMP280_Temp(Adafruit_BMP280 *parent) { _theBMP280 = parent; }
|
||||
bool getEvent(sensors_event_t *);
|
||||
void getSensor(sensor_t *);
|
||||
|
||||
private:
|
||||
int _sensorID = 280;
|
||||
Adafruit_BMP280 *_theBMP280 = NULL;
|
||||
};
|
||||
|
||||
/** Adafruit Unified Sensor interface for pressure component of BMP280 */
|
||||
class Adafruit_BMP280_Pressure : public Adafruit_Sensor {
|
||||
public:
|
||||
/** @brief Create an Adafruit_Sensor compatible object for the pressure sensor
|
||||
@param parent A pointer to the BMP280 class */
|
||||
Adafruit_BMP280_Pressure(Adafruit_BMP280 *parent) { _theBMP280 = parent; }
|
||||
bool getEvent(sensors_event_t *);
|
||||
void getSensor(sensor_t *);
|
||||
|
||||
private:
|
||||
int _sensorID = 0;
|
||||
Adafruit_BMP280 *_theBMP280 = NULL;
|
||||
};
|
||||
|
||||
/**
|
||||
* Driver for the Adafruit BMP280 barometric pressure sensor.
|
||||
*/
|
||||
class Adafruit_BMP280 {
|
||||
public:
|
||||
/** Oversampling rate for the sensor. */
|
||||
enum sensor_sampling {
|
||||
/** No over-sampling. */
|
||||
SAMPLING_NONE = 0x00,
|
||||
/** 1x over-sampling. */
|
||||
SAMPLING_X1 = 0x01,
|
||||
/** 2x over-sampling. */
|
||||
SAMPLING_X2 = 0x02,
|
||||
/** 4x over-sampling. */
|
||||
SAMPLING_X4 = 0x03,
|
||||
/** 8x over-sampling. */
|
||||
SAMPLING_X8 = 0x04,
|
||||
/** 16x over-sampling. */
|
||||
SAMPLING_X16 = 0x05
|
||||
};
|
||||
|
||||
/** Operating mode for the sensor. */
|
||||
enum sensor_mode {
|
||||
/** Sleep mode. */
|
||||
MODE_SLEEP = 0x00,
|
||||
/** Forced mode. */
|
||||
MODE_FORCED = 0x01,
|
||||
/** Normal mode. */
|
||||
MODE_NORMAL = 0x03,
|
||||
/** Software reset. */
|
||||
MODE_SOFT_RESET_CODE = 0xB6
|
||||
};
|
||||
|
||||
/** Filtering level for sensor data. */
|
||||
enum sensor_filter {
|
||||
/** No filtering. */
|
||||
FILTER_OFF = 0x00,
|
||||
/** 2x filtering. */
|
||||
FILTER_X2 = 0x01,
|
||||
/** 4x filtering. */
|
||||
FILTER_X4 = 0x02,
|
||||
/** 8x filtering. */
|
||||
FILTER_X8 = 0x03,
|
||||
/** 16x filtering. */
|
||||
FILTER_X16 = 0x04
|
||||
};
|
||||
|
||||
/** Standby duration in ms */
|
||||
enum standby_duration {
|
||||
/** 1 ms standby. */
|
||||
STANDBY_MS_1 = 0x00,
|
||||
/** 62.5 ms standby. */
|
||||
STANDBY_MS_63 = 0x01,
|
||||
/** 125 ms standby. */
|
||||
STANDBY_MS_125 = 0x02,
|
||||
/** 250 ms standby. */
|
||||
STANDBY_MS_250 = 0x03,
|
||||
/** 500 ms standby. */
|
||||
STANDBY_MS_500 = 0x04,
|
||||
/** 1000 ms standby. */
|
||||
STANDBY_MS_1000 = 0x05,
|
||||
/** 2000 ms standby. */
|
||||
STANDBY_MS_2000 = 0x06,
|
||||
/** 4000 ms standby. */
|
||||
STANDBY_MS_4000 = 0x07
|
||||
};
|
||||
|
||||
Adafruit_BMP280(TwoWire *theWire = &Wire1);
|
||||
Adafruit_BMP280(int8_t cspin, SPIClass *theSPI = &SPI);
|
||||
Adafruit_BMP280(int8_t cspin, int8_t mosipin, int8_t misopin, int8_t sckpin);
|
||||
~Adafruit_BMP280(void);
|
||||
|
||||
bool begin(uint8_t addr = BMP280_ADDRESS, uint8_t chipid = BMP280_CHIPID);
|
||||
void reset(void);
|
||||
uint8_t getStatus(void);
|
||||
|
||||
float readTemperature();
|
||||
float readPressure(void);
|
||||
float readAltitude(float seaLevelhPa = 1013.25);
|
||||
float seaLevelForAltitude(float altitude, float atmospheric);
|
||||
float waterBoilingPoint(float pressure);
|
||||
|
||||
Adafruit_Sensor *getTemperatureSensor(void);
|
||||
Adafruit_Sensor *getPressureSensor(void);
|
||||
|
||||
// void takeForcedMeasurement();
|
||||
void setSampling(sensor_mode mode = MODE_NORMAL,
|
||||
sensor_sampling tempSampling = SAMPLING_X16,
|
||||
sensor_sampling pressSampling = SAMPLING_X16,
|
||||
sensor_filter filter = FILTER_OFF,
|
||||
standby_duration duration = STANDBY_MS_1);
|
||||
|
||||
private:
|
||||
TwoWire *_wire; /**< Wire object */
|
||||
SPIClass *_spi; /**< SPI object */
|
||||
|
||||
Adafruit_BMP280_Temp *temp_sensor = NULL;
|
||||
Adafruit_BMP280_Pressure *pressure_sensor = NULL;
|
||||
|
||||
/** Encapsulates the config register */
|
||||
struct config {
|
||||
/** Inactive duration (standby time) in normal mode */
|
||||
unsigned int t_sb : 3;
|
||||
/** Filter settings */
|
||||
unsigned int filter : 3;
|
||||
/** Unused - don't set */
|
||||
unsigned int none : 1;
|
||||
/** Enables 3-wire SPI */
|
||||
unsigned int spi3w_en : 1;
|
||||
/** Used to retrieve the assembled config register's byte value. */
|
||||
unsigned int get() { return (t_sb << 5) | (filter << 2) | spi3w_en; }
|
||||
};
|
||||
|
||||
/** Encapsulates trhe ctrl_meas register */
|
||||
struct ctrl_meas {
|
||||
/** Temperature oversampling. */
|
||||
unsigned int osrs_t : 3;
|
||||
/** Pressure oversampling. */
|
||||
unsigned int osrs_p : 3;
|
||||
/** Device mode */
|
||||
unsigned int mode : 2;
|
||||
/** Used to retrieve the assembled ctrl_meas register's byte value. */
|
||||
unsigned int get() { return (osrs_t << 5) | (osrs_p << 2) | mode; }
|
||||
};
|
||||
|
||||
void readCoefficients(void);
|
||||
uint8_t spixfer(uint8_t x);
|
||||
void write8(byte reg, byte value);
|
||||
uint8_t read8(byte reg);
|
||||
uint16_t read16(byte reg);
|
||||
uint32_t read24(byte reg);
|
||||
int16_t readS16(byte reg);
|
||||
uint16_t read16_LE(byte reg);
|
||||
int16_t readS16_LE(byte reg);
|
||||
|
||||
uint8_t _i2caddr;
|
||||
|
||||
int32_t _sensorID;
|
||||
int32_t t_fine;
|
||||
int8_t _cs, _mosi, _miso, _sck;
|
||||
bmp280_calib_data _bmp280_calib;
|
||||
config _configReg;
|
||||
ctrl_meas _measReg;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,958 @@
|
|||
#include <TinyGPS++.h>
|
||||
#include <SD.h>
|
||||
#include "Adafruit_BMP280.h"
|
||||
#include <SBUS.h>
|
||||
#include <EasyBuzzer.h>
|
||||
#include <Watchdog.h>
|
||||
#include <PWMServo.h>
|
||||
#include <movingAvg.h>
|
||||
#include <Adafruit_NeoPixel.h>
|
||||
|
||||
// ----------------------------------- SETUP PANEL ----------------------------------- //
|
||||
|
||||
int parallax = true;
|
||||
|
||||
float dep_altitude = 110;
|
||||
float vup = 1.5;
|
||||
float vdown = -3.5;
|
||||
|
||||
float bcritical = 10.8;
|
||||
float blow = 11.4;
|
||||
|
||||
int servo_man_min = 1100;
|
||||
int servo_man_max = 1900;
|
||||
|
||||
int servo_auto_min = 1350;
|
||||
int servo_auto_max = 1650;
|
||||
|
||||
float gliding_timer = 5000;
|
||||
|
||||
// ----------------------------------- GLOBAL VARIABLES ----------------------------------- //
|
||||
|
||||
// BARO //
|
||||
Adafruit_BMP280 bmp;
|
||||
float alt_baro = 0;
|
||||
float prev_alt = 0;
|
||||
float vspeed = 0;
|
||||
float baro_set = 1000;
|
||||
float baro_count = 0;
|
||||
int vspeed_count = 0;
|
||||
boolean new_baro = false;
|
||||
float dt = 0;
|
||||
movingAvg al(5);
|
||||
movingAvg vs(5);
|
||||
|
||||
|
||||
// BATTERY //
|
||||
movingAvg voltage(50);
|
||||
float vpin = A17;
|
||||
float vbatt = 0;
|
||||
boolean batt_critical = false;
|
||||
boolean batt_low = false;
|
||||
|
||||
// GPS //
|
||||
TinyGPSPlus gps;
|
||||
TinyGPSCustom fix_type(gps, "GNGSA", 2);
|
||||
float prev_cog = 0;
|
||||
int cog_count = 2;
|
||||
int gps_count = 0;
|
||||
int valid_count = 0;
|
||||
float prev_gps = 0;
|
||||
boolean new_gps = false;
|
||||
boolean cog_ok = 0;
|
||||
|
||||
|
||||
// LED //
|
||||
#define LED_PIN 3
|
||||
#define LED_COUNT 1
|
||||
#define BRIGHTNESS 15
|
||||
Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRBW + NEO_KHZ800);
|
||||
int duration = 0;
|
||||
int timeled = 0;
|
||||
int lastled = 0;
|
||||
|
||||
// MISC //
|
||||
int buzzer = 2;
|
||||
int cam = 23;
|
||||
|
||||
// NAV //
|
||||
float merged_alt = 0;
|
||||
float setPoint_Home = 0;
|
||||
float errHome = 0;
|
||||
float baro_weight = 1;
|
||||
float gps_weight = 1;
|
||||
float lat_B = 0;
|
||||
float lon_B = 0;
|
||||
float cmd = 0;
|
||||
|
||||
|
||||
// RX //
|
||||
SBUS rx(Serial1);
|
||||
movingAvg s_steer(50);
|
||||
uint16_t channels[16];
|
||||
bool failSafe;
|
||||
bool lostFrame;
|
||||
|
||||
// SD CARD //
|
||||
File dataFile;
|
||||
const int chipSelect = BUILTIN_SDCARD;
|
||||
char namebuff[20];
|
||||
unsigned int addr = 0;
|
||||
String filename;
|
||||
int datatest = 0;
|
||||
int count = 0;
|
||||
|
||||
// SERVO /
|
||||
movingAvg st(5);
|
||||
PWMServo steer;
|
||||
PWMServo deploy;
|
||||
float steer_man = 1500;
|
||||
float steer_auto = 1500;
|
||||
float servo_steer = 1500;
|
||||
float parallax_steer = 1500;
|
||||
float servo_deploy = 2000;
|
||||
|
||||
byte feedback_pin = 6;
|
||||
float feedback_value = 0;
|
||||
|
||||
float Kp = 0.8;
|
||||
float Kd = 3;
|
||||
float cumerror = 0;
|
||||
float lasterror = 0;
|
||||
|
||||
// STATUS //
|
||||
int flight_mode = 0;
|
||||
int prev_mode = 0;
|
||||
bool deployed = false;
|
||||
bool baro_stab = false;
|
||||
bool gps_stab = false;
|
||||
bool initialised = false;
|
||||
bool wing_opened = false;
|
||||
|
||||
bool sd_ok = false;
|
||||
bool gps_ok = false;
|
||||
|
||||
int packet_count = 0;
|
||||
|
||||
|
||||
// STRING //
|
||||
char sdnamebuff[20];
|
||||
char lat_A_text[30];
|
||||
char lon_A_text[30];
|
||||
char alt_gps_text[30];
|
||||
char cog_text[30];
|
||||
char speed_text[30];
|
||||
char sat_text[30];
|
||||
char fix_type_text[10];
|
||||
char hdop_text[10];
|
||||
char pos_age_text[10];
|
||||
char count_text[10];
|
||||
char time_text[10];
|
||||
|
||||
char alt_baro_text[30];
|
||||
char vspeed_text[30];
|
||||
|
||||
char merged_alt_text[30];
|
||||
char baro_weight_text[30];
|
||||
char gps_weight_text[30];
|
||||
char setPoint_Home_text[30];
|
||||
char lat_B_text[30];
|
||||
char lon_B_text[30];
|
||||
char errHome_text[30];
|
||||
|
||||
char a_text[10];
|
||||
char b_text[10];
|
||||
char c_text[10];
|
||||
char d_text[10];
|
||||
char e_text[10];
|
||||
char f_text[10];
|
||||
char g_text[10];
|
||||
|
||||
char steer_text[10];
|
||||
char deploy_text[10];
|
||||
|
||||
char gps_ok_text[10];
|
||||
char cog_ok_text[10];
|
||||
char failSafe_text[10];
|
||||
char baro_stab_text[10];
|
||||
char gps_stab_text[10];
|
||||
char deployed_text[10];
|
||||
char flight_mode_text[10];
|
||||
char vbatt_text[10];
|
||||
char looptime_text[10];
|
||||
char packet_count_text[10];
|
||||
char initialised_text[10];
|
||||
char wing_opened_text[10];
|
||||
char feedback_value_text[10];
|
||||
|
||||
char mainSD[240];
|
||||
char mainTLM[250];
|
||||
|
||||
char mintext[120];
|
||||
char gps_text[120];
|
||||
char baro_text[120];
|
||||
char rc_text[120];
|
||||
char servo_text[120];
|
||||
char status_text[120];
|
||||
char alt_text[120];
|
||||
char nav_text[120];
|
||||
|
||||
char date_time[80];
|
||||
|
||||
char date_year[10];
|
||||
char date_month[10];
|
||||
char date_day[10];
|
||||
char time_hour[10];
|
||||
char time_minute[10];
|
||||
char time_seconde[10];
|
||||
|
||||
// TIMERS //
|
||||
|
||||
unsigned long baroA = 0;
|
||||
unsigned long baroB = 0;
|
||||
unsigned long tstab = 0;
|
||||
unsigned long tlm = 0;
|
||||
unsigned long sd = 0;
|
||||
unsigned long baro_blk = 0;
|
||||
unsigned long init_time = 0;
|
||||
unsigned long sat_buzz = 0;
|
||||
unsigned long tparallax = 0;
|
||||
unsigned long tpwm = 0;
|
||||
unsigned long batt_buzz = 0;
|
||||
unsigned long long tloop = 0;
|
||||
unsigned long long tgps = 0;
|
||||
|
||||
|
||||
int loop_time = 999;
|
||||
|
||||
int delaySD = 100; // Datalog
|
||||
int delayTLM = 1000; // Tlm
|
||||
|
||||
|
||||
// WATCHDOG //
|
||||
Watchdog watchdog;
|
||||
|
||||
|
||||
void setup() {
|
||||
|
||||
pinMode(A17, INPUT);
|
||||
pinMode(buzzer, OUTPUT);
|
||||
pinMode(cam, OUTPUT);
|
||||
digitalWrite(cam, LOW);
|
||||
|
||||
Serial.begin(115200);
|
||||
Serial5.begin(57600);
|
||||
|
||||
gpset(57600, 5, 0, 1, 0);
|
||||
|
||||
rx.begin();
|
||||
|
||||
EasyBuzzer.setPin(buzzer);
|
||||
|
||||
voltage.begin();
|
||||
s_steer.begin();
|
||||
st.begin();
|
||||
vs.begin();
|
||||
al.begin();
|
||||
|
||||
steer.attach(7);
|
||||
deploy.attach(8);
|
||||
|
||||
bmp.begin(0X76);
|
||||
|
||||
bmp.setSampling(Adafruit_BMP280::MODE_NORMAL, /* Operating Mode. */
|
||||
Adafruit_BMP280::SAMPLING_X1, /* Temp. oversampling */
|
||||
Adafruit_BMP280::SAMPLING_X2, /* Pressure oversampling */
|
||||
Adafruit_BMP280::FILTER_X16, /* Filtering. */
|
||||
Adafruit_BMP280::STANDBY_MS_1);
|
||||
|
||||
strip.begin();
|
||||
strip.show();
|
||||
strip.setBrightness(BRIGHTNESS);
|
||||
|
||||
watchdog.enable(Watchdog::TIMEOUT_1S);
|
||||
|
||||
tone(buzzer, 523);
|
||||
delay(200);
|
||||
tone(buzzer, 582);
|
||||
delay(200);
|
||||
tone(buzzer, 762);
|
||||
delay(300);
|
||||
noTone(buzzer);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
tloop = micros();
|
||||
|
||||
getdata();
|
||||
datacmpt();
|
||||
|
||||
flight_state();
|
||||
|
||||
applycmd();
|
||||
updatecmd(100);
|
||||
|
||||
userinter();
|
||||
|
||||
loop_time = (micros()-tloop);
|
||||
if (loop_time<100000) {watchdog.reset();}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void getdata() {
|
||||
|
||||
// -------------------------- Get GPS -------------------------- //
|
||||
|
||||
while (Serial7.available()) { gps.encode(Serial7.read()); }
|
||||
|
||||
// -------------------------- Get BARO & COMPASS -------------------------- //
|
||||
|
||||
if ((millis()-baroA)>=10) {
|
||||
baroA = millis();
|
||||
alt_baro = al.reading(bmp.readAltitude(baro_set)*100);
|
||||
alt_baro = (alt_baro/100);
|
||||
baro_count = (baro_count + 1);;
|
||||
|
||||
if (baro_count >= 10) {
|
||||
baro_count = 0;
|
||||
new_baro = true;
|
||||
dt = baroA-baroB;
|
||||
baroB = millis();
|
||||
}
|
||||
}
|
||||
|
||||
// -------------------------- Get RC -------------------------- //
|
||||
|
||||
rx.read(&channels[0], &failSafe, &lostFrame);
|
||||
|
||||
if ((channels[3])>=1500 or (channels[3])==0) { failSafe = true; }
|
||||
else { failSafe = false; }
|
||||
|
||||
steer_man = map(s_steer.reading(channels[0]), 50, 1950, 1200, 1800);
|
||||
steer_man = constrain(steer_man, servo_man_min, servo_man_max);
|
||||
|
||||
// -------------------------- Get Vbatt -------------------------- //
|
||||
|
||||
vbatt = analogRead(A17);
|
||||
vbatt = (voltage.reading(vbatt)*0.0141796875);
|
||||
|
||||
}
|
||||
|
||||
// ----------------------------------------------------- Data CMPT ----------------------------------------------------- //
|
||||
|
||||
void datacmpt() {
|
||||
|
||||
// -------------------------- NAV -------------------------- //
|
||||
|
||||
// -------------------- GPS Healt -------------------- //
|
||||
|
||||
if (((gps.location.age()) < 999) and (atof(fix_type.value()) == 3)) { gps_ok = true; }
|
||||
else { gps_ok = false; }
|
||||
|
||||
// -------------------------- STUFF DEPENDING ON GPS COURSE OVER GROUND -------------------------- //
|
||||
|
||||
// -------------------- Autopilot -------------------- //
|
||||
|
||||
if (new_gps == true) {
|
||||
new_gps = false;
|
||||
if (cog_valid(cog_count) == true) {
|
||||
cog_ok = true;
|
||||
setPoint_Home = TinyGPSPlus::courseTo(gps.location.lat(),gps.location.lng(),lat_B,lon_B);
|
||||
errHome = getangle(gps.course.deg(), setPoint_Home);
|
||||
}
|
||||
else { cog_ok = false; }
|
||||
prev_cog = gps.course.deg();
|
||||
}
|
||||
|
||||
if ((gps_ok == true) and (cog_ok == true)) {
|
||||
steer_auto = map(errHome, -180, +180, 1200, 1800);
|
||||
steer_auto = constrain(steer_auto, servo_auto_min, servo_auto_max);
|
||||
}
|
||||
|
||||
else { steer_auto = 1500; }
|
||||
|
||||
// -------------------- Vertical Speed -------------------- //
|
||||
|
||||
if (new_baro == true) {
|
||||
|
||||
new_baro = false;
|
||||
|
||||
if (initialised == false) { baro_set = (baro_set + ((0-alt_baro)/100)); }
|
||||
if (millis()<=5000) { alt_baro = 0; }
|
||||
|
||||
float da = (alt_baro - prev_alt);
|
||||
float vps = (da/(dt/1000));
|
||||
vspeed = vs.reading(vps*100);
|
||||
vspeed = (vspeed/100);
|
||||
|
||||
prev_alt = alt_baro;
|
||||
|
||||
}
|
||||
|
||||
// -------------------- Alt Fusion -------------------- //
|
||||
|
||||
if (initialised == true) {
|
||||
baro_weight = (1+(abs(vspeed)/10));
|
||||
gps_weight = (50/gps.hdop.value());
|
||||
merged_alt = ((alt_baro*baro_weight)+(gps.altitude.meters()*gps_weight))/(baro_weight+gps_weight);
|
||||
}
|
||||
|
||||
// -------------------- Stationary -------------------- //
|
||||
|
||||
if ((millis()-tstab) >= 1000) {
|
||||
tstab = millis();
|
||||
|
||||
if (vspeed < 0.2 and vspeed > -0.2) { vspeed_count = (vspeed_count + 1); }
|
||||
else { vspeed_count = 0; }
|
||||
|
||||
if (abs(prev_gps-gps.altitude.meters())<1 and (gps.altitude.meters() != 0)) { gps_count = (gps_count + 1); }
|
||||
else { gps_count = 0; }
|
||||
prev_gps = gps.altitude.meters();
|
||||
|
||||
if (vspeed_count >= 10) { baro_stab = true; }
|
||||
else { baro_stab = false; }
|
||||
|
||||
if (gps_count >= 10) { gps_stab = true; }
|
||||
else { gps_stab = false; }
|
||||
|
||||
}
|
||||
|
||||
// -------------------------- String -------------------------- //
|
||||
|
||||
|
||||
snprintf(date_time, 80, "%s:%s:%s,%s:%s:%s", date_year, date_month, date_day, time_hour, time_minute, time_seconde);
|
||||
snprintf(sdnamebuff, 80, "%s%s%s%s", date_day, time_hour, time_minute, time_seconde);
|
||||
|
||||
dtostrf(gps.location.lat(), 10, 10, lat_A_text);
|
||||
dtostrf(gps.location.lng(), 10, 10, lon_A_text);
|
||||
dtostrf(gps.altitude.meters(), 1, 1, alt_gps_text);
|
||||
dtostrf(gps.course.deg(), 1, 0, cog_text);
|
||||
dtostrf(gps.speed.mps(), 1, 1, speed_text);
|
||||
dtostrf(gps.satellites.value(), 1, 0, sat_text);
|
||||
dtostrf(atof(fix_type.value()), 1, 0, fix_type_text);
|
||||
dtostrf(gps.hdop.value(), 1, 0, hdop_text);
|
||||
|
||||
if (gps.location.age()>10000) { dtostrf(9999, 1, 0, pos_age_text); }
|
||||
else { dtostrf(gps.location.age(), 1, 0, pos_age_text); }
|
||||
|
||||
snprintf(gps_text, 120, "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s", date_time, lat_A_text, lon_A_text, alt_gps_text, cog_text, speed_text, sat_text, hdop_text, pos_age_text, fix_type_text);
|
||||
|
||||
dtostrf(alt_baro, 1, 3, alt_baro_text);
|
||||
dtostrf(vspeed, 1, 2, vspeed_text);
|
||||
|
||||
snprintf(baro_text, 120, "%s,%s", alt_baro_text, vspeed_text);
|
||||
|
||||
dtostrf(merged_alt, 2, 3, merged_alt_text);
|
||||
dtostrf(gps_weight, 2, 2, gps_weight_text);
|
||||
dtostrf(baro_weight, 2, 3, baro_weight_text);
|
||||
|
||||
dtostrf(setPoint_Home, 2, 2, setPoint_Home_text);
|
||||
dtostrf(errHome, 1, 2, errHome_text);
|
||||
|
||||
dtostrf(lat_B, 1, 5, lat_B_text);
|
||||
dtostrf(lon_B, 1, 5, lon_B_text);
|
||||
|
||||
snprintf(nav_text, 120, "%s,%s,%s,%s,%s,%s,%s", merged_alt_text, baro_weight_text, gps_weight_text, setPoint_Home_text, errHome_text, lat_B_text, lon_B_text);
|
||||
|
||||
dtostrf(channels[0], 2, 0, a_text);
|
||||
dtostrf(channels[1], 2, 0, b_text);
|
||||
dtostrf(channels[2], 2, 0, c_text);
|
||||
dtostrf(channels[3], 2, 0, d_text);
|
||||
dtostrf(channels[4], 2, 0, e_text);
|
||||
dtostrf(channels[5], 2, 0, f_text);
|
||||
dtostrf(channels[6], 2, 0, g_text);
|
||||
|
||||
snprintf(rc_text, 120, "%s,%s,%s,%s,%s,%s,%s", a_text, b_text, c_text, d_text, e_text, f_text, g_text);
|
||||
|
||||
dtostrf(servo_steer, 2, 0, steer_text);
|
||||
dtostrf(servo_deploy, 2, 0, deploy_text);
|
||||
dtostrf(feedback_value, 2, 0, feedback_value_text);
|
||||
|
||||
snprintf(servo_text, 40, "%s,%s,%s", steer_text, feedback_value_text, deploy_text);
|
||||
|
||||
dtostrf(millis(), 1, 0, time_text);
|
||||
dtostrf(flight_mode, 1, 0, flight_mode_text);
|
||||
dtostrf(gps_ok, 1, 0, gps_ok_text);
|
||||
dtostrf(cog_ok, 1, 0, cog_ok_text);
|
||||
dtostrf(failSafe, 1, 0, failSafe_text);
|
||||
dtostrf(gps_stab, 1, 0, gps_stab_text);
|
||||
dtostrf(baro_stab, 1, 0, baro_stab_text);
|
||||
dtostrf(deployed, 1, 0, deployed_text);
|
||||
dtostrf(wing_opened, 1, 0, wing_opened_text);
|
||||
dtostrf(vbatt, 1, 3, vbatt_text);
|
||||
dtostrf(loop_time, 1, 0, looptime_text);
|
||||
dtostrf(packet_count, 1, 0, packet_count_text);
|
||||
dtostrf(initialised, 1, 0, initialised_text);
|
||||
|
||||
snprintf(status_text, 60, "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s", time_text, packet_count_text, flight_mode_text, gps_ok_text, cog_ok_text, failSafe_text, gps_stab_text, baro_stab_text, deployed_text, wing_opened_text, initialised_text, vbatt_text, looptime_text);
|
||||
|
||||
snprintf(mainSD, 290, "%s,%s,%s,%s,%s,%s", status_text, gps_text, baro_text, nav_text, rc_text, servo_text);
|
||||
|
||||
snprintf(mainTLM, 250, "/*%s,%s,%s,%s*/", status_text, gps_text, baro_text, servo_text);
|
||||
|
||||
// -------------------------- TLM -------------------------- //
|
||||
|
||||
switch(flight_mode) {
|
||||
|
||||
case 0:
|
||||
delaySD = 100;
|
||||
delayTLM = 5000;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
case 2:
|
||||
case 3:
|
||||
case 4:
|
||||
case 5:
|
||||
case 6:
|
||||
delaySD = 50;
|
||||
delayTLM = 1000;
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
if (millis()-tlm>=delayTLM) {
|
||||
tlm = millis();
|
||||
packet_count = (packet_count +1);
|
||||
Serial5.println(mainTLM);
|
||||
Serial.println(mainTLM);
|
||||
}
|
||||
|
||||
// -------------------------- SD DATALOG -------------------------- //
|
||||
|
||||
if ((initialised == true) and ((flight_mode != 8) or (flight_mode != 9))) {
|
||||
|
||||
if ((millis()-sd)>=delaySD) {
|
||||
sd = millis();
|
||||
|
||||
if (!SD.begin(chipSelect)) { sd_ok = false; }
|
||||
else {
|
||||
sd_ok = true;
|
||||
dataFile = SD.open(namebuff, FILE_WRITE);
|
||||
|
||||
if (dataFile) { dataFile.println(mainSD); dataFile.close(); }
|
||||
else { sd_ok = false; }
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// ----------------------------------------------------- State machine ----------------------------------------------------- //
|
||||
|
||||
|
||||
void flight_state() {
|
||||
|
||||
if (flight_mode != prev_mode) {
|
||||
|
||||
packet_count = (packet_count +1);
|
||||
Serial5.println(mainTLM);
|
||||
Serial.println(mainTLM);
|
||||
|
||||
if (!SD.begin(chipSelect)) {}
|
||||
else {
|
||||
dataFile = SD.open(namebuff, FILE_WRITE);
|
||||
if (dataFile) { dataFile.println(mainSD); dataFile.close(); }
|
||||
}
|
||||
prev_mode = flight_mode;
|
||||
}
|
||||
|
||||
switch(flight_mode) {
|
||||
|
||||
case 0:
|
||||
flight_init();
|
||||
setled(255, 0, 0, 25, 2000);
|
||||
break;
|
||||
|
||||
case 1:
|
||||
ready_steady();
|
||||
setled(0, 255, 0, 25, 500);
|
||||
break;
|
||||
|
||||
case 2:
|
||||
flight_ascent();
|
||||
setled(0, 0, 255, 25, 250);
|
||||
break;
|
||||
|
||||
case 3:
|
||||
flight_descent();
|
||||
setled(255, 128, 0, 25, 1000);
|
||||
break;
|
||||
|
||||
case 4:
|
||||
flight_gliding();
|
||||
break;
|
||||
|
||||
case 5:
|
||||
flight_gliding_auto();
|
||||
setled(255, 255, 0, 25, 1000);
|
||||
break;
|
||||
|
||||
case 6:
|
||||
flight_gliding_manual();
|
||||
setled(0, 255, 255, 25, 1000);
|
||||
break;
|
||||
|
||||
case 7:
|
||||
landed();
|
||||
setled(128, 0, 255, 25, 1000);
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
//------------------- 0 -------------------//
|
||||
|
||||
void flight_init() {
|
||||
|
||||
if ((gps.satellites.value() >= 6) and (gps_ok == true) and (gps_stab == true) and (millis()>5000)) {
|
||||
|
||||
EasyBuzzer.beep(3000,100,50,10,500,1);
|
||||
lat_B = gps.location.lat();
|
||||
lon_B = gps.location.lng();
|
||||
|
||||
|
||||
while (abs(alt_baro-gps.altitude.meters())>0.01) {
|
||||
if ((micros()-baro_blk)>10) {
|
||||
baro_blk = millis();
|
||||
alt_baro = (bmp.readAltitude(baro_set));
|
||||
baro_set = (baro_set + ((gps.altitude.meters()-alt_baro)/8));
|
||||
prev_alt = alt_baro;
|
||||
watchdog.reset();
|
||||
}
|
||||
}
|
||||
|
||||
dep_altitude = (dep_altitude+gps.altitude.meters());
|
||||
|
||||
strip.setBrightness(50);
|
||||
setcam(1);
|
||||
|
||||
sprintf(namebuff, "%s.txt", sdnamebuff);
|
||||
if (!SD.begin(chipSelect)) {}
|
||||
else {
|
||||
SdFile::dateTimeCallback(dateTime);
|
||||
dataFile = SD.open(namebuff, FILE_WRITE);
|
||||
if (dataFile) {
|
||||
dataFile.println("onTime, PacketCount, Mode, GPSok, COGok, FailSafe, GPS stab, Baro stab, Deployed, WingOpened, Initialised, Vbatt, Looptime, Date, Time, Latitude, Longitude, AltGPS, CoG, Speed, Sat in use, HDOP, Posage, Fixtype, AltBARO, Vspeed, AltMERGED, BaroWeight, GPSWeight, SetPoint Home, errHome, LatB, LonB, Ch 0, Ch 1, Ch 2, Ch 3, Ch 4, Ch 5, Ch 6, ServoSteer, Pwm value, ServoDeploy");
|
||||
dataFile.close();
|
||||
}
|
||||
}
|
||||
|
||||
initialised = true;
|
||||
flight_mode = 1;
|
||||
init_time = millis();
|
||||
|
||||
}
|
||||
|
||||
if (millis()-sat_buzz>5000) {
|
||||
sat_buzz = millis();
|
||||
EasyBuzzer.beep(2000,50,25,gps.satellites.value(),100,1);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//------------------- 1 -------------------//
|
||||
|
||||
void ready_steady() {
|
||||
|
||||
if (millis()-init_time>=1000) {
|
||||
|
||||
if (vspeed>vup) { flight_mode = 2; EasyBuzzer.beep(1000,100,50,2,500,1); strip.setBrightness(75); }
|
||||
if (vspeed<vdown) { flight_mode = 3; EasyBuzzer.beep(3000,100,50,3,500,1); strip.setBrightness(100); }
|
||||
if (atof(fix_type.value()) < 2) { flight_mode = 0; EasyBuzzer.beep(700,100,50,3,500,1); strip.setBrightness(50); }
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//------------------- 2 -------------------//
|
||||
|
||||
void flight_ascent() { if (vspeed<0.5) {flight_mode = 1; strip.setBrightness(50);} }
|
||||
|
||||
//------------------- 3 -------------------//
|
||||
|
||||
void flight_descent() {
|
||||
|
||||
if (vspeed>-0.5) {flight_mode = 1; strip.setBrightness(50);}
|
||||
if (merged_alt < dep_altitude) { flight_mode = 4; EasyBuzzer.beep(3000,100,50,5,500,1); strip.setBrightness(255); deployed = true; init_time = millis(); }
|
||||
|
||||
}
|
||||
|
||||
//------------------- 4 -------------------//
|
||||
|
||||
void flight_gliding() {
|
||||
|
||||
if (((millis()-init_time) >= gliding_timer) and (vspeed > (vdown+1))) {
|
||||
wing_opened = true;
|
||||
if (failSafe == true) {flight_mode = 5; strip.setBrightness(100);}
|
||||
else {flight_mode = 6; strip.setBrightness(100);}
|
||||
}
|
||||
}
|
||||
|
||||
//------------------- 5 -------------------//
|
||||
|
||||
void flight_gliding_auto() {
|
||||
if (failSafe == false) {flight_mode = 6;}
|
||||
}
|
||||
|
||||
//------------------- 6 -------------------//
|
||||
|
||||
void flight_gliding_manual() {
|
||||
if (failSafe == true) {flight_mode = 5;}
|
||||
}
|
||||
|
||||
//------------------- 7 -------------------//
|
||||
|
||||
void landed() { if ((baro_stab == false) or (gps_stab == false)) {flight_mode = 1; strip.setBrightness(50);} }
|
||||
|
||||
// ----------------------------------------------------- Apply command ----------------------------------------------------- //
|
||||
|
||||
void applycmd() {
|
||||
|
||||
// -------------------------- Steering Servo -------------------------- //
|
||||
|
||||
switch(flight_mode) {
|
||||
|
||||
case 0:
|
||||
case 1:
|
||||
case 6:
|
||||
case 8:
|
||||
case 9:
|
||||
if (failSafe == false) { servo_steer = steer_man; }
|
||||
else { servo_steer = 1500; }
|
||||
break;
|
||||
|
||||
case 2:
|
||||
case 3:
|
||||
case 4:
|
||||
servo_steer = 1500;
|
||||
break;
|
||||
|
||||
case 5:
|
||||
servo_steer = steer_auto;
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
// -------------------------- Deployment Servo -------------------------- //
|
||||
|
||||
if (deployed == true) { servo_deploy = 1500; }
|
||||
|
||||
else {
|
||||
if (failSafe == true) { servo_deploy = 2000; }
|
||||
else { servo_deploy = map(channels[6], 50, 1950, 1000, 2000);}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// ----------------------------------------------------- Update command ----------------------------------------------------- //
|
||||
|
||||
void updatecmd(float a) {
|
||||
|
||||
if (parallax == true) {
|
||||
|
||||
if ((millis()-tparallax)>=(1000/a)) {
|
||||
|
||||
tparallax = millis();
|
||||
|
||||
parallax_steer = map(servo_steer, 1000, 2000, 1950, 1050);
|
||||
feedback_value = (((pulseIn(feedback_pin, HIGH, 1200))-30)/1.050);
|
||||
feedback_value = st.reading(feedback_value);
|
||||
feedback_value = map(feedback_value, 0, 1000, 1000, 2000);
|
||||
|
||||
float error = (parallax_steer - feedback_value);
|
||||
float raterror = (lasterror-error);
|
||||
|
||||
float PIDsum = ((Kp*error)+(Kd*(-raterror)));
|
||||
float cmd = 1500 - PIDsum;
|
||||
|
||||
constrain(cmd, 1000, 2000);
|
||||
cmd = map(cmd, 1000, 2000, 0, 180);
|
||||
lasterror = error;
|
||||
|
||||
steer.write(cmd);
|
||||
deploy.write(map(servo_deploy,1000,2000,40,150));
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
else {
|
||||
if ((millis()-tpwm)>=(1000/a)) {
|
||||
tpwm = millis();
|
||||
steer.write(map(servo_steer, 1000, 2000, 50, 130));
|
||||
deploy.write(map(servo_deploy,1000,2000,40,140));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ----------------------------------------------------- User Interface ----------------------------------------------------- //
|
||||
|
||||
void userinter() {
|
||||
|
||||
// -------------------------- Buzzer -------------------------- //
|
||||
|
||||
EasyBuzzer.update();
|
||||
|
||||
// -------------------------- Voltage monitoring -------------------------- //
|
||||
|
||||
if ((vbatt < bcritical) and (vbatt > 3)) {
|
||||
batt_critical = true;
|
||||
if (millis()-batt_buzz>=100) {
|
||||
batt_buzz = millis();
|
||||
EasyBuzzer.singleBeep(3000,25);
|
||||
}
|
||||
}
|
||||
|
||||
else { batt_critical = false; }
|
||||
|
||||
if ((vbatt < blow) and (vbatt > bcritical)) {
|
||||
batt_low = true;
|
||||
if (millis()-batt_buzz>=200) {
|
||||
batt_buzz = millis();
|
||||
EasyBuzzer.singleBeep(3000,50);
|
||||
}
|
||||
}
|
||||
|
||||
else { batt_low = false; }
|
||||
|
||||
|
||||
// -------------------------- RGB LED -------------------------- //
|
||||
|
||||
updateled();
|
||||
|
||||
}
|
||||
|
||||
// ----------------------------------------------------- Misc Functions ----------------------------------------------------- //
|
||||
|
||||
void setled(int a, int b, int c, int d, int e) {
|
||||
|
||||
if (batt_critical == true) { a = 255; b = 0; c = 0; d = 25; e = 50; }
|
||||
if (batt_low == true) { a = 255; b = 0; c = 0; d = 25; e = 200; }
|
||||
|
||||
if ((millis()-lastled)>=e) {
|
||||
lastled = millis();
|
||||
colorWipe(strip.Color(b,a,c), 0);
|
||||
duration = d;
|
||||
timeled = millis();
|
||||
}
|
||||
}
|
||||
|
||||
void updateled() {
|
||||
if ((millis()-timeled)>=duration) {
|
||||
colorWipe(strip.Color(0,0,0), 0);
|
||||
}
|
||||
}
|
||||
|
||||
void colorWipe(uint32_t color, int wait) {
|
||||
for(int i=0; i<strip.numPixels(); i++) {
|
||||
strip.setPixelColor(i, color);
|
||||
strip.show();
|
||||
}
|
||||
}
|
||||
|
||||
boolean cog_valid(int a) {
|
||||
if (abs(gps.course.deg()-prev_cog)<0.1) { valid_count++; }
|
||||
else { valid_count = 0; }
|
||||
if (valid_count >= a) { return false; }
|
||||
else { return true; }
|
||||
}
|
||||
|
||||
float getangle(float a, float b) {
|
||||
float angle = 0;
|
||||
if (abs(a-b) < 180) { angle = (b-a);}
|
||||
else {
|
||||
if ((a-b) < 0) { angle = (-360) +(b-a);}
|
||||
else { angle = (360) + (b-a);}
|
||||
}
|
||||
return angle;
|
||||
}
|
||||
|
||||
void setcam(int a) {
|
||||
switch(a) {
|
||||
case 1 :
|
||||
digitalWrite(cam, HIGH);
|
||||
break;
|
||||
case 0 :
|
||||
digitalWrite(cam, LOW);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void dateTime(uint16_t* date, uint16_t* time){
|
||||
*date = FAT_DATE(gps.date.year(), gps.date.month(), gps.date.day());
|
||||
*time = FAT_TIME(gps.time.hour(), gps.time.minute(), gps.time.second());
|
||||
}
|
||||
|
||||
void gpset(int a, int b, int c, int d, int e){
|
||||
|
||||
if (a == 9600) {
|
||||
Serial7.begin(9600);
|
||||
delay(100);
|
||||
byte packet1[] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x80, 0x25, 0x00, 0x00, 0x07, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x92, 0xB5};
|
||||
sendPacket(packet1, sizeof(packet1));
|
||||
}
|
||||
|
||||
if (a == 57600) {
|
||||
Serial7.begin(9600);
|
||||
delay(100);
|
||||
byte packet2[] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x00, 0xE1, 0x00, 0x00, 0x07, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0xCE, 0xC9};
|
||||
sendPacket(packet2, sizeof(packet2));
|
||||
Serial7.end();
|
||||
Serial7.begin(57600);
|
||||
delay(100);
|
||||
}
|
||||
|
||||
if (a == 115200) {
|
||||
Serial7.begin(9600);
|
||||
delay(100);
|
||||
byte packet3[] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x00, 0xC2, 0x01, 0x00, 0x07, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB0, 0x7E};
|
||||
sendPacket(packet3, sizeof(packet3));
|
||||
Serial7.end();
|
||||
Serial7.begin(115200);
|
||||
delay(100);
|
||||
}
|
||||
|
||||
if (b == 1) {
|
||||
byte packet4[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xE8, 0x03, 0x01, 0x00, 0x01, 0x00, 0x01, 0x39};
|
||||
sendPacket(packet4, sizeof(packet4));
|
||||
}
|
||||
|
||||
if (b == 5) {
|
||||
byte packet5[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A};
|
||||
sendPacket(packet5, sizeof(packet5));
|
||||
}
|
||||
|
||||
if (b == 10) {
|
||||
byte packet6[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0x64, 0x00, 0x01, 0x00, 0x01, 0x00, 0x7A, 0x12};
|
||||
sendPacket(packet6, sizeof(packet6));
|
||||
}
|
||||
|
||||
if (c == 0) {
|
||||
byte packet7[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x5E, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7E, 0x3C};
|
||||
sendPacket(packet7, sizeof(packet7));
|
||||
}
|
||||
|
||||
if (c == 1) {
|
||||
byte packet8[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x08, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x5E, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x86, 0x4C};
|
||||
sendPacket(packet8, sizeof(packet8));
|
||||
}
|
||||
|
||||
if (d == 1) {
|
||||
byte packet9[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x01, 0x01, 0x00, 0x00, 0x01, 0x01, 0x00, 0x03, 0x35};
|
||||
byte packet10[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x03, 0x01, 0x00, 0x00, 0x01, 0x01, 0x00, 0x05, 0x43};
|
||||
sendPacket(packet9, sizeof(packet9));
|
||||
sendPacket(packet10, sizeof(packet10));
|
||||
}
|
||||
|
||||
if (e == 1) {
|
||||
byte packet11[] = {0xB5, 0x62, 0x06, 0x1E, 0x14, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0A, 0x32, 0x00, 0x00, 0x99, 0x4C, 0x00, 0x00, 0x5B, 0x10};
|
||||
sendPacket(packet11, sizeof(packet11));
|
||||
}
|
||||
}
|
||||
|
||||
void sendPacket(byte *packet, byte len){
|
||||
for (byte i = 0; i < len; i++) { Serial7.write(packet[i]); }
|
||||
}
|
||||
Ładowanie…
Reference in New Issue