File content as of revision 68:0c96bb3d73a7:

#include "TA.h"
#include "Radio.hh"

#include <nrf51.h>
#include <mbed.h>
#include <device.h>
#include <vector> 
#include <RFM69.h>

/* !SR
 * Can't find any boards which use the nrf51822 and have analog output enabled. Currently
 * all analog stuff has been commented out.
 */
 
 #define NEED_CONSOLE_OUTPUT 1 /* Set this if you need //////////////////////DEBUG messages on the console;
                               * it will have an impact on code-size and power consumption. */

#define LOOPBACK_MODE       0  // Loopback mode

#if NEED_CONSOLE_OUTPUT
#define DEBUG(...) { printf(__VA_ARGS__); }
#else
#define DEBUG(...) /* nothing */
#endif /* #if NEED_CONSOLE_OUTPUT */
 

extern unsigned long millis();
extern unsigned long micros();
extern int random(int numberone, int numbertwo);

uint8_t TA::msg_id = 0;  // Message ID which replaces '%' in radio send packet for duplicate message detection.

ByteBuffer TA::send_buffer;
ByteBuffer TA::receive_buffer;

//// These are currently no longer used...
uint8_t TA::node_id;//        1  //network ID used for this unit
uint8_t TA::network_id;//    99  //network ID used for this network
uint8_t TA::gateway_id;//     1  //the ID of the network controller
uint8_t TA::ack_time;     // 50  // # of ms to wait for an ack

//encryption is OPTIONAL
//to enable encryption you will need to:
// - provide a 16-byte encryption KEY (same on all nodes that talk encrypted)
// - to call .Encrypt(KEY) to start encrypting
// - to stop encrypting call .Encrypt(NULL)
uint8_t TA::KEY[] = "ABCDABCDABCDABCD";
uint16_t TA::interPacketDelay;// = 1000; //wait this many ms between sending packets

// Need an instance of the Radio Module
//byte TA::sendSize;//=0;
//char TA::payload[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZ";
//bool TA::requestACK;//=true;

//neopixels_spi TA::neopixels;

bool gotACK = false;  //// If we got an ACK between retrying attempts.

unsigned long millis();

unsigned long micros();

extern RFM69 radio;

/** Macro for min() 
 *
 * @param any
 */
#define min(a,b)                  ((a)<(b)?(a):(b))
/** Macro for max()
 *
 * @param any
 */
#define max(a,b)                  ((a)>(b)?(a):(b))

/** generates a random number between two numbers
 *
 * @param numberone minimum value for random number
 * @param numbertwo maximum value for random number
 */
int random(int numberone, int numbertwo) {
    int random = 0;
    if ((numberone < 0) && (numbertwo < 0)) {
        numberone = numberone * -1;
        numbertwo = numbertwo * -1;
        random = -1 * (rand()%(numberone + numbertwo));
    }
    if ((numbertwo < 0) && (numberone >= 0)) {
        numbertwo = numbertwo * -1;
        random = (rand()%(numberone + numbertwo)) - numbertwo;
    }
    if ((numberone < 0) && (numbertwo >= 0)) {
        numberone = numberone * -1;
        random = (rand()%(numberone + numbertwo)) - numberone;
    } else {
        random = (rand()%(numberone + numbertwo)) - min(numberone, numbertwo);
    }
    return random;
}

// ########## End code taken from audrino library ##########

uint8_t TA::mask;

// outputs
//uint8_t TA::buzzPin;
uint8_t TA::red;
uint8_t TA::green;
uint8_t TA::blue;

neopixel_strip_t m_strip;

uint8_t dig_pin_num =  16;
uint8_t leds_per_strip = 18;
uint8_t led_to_enable = 1;

#if 0
DigitalOut TA::enable_1(p4);
DigitalOut TA::enable_2(p7);
DigitalOut TA::enable_3(p8);
#endif

DigitalOut TA::cap_enable( p1, 0 );
/// DigitalIn  touch_top(p1);
/// DigitalIn  touch(p12);

#define BUZZ_ON   0
#define BUZZ_OFF  1

DigitalOut TA::buzzPin( p20, BUZZ_OFF );

bool    TA::batteryLow    = false;
uint8_t TA::buttonsRising = 0;

#if 1
// inputs
DigitalIn TA::touch_1( p12,PullDown );  //  Test  button (p6 [was] RSVD for lower button--p6 is last analog-in though.)
EdgeDigIn TA::touch_2( p0, PullNone );  //  Top touch sensor.
DigitalIn TA::touch_3( p3, PullNone );  // /Power button (even though not a touch button.)
#endif

//NeoStrip *neo;

TA::TA()
{
    setMask( DEFTOUCHMASK );
    buzzPin = BUZZ_OFF;
    neopixel_init(&m_strip, dig_pin_num, leds_per_strip);
    neopixel_clear(&m_strip); 
}

void TA::post_color(uint32_t rgb)
{
    if (rgb == 0)
    {
          neopixel_clear(&m_strip);  
          return;
    }
    
    for (int i = 0; i <= leds_per_strip; ++i)
    {
        neopixel_set_color_and_show(&m_strip, i, (rgb >> 16) & 0xFF, (rgb >> 8) & 0xFF, rgb & 0xFF);
        //neo->setPixel(i, rgb);
    } 
    
    /// buzzPin = 1;
}

void TA::mask_color(uint32_t rgb)
{
   // enable_1 = 0;
   // enable_2 = 0;
   // enable_3 = 0;
  //neopixels.setRGBStrip1((rgb >> 16) & 0xFF, (rgb >> 8) & 0xFF, rgb & 0xFF);
  
  //enable_1 = (mask & 0x01)?1:0;
  //enable_2 = (mask & 0x02)?1:0;
  //enable_3 = (mask & 0x04)?1:0;

  if( mask & TOUCHLIGHTS )  // Even though we now only use one sensor, we still have DARK ALERT.
    for (int i = 0; i <= leds_per_strip; ++i)
    {
          neopixel_set_color_and_show(&m_strip, i, (rgb >> 16) & 0xFF, (rgb >> 8) & 0xFF, rgb & 0xFF);
    }
  else
    neopixel_clear(&m_strip);
}

void TA::beep(uint16_t ms)
{
    beeping = true;
    beep_start = millis();
    beep_duration = ms;
}

void TA::beep_off(void)
{
    beeping = false;   
}

void TA::powerup(uint8_t sound)
{
    powerup_start = millis();
    beeping = false;
    pulsing = false;
    powering_up1 = false;
    powering_up2 = false;

    if(sound == 1)
    {
        powering_up1 = true;
        powerup_toggle = 300;
    }
    
    if(sound == 2)
    {
        powering_up2 = true;
        powerup_toggle = 20;
    }
}

void TA::pulse(uint16_t on_time, uint16_t period, uint16_t ms, uint32_t rgb)
{
    current_color = rgb;
    pulsing = true;
    pulse_start = millis();
    pulse_period = period;
    pulse_on = on_time; // (on_time < 126 ? 126 : on_time);
    pulse_duration = ms;   
}

void TA::pulse_off(void)
{
    pulsing = false;
}

int TA::get_buffer_size(void)
{
    return send_buffer.getSize();    
}

bool TA::send( Message *m ) //// Restored existing architecture to use buffer.
{
  int s = (int)send_buffer.getCapacity() - (int)send_buffer.getSize();
  //DEBUG( "buffer space: %d\r\n", s );
  if( s < 7 )
  {
    DEBUG( "Warning, radio buffer overflow!" );
    return  false;
  }
  //DEBUG( "queued" );
  send_buffer.put(     (byte)m->command );
  send_buffer.putLong( (long)m->value   );
  send_buffer.put(     (byte)m->cone    );
  send_buffer.put(     '%'              );
 /*
  DEBUG( "Sent: \r\n" );
  DEBUG( "%d\r\n", m->cone );
  DEBUG( "%c\r\n", (char)m->command );
  DEBUG( "%d\r\n", m->value );
  DEBUG( "%%\r\n" );
  DEBUG( "\r\n" );

  DEBUG( "Raw send:\r\n" );
  DBITS( (byte)m->cone,        8 );  DEBUG( "\r\n" );
  DBITS( (byte)m->command,     8 );  DEBUG( "\r\n" );
  DBITS( (byte)(m->value>>24), 8 );  DEBUG( "\r\n" );
  DBITS( (byte)(m->value>>16), 8 );  DEBUG( "\r\n" );
  DBITS( (byte)(m->value>>8),  8 );  DEBUG( "\r\n" );
  DBITS( (byte)m->value,       8 );  DEBUG( "\r\n" );
  DBITS( '%',                  8 );  DEBUG( "\r\n" );
 */
 /*
  DBITS( receive_buffer.peek(0), 8 );  DEBUG( "\r\n" ); //<-cone?
  DBITS( send_buffer.peek(1),    8 );  DEBUG( "\r\n" ); //<-command
  DBITS( send_buffer.peek(2),    8 );  DEBUG( "\r\n" ); // value byte4
  DBITS( send_buffer.peek(3),    8 );  DEBUG( "\r\n" ); // value byte2
  DBITS( send_buffer.peek(4),    8 );  DEBUG( "\r\n" ); // value byte1
  DBITS( send_buffer.peek(5),    8 );  DEBUG( "\r\n" ); // value byte0 (lsb)
  DBITS( send_buffer.peek(6),    8 );  DEBUG( "\r\n" ); //<-%
 */
  //DEBUG( "\r\n" );
 /*
  int i = 0;
  DEBUG( "\r\n" );
  DEBUG( "Sent\r\n" );
  for( i=0; i<8; i++ ){  DBITS( send_buffer.peek(i), 8 );  DEBUG( "\r\n" );  }
  DEBUG( "\r\n" );
 */

  return  true;
}

void TA::send_immediate(Message *m)  //// Does not req. ACK.
{
    radio_send( m, false );
}

bool TA::sendRaw(uint8_t *message, uint8_t len, uint8_t cone)  //// Currently not used.
{
    radio.send(cone, message, len);
    return true;    
}

// Restored existing architecture to use buffer.
bool TA::recieve(Message *m)
{
 /*
  if( receive_buffer.getSize() > 1 )
  {
    DEBUG( "buffer size is: %d\r\n", receive_buffer.getSize() );
  }
 */
  if( receive_buffer.getSize() < 7 )  return  false;
  int i = 0;
  // DEBUG( "\r\n" );
  // DEBUG( "Received\r\n" );
  // for( i=0; i<8; i++ )  DBITS( receive_buffer.peek(i), 8 );
  // DEBUG( "\r\n" );

 /*
  DEBUG( "%d\r\n", receive_buffer.peek(0) );
  DEBUG( "%d\r\n", receive_buffer.peek(1) );
  DEBUG( "%d\r\n", receive_buffer.peek(2) );
  DEBUG( "%d\r\n", receive_buffer.peek(3) );
  DEBUG( "%d\r\n", receive_buffer.peek(4) );
  DEBUG( "%d\r\n", receive_buffer.peek(5) );
 */
  
  //DEBUG( "%d\r\n", receive_buffer.getSize() );
  while( (receive_buffer.getSize() > 7) && ((char)receive_buffer.peek(6) != '%') )  receive_buffer.get();
  //DEBUG( "%d\r\n", receive_buffer.getSize() );

  if( receive_buffer.getSize() > 6 )// && receive_buffer.peek(6) == '%')
  {
    //DEBUG( "\r\n" );
   /*
    DEBUG( "Received:\r\n" );
    DBITS( receive_buffer.peek(0), 8 );  DEBUG( "\r\n" ); //<-cone?
    DBITS( receive_buffer.peek(1), 8 );  DEBUG( "\r\n" ); //<-command
    DBITS( receive_buffer.peek(2), 8 );  DEBUG( "\r\n" ); // value byte4
    DBITS( receive_buffer.peek(3), 8 );  DEBUG( "\r\n" ); // value byte2
    DBITS( receive_buffer.peek(4), 8 );  DEBUG( "\r\n" ); // value byte1
    DBITS( receive_buffer.peek(5), 8 );  DEBUG( "\r\n" ); // value byte0 (lsb)
    DBITS( receive_buffer.peek(6), 8 );  DEBUG( "\r\n" ); //<-%
   */
    m->cone    = receive_buffer.get();
    m->command = receive_buffer.get();
    m->value   = (uint32_t)receive_buffer.get();
    m->value   = m->value << 8;
    m->value  += (uint32_t)receive_buffer.get();
    m->value   = m->value << 8;
    m->value  += (uint32_t)receive_buffer.get();
    m->value   = m->value << 8;
    m->value  += (uint32_t)receive_buffer.get();

 /*
  DEBUG( "Raw receive:\r\n" );
  DBITS( (byte)m->cone,        8 );  DEBUG( "\r\n" );
  DBITS( (byte)m->command,     8 );  DEBUG( "\r\n" );
  DBITS( (byte)(m->value>>24), 8 );  DEBUG( "\r\n" );
  DBITS( (byte)(m->value>>16), 8 );  DEBUG( "\r\n" );
  DBITS( (byte)(m->value>>8),  8 );  DEBUG( "\r\n" );
  DBITS( (byte)m->value,       8 );  DEBUG( "\r\n" );
  DBITS( '%',                  8 );  DEBUG( "\r\n" );
 */

   /*
    DEBUG( "\r\n" );
    DEBUG( "Received:\r\n" );
    DEBUG( "%d\r\n", m->cone );
    DEBUG( "%c\r\n", (char)m->command );
    DEBUG( "%d\r\n", m->value );
    DEBUG( "\r\n" );
   */
    return  true;

  } else
    {
      DEBUG( "Bad Packet, size is: %d\r\n", receive_buffer.getSize() );
      while( receive_buffer.getSize() )  DEBUG( "%c", (char)receive_buffer.get() );
      DEBUG( "\r\n" );
      return  false;
    }
}

bool TA::waitForAck(int cone)  //// Currently not used.
{
    long start = micros();
    long timeout = ack_time;// + random(0,2000);
  while (micros() - start < timeout){
    if( radio_ack_received( cone ) ){
      //Serial.println(millis() - now);
      return true;
    }
  }
  //Serial.println(millis() - start);
  return false;
}

void TA::resetTouch()
{
    cap_enable = 1;
    wait_ms(100);
    cap_enable = 0;
}

#define MAXSENDTRIES  15

void TA::spin(void)
{
  static byte payload [6];
  static bool message_in_queue = false;
  static bool waiting_for_ack = false;
  static byte dest_cone = 0;
  static uint16_t index = 0;
  static uint16_t ack_tries = 0;
  static uint8_t send_tries = 0;
  static unsigned long ack_start = 0;
  static unsigned long random_wait = 0;
  static unsigned long random_wait_start = 0;
  static unsigned long mem_monitor_start = 0;

  static Message m; ////

  static unsigned long last_touch = 0;

  if (last_touch == 0 || millis()-last_touch > 3000)
  {
        //writeToPhone("Touch value: %d\r\n", touch_2);
        last_touch = millis();  
  }

  if(tripped())
  {
    resetTouch();
    writeToPhone("TCP\r\n");
    //Serial.println(F("toggled cap sense power"));
  }

  if(powering_up2)
  {
    unsigned long t = millis() - powerup_start;
    if(t > powerup_toggle)
    {
      buzzPin = !buzzPin;
      powerup_toggle *= 1.2;
    }
    if(t > 1250)
    {
      buzzPin = BUZZ_OFF;
      powering_up2 = false;
    }
  }
  else if(powering_up1)
  {
    unsigned long t = millis() - powerup_start;
    if(t > powerup_toggle)
    {
      buzzPin = !buzzPin;
      powerup_toggle *= 0.95;
      powerup_start = millis();
    }
    if(powerup_toggle < 10)
    {
      buzzPin = BUZZ_OFF;
      powering_up1 = false;
    }
  }
  else
  {
    if(beeping && (millis()-beep_start > beep_duration)) 
        beeping = false;
    if(pulsing && (millis()-pulse_start > pulse_duration)) 
        pulsing = false;
    if(beeping)
        buzzPin = BUZZ_ON;
    else if( pulsing && (((millis()-pulse_start) % pulse_period) < pulse_on))
    {
      if(!(mask & SILENT))
        buzzPin = BUZZ_ON;
      mask_color(0);
    }
    else
    {
      if(pulsing)mask_color(current_color);
      buzzPin = BUZZ_OFF;
    }
  }

  gotACK = false;  ////
  if( !waiting_for_ack && radio_receive( &m ) )  ////
  {
    if( get_crc_ok() )
    {
      if( radio.ACK_RECEIVED )  gotACK = true;  ////
        else
        {
          receive_buffer.put( radio.SENDERID );

          for( byte i=0; i < radio.DATALEN-1; i++ )
          {
            receive_buffer.put( radio.DATA[i] );
          }
          receive_buffer.put( '%' );
         /*
          DEBUG( "\r\n" );
          DEBUG( "Recieved: \r\n" );
          for( byte i=0; i < radio.DATALEN; i++ )
          {
            DBITS( radio.DATA[i], 8 );  DEBUG( "\r\n" );
          }
          DEBUG( "\r\n" );
         */
          if( radio.ACKRequested() )
          {
////           wait_us( 1300 );
            radio_send_ack();
            //DEBUG( "Sent ACK\r\n" );
          }
        }

    } else writeToPhone( "BAD-CRC\r\n" );
  }

  //if(index > 4999 || waiting_for_ack){
  if( waiting_for_ack )
  {
    //DEBUG( "Waiting for ack\r\n" );
    bool success = radio_ack_received( dest_cone );
    if( success || (send_tries > MAXSENDTRIES) )
    {
      //DEBUG( "dequeued: %u\r\n", (uint16_t)payload[1]<<8 + payload[2] );
      message_in_queue = false;
      waiting_for_ack  = false;
      unsigned long t = micros() - ack_start;
      //DEBUG( "Received ACK, microseconds: %u\r\n", t );
      //ack_tries = 0;
      if( !success )
      {
        //// DEBUG( "Failed to deliver message to cone %d\r\n", dest_cone );
        RA_DEBUG( "Fail msg to cone %d\r\n", dest_cone );  ////

      } else
        {
          if( send_tries > 1 )
          {
           /*
            DEBUG( "Sent message to cone %d, tries = %d\r\n", dest_cone, send_tries );
           */
            RA_DEBUG( "Rcvd ACK cone %d try %d, %uus\r\n", dest_cone, send_tries, t );

          } else
            {
              RA_DEBUG( "Rcvd ACK, %uus\r\n", t );  ////
            }
        }

      message_in_queue = false;
      send_tries       = 0;
     /*
      DEBUG( "Sent %c, to cone %d\r\n", (char)payload[0], dest_cone );
     */

    } else  // Still trying to get an ACK...
      {
        if( micros() - ack_start > 15000 /* 10000 */ )  ////
        {
          RA_DEBUG( "No ACK after 15ms\r\n" );
          waiting_for_ack   = false;
          random_wait_start = micros();
          random_wait       = random( 1500, 3000 );
         /*
          if( send_tries > MAXSENDTRIES )
          {
            DEBUG( "Failed to deliver message to cone %d\r\n", dest_cone );
            message_in_queue = false;
            send_tries       = 0;
          }
         */
          if( send_tries > 4 )  random_wait = random( 3000, 9000 );
          //DEBUG( "Failed to deliver message, waiting %d\r\n", random_wait );
          //ack_tries = 0;

         /*
          if( send_tries%50 == 0 )
          {
            DEBUG( "%d tries\r\n", send_tries );
          }
         */
        }
      }
    // ack_tries++;

  } else if( message_in_queue && (micros() - random_wait > random_wait_start) )// && index%64 == 0)
    { //// Continued from ack timeout in a previous spin()...
      if( gotACK )  ////
      {
          message_in_queue = false;
          send_tries       = 0;
          RA_DEBUG( "Rcvd B ACK %uus\r\n", micros() -ack_start );  ////

      } else
        {
            requestACK = true;
            //DEBUG( "sending\r\n" );
///          writeToPhone("Sending message to: %d\r\n", dest_cone);
            radio.send( dest_cone, payload, 6, requestACK );
            //DEBUG( "sent\r\n" );
            //DEBUG( "Trying to send: %d\r\n", (uint8_t)payload[0] );
            //uint16_t temp = (uint16_t)payload[1]<<8;
            //temp += (uint8_t)payload[2];
            send_tries++;
            ack_start = micros();
            if( !radio_receive_complete() /* !radio_ack_received( dest_cone ) */ ) // the 'if' is here to prevent the radio from going to sleep and missing the ACK
            {
                waiting_for_ack = true;  //// Check for ack on next spin()

            } else  // Shouldn't happen...
              {
                  if( radio.ACK_RECEIVED )  message_in_queue = false;
                  RA_DEBUG( "Rcvd Immed %uus\r\n", micros() -ack_start );  ////
              }
        }

    } else
      {
       /*
        if( (send_buffer.getSize() > 0) && !message_in_queue )
        {
          payload[0] = send_buffer.get();
          //DEBUG( "Got from queue: %c\r\n", (char)payload[0] );
          message_in_queue = true;
        }
       */
        while( (send_buffer.getSize() > 4) && !message_in_queue )
        {
          payload[0] = send_buffer.get();
          payload[1] = send_buffer.get();
          payload[2] = send_buffer.get();
          payload[3] = send_buffer.get();
          payload[4] = send_buffer.get();
          dest_cone  = send_buffer.get();
          payload[5] = send_buffer.get();
         /*
          DEBUG( "\r\n" );
          DEBUG( "sending...\r\n" );
          DBITS( payload[0], 8 );  DEBUG( "\r\n" );
          DBITS( payload[1], 8 );  DEBUG( "\r\n" );
          DBITS( payload[2], 8 );  DEBUG( "\r\n" );
          DBITS( payload[3], 8 );  DEBUG( "\r\n" );
          DBITS( payload[4], 8 );  DEBUG( "\r\n" );
          DBITS( dest_cone,  8 );  DEBUG( "\r\n" );
          DBITS( payload[5], 8 );  DEBUG( "\r\n" );
          DEBUG( "\r\n" );
         */
          if( (char)payload[5] == '%' )
          {
              message_in_queue = true;
              payload[5] = ++msg_id;  //// Message ID used for duplicate message check.

          } else
            {
              //DEBUG( "bad message format\r\n" );
              while( (send_buffer.getSize() > 0) && (send_buffer.get() != '%') ) ; // if we didn't land on the end of a message, peel stuff off until we are
            }
        }
      }
}

bool TA::activated(void)
{
    /* Default to enabling activation on disabled--Except if mask has DIS_ON_DARK set */
    uint8_t maskOrDark = ((!(mask & TOUCHLIGHTS) != !!(mask & DIS_ON_DARK)) ? TOUCHLIGHTS : mask);

    bool retval = (((buttons() | buttonsRising) & maskOrDark) ? true : false);

    TA::buttonsRising = 0;

    return  retval;
}

bool TA::tripped(void)
{
  // detect if something is staying on the cap sensor (i.e. the floor is causing the sensor to overload)
  // in some cases the sensor output is choppy, so we must detect those situations also.
  uint8_t current = 0;
  static bool sensor_hysteresis = false;
  static uint8_t last;
  static unsigned long activated_start;
  static unsigned long sensor_hyst_start;

  current = buttons();
  if(current && !last && (millis() - sensor_hyst_start > 250)){
     activated_start = millis();
     //Serial.println(current | 0b10000000, BIN);
  }
  if(!current && last) sensor_hyst_start = millis();
  last = current;

  return (current && (millis() - activated_start > 4000))?true:false;

}

uint8_t TA::buttons(void)
{
  uint8_t buttons = 0;

  buttons |= touch_1?1:0;
  buttons |= touch_2?2:0;
  buttons |= touch_3?0:4;
  return buttons; /// touch;

}

void TA::setMask(uint8_t the_mask)
{
    mask = the_mask;
}

void TA::initialize(uint8_t address)
{
  send_buffer.init(96); //48//64//128 is too big, seems to corrupt the data, WTF????
  receive_buffer.init(96);

  resetTouch();

  ack_time = ACK_TIME;  //// Currently not used.

  radio_init();

  beep(50);
}

/* Can't find a way of implementing this function.
 *
 * !SR
 */
void TA::Ram_TableDisplay(void)
{
    
}

/* Not sure if this work. Taken from: https://developer.mbed.org/questions/6994/How-to-print-Free-RAM-available-RAM-or-u/
 *
 * !SR
 */
void TA::get_free_memory(void)
{
    char   stackVariable;
    char   *heap;
    unsigned long result;
    heap  = (char*)malloc(4);
    result  = &stackVariable - heap;
    free(heap);
    
    //serial->printf("Free memory: %ul\n\n",result);
}

void TA::check_mem(void)
{
    uint8_t * heapptr, * stackptr;
    unsigned int stack, heap = 0;

    stackptr = (uint8_t *)malloc(4);          // use stackptr temporarily
    heapptr = stackptr;                     // save value of heap pointer
    free(stackptr);      // free up the memory again (sets stackptr to 0)
  
    stack = __current_sp() ;
    heap = (uint16_t)heapptr;
    uint16_t last_call = *(stackptr++);
    
    //serial->printf("Stack: 0x");
    //serial->printf("%x ",stack);
    //serial->printf("Heap: 0x");
    //serial->printf("%x\n",heap);
    //serial->printf("Last call: 0x");
   // serial->printf("%x\n",last_call);
    get_free_memory();
}