o Implementing getting and setting of values.HEADmaster

1 files changed, 43 insertions, 9 deletions

diff --git a/app.cpp b/app.cpp
index 8163ad8..ce3b15e 100644
--- a/app.cpp
+++ b/app.cpp
@@ -1,5 +1,6 @@
 #include "app.h"
 
+#include <Arduino.h>
 #include <HardwareSerial.h>
 
 // http://bleaklow.com/2010/09/05/progmem_and_gcc_bug_34734.html
@@ -9,8 +10,14 @@
 #define GAUGE_COUNT 4
 
 // See http://redbearlab.com/blendmicro/
-int gauge_pins[GAUGE_COUNT] = {
-  9, 10, 11, 13
+struct gauge {
+  uint8_t pin;
+  uint8_t value;
+} gauges[] = {
+  { 9, 0},
+  {10, 0},
+  {11, 0},
+  {13, 0}
 };
 
 void on_gauge_data(uint8_t *data, uint8_t len) {
@@ -21,21 +28,48 @@ void on_gauge_data(uint8_t *data, uint8_t len) {
 }
 
 void on_gauge_ctrl(uint8_t *data, uint8_t len) {
-  Serial.print(F("on_gauge_data, data[0]="));
+  Serial.print(F("on_gauge_ctrl, data[0]="));
   Serial.print(data[0], HEX);
   Serial.print(F(", data[1]="));
   Serial.println(data[1], HEX);
 
-  uint8_t res[2];
+  struct fsp_req *cmd = (struct fsp_req *) data;
+  struct fsp_res res;
 
-  switch(data[0]) {
-    case FSP_CMD_GAUGE_COUNT:
-      res[0] = FSP_CMD_GAUGE_COUNT;
-      res[1] = GAUGE_COUNT;
-      send_ctrl(res, 2);
+  res.code = cmd->code;
+
+  Serial.print("code=");
+  Serial.println(cmd->code, DEC);
+
+  switch(cmd->code) {
+    case FSP_CMD_GET_GAUGE_COUNT:
+      res.len = sizeof(struct fsp_get_gauge_count_res);
+      res.get_gauge_count.count = GAUGE_COUNT;
+      break;
+    case FSP_CMD_SET_GAUGE:
+      res.len = sizeof(struct fsp_set_gauge_res);
+      if (cmd->set_gauge.gauge < GAUGE_COUNT) {
+        analogWrite(gauges[cmd->set_gauge.gauge].pin, cmd->set_gauge.value);
+        gauges[cmd->set_gauge.gauge].value = cmd->set_gauge.value;
+      }
+
+      break;
+    case FSP_CMD_GET_GAUGE:
+      res.len = sizeof(struct fsp_get_gauge_res);
+      if (cmd->get_gauge.gauge < GAUGE_COUNT) {
+        res.get_gauge.gauge = cmd->get_gauge.gauge;
+        res.get_gauge.value = gauges[cmd->get_gauge.gauge].value;
+      } else {
+        res.code = FSP_CMD_FAIL;
+        res.len = 0;
+      }
       break;
     default:
+      res.code = FSP_CMD_FAIL;
+      res.len = 0;
       break;
   }
+
+  send_ctrl(&res);
 }