Adding functionality

I need to add some functionality to the GrovePi I have:

  • run hundreds of A/D samples per second and do an exponential average on it
  • do the same thing for the ultrasonic sensor without converting it to centimeters first

or at least

  • send the ultrasonic range as 0.1mm instead of 10mm resolution

so I want to expand the software that is running on the GrovePI board.

The source of the firmware is easy to understand and I’m pretty confident I can do what I want with it. However, I have some questions:

  1. are there any specific instructions on how to construct a new .hex?

  2. are there any risks of bricking it?

  3. are there any documentation on the interface? It looks like it’s an I2C-to-serial hardware, which is rather confusing - I can work with that without knowing exactly what it does, but some information would be nice.

My plan is to add “subscriptions” so that the GrovePI hardware will always sample a few things a number of times every second, and add commands to read out those already sampled and averaged values.

Of course I will share this back if it gets pretty enough.

thanks
/Mirar

Hey Mirar,
There are no specific instructions as such for developing for GrovePi, but it usually easier to use the Raspberry Pi itself for development as well as uploading the code to the GrovePi. The discussion here might be interesting if you want to know how other folks modify the firmware: http://www.dexterindustries.com/forum/?topic=grovepi-with-arduino-ide/#post-4158.

The GrovePi can easily be recovered if you brick it by running the firmware update. But be a bit cautious with the Fuse setting if you see them anywhere and it might be better not to play with it wrong settings might permanently brick the GrovePi.

Do let us know how it goes for you and do share the progress with us.

-Karan

Ok, that wasn’t so hard. Hardest part was to setup an Arduino build environment.


I did two things now.

  1. The command is overwritten if a new command comes in, so commands might be missed. I made a command FIFO:
--- grove_pi_v1_2_2.ino	2015-06-03 18:08:30.913567455 +0200
+++ grovepi_cmd_queue.ino	2015-06-03 18:09:50.668564799 +0200
@@ -2,7 +2,7 @@
 #include "MMA7660.h"
 #include "DS1307.h"
 #include "DHT.h"
-#include "Grove_LED_bar.h"
+#include "Grove_LED_Bar.h"
 #include "TM1637.h"
 #include "ChainableLED.h"
 MMA7660 acc;
@@ -14,15 +14,16 @@
 
 #define SLAVE_ADDRESS 0x04
 
-int cmd[5];
-int index=0;
 int flag=0;
 int i;
-byte val=0,b[9],float_array[4];
+byte val=0,b[32],float_array[4];
 int aRead=0;
 byte accFlag=0,clkFlag=0;
 int8_t accv[3];
 byte rgb[] = { 0, 0, 0 };
+
+volatile int last_cmd=0;
+
 void setup()
 {
     Serial.begin(9600);         // start serial for output
@@ -33,14 +34,36 @@
 
     Serial.println("Ready!");
 }
-int pin;
-int j;
+
+// command FIFO
+#define QCMDS 32
+volatile byte qcmd[QCMDS][4];
+volatile byte cmdw=0,cmdr=0;
+
+byte debug[32],debugw;
+
+void execute_command(byte *cmd);
+
 void loop()
 {
-  long dur,RangeCm;
-  if(index==4 && flag==0)
+  while (cmdw!=cmdr)
   {
-    flag=1;
+     byte cmd[4];
+     for (int i=0; i<4; i++) cmd[i]=qcmd[cmdr][i];
+
+     execute_command(cmd);
+     cmdr++;
+     if (cmdr==QCMDS) cmdr=0;
+  }
+}
+
+int pin;
+int j;
+
+void execute_command(byte *cmd)
+{   
+  long dur,RangeCm;
+
     //Digital Read
     if(cmd[0]==1)
       val=digitalRead(cmd[1]);
@@ -83,12 +106,13 @@
       //Serial.println(b[1]);
       //Serial.println(b[2]);
     }
+
     //Firmware version
     if(cmd[0]==8)
     {
       b[1] = 1;
       b[2] = 2;
-      b[3] = 2;
+      b[3] = 3;
     }
     //Accelerometer x,y,z, read
     if(cmd[0]==20)
@@ -482,32 +506,47 @@
     }
 
     // end Grove Chainable RGB LED
-  }
+
+    last_cmd=cmd[0];
+}
+
+void got_command(byte *cmd)
+{
+   for (int i=0; i<4; i++)
+      qcmd[cmdw][i]=cmd[i];
+
+   cmdw++;
+   if (cmdw==QCMDS) cmdw=0;
 }
 
 void receiveData(int byteCount)
 {
+   static byte cmd[4];
+   static int index=0;
+
     while(Wire.available())
     {
-      if(Wire.available()==4)
-      {
-        flag=0;
-        index=0;
-      }
-        cmd[index++] = Wire.read();
+       if (Wire.available()>=4) index=0; // ???!?
+       cmd[index++]=Wire.read();
+
+       if (index==4)
+       {
+          got_command(cmd);
+          index=0;
+       }
     }
 }
 
 // callback for sending data
 void sendData()
 {
-  if(cmd[0] == 1)
+  if(last_cmd == 1)
     Wire.write(val);
-  if(cmd[0] == 3 || cmd[0] == 7 || cmd[0] == 56)
+  else if(last_cmd == 3 || last_cmd == 7 || last_cmd == 56)
     Wire.write(b, 3);
-  if(cmd[0] == 8 || cmd[0] == 20)
+  else if(last_cmd == 8 || last_cmd == 20)
     Wire.write(b, 4);
-  if(cmd[0] == 30 || cmd[0] == 40)
+  else if(last_cmd == 30 || last_cmd == 40)
     Wire.write(b, 9);
 }

A patch like that is more or less needed. I’ve had missed commands, but now I know why… The command handling is still not good, but I don’t understand what the I2C<->“Wire” converter is doing. If I knew, I’d write something better, so if you have any information…

…continued…

The second thing I wrote was “subscriptions”. I wanted the GrovePi hardware to read out analog (and digital) ports and do averaging on the GrovePi processors, so that the RPi can read out already finished values.

Right now I made two types, one for analog read and one for the ultrasonic sensor (pulse length read).

The Python API part of the subscription looks like this:

def setupSubscription(n, typ, pin, flag):
        write_i2c_block(address, [100+n] + [typ,pin,flag])
        return 1

def readSubscription(start=0,n=8):
        write_i2c_block(address, [116] + [start,n,0]);
        time.sleep(.05)
        bus.read_byte(address)
        number = bus.read_i2c_block_data(address, 1)
        v=[ number[i*2:(i+1)*2] for i in range(n) ]
        res=[]
        for x in v: res.append((x[0]&lt;&lt;8)+x[1])
        return res

Each subscription has a type, a pin, and flags. You can use subscription number 0 to and including 15.

When reading subscriptions they can be read grouped, so you can read out several at once (or even all).

In the application code they are used like this:

File Edit Options Buffers Tools Python Help                                     
import time
import grovepi

ultrasonic_ranger = 7
infrared_sensor = 1
grovepi.pinMode(infrared_sensor,&quot;INPUT&quot;)

grovepi.setupSubscription(1,3,infrared_sensor,0x10|0x08|0x07);
grovepi.setupSubscription(0,8,ultrasonic_ranger,0x10|0x08|0x05);

while True:
    try:
        time.sleep(.1)
        numbers=grovepi.readSubscription(0,2)
        print &quot;ultrasonic &quot; + repr(numbers[0]/(16*29.0*2))+&quot; cm&quot;
        print &quot;infrared &quot; + repr(numbers[1]*(5.0/(1024*16.0)))+&quot; V&quot;;

    except IOError:
        print &quot;Error&quot;

The flags here means:

0x10 : send number * 16 for higher precision
0x08 : run median filter to cancel out single bad readings
0x07 : run exponential average filter on the reading, “128” length (2<<7)
0x05 : run exponential average filter on the reading, “32” length (2<<5)

The ultrasonic reads with a pace of 10Hz. I found this made it non-glitchy.
The analog port reads with a pace of 100Hz.

With this I get a precision and noise level of the ultrasonic of ±0.5mm (it walks up and down in that interval; I’m unsure why).

Here is the patch (also in the zip above):

--- grovepi_cmd_queue.ino	2015-06-03 18:09:50.668564799 +0200
+++ grovepi_subscriptions.ino	2015-06-03 18:08:10.301826579 +0200
@@ -24,6 +24,152 @@
 
 volatile int last_cmd=0;
 
+
+#define SUBSCRIPTIONS 16
+// subscriptions:   number: result without flags:
+// analog read          3   analog read (1023 = Vref)
+// ultrasonic           7   return time (us) (n/29/2 for cm)
+
+// flags:
+// bit 0..2 (1..7)   exponential average of 2^n = 2..128 samples
+// bit 3 (0x08)      median filter of 3 (n = median of n_t-2, n_t-1, n)
+// bit 4 (0x10)      send result as x16 for better precision
+// internal:
+// bit 8 (0x100)     median filter initialized
+// bit 9 (0x200)     average filter initialized
+
+struct subscription_s
+{
+   byte type;
+   byte pin;
+   byte median_w;
+   uint16_t flags;
+   uint16_t result;
+   float average;
+   float median[3];
+   unsigned long last; // pacing
+   unsigned long pace; // pacing
+} subscription[SUBSCRIPTIONS];
+
+static float exponential_average(float old, float y, int bits)
+{
+   float mul = 1.0 / (1&lt;&lt;bits);
+   return old*(1-mul) + y*mul;
+}
+
+void setup_subscription(int n,int type,int pin,int flags)
+{
+   subscription[n].type=type;
+   subscription[n].pin=pin;
+   subscription[n].flags=flags; // clears median/average init flag
+   subscription[n].median_w=0;
+   subscription[n].last=micros();
+   subscription[n].pace=0;
+   subscription[n].result=0xfffe;
+   subscription[n].average=0.0;
+}
+
+static long ultrasonic_read(int pin,int max)
+{
+   pinMode(pin, OUTPUT);
+   digitalWrite(pin, LOW);
+   delayMicroseconds(2);
+   digitalWrite(pin, HIGH);
+   delayMicroseconds(5);
+   digitalWrite(pin,LOW);
+   pinMode(pin,INPUT);
+   return pulseIn(pin,HIGH,0xffff);
+}
+
+void execute_subscription(struct subscription_s *sub)
+{
+   float value=0.0;
+   
+   unsigned long now=micros();
+   if (sub-&gt;last &amp;&amp; now-sub-&gt;last&lt;sub-&gt;pace) return;
+   sub-&gt;last=now;
+
+   switch (sub-&gt;type)
+   {
+      case 3: // analog read
+         value = analogRead(sub-&gt;pin);
+         sub-&gt;pace = 10000; // 100Hz
+         break;
+      case 7: // ultrasonic read, long distance (~11 meters)
+         value = ultrasonic_read(sub-&gt;pin,0xffff);
+         sub-&gt;pace = 500000; // 2Hz
+         break;
+      case 8: // ultrasonic read, short distance (~1 meter)
+         value = ultrasonic_read(sub-&gt;pin,1000);
+         sub-&gt;pace = 100000; // 10Hz
+         break;
+
+      case 0:
+      default: // do nothing
+         return;
+   }
+
+   // value=500;
+
+   // value=sub-&gt;flags;
+   // value+=(sub-&gt;flags*256);
+
+#if 1
+   // median
+   if ( (sub-&gt;flags &amp; 0x08) )
+   {
+      sub-&gt;median[sub-&gt;median_w++]=value;
+      if (sub-&gt;median_w&gt;=3) 
+      { 
+         sub-&gt;median_w=0; 
+         sub-&gt;flags|=0x100;
+      }
+      // leave value if we&#039;re not initialized
+      if (sub-&gt;flags&amp;0x100)
+      {
+         // sort the three values, pick the middle
+         float a=sub-&gt;median[0];
+         float b=sub-&gt;median[1];
+         float c=sub-&gt;median[2];
+         float t;
+         if (b&gt;a) t=a,a=b,b=t; 
+         if (c&gt;b) t=b,b=c,c=t; 
+         if (b&gt;a) t=a,a=b,b=t; 
+         value=b;
+      }
+   }
+
+   // average
+   if ( (sub-&gt;flags &amp; 0x07) )
+   {
+      if (!(sub-&gt;flags &amp; 0x200)) // initialize
+      {
+         sub-&gt;average=value;
+         sub-&gt;flags|=0x200;
+      }
+      value = sub-&gt;average =
+         exponential_average(sub-&gt;average,value,sub-&gt;flags&amp;0x07);
+   }
+   
+   if ( (sub-&gt;flags &amp; 0x10) )
+      value*=16;
+#endif
+
+   value=round(value);
+
+   if (value&lt;0) value=0;
+   else if (value&gt;65535) value=65535;
+
+   sub-&gt;result=(uint16_t)value;
+}
+
+
+void execute_subscriptions()
+{
+   for (int i=0; i&lt;SUBSCRIPTIONS; i++)
+      execute_subscription(subscription+i);
+}
+
 void setup()
 {
     Serial.begin(9600);         // start serial for output
@@ -46,6 +192,8 @@
 
 void loop()
 {
+  execute_subscriptions();
+
   while (cmdw!=cmdr)
   {
      byte cmd[4];
@@ -507,6 +655,30 @@
 
     // end Grove Chainable RGB LED
 
+    // ----------------
+    // subscriptions
+
+    if (cmd[0]&gt;=100 &amp;&amp; cmd[0]&lt;=115) // subscribe n-100, type, pin, flags
+    {
+       setup_subscription(cmd[0]-100,cmd[1],cmd[2],cmd[3]);
+    }
+
+    if (cmd[0]&gt;=116) // read subscriptions cmd[1] to cmd[1]+cmd[2]
+    {
+       int base = cmd[1];
+       int nread = cmd[2];
+       if (base&lt;0 || base&gt;3) base=0;
+       if (SUBSCRIPTIONS-base&lt;nread) nread=SUBSCRIPTIONS-base;
+       if (nread&gt;16) nread=16; // 32 bytes
+
+       for (int i=0; i&lt;nread; i++)
+       {
+          struct subscription_s *sub = subscription+i+base;
+          b[i*2 + 0] = (sub-&gt;result&gt;&gt;8)&amp;0xff;
+          b[i*2 + 1] = (sub-&gt;result&gt;&gt;0)&amp;0xff;
+       }
+    }
+
     last_cmd=cmd[0];
 }
 
@@ -548,5 +720,7 @@
     Wire.write(b, 4);
   else if(last_cmd == 30 || last_cmd == 40)
     Wire.write(b, 9);
+  else if (last_cmd&gt;=116 &amp;&amp; last_cmd&lt;=119)
+    Wire.write(b, 32);
 }
 

I’d be happy if these ideas are added to the normal distributions. I’m sure others would find this useful. There should be nothing that will cause problems or backwards compatibility issues in these patches.

Hey,
Thanks a lot for all the detailed experiments and they really look interesting.

The I2C queue really sounds like a good idea, but a big bottlenck here would be the time it takes for data transfers. Right now it takes a couple of milliseconds to do that and there are some delays in the python library itself. Also, wire is nothing but the name of the I2C library for Arduino. You can find more information about it here.

The averaging is code is really exciting. I am pretty amazed that you got that kind of precision with the ultrasonic sensor.

It is a bit hard to read the source and understand them here. Can you send a link to Github where we can have a look at the source and decide how to get this in our firmware.

Thanks,
Karan

The problem is that the bottleneck isn’t the I2C. A lot of the operations have ‘sleep’ in them, which means that three commands following each other might miss the middle one. The queue fixes that, if it doesn’t get full. I had the problem with a digital output that didn’t always toggle and I previously solved the problem with sleep and some spamming of the command, not pretty.

I’ll investigate the “Wire” for the arduino. Maybe I can come up with something even more reliable.

I can upload to github (I’m not used to working with github), but there’s also the attached zip above with both patches and finished .ino files. Tell me if you still need me to upload to github.

Exponential averaging and median filter is what I usually have to do on the sensor I work with for work (I’m an embedded programmer for a living - I just don’t like soldering and designing boards). Especially the median filter is a nice bit of signal magic.

Note that the accuracy of the ultrasonic might still be very off - on a longer distance it can be a few centimeters off depending on the precision of the clock of the GrovePi, room temperature, pressure, humidity and what the sensor is bounced off on, all this could be measured separatedly and/or calibrated for. But stability and noise is around ±0.5mm.

(I also ordered some more I2C sensors to play with, 3DOF and barometric pressure. Will add those and send patches when I get them.)

Hey,
Thanks a lot for digging this deep into the code to improve the functionality. Do let us know if you find a good alternative to Wire. It would be a lot more easier to use something that is better.

Got the source codes. We’ll try to integrate things from it when we are going to push the next firmware and software update.

Thanks again,
Karan