Hi all,
I’m passing on an ESP32/Pixhawk based research project to a new cohort of students in our lab. The communication between the ESP and pixhawk has been working fine, but I am a novice in these things, and want to make sure I’m not setting them off on the wrong foot.
Thanks in advance for any input!
Version Details:
Pixhawk 6C (Holybro)
Arducopter V4.5.4
Generic ESP32 Arduino framework
Mission Planner 1.3.81
Flowchart:
Relevant bits:
- The refresh rate on the data displayed on the GCS is not that critical as the ESP also has onboard logging. The main goal is to ensure the sensors are still operating.
- The ESP32 could be removed from the equation for this simple setup, but there are more sensors and data processing involved in the larger project, and there is a lot of sensor testing involved which is generally easier to Protoype in the Arduino framework.
Concerns:
- Strings are evil, right? Everyone says strings are evil, and I’m using strings. Should they all be char arrays?
- Is there a better way to send multiple values between the Pixhawk and GCS? In this thread @Yuri_Rage says that stacking a bunch of gcs:send_named_float() commands is clunky, but it seems to have worked up to 12 values so far.
- I need to add some filtering to the lua script to interpret NaN values or sensor errors in a controlled way.
ESP32 Script:
/*
For ESP32 running Arduino
This script recieves temp, pressure, and resistance values from four BME688 i2C connected sensors.
The values are put into an array, and then sent as a psuedo CSV line to a pixhawk.
For now, only the first four temperature values are being sent.
A lua script running on the Pixhawk recieves and parses the data.
Serial(0) is the default ESP32<->Computer programming serial comm port
pixhawkSerial(2) is the UART port communicating with the pixhawk
*/
#include <Arduino.h>
#include <Wire.h>
#include "Adafruit_BME680.h"
#define PCAADDR 0x70 //address for multiplexer board
#define TXD2 17 // pins used to initialize pixhawkSerial(2)
#define RXD2 16
HardwareSerial pixhawkSerial(2); // pixhawk comms use UART2, pins 16 and 17
Adafruit_BME680 bme[4]; //BME Sensor instances, four total
//PCA select handles multiplexer:
void pcaselect(uint8_t i) {
if (i > 7) return;
Wire.beginTransmission(PCAADDR);
Wire.write(1 << i);
Wire.endTransmission();
}
void tvoc_sensor_scan(float *tvoc_array, size_t numElements){
for(int i=0; i<numElements/3; i++){ //numElements/3 = num of sensors
//array is filled as [T0, T1, T2, T3, G0, G1, G2, G3, P0, P1, P2, P3]
int j=i+4;
int k=i+8;
pcaselect(i);
if (bme[i].performReading()) {
tvoc_array[i]=bme[i].temperature; //temp in celcius
tvoc_array[j]=(bme[i].gas_resistance/1000.0); //kOhms
tvoc_array[k]=(bme[i].pressure/100); //pressure in hPa
}else{
Serial.println("BME" + String(i) + " Error");
tvoc_array[i]=NAN; //temp in celcius
tvoc_array[j]=NAN; //kOhms
tvoc_array[k]=NAN; //pressure in hPa
}
}
}
void setup()
{
Serial.begin(115200);
pixhawkSerial.begin(115200, SERIAL_8N1, RXD2, TXD2);
Wire.begin();
//BME Sensors Initialization:
for(int i=0; i<4; i++){
pcaselect(i);
if (!bme[i].begin(0x77)) {
Serial.println("BME680" + String(i) + " error, check wiring!");
while (1);
}
bme[i].setTemperatureOversampling(BME680_OS_8X);
bme[i].setHumidityOversampling(BME680_OS_2X);
bme[i].setPressureOversampling(BME680_OS_4X);
bme[i].setIIRFilterSize(BME680_FILTER_SIZE_3);
Serial.println("BME Sensor " + String(i) + " Setup Success");
delay(10);
}
Serial.println("\nSetup Complete");
Serial.println("\n");
}
void loop()
{
const int tvoc_arraySize = 12;
float tvoc_array[tvoc_arraySize];
tvoc_sensor_scan(tvoc_array, tvoc_arraySize);
//tvoc_array is filled as [T0, T1, T2, T3, G0, G1, G2, G3, P0, P1, P2, P3]
String pixhawkDataLine = String(tvoc_array[0]) + "," + String(tvoc_array[1]) + "," + String(tvoc_array[2]) + "," + String(tvoc_array[3]);
Serial.println(pixhawkDataLine);
pixhawkSerial.println(pixhawkDataLine);
}
Lua Script:
--[[
This script recieves ASCII bytes through UART Serial from an ESP32/Arduino.
It reads until the newline ('/n', in ASCII code 10, included at the end of Arduino's Serial.println("") transmission.
The ESP is sending a psuedo CSV line, with multiple sensor float values sent as a string, aka "123.45,678.910,11.0,12.0"
In the Lua script, bytes are read into a buffer, that is unpacked into a string.
The string is then split at the commas and the values are put into a table as lua number types
Finally the gcs:send_named_float() command is used to transmit the data to Mission planner.
This was all referencing the serial_dump.lua file in the ardupilit AP_Scripting library:
https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Scripting/examples/Serial_Dump.lua
--]]
---@diagnostic disable: param-type-mismatch
---@diagnostic disable: need-check-nil
---@diagnostic disable: cast-local-type
local baud_rate = 115200
-- find the serial first (0) scripting serial port instance
-- SERIALx_PROTOCOL 28
local port = assert(serial:find_serial(0), "Could not find Scripting Serial Port")
-- begin the serial port
port:begin(baud_rate)
port:set_flow_control(0)
function update() -- this is the loop which periodically runs
local n_bytes = port:available()
local dataLineCompleteFlag = 0
while (n_bytes > 0) and (dataLineCompleteFlag == 0) do
-- only read a max of 515 bytes in a go
-- this limits memory consumption
local buffer = {} -- table to buffer data
local bytes_target = n_bytes - math.min(n_bytes, 128)
while n_bytes > bytes_target do
--values are read in in ASCII decimal, i.e. 97=a
local value = port:read()
table.insert(buffer, value)
n_bytes = n_bytes - 1
-- the ASCII decimal value for endl is 10, if the endline is detected
-- we break out of the while loops and set the complete flag to true
if value == 10 then
dataLineCompleteFlag = 1
break
end
end
-- first take the decimal valued buffer and unpack into a single string
dataString = string.char(table.unpack(buffer))
-- gcs:send_text(6, dataString)
-- split that string at the commas, convert the values to numbers, and
-- fill them into a new table
numericalTable = {}
for entry in string.gmatch(dataString, '([^,]+)') do
-- print(tonumber(entry))
table.insert(numericalTable, tonumber(entry))
-- -- give named values to all the numbers
-- T1, T2, T3, T4 = table.unpack(numericalTable)
end
end
-- check to see if numericalTable has values, if the initial lua loop
-- runs out of sync with the incoming serial data it'll be empty
if next(numericalTable) ~= nil then
-- send those values to the GCS as a float
-- they will appear as MAV_foo in the "Quick" tab
gcs:send_named_float('TEMP1', numericalTable[1])
gcs:send_named_float('TEMP2', numericalTable[2])
gcs:send_named_float('TEMP3', numericalTable[3])
gcs:send_named_float('TEMP4', numericalTable[4])
end
return update, 100 -- reschedules the loop
end
return update() -- run immediately before starting to reschedule