From a97fc7f0874a3a35419f5af53cc11f7840a91530 Mon Sep 17 00:00:00 2001
From: Karol Hennessy <revkarol@gmail.com>
Date: Sat, 11 Apr 2020 15:07:59 +0200
Subject: [PATCH] update to lib for breathing with comms
---
.../breathing_with_comms/src/MemoryFree.cpp | 52 +++++++++++++
arduino/breathing_with_comms/src/MemoryFree.h | 18 +++++
arduino/breathing_with_comms/src/main.cpp | 24 ++++--
.../breathing_with_comms/src/test_hw_loop.cpp | 4 +-
.../common/lib/commsControl/commsConstants.h | 74 ++++++++++---------
.../common/lib/commsControl/commsControl.cpp | 37 +++++-----
.../common/lib/commsControl/commsControl.h | 6 +-
.../common/lib/commsControl/commsFormat.cpp | 1 -
arduino/common/lib/commsControl/commsFormat.h | 14 ++++
raspberry-dataserver/commsDebug.py | 15 +++-
10 files changed, 176 insertions(+), 69 deletions(-)
create mode 100644 arduino/breathing_with_comms/src/MemoryFree.cpp
create mode 100644 arduino/breathing_with_comms/src/MemoryFree.h
diff --git a/arduino/breathing_with_comms/src/MemoryFree.cpp b/arduino/breathing_with_comms/src/MemoryFree.cpp
new file mode 100644
index 00000000..bf571845
--- /dev/null
+++ b/arduino/breathing_with_comms/src/MemoryFree.cpp
@@ -0,0 +1,52 @@
+#if (ARDUINO >= 100)
+#include <Arduino.h>
+#else
+#include <WProgram.h>
+#endif
+
+extern unsigned int __heap_start;
+extern void *__brkval;
+
+/*
+ * The free list structure as maintained by the
+ * avr-libc memory allocation routines.
+ */
+struct __freelist
+{
+ size_t sz;
+ struct __freelist *nx;
+};
+
+/* The head of the free list structure */
+extern struct __freelist *__flp;
+
+#include "MemoryFree.h"
+
+/* Calculates the size of the free list */
+int freeListSize()
+{
+ struct __freelist* current;
+ int total = 0;
+ for (current = __flp; current; current = current->nx)
+ {
+ total += 2; /* Add two bytes for the memory block's header */
+ total += (int) current->sz;
+ }
+
+ return total;
+}
+
+int freeMemory()
+{
+ int free_memory;
+ if ((int)__brkval == 0)
+ {
+ free_memory = ((int)&free_memory) - ((int)&__heap_start);
+ }
+ else
+ {
+ free_memory = ((int)&free_memory) - ((int)__brkval);
+ free_memory += freeListSize();
+ }
+ return free_memory;
+}
diff --git a/arduino/breathing_with_comms/src/MemoryFree.h b/arduino/breathing_with_comms/src/MemoryFree.h
new file mode 100644
index 00000000..f9d7cad7
--- /dev/null
+++ b/arduino/breathing_with_comms/src/MemoryFree.h
@@ -0,0 +1,18 @@
+// MemoryFree library based on code posted here:
+// http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1213583720/15
+// Extended by Matthew Murdoch to include walking of the free list.
+
+#ifndef MEMORY_FREE_H
+#define MEMORY_FREE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int freeMemory();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/arduino/breathing_with_comms/src/main.cpp b/arduino/breathing_with_comms/src/main.cpp
index 621a77b4..2d2de750 100644
--- a/arduino/breathing_with_comms/src/main.cpp
+++ b/arduino/breathing_with_comms/src/main.cpp
@@ -1,9 +1,16 @@
#include <Arduino.h>
+#include <MemoryFree.h>
#include "commsControl.h"
#include "test_hw_loop.h"
#include "common.h"
int ventilation_mode = HEV_MODE_PS;
+
+const uint16_t report_freq = 1 ; // in Hz
+const uint16_t update_freq = 100 ; // in Hz
+
+uint16_t report_cnt = 0;
+
float working_pressure = 1; //?
float inspiratory_minute_volume = 6000; // ml/min
float respiratory_rate = 15; // 10-40 +-1 ;aka breaths_per_min
@@ -106,23 +113,28 @@ void loop()
data.readback_valve_inhale = vinhale;
data.readback_valve_exhale = vexhale;
data.readback_valve_purge = vpurge;
+ data.pressure_o2_supply = freeMemory() & 0xFFFF;
+ data.pressure_o2_regulated = freeMemory() >> 16;
// TODO ; add to dataFormat
// data.readback_valve_atmosphere = vpurge;
- //breath_cycle();
FSM_assignment();
FSM_breath_cycle();
- plSend.setType(payloadType::payloadData);
- plSend.setData(&data);
- comms.writePayload(&plSend);
+ report_cnt++;
+ if(report_cnt % (update_freq/report_freq) == 0)
+ {
+ plSend.setType(payloadType::payloadData);
+ plSend.setData(&data);
+ comms.writePayload(plSend);
+ }
// per cycle sender
comms.sender();
// per cycle receiver
comms.receiver();
uint8_t cmdCode = 0;
- if(comms.readPayload(&plReceive)){
+ if(comms.readPayload(plReceive)){
if (plReceive.getType() == payloadType::payloadCmd) {
cmdCode = (plReceive.getCmd()->cmdCode);
plReceive.setType(payloadType::payloadUnset);
@@ -138,5 +150,5 @@ void loop()
default:
break;
}
- delay(10);
+ delay(1000/update_freq);
}
diff --git a/arduino/breathing_with_comms/src/test_hw_loop.cpp b/arduino/breathing_with_comms/src/test_hw_loop.cpp
index 2e84c744..14597919 100644
--- a/arduino/breathing_with_comms/src/test_hw_loop.cpp
+++ b/arduino/breathing_with_comms/src/test_hw_loop.cpp
@@ -26,7 +26,7 @@ void FSM_assignment( ) {
switch (bs_state)
{
case BS_IDLE:
- if (running == false) {
+ if (running == true) {
next_state = BS_BUFF_PREFILL;
FSM_time = millis();
// Serial.println("Exit IDLE") ;
@@ -99,7 +99,7 @@ void FSM_breath_cycle()
switch (bs_state) {
case BS_IDLE:
- if (running == false) {
+ if (running == true) {
FSM_time = millis();
} else {
timeout = 1000;
diff --git a/arduino/common/lib/commsControl/commsConstants.h b/arduino/common/lib/commsControl/commsConstants.h
index 4a633c95..072b4183 100644
--- a/arduino/common/lib/commsControl/commsConstants.h
+++ b/arduino/common/lib/commsControl/commsConstants.h
@@ -5,7 +5,7 @@
#define CONST_TIMEOUT_ALARM 5
#define CONST_TIMEOUT_DATA 10
-#define CONST_TIMEOUT_CMD 50
+#define CONST_TIMEOUT_CMD 5
#define CONST_MAX_SIZE_RB_RECEIVING 10
@@ -31,20 +31,14 @@
#define PACKET_DATA 0x40
#define PACKET_SET 0x20 //set vs get ?
-// TODO: make sensible
-#define PRESSURE_PEEP_LOW 0x01
-#define PRESSURE_PEEP_HIGH 0x02
-
#define HEV_FORMAT_VERSION 0xA0
-enum command_codes {CMD_START = 1,
+enum command_codes {CMD_START = 1,
CMD_STOP = 2};
// struct for all data sent
-
struct dataFormat {
- uint8_t version = HEV_FORMAT_VERSION; //
- uint8_t size = 27; // in bytes (includes version and size itself)
+ uint8_t version = HEV_FORMAT_VERSION; //
uint8_t fsm_state = 0;
uint16_t pressure_air_supply = 0;
uint16_t pressure_air_regulated = 0;
@@ -60,19 +54,17 @@ struct dataFormat {
uint8_t readback_valve_inhale = 0;
uint8_t readback_valve_exhale = 0;
uint8_t readback_valve_purge = 0;
- uint8_t readback_mode = 0;
+ uint8_t readback_mode = 0;
};
struct cmdFormat {
- uint8_t version = HEV_FORMAT_VERSION; //
- uint8_t size = 7; // in bytes (includes version and size itself)
+ uint8_t version = HEV_FORMAT_VERSION; //
uint8_t cmdCode = 0;
uint32_t param = 0;
};
struct alarmFormat{
- uint8_t version = HEV_FORMAT_VERSION; //
- uint8_t size = 7; // in bytes (includes version and size itself)
+ uint8_t version = HEV_FORMAT_VERSION; //
uint8_t alarmCode = 0;
uint32_t param = 0;
// do we do the same as dataFormat and put all alarms in one message?
@@ -91,28 +83,44 @@ enum payloadType {
// information is set as information in the protocol
class payload {
public:
- payload(payloadType type = payloadType::payloadUnset) {type_ = type; data_ = nullptr; cmd_ = nullptr; alarm_ = nullptr; }
- ~payload() {unsetData(); unsetAlarm(); unsetCmd();}
+ payload(payloadType type = payloadType::payloadUnset) {type_ = type; } //data_ = nullptr; cmd_ = nullptr; alarm_ = nullptr; }
+ payload(const payload &other) {
+// unsetAll();
+ type_ = other.type_;
+ /*if ( other. data_ != nullptr) { data_ = new dataFormat;*/ memcpy(& data_, & other.data_, sizeof( dataFormat));// } else { data_ = nullptr; }
+ /*if ( other. cmd_ != nullptr) { cmd_ = new cmdFormat;*/ memcpy(& cmd_, & other.cmd_, sizeof( cmdFormat));// } else { cmd_ = nullptr; }
+ /*if ( other.alarm_ != nullptr) { alarm_ = new alarmFormat;*/ memcpy(&alarm_, &other.alarm_, sizeof(alarmFormat));// } else {alarm_ = nullptr; }
+ }
+ payload& operator=(const payload& other) {
+// unsetAll();
+ type_ = other.type_;
+ /*if ( other. data_ != nullptr) { data_ = new dataFormat;*/ memcpy(& data_, & other.data_, sizeof( dataFormat));// } else { data_ = nullptr; }
+ /*if ( other. cmd_ != nullptr) { cmd_ = new cmdFormat;*/ memcpy(& cmd_, & other.cmd_, sizeof( cmdFormat));// } else { cmd_ = nullptr; }
+ /*if ( other.alarm_ != nullptr) { alarm_ = new alarmFormat;*/ memcpy(&alarm_, &other.alarm_, sizeof(alarmFormat));// } else {alarm_ = nullptr; }
+
+ return *this;
+ }
+
+ ~payload() {;}//unsetData(); unsetAlarm(); unsetCmd();}
void setType(payloadType type) { type_ = type; }
payloadType getType() {return type_; }
// requires argument as new struct
- void setData (dataFormat *data) { unsetAll(); data_ = new dataFormat; memcpy( data_, data, data->size); }
- void setCmd (cmdFormat *cmd) { unsetAll(); cmd_ = new cmdFormat; memcpy( cmd_, cmd, cmd->size); }
- void setAlarm(alarmFormat *alarm) { unsetAll(); alarm_ = new alarmFormat; memcpy(alarm_, alarm, alarm->size); }
+ void setData (dataFormat *data) { /*unsetAll(); type_ = payloadType::payloadData; data_ = new dataFormat( *data); }*/ memcpy(& data_, data, sizeof( dataFormat)); }
+ void setCmd (cmdFormat *cmd) { /*unsetAll(); type_ = payloadType::payloadCmd; cmd_ = new cmdFormat( *cmd); }*/ memcpy(& cmd_, cmd, sizeof( cmdFormat)); }
+ void setAlarm(alarmFormat *alarm) { /*unsetAll(); type_ = payloadType::payloadAlarm; alarm_ = new alarmFormat( *alarm); }*/ memcpy(&alarm_, alarm, sizeof(alarmFormat)); }
- dataFormat *getData () {return data_; }
- cmdFormat *getCmd () {return cmd_; }
- alarmFormat *getAlarm() {return alarm_; }
+ dataFormat *getData () {return &data_; }
+ cmdFormat *getCmd () {return &cmd_; }
+ alarmFormat *getAlarm() {return &alarm_; }
- void unsetAll() { unsetData(); unsetAlarm(); unsetCmd(); }
- void unsetData() { if ( data_ != nullptr) { delete data_; data_ = nullptr; } }
- void unsetCmd() { if ( cmd_ != nullptr) { delete cmd_; cmd_ = nullptr; } }
- void unsetAlarm() { if (alarm_ != nullptr) { delete alarm_; alarm_ = nullptr; } }
+// void unsetAll() { unsetData(); unsetAlarm(); unsetCmd(); type_ = payloadType::payloadUnset; }
+// void unsetData() { if ( data_ != nullptr) { delete data_; data_ = nullptr; } }
+// void unsetCmd() { if ( cmd_ != nullptr) { delete cmd_; cmd_ = nullptr; } }
+// void unsetAlarm() { if (alarm_ != nullptr) { delete alarm_; alarm_ = nullptr; } }
void setPayload(payloadType type, void* dt) {
- setType(type);
switch (type) {
case payloadType::payloadData:
setData(reinterpret_cast<dataFormat*>(dt));
@@ -145,11 +153,11 @@ public:
uint8_t getInformationSize() {
switch (type_) {
case payloadType::payloadData:
- return getData()->size;
+ return static_cast<uint8_t>(sizeof( dataFormat));
case payloadType::payloadCmd:
- return getCmd()->size;
+ return static_cast<uint8_t>(sizeof( cmdFormat));
case payloadType::payloadAlarm:
- return getAlarm()->size;
+ return static_cast<uint8_t>(sizeof(alarmFormat));
default:
return 0;
}
@@ -158,9 +166,9 @@ public:
private:
payloadType type_;
- dataFormat *data_;
- cmdFormat *cmd_;
- alarmFormat *alarm_;
+ dataFormat data_;
+ cmdFormat cmd_;
+ alarmFormat alarm_;
};
#endif
diff --git a/arduino/common/lib/commsControl/commsControl.cpp b/arduino/common/lib/commsControl/commsControl.cpp
index 86c7570d..62a89385 100644
--- a/arduino/common/lib/commsControl/commsControl.cpp
+++ b/arduino/common/lib/commsControl/commsControl.cpp
@@ -19,7 +19,7 @@ commsControl::commsControl(uint32_t baudrate) {
queueData_ = new RingBuf<commsFormat *, CONST_MAX_SIZE_RB_SENDING>();
queueCmd_ = new RingBuf<commsFormat *, CONST_MAX_SIZE_RB_SENDING>();
- queueReceived_ = new RingBuf<payload *, CONST_MAX_SIZE_RB_RECEIVING>();
+ queueReceived_ = new RingBuf<payload, CONST_MAX_SIZE_RB_RECEIVING>();
commsTmp_ = commsFormat(CONST_MAX_SIZE_PACKET - CONST_MIN_SIZE_PACKET );
@@ -135,45 +135,44 @@ void commsControl::receiver() {
}
}
-bool commsControl::writePayload(payload *pl) {
+bool commsControl::writePayload(payload &pl) {
commsFormat* tmpComms;
+ payloadType type = pl.getType();
// switch on different received payload types
// TODO simplify the static functions
- switch(pl->getType()) {
+ switch(type) {
case payloadAlarm:
- tmpComms = commsFormat::generateALARM(pl);
+ tmpComms = commsFormat::generateALARM(&pl);
break;
case payloadData:
- tmpComms = commsFormat::generateDATA(pl);
+ tmpComms = commsFormat::generateDATA(&pl);
break;
case payloadCmd:
- tmpComms = commsFormat::generateCMD(pl);
+ tmpComms = commsFormat::generateCMD(&pl);
break;
default:
return false;
}
+ RingBuf<commsFormat *, CONST_MAX_SIZE_RB_SENDING> *tmpQueue = getQueue(&type);
// add new entry to the queue
- if (queueData_->isFull()) {
+ if (tmpQueue->isFull()) {
commsFormat *tmpCommsRm;
- if (queueData_->pop(tmpCommsRm)) {
+ if (tmpQueue->pop(tmpCommsRm)) {
delete tmpCommsRm;
}
}
- if (queueData_->push(tmpComms) ) {
+ if (tmpQueue->push(tmpComms) ) {
return true;
}
return false;
}
-bool commsControl::readPayload( payload* pl) {
+bool commsControl::readPayload( payload &pl) {
if ( !queueReceived_->isEmpty()) {
- payload *tmpPl;
- if (queueReceived_->pop(tmpPl)) {
- memcpy(pl, tmpPl, sizeof(payload));
- delete tmpPl;
+ if (queueReceived_->pop(pl)) {
return true;
}
}
@@ -259,9 +258,8 @@ void commsControl::resendPacket(RingBuf<commsFormat *, CONST_MAX_SIZE_RB_SENDING
// receiving anything of commsFormat
-// TODO remove address and use commsTmp only
bool commsControl::receivePacket(payloadType *type) {
- payload *tmpPl = new payload(*type);
+ payload tmpPl = payload(*type);
void *tmpInformation = nullptr;
switch (*type) {
@@ -282,13 +280,12 @@ bool commsControl::receivePacket(payloadType *type) {
return false;
}
memcpy(tmpInformation, commsTmp_.getInformation(), commsTmp_.getInfoSize());
- tmpPl->setPayload(*type, tmpInformation);
+ tmpPl.setPayload(*type, tmpInformation);
// remove first entry if RB is full
if (queueReceived_->isFull()) {
- payload *tmpDataRm = nullptr;
- if (queueReceived_->pop(tmpDataRm)) {
- delete tmpDataRm;
+ payload tmpPlRm;
+ if (queueReceived_->pop(tmpPlRm)) {
}
}
diff --git a/arduino/common/lib/commsControl/commsControl.h b/arduino/common/lib/commsControl/commsControl.h
index e0417884..789f3605 100644
--- a/arduino/common/lib/commsControl/commsControl.h
+++ b/arduino/common/lib/commsControl/commsControl.h
@@ -19,8 +19,8 @@ public:
void beginSerial();
- bool writePayload(payload *pl);
- bool readPayload (payload *pl);
+ bool writePayload(payload &pl);
+ bool readPayload (payload &pl);
void sender();
void receiver();
@@ -50,7 +50,7 @@ private:
RingBuf<commsFormat *, CONST_MAX_SIZE_RB_SENDING> *queueData_;
RingBuf<commsFormat *, CONST_MAX_SIZE_RB_SENDING> *queueCmd_;
- RingBuf<payload *, CONST_MAX_SIZE_RB_RECEIVING> *queueReceived_;
+ RingBuf<payload, CONST_MAX_SIZE_RB_RECEIVING> *queueReceived_;
commsFormat commsTmp_;
diff --git a/arduino/common/lib/commsControl/commsFormat.cpp b/arduino/common/lib/commsControl/commsFormat.cpp
index affb4200..b4951e59 100644
--- a/arduino/common/lib/commsControl/commsFormat.cpp
+++ b/arduino/common/lib/commsControl/commsFormat.cpp
@@ -73,7 +73,6 @@ void commsFormat::generateCrc(bool assign) {
// assign crc to fcs
if (assign) {
- ;
assignBytes(getFcs(), reinterpret_cast<uint8_t*>(&crc_), 2, false);
}
}
diff --git a/arduino/common/lib/commsControl/commsFormat.h b/arduino/common/lib/commsControl/commsFormat.h
index ddfc6e3a..81b2ca47 100644
--- a/arduino/common/lib/commsControl/commsFormat.h
+++ b/arduino/common/lib/commsControl/commsFormat.h
@@ -14,6 +14,20 @@
class commsFormat {
public:
commsFormat(uint8_t infoSize = 0, uint8_t address = 0x00, uint16_t control = 0x0000);
+ commsFormat(const commsFormat& other) {
+ crc_ = other.crc_;
+ packetSize_ = other.packetSize_;
+ infoSize_ = other.infoSize_;
+ memcpy(data_, other.data_, CONST_MAX_SIZE_PACKET);
+ }
+ commsFormat& operator=(const commsFormat& other) {
+ crc_ = other.crc_;
+ packetSize_ = other.packetSize_;
+ infoSize_ = other.infoSize_;
+ memcpy(data_, other.data_, CONST_MAX_SIZE_PACKET);
+
+ return *this;
+ }
uint8_t* getData() { return data_; }
uint8_t getSize() { return packetSize_; }
diff --git a/raspberry-dataserver/commsDebug.py b/raspberry-dataserver/commsDebug.py
index 47effb6b..09cfaa5c 100755
--- a/raspberry-dataserver/commsDebug.py
+++ b/raspberry-dataserver/commsDebug.py
@@ -1,5 +1,6 @@
#!/usr/bin/env python3
from commsControl import commsControl
+from commsConstants import *
import logging
logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')
import sys
@@ -18,17 +19,23 @@ class Dependant(object):
logging.info(f"payload received: {payload}")
self._llipacket = payload.getDict() # returns a dict
# pop from queue - protects against Dependant going down and not receiving packets
- self._lli.pop_payloadrecv()
+ self._lli.poppayloadrecv_()
dep = Dependant(comms)
-
start = 0x1
stop = 0x2
-#comms.registerData(start)
-# comms.sender()
+cmd = commandFormat()
+cmd.cmdCode = start
+cmd.toByteArray()
+
+comms.writePayload(cmd)
+#comms.sender()
while True:
time.sleep(30)
+ cmd.cmdCode = stop
+ cmd.toByteArray()
+ comms.writePayload(cmd)
#comms.registerData(stop)
pass
--
GitLab