Can we detect how much stack space and heap memory is used by a sketch?

KurtE commented 5 years ago

Not sure if this is the best place to ask? This is not totally specific to OpenCM, this equally will apply to OpenCR, XEL, Ros To Arduino... But the actual code may differ.

Again not sure if this is better to ask here or RobotSource or Robotis Forum?

But wondering if there are any suggestions on how to detect how much memory is being used by a sketch. For example I am pretty sure that when I try to blindly merge in the current Dyanmixel SDK, my test apps were failing to properly run as the stack and heap corrupted each other.

Would be great if we could somehow find out how much non-static memory a program is using, which would would help to measure reductions in usage.

I don't know for example if the new operation as well as malloc(), maybe call _sbrk(). If so might be able to enable test for collision... (Would need to maybe change the commented out write statement...

But again this does not show us how close we are getting...

Was wondering if it would make sense to do something like, maybe have the startup code, do something like write a standard byte or the like to all memory over "end" to some known quantity.

Again assuming that _sbrk is correct and we can hack it slightly, we can hopefully find out how far the heap has grown.

We could then scan from the heap_end back to at worst case back to current stack pointer, but basically keep walking up in memory until we hit something that is NOT our prefill values and assume the stack grew at least to there... Obviously the stack could have grown beyond that if it had used those magic values, but hopefully close enough...

In my test app that failed earlier. I added some init code:

 // Print out some memory information
  Serial.printf("End Memory location: %x\n", (uint32_t)&end);
  Serial.printf("Stack Pointer: %x\n", (uint32_t)stack_ptr);

Which Output:

End Memory location: 20003450
Stack Pointer: 20004fd8

Which if I subtract the two I see they differ by 7048 bytes. Which might explain why increasing the size of buffers from 2K to 4K and a write allocated a 4K buffer and the stuffing function also used 4K on stack where these two collided.

Does this make sense? Has anyone already setup something like this?

OpusK commented 5 years ago

Hi, @KurtE

I think it would be better to deal with this issue here than elsewhere. Among the ROBOTIS OpenHardware, OpenCM 9.04 has the fewest resources, so I think we can do a good test here.

It would be nice to be able to measure the stack and heap usage of sketches. In an IDE such as TrueStudio, the memory area is measured and displayed in the GUI. I do not have experience with this, so I think we should try to think together.

KurtE commented 5 years ago

Hi @OpusK @routiful @kijongGil ,

For what it is worth, I have hacked up my own AX 12_test_OpenCM sketch to try it out. Here is my current sketch: Note: I have also checked in the changes into my github project: https://github.com/KurtE/Open_CM_CR_Arduino_Sketches

//====================================================================================================
// Kurts Test program to try out different ways to manipulate the AX12 servos on the PhantomX
// This is a test, only a test...
//
// This version for Robotis OpenCM9.04
//====================================================================================================
//============================================================================
// Global Include files
//=============================================================================
#include <DynamixelSDK.h>
#include <syscalls.h>
//=============================================================================
// Options...
//=============================================================================
#if defined(__OPENCM904__)
uint8_t port_handler_numbers[] = {1, 3};    // Setup to handle both ports of openCM
#endif
#if defined(__OPENCR__)
uint8_t port_handler_numbers[] = {3};    // Setup to handle both ports of openCR
#define SERVO_POWER_ENABLE_PIN BDPIN_DXL_PWR_EN
#endif

#if defined(TEENSYDUINO)
uint8_t port_handler_numbers[] = {1};     // Default to Serial1
#if defined(__MK66FX1M0__)
// Try with the T3.6 board
#define SERVOBUS Serial1
#define SERVO_RX_PIN           27
#define SERVO_TX_PIN           26
#define SERVO_DIRECTION_PIN 28
#define SERVO_POWER_ENABLE_PIN  29
#else
#define SERVO_POWER_ENABLE_PIN 2
#endif
#endif
#define COUNT_PORTHANDLERS  sizeof(port_handler_numbers)
// Uncomment the next line if building for a Quad instead of a Hexapod.
//#define QUAD_MODE
//#define TURRET
//#define DEBUG_IO_PINS

//#define VOLTAGE_ANALOG_PIN 0
//#define SOUND_PIN 1
#define SERVO1_SPECIAL  19     // We wish to reserve servo 1 so we can see servo reset

//=============================================================================
// Define differnt robots..
//=============================================================================

// Protocol version
#define PROTOCOL_VERSION                1.0                 // See which protocol version is used in the Dynamixel
#define PROTOCOL_VERSION2                2.0                 // See which protocol version is used in the Dynamixel
#define DEVICENAME                      "3"                 // Check which port is being used on your controller
#define DXL_BAUDRATE 1000000

// Constants
/* Servo IDs */
#define     RF_COXA       2
#define     RF_FEMUR      4
#define     RF_TIBIA      6

#define     RM_COXA      14
#define     RM_FEMUR     16
#define     RM_TIBIA     18

#define     RR_COXA       8
#define     RR_FEMUR     10
#define     RR_TIBIA     12

#ifdef SERVO1_SPECIAL
#define     LF_COXA       19
#else
#define     LF_COXA       1
#endif
#define     LF_FEMUR      3
#define     LF_TIBIA      5

#define     LM_COXA      13
#define     LM_FEMUR     15
#define     LM_TIBIA     17

#define     LR_COXA       7
#define     LR_FEMUR      9
#define     LR_TIBIA     11

#ifdef TURRET
#define     TURRET_ROT    20
#define     TURRET_TILT   21
#endif

static const byte pgm_axdIDs[] = {
  LF_COXA, LF_FEMUR, LF_TIBIA,
#ifndef QUAD_MODE
  LM_COXA, LM_FEMUR, LM_TIBIA,
#endif
  LR_COXA, LR_FEMUR, LR_TIBIA,
  RF_COXA, RF_FEMUR, RF_TIBIA,
#ifndef QUAD_MODE
  RM_COXA, RM_FEMUR, RM_TIBIA,
#endif
  RR_COXA, RR_FEMUR, RR_TIBIA
#ifdef TURRET
  , TURRET_ROT, TURRET_TILT
#endif
};

#define NUM_SERVOS ((int)(sizeof(pgm_axdIDs)/sizeof(pgm_axdIDs[0])))
const char* IKPinsNames[] = {
  "LFC", "LFF", "LFT",
#ifndef QUAD_MODE
  "LMC", "LMF", "LMT",
#endif
  "LRC", "LRF", "LRT",
  "RFC", "RFF", "RFT",
#ifndef QUAD_MODE
  "RMC", "RMF", "RMT",
#endif
  "RRC", "RRF", "RRT",
#ifdef TURRET
  "T-ROT", "T-TILT"
#endif
};

/** EEPROM AREA **/
#define AX_MODEL_NUMBER_L           0
#define AX_MODEL_NUMBER_H           1
#define AX_VERSION                  2
#define AX_ID                       3
#define AX_BAUD_RATE                4
#define AX_RETURN_DELAY_TIME        5
#define AX_CW_ANGLE_LIMIT_L         6
#define AX_CW_ANGLE_LIMIT_H         7
#define AX_CCW_ANGLE_LIMIT_L        8
#define AX_CCW_ANGLE_LIMIT_H        9
#define AX_SYSTEM_DATA2             10
#define AX_LIMIT_TEMPERATURE        11
#define AX_DOWN_LIMIT_VOLTAGE       12
#define AX_UP_LIMIT_VOLTAGE         13
#define AX_MAX_TORQUE_L             14
#define AX_MAX_TORQUE_H             15
#define AX_RETURN_LEVEL             16
#define AX_ALARM_LED                17
#define AX_ALARM_SHUTDOWN           18
#define AX_OPERATING_MODE           19
#define AX_DOWN_CALIBRATION_L       20
#define AX_DOWN_CALIBRATION_H       21
#define AX_UP_CALIBRATION_L         22
#define AX_UP_CALIBRATION_H         23
/** RAM AREA **/
#define AX_TORQUE_ENABLE            24
#define AX_LED                      25
#define AX_CW_COMPLIANCE_MARGIN     26
#define AX_CCW_COMPLIANCE_MARGIN    27
#define AX_CW_COMPLIANCE_SLOPE      28
#define AX_CCW_COMPLIANCE_SLOPE     29
#define AX_GOAL_POSITION_L          30
#define AX_GOAL_POSITION_H          31
#define AX_GOAL_SPEED_L             32
#define AX_GOAL_SPEED_H             33
#define AX_TORQUE_LIMIT_L           34
#define AX_TORQUE_LIMIT_H           35
#define AX_PRESENT_POSITION_L       36
#define AX_PRESENT_POSITION_H       37
#define AX_PRESENT_SPEED_L          38
#define AX_PRESENT_SPEED_H          39
#define AX_PRESENT_LOAD_L           40
#define AX_PRESENT_LOAD_H           41
#define AX_PRESENT_VOLTAGE          42
#define AX_PRESENT_TEMPERATURE      43
#define AX_REGISTERED_INSTRUCTION   44
#define AX_PAUSE_TIME               45
#define AX_MOVING                   46
#define AX_LOCK                     47
#define AX_PUNCH_L                  48
#define AX_PUNCH_H                  49
//=============================================================
// Defines for X series (XL430)
//=============================================================================
#define DXL_PING                    1
#define DXL_READ_DATA               2
#define DXL_WRITE_DATA              3
#define DXL_REG_WRITE               4
#define DXL_ACTION                  5
#define DXL_RESET                   6
#define DXL_SYNC_READ               0x82
#define DXL_SYNC_WRITE              0x83

/** EEPROM AREA **/
#define DXL_X_MODEL_NUMBER         0 //2 1060(xl430-250)
#define DXL_X_MODEL_INFORMATION    2 //4
#define DXL_X_VERSION              6
#define DXL_X_ID                   7   // 1 1
#define DXL_X_BAUD_RATE            8   // 1 1
#define DXL_X_RETURN_DELAY_TIME    9   // 1 250
#define DXL_X_DRIVE_MODE           10  // 1 0
#define DXL_X_OPERATING_MODE       11  // 1 3
#define DXL_X_SECONDARY_ID         12  // 1 255
#define DXL_X_PROTOCOL_VERSION     13  // 1 - 2
#define DXL_X_HOMING_OFFSET        20  // 4 - 0
#define DXL_X_MOVING_THRESHOLD     24  // 4 - 10
#define DXL_X_TEMPERATURE_LIMIT    31  //1 - 72
#define DXL_X_MAX_VOLTAGE_LIMIT    32  // 2 140
#define DXL_X_MIN_VOLTAGE_LIMIT    34  // 2 60
#define DXL_X_PWM_LIMIT            36  // 2 885
#define DXL_X_ACCELERATION_LIMIT   40  // 4 32767
#define DXL_X_VElOCITY_LIMIT       44  // 4 415
#define DXL_X_MAX_POSITION_LIMIT   48  // 4 4095
#define DXL_X_MIN_POSITION_LIMIT   52  // 4 0
#define DXL_X_SHUTDOWN             63  // 1 52

/** RAM AREA **/
#define DXL_X_TORQUE_ENABLE        64  // 1 0
#define DXL_X_LED                  65  // 1 0
#define DXL_X_STATUS_RETURN_LEVEL  68  // 1 2
#define DXL_X_REGISTERED_INSTRUCTION 69 //1 (R)
#define DXL_X_HARDWARE_ERROR_STATUS  70 // 1 (R)
#define DXL_X_VELOCITY_I_GAIN      76  // 2 1000 
#define DXL_X_VELOCITY_P_GAIN      78  // 2 100 
#define DXL_X_POSITION_D_GAIN      80  // 2 4000 
#define DXL_X_POSITION_I_GAIN      82  // 2 0
#define DXL_X_POSITION_P_GAIN      84  // 2 640
#define DXL_X_FEEDFORWARD_2_GAIN   88  // 2 0
#define DXL_X_FEEDFORWARD_1_GAIN   90  // 2 0
#define DXL_X_BUS_WATCHDOG         98  // 1 -
#define DXL_X_GOAL_PWM             100 // 2 
#define DXL_X_GOAL_VELOCITY        104 // 4
#define DXL_X_PROFILE_ACCELERATION 108 // 4
#define DXL_X_PROFILE_VELOCITY     112 // 4
#define DXL_X_GOAL_POSITION        116 // 4
#define DXL_X_REALTIME_TICK        120 // 2 (R)
#define DXL_X_MOVING               122 // 1 (R)
#define DXL_X_MOVING_STATUS        123 // 1 (R)
#define DXL_X_PRESENT_PWN          124 // 2 (R)
#define DXL_X_PRESENT_LOAD         126 // 2 (R)
#define DXL_X_PRESENT_VELOCITY     128 // 4 (R)
#define DXL_X_PRESENT_POSITION     132 // 4 (R)
#define DXL_X_VELOCITY_TRAJECTORY  136 // 4 (R)
#define DXL_X_POSITION_TRAJECTORY  140 // 4 (R)
#define DXL_X_PRESENT_INPUT_VOLTAGE  144 // 2 (R)
#define DXL_X_PRESENT_TEMPERATURE  146 // 1 (R)

//=============================================================================
// Globals
//=============================================================================
// Global objects
// Handle to port handler and packet handler;
dynamixel::PortHandler *portHandlers[COUNT_PORTHANDLERS];

dynamixel::PacketHandler *packetHandler1;
dynamixel::PacketHandler *packetHandler2;

word           g_wVoltage;
uint8_t        g_servo_index_voltage = 0;
char           g_aszCmdLine[80];
uint8_t        g_iszCmdLine;
boolean        g_fTrackServos = false;

// g_servo_protocol
typedef union {
  struct {
    uint8_t protocol: 2; // define which protocol to use
    uint8_t port: 6;    // Define which port it is on
  } b;
  uint8_t val;
} SERVO_PROT_PORT_t;

enum           {SERVO_NOT_FOUND = 0, SERVO_PROTOCOL1 = 1, SERVO_PROTOCOL2};

SERVO_PROT_PORT_t        g_servo_protocol[255] = {SERVO_NOT_FOUND};  // What type of servos do we have????

uint8_t        g_count_servos_found = 0;

// Values to use for servo position...
byte          g_bServoID;
word          g_wServoGoalPos;
word          g_wServoGoalSpeed;

//====================================================================================================
// forward reference
//====================================================================================================
extern bool IsValidServo(uint8_t servo_id);
//====================================================================================================
// Setup
//====================================================================================================
void setup() {
  while (!Serial && (millis() < 3000)) ;  // Give time for Teensy and other USB arduinos to create serial port
  Serial.begin(38400);  // start off the serial port.
  Serial.println("\nCM9.04 Servo Test program");
  initMemoryUsageTest();

  pinMode(0, OUTPUT);
#if defined(SERVO_RX_PIN)
  SERVOBUS.setRX(SERVO_RX_PIN);
#endif
#if defined(SERVO_TX_PIN)
  SERVOBUS.setTX(SERVO_TX_PIN);
#endif

#ifdef SERVO_POWER_ENABLE_PIN
  Serial.printf("Enable Servo power: %d\n", SERVO_POWER_ENABLE_PIN);
  pinMode(SERVO_POWER_ENABLE_PIN, OUTPUT);
  digitalWrite(SERVO_POWER_ENABLE_PIN, HIGH);
#endif
  pinMode(4, OUTPUT);

  // Initialize PacketHandler instance
  Serial.println("Get Packet Handlers");
  packetHandler1 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
  packetHandler2 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION2);
  // Open port

  // Set the protocol version
  // Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
  // Initialize PortHandler instances

  char port_string[5];
  for (uint8_t i = 0; i < COUNT_PORTHANDLERS; i++) {
    itoa(port_handler_numbers[i], port_string, sizeof(port_string));
    portHandlers[i] = dynamixel::PortHandler::getPortHandler(port_string);
    Serial.printf("Get Port Handler %s %x\n", port_string, (uint32_t)portHandlers[i]);

    // Lets init the two different port handlers.
#if defined(SERVO_DIRECTION_PIN)
    portHandlers[i]->setTXEnablePin(SERVO_DIRECTION_PIN);
#endif
    Serial.println("    Call Open Port");
    if (!portHandlers[i]->openPort()) {
      Serial.print("Failed to open port 1 the Dynamixel port: ");
      Serial.println(port_string);
    }
    Serial.printf("    Set Baud Rate: %d\n", DXL_BAUDRATE);
    if (!portHandlers[i]->setBaudRate(DXL_BAUDRATE)) {
      Serial.print("Failed to change the Port 1 Dynamixel baudrate: ");
      Serial.println(port_string);
    }
  }
  delay(1000);
  // Lets start of trying to locate all servos.
  FindServos();

  PrintServoVoltage();

}

//====================================================================================================
// Loop
//====================================================================================================
void loop() {
  // Output a prompt
  PrintServoVoltage();
  printMemoryUsage();

  // lets toss any charcters that are in the input queue
  while (Serial.read() != -1)
    ;

  Serial.println("0 - All Servos off");
  Serial.println("1 - All Servos center");
  Serial.println("2 - Set Servo position [<Servo>] <Position> [<Speed>]");
  Serial.println("3 - Set Servo Angle");
  Serial.println("4 - Get Servo Positions");
  Serial.println("5 - Find All Servos");
  Serial.println("6 - Set Servo return delay time");
  Serial.println("8 - Set ID: <old> <new>");
  Serial.println("9 - Print Servo Values");
  Serial.println("b - Baud <new baud>");
  Serial.println("t - Toggle track Servos");
  Serial.println("h - hold [<sn>]");
  Serial.println("f - free [<sn>]");
  Serial.println("r - Reboot [<sn>]");
  Serial.println("w - write <servo> <reg> <val> (<val2>...)\n\r");

  Serial.print(":");
  Serial.flush();  // make sure the complete set of prompts has been output...
  // Get a command
  if (GetCommandLine()) {
    Serial.println("");
    Serial.print("Cmd: ");
    Serial.println(g_aszCmdLine);
    g_iszCmdLine = 1;  // skip over first byte...
    switch (g_aszCmdLine[0]) {
      case '0':
        AllServosOff();
        break;
      case '1':
        AllServosCenter();
        break;
      case '2':
        SetServoPosition();
        break;
      case '3':
        break;
      case '4':
        GetServoPositions();
        break;
      case '5':
        FindServos();
        break;
      case '6':
        SetServoReturnDelayTime();
        break;
      case '8':
        SetServoID();
        break;
      case '9':
        PrintServoValues();
        break;
      case 'b':
      case 'B':
        SetBaudRate();
        break;
      case 'f':
      case 'F':
        HoldOrFreeServos(0);
        break;
      case 'h':
      case 'H':
        HoldOrFreeServos(1);
        break;
      case 'r':
      case 'R':
        RebootServos();
        break;
      case 't':
      case 'T':
        g_fTrackServos = !g_fTrackServos;
        if (g_fTrackServos) {
          Serial.println("Tracking On");
          TrackServos(true);  // call to initialize all of the positions.
        }
        else
          Serial.println("Tracking Off");
        TrackPrintMinsMaxs();
        break;
      case 'w':
      case 'W':
        WriteServoValues();
        break;
    }
  }
}

//====================================================================================================
void PrintServoVoltage() {
  // Lets try reading in the current voltage for the next servo we found...
  if (g_count_servos_found == 0) return; // no servos found
  Serial.println("PrintServo Voltage called");
  g_servo_index_voltage++;    // will wrap around...
  uint8_t sanity_test_count = 0;
  while (!g_servo_protocol[g_servo_index_voltage].val) {
    g_servo_index_voltage++;
    if (g_servo_index_voltage >= 254) g_servo_index_voltage = 0;
    sanity_test_count--;
    if (sanity_test_count == 0) {
      Serial.println("PSV Sanity Test fail");
      return;
    }
  }
  uint16_t wNewVoltage;
  dynamixel::PortHandler *portHandler = portHandlers[g_servo_protocol[g_servo_index_voltage].b.port];
  if (g_servo_protocol[g_servo_index_voltage].b.protocol == SERVO_PROTOCOL1) {
    uint8_t bVoltage;
    packetHandler1->read1ByteTxRx(portHandler, g_servo_index_voltage, AX_PRESENT_VOLTAGE, &bVoltage);
    wNewVoltage = bVoltage;
  } else {
    packetHandler2->read2ByteTxRx(portHandler, g_servo_index_voltage, DXL_X_PRESENT_INPUT_VOLTAGE, &wNewVoltage);
  }
  if (wNewVoltage != g_wVoltage) {
    g_wVoltage = wNewVoltage;
    Serial.print("Servo: ");
    Serial.print(g_servo_index_voltage, DEC);
    Serial.print(" Voltage in 10ths: ");
    Serial.println(g_wVoltage, DEC);
  }
}

//====================================================================================================
// Helper function to read in a command line
uint8_t GetCommandLine(void) {
  int ch;
  uint8_t ich = 0;
  g_iszCmdLine = 0;

  for (;;) {
    // throw away any thing less than CR character...
    ch = Serial.read();
    if ((ch >= 10) && (ch <= 15)) {
      g_aszCmdLine[ich] = 0;
      return ich;
    }
    if (ch != -1)
      g_aszCmdLine[ich++] = ch;

    if (g_fTrackServos)
      TrackServos(false);
  }
}

//=======================================================================================
boolean FGetNextCmdNum(word * pw ) {
  // Skip all leading num number characters...
  while ((g_aszCmdLine[g_iszCmdLine] < '0') || (g_aszCmdLine[g_iszCmdLine] > '9')) {
    if (g_aszCmdLine[g_iszCmdLine] == 0)
      return false;  // end of the line...
    g_iszCmdLine++;
  }
  *pw = 0;
  while ((g_aszCmdLine[g_iszCmdLine] >= '0') && (g_aszCmdLine[g_iszCmdLine] <= '9')) {
    *pw = *pw * 10 + (g_aszCmdLine[g_iszCmdLine] - '0');
    g_iszCmdLine++;
  }
  return true;
}

//=======================================================================================
void AllServosOff(void) {
  // Quick and dirty way to do it by broadcast...
  for (uint8_t i = 0; i < COUNT_PORTHANDLERS; i++) {
    packetHandler1->write1ByteTxRx(portHandlers[i], 0xfe, AX_TORQUE_ENABLE, 0x0);
    packetHandler2->write1ByteTxRx(portHandlers[i], 0xfe, DXL_X_TORQUE_ENABLE, 0x0);
  }
}

//=======================================================================================
bool ReportAnyErrors(const char *psz, uint8_t servo_id, int retval, uint8_t error) {

  if ((retval == COMM_SUCCESS) && (error == 0)) return false; // no error
  Serial.print(psz);
  Serial.print(":");
  Serial.print(servo_id, DEC);
  Serial.print("(");
  switch (retval) {
    case COMM_PORT_BUSY : Serial.print("BUSY"); break;
    case COMM_TX_FAIL   : Serial.print("TX FAIL"); break;
    case COMM_RX_FAIL   : Serial.print("RX FAIL"); break;
    case COMM_TX_ERROR  : Serial.print("TX ERROR"); break;
    case COMM_RX_WAITING: Serial.print("RX WAIT"); break;
    case COMM_RX_TIMEOUT: Serial.print("TIMEOUT"); break;
    case COMM_RX_CORRUPT: Serial.print("CORRUPT"); break;
    default: Serial.print(retval, DEC);
  }
  Serial.print(",");
  Serial.print(error, HEX);
  switch (error) {
    case 1: Serial.print(" Result"); break;
    case 2: Serial.print(" Instruction"); break;
    case 3: Serial.print(" CRC"); break;
    case 4: Serial.print(" Range"); break;
    case 5: Serial.print(" Length"); break;
    case 6: Serial.print(" Limit"); break;
    case 7:  Serial.print(" Access"); break;
  }
  Serial.print(") ");
  return true;
}

//=======================================================================================
void AllServosCenter(void) {
  bool any_errors = false;
  uint8_t error;
  int retval;
  // First make sure all of the motors are turned on.
  for (int i = 0; i < 255; i++) {
    dynamixel::PortHandler *portHandler = portHandlers[g_servo_protocol[i].b.port];

    if (g_servo_protocol[i].b.protocol == SERVO_PROTOCOL1) {
      // See if this turns the motor off and I can turn it back on...
      retval = packetHandler1->write1ByteTxRx(portHandler, i, AX_TORQUE_ENABLE, 0x1, &error);
      any_errors |= ReportAnyErrors("TQ ON", i, retval, error);
      retval = packetHandler1->write2ByteTxRx(portHandler, i, AX_GOAL_POSITION_L, 0x1ff, &error);
      any_errors |= ReportAnyErrors("Goal", i, retval, error);
    } else if (g_servo_protocol[i].b.protocol == SERVO_PROTOCOL2) {
      retval = packetHandler2->write1ByteTxRx(portHandler, i, DXL_X_TORQUE_ENABLE, 0x1, &error);
      any_errors |= ReportAnyErrors("TQ ON", i, retval, error);
      retval = packetHandler2->write4ByteTxRx(portHandler, i, DXL_X_GOAL_POSITION, 2047, &error);
      any_errors |= ReportAnyErrors("Goal", i, retval, error);
    }
  }

  if (any_errors) Serial.println();
}

//=======================================================================================
void HoldOrFreeServos(byte fHold) {
  word iServo;
  if (!FGetNextCmdNum(&iServo)) {
    for (int i = 0; i < 255; i++) {
      dynamixel::PortHandler *portHandler = portHandlers[g_servo_protocol[i].b.port];
      if (g_servo_protocol[i].b.protocol == SERVO_PROTOCOL1) {
        // See if this turns the motor off and I can turn it back on...
        packetHandler1->write1ByteTxRx(portHandler, i, AX_TORQUE_ENABLE, fHold);
      } else if (g_servo_protocol[i].b.protocol == SERVO_PROTOCOL2) {
        packetHandler2->write1ByteTxRx(portHandler, i, DXL_X_TORQUE_ENABLE, fHold);
      }
    }
  }
  else {
    if (IsValidServo(iServo)) {
      dynamixel::PortHandler *portHandler = portHandlers[g_servo_protocol[iServo].b.port];
      if (g_servo_protocol[iServo].b.protocol == SERVO_PROTOCOL1) {
        // See if this turns the motor off and I can turn it back on...
        packetHandler1->write1ByteTxRx(portHandler, iServo, AX_TORQUE_ENABLE, fHold);
      } else if (g_servo_protocol[iServo].b.protocol == SERVO_PROTOCOL2) {
        packetHandler2->write1ByteTxRx(portHandler, iServo, DXL_X_TORQUE_ENABLE, fHold);
      }

    }
  }
}

//=======================================================================================
//=======================================================================================
void RebootServos() {
  word iServo;
  while (FGetNextCmdNum(&iServo)) {
    if (IsValidServo(iServo)) {
      dynamixel::PortHandler *portHandler = portHandlers[g_servo_protocol[iServo].b.port];
      if (g_servo_protocol[iServo].b.protocol == SERVO_PROTOCOL1) {
        // See if this turns the motor off and I can turn it back on...
        packetHandler1->reboot(portHandler, iServo);
      } else if (g_servo_protocol[iServo].b.protocol == SERVO_PROTOCOL2) {
        packetHandler2->reboot(portHandler, iServo);
      }
    }
  }
}

//=======================================================================================
void SetServoPosition(void) {
  word w1;
  word w2;
  dynamixel::PortHandler *portHandler;

  if (!FGetNextCmdNum(&w1))
    return;    // no parameters so bail.

  Serial.println("Set Servo Position");
  if (FGetNextCmdNum(&w2)) {  // We have at least 2 parameters
    g_bServoID = w1;    // So first is which servo

    if (!IsValidServo(g_bServoID)) {
      Serial.println("Servo not found");
      return;
    }

    portHandler = portHandlers[g_servo_protocol[g_bServoID].b.port];
    g_wServoGoalPos = w2;
    if (FGetNextCmdNum(&w2)) {  // We have at least 3 parameters
      g_wServoGoalSpeed = w2;
      if (g_servo_protocol[g_bServoID].b.protocol == SERVO_PROTOCOL1) {
        packetHandler1->write2ByteTxRx(portHandler, g_bServoID, AX_GOAL_SPEED_L, g_wServoGoalSpeed);
      } else {
        packetHandler2->write4ByteTxRx(portHandler, g_bServoID, DXL_X_GOAL_VELOCITY, g_wServoGoalSpeed);
      }
      Serial.print("Goal Speed: ");
      Serial.print(g_wServoGoalSpeed, DEC);
    }
  }
  else {
    if (!IsValidServo(g_bServoID)) {
      Serial.println("Servo not found");
      return;
    }

    portHandler = portHandlers[g_servo_protocol[g_bServoID].b.port];
    g_wServoGoalPos = w1;  // Only 1 paramter so assume it is the new position
  }

  // Now lets try moving that servo there
  if (g_servo_protocol[g_bServoID].b.protocol == SERVO_PROTOCOL1) {
    packetHandler1->write2ByteTxRx(portHandler, g_bServoID, AX_GOAL_POSITION_L, g_wServoGoalPos);
  } else {
    packetHandler2->write4ByteTxRx(portHandler, g_bServoID,  DXL_X_GOAL_POSITION, g_wServoGoalPos);
  }
  Serial.print(" ID: ");
  Serial.print(g_bServoID, DEC);
  Serial.print(" ");
  Serial.println(g_wServoGoalPos, DEC);

}

//=======================================================================================
bool IsValidServo(uint8_t servo_id) {
  if (g_servo_protocol[servo_id].val)
    return true;  // was found before.

  // First lets try Protocol1 ping
  for (uint8_t i = 0; i < COUNT_PORTHANDLERS; i++) {
    //Serial.printf("IsValidServo: %d Need to ping\n", servo_id);
    if (packetHandler1->ping(portHandlers[i], servo_id) == COMM_SUCCESS) {
      g_servo_protocol[servo_id].b.protocol = SERVO_PROTOCOL1;
      g_servo_protocol[servo_id].b.port = i;
      g_count_servos_found++;
      return true;
    }
    if (packetHandler2->ping(portHandlers[i], servo_id) == COMM_SUCCESS) {
      g_servo_protocol[servo_id].b.protocol = SERVO_PROTOCOL2;
      g_servo_protocol[servo_id].b.port = i;
      g_count_servos_found++;
      return true;
    }
  }
  return false;
}

//=======================================================================================
void SetServoReturnDelayTime(void) {
  word w1;
  word w2;

  if (!FGetNextCmdNum(&w1))
    return;    // no parameters so bail.

  if (!FGetNextCmdNum(&w2))
    w2 = 0;   // we will default to 0 (our desired)

  Serial.print("Set Servo ID: ");
  Serial.print(w1, DEC);
  Serial.print(" return delay time: ");
  Serial.println(w2, DEC);

  if (!IsValidServo(w1)) {
    Serial.print("Servo: ");
    Serial.print(w1, DEC);
    Serial.println("Was not found");
    return;
  }

  // Now lets update that servo
  int retval;
  uint8_t error;
  dynamixel::PortHandler *portHandler = portHandlers[g_servo_protocol[w1].b.port];
  if (g_servo_protocol[w1].b.protocol == SERVO_PROTOCOL1) {
    retval = packetHandler1->write1ByteTxRx(portHandler, w1, AX_RETURN_DELAY_TIME, w2, &error);
  } else {
    retval = packetHandler2->write1ByteTxRx(portHandler, w1, DXL_X_RETURN_DELAY_TIME, w2, &error);
  }
  ReportAnyErrors("Set return delay", w1, retval, error);
}

//=======================================================================================
void SetServoID(void) {
  word w1;
  word w2;

  if (!FGetNextCmdNum(&w1))
    return;    // no parameters so bail.

  if (!FGetNextCmdNum(&w2))
    return;    // no parameters so bail.

  Serial.print("Set Servo ID From: ");
  Serial.print(w1, DEC);
  Serial.print(" To: ");
  Serial.println(w2, DEC);

  int retval;
  uint8_t error;
  dynamixel::PortHandler *portHandler = portHandlers[g_servo_protocol[w1].b.port];
  if (g_servo_protocol[w1].b.protocol == SERVO_PROTOCOL1) {
    retval = packetHandler1->write1ByteTxRx(portHandler, w1, AX_ID, w2, &error);
  } else {
    retval = packetHandler2->write1ByteTxRx(portHandler, w1, DXL_X_ID, w2, &error);
  }
  ReportAnyErrors("Set ID", w1, retval, error);
}

//=======================================================================================
void GetServoPositions(void) {

  unsigned long ulBefore;
  unsigned long ulDelta;
  uint16_t w;
  uint32_t pos;
  uint8_t err;
  int retval;

  if (!g_count_servos_found) {
    Serial.println("Previous Find Servos failed to locate any servos: so retry");
    FindServos();
    return;
  }

  for (int i = 0; i < 255; i++) {
    if (!g_servo_protocol[i].val) continue;  // No servo found on that index
    dynamixel::PortHandler *portHandler = portHandlers[g_servo_protocol[i].b.port];
    Serial.print(i, DEC);
    Serial.print(":");
    ulBefore = micros();
    uint8_t delay_time_reg;
    if (g_servo_protocol[i].b.protocol == SERVO_PROTOCOL1) {
      retval = packetHandler1->read2ByteTxRx(portHandler, i, AX_PRESENT_POSITION_L, &w, &err);
      delay_time_reg = AX_RETURN_DELAY_TIME;
      pos = w;
    } else {
      retval = packetHandler2->read4ByteTxRx(portHandler, i, DXL_X_PRESENT_POSITION, &pos, &err);
      delay_time_reg = DXL_X_RETURN_DELAY_TIME;
    }
    if (!ReportAnyErrors("Current Pos", i, retval, err)) {
      ulDelta = micros() - ulBefore;
      Serial.print(pos, DEC);
      Serial.print(" ");
      Serial.print(ulDelta, DEC);
      Serial.print(" ");
      Serial.println(getServoByte(i, delay_time_reg), DEC);
    } else {
      ulDelta = micros() - ulBefore;
      Serial.print("** Failed(" );
      Serial.print(err, DEC);
      Serial.println(")" );
    }
  }
}

//=======================================================================================

void FindServos(void) {

  g_count_servos_found = 0;
  uint16_t w;
  Serial.println("\nSearch for all servos");

  // Initialize to no servos...
  for (int i = 0; i < 254; i++) {
    g_servo_protocol[i].val = SERVO_NOT_FOUND;
  }

  for (uint8_t port_index = 0; port_index < COUNT_PORTHANDLERS; port_index++) {
    dynamixel::PortHandler *portHandler = portHandlers[port_index];
    Serial.print("Begin Searching on Port: ");
    //Serial.println(port_handler_numbers[port_index], DEC);
    Serial.println(portHandler->getPortName());

    Serial.print("  Begin Protocol 1: ");
    Serial.println(packetHandler1->getProtocolVersion());
    for (int i = 1; i < 254; i++) {
      Serial.print(".");
      if ((i & 0x3f) == 0) Serial.println();
      if (packetHandler1->read2ByteTxRx(portHandler, i, AX_PRESENT_POSITION_L, &w) == COMM_SUCCESS) {
        if (g_servo_protocol[i].val) {
          Serial.println("Multiple servos found with same ID");
        } else {
          g_count_servos_found++;
        }
        g_servo_protocol[i].b.protocol = SERVO_PROTOCOL1;
        g_servo_protocol[i].b.port = port_index;
        g_count_servos_found++;
        Serial.print("    ");
        Serial.print(i, DEC);
        Serial.print(" - ");
        Serial.println(w, DEC);
      }
    }

    Serial.println("  Done");
    Serial.print("  Begin Protocol 2: ");
    Serial.println(packetHandler2->getProtocolVersion());
    for (int i = 1; i < 254; i++) {
      uint16_t model_number;
      uint32_t position;
      if (packetHandler2->ping(portHandler, i, &model_number) == COMM_SUCCESS) {
        if (g_servo_protocol[i].val) {
          Serial.println("Multiple servos found with same ID");
        } else {
          g_count_servos_found++;
        }
        g_servo_protocol[i].b.protocol = SERVO_PROTOCOL2;
        g_servo_protocol[i].b.port = port_index;
        Serial.print("    ");
        Serial.print(i, DEC);
        Serial.print(", Model:");
        Serial.print(model_number, HEX);
        packetHandler2->read4ByteTxRx(portHandler, i, DXL_X_PRESENT_POSITION, &position);
        Serial.print(i, DEC);
        Serial.print(" - ");
        Serial.println(position, DEC);
      }
    }
    Serial.println("  Done");
  }
}

//=======================================================================================
int g_asPositionsPrev[255];
int g_asMins[255];
int g_asMaxs[255];

void TrackServos(boolean fInit) {

  uint16_t w;
  uint32_t dw;
  int pos;
  bool fSomethingChanged = false;
  for (int i = 0; i < 254; i++) {
    if (!g_servo_protocol[i].val) continue;
    dynamixel::PortHandler *portHandler = portHandlers[g_servo_protocol[i].b.port];
    pos = 0xffff;
    if (g_servo_protocol[i].b.protocol == SERVO_PROTOCOL1) {
      if (packetHandler1->read2ByteTxRx(portHandler, i, AX_PRESENT_POSITION_L, &w) == COMM_SUCCESS)
        pos = w;
    } else {
      if (packetHandler2->read4ByteTxRx(portHandler, i, DXL_X_PRESENT_POSITION, &dw) == COMM_SUCCESS)
        pos = dw;
    }
    if (fInit) {
      g_asMins[i] = pos;
      g_asMaxs[i] = pos;
    }
    if (pos != g_asPositionsPrev[i]) {
      if (!fInit) {
        // only print if we moved more than some deltas
        if (abs(w - g_asPositionsPrev[i]) > 3) {
          Serial.print(IKPinsNames[i]);
          Serial.print("(");
          Serial.print((byte)pgm_axdIDs[i], DEC);
          Serial.print("):");
          Serial.print(pos, DEC);
          /*          Serial.print("(");
                    Serial.print((((long)(w - 512)) * 375L) / 128L, DEC);
                    Serial.print(") "); */
          Serial.print(" ");
          fSomethingChanged = true;
        }
      }
      g_asPositionsPrev[i] = pos;
      if (g_asMins[i] > pos)
        g_asMins[i] = pos;

      if (g_asMaxs[i] < pos)
        g_asMaxs[i] = pos;
    }
  }
  if (fSomethingChanged)
    Serial.println();
}

void TrackPrintMinsMaxs(void) {
  for (int i = 0; i < 254; i++) {
    if (!g_servo_protocol[i].val) continue;
    Serial.print(i, DEC);
    Serial.print(":");
    Serial.print(g_asMins[i], DEC);
    Serial.print(" - ");
    Serial.println(g_asMaxs[i], DEC);
  }
}

//=======================================================================================
void PrintServoValues(void) {

  word wID;
  word w;
  word w_reg_count;
#ifdef DEBUG_IO_PINS
  pinMode(A2, OUTPUT);
  pinMode(A3, OUTPUT);
#endif
  if (!FGetNextCmdNum(&wID))
    return;
  if (!FGetNextCmdNum(&w_reg_count))
    w_reg_count = 50;
  for (int i = 0; i < w_reg_count; i++) {
    Serial.print(i, DEC);
    Serial.print(":");
#ifdef DEBUG_IO_PINS
    digitalWrite(A2, HIGH);
#endif
    w = getServoByte(wID, i);
#ifdef DEBUG_IO_PINS
    digitalWrite(A2, LOW);
    if (w == (word) - 1)
      digitalWrite(A3, !digitalRead(A3));
#endif
    Serial.print(w, HEX);
    Serial.print(" ");
    if ((i % 10) == 9)
      Serial.println("");
    Serial.flush();  // try to avoid any interrupts while processing.
    // delay(5);
  }
}

//=======================================================================================
void WriteServoValues() {
  word wID;
  word wReg;
  word wVal;
  uint8_t error;
  int retval;
  if (!FGetNextCmdNum(&wID))
    return;    // no parameters so bail.

  if (!IsValidServo(wID)) {
    Serial.print("Write register ID: ");
    Serial.print(wID, DEC);
    Serial.println(" Servo not found");
    return;
  }
  dynamixel::PortHandler *portHandler = portHandlers[g_servo_protocol[wID].b.port];

  if (!FGetNextCmdNum(&wReg))
    return;    // no parameters so bail.

  while (FGetNextCmdNum(&wVal)) {
    Serial.print("Write register ID: ");
    Serial.print(wID, DEC);
    Serial.print(" Reg: ");
    Serial.print(wReg, DEC);
    Serial.print(" Val: ");
    Serial.print(wVal, DEC);
    if (g_servo_protocol[wID].b.protocol == SERVO_PROTOCOL1) {
      retval = packetHandler1->write1ByteTxRx(portHandler, wID, wReg, wVal, &error);
    } else {
      retval = packetHandler2->write1ByteTxRx(portHandler, wID, wReg, wVal, &error);
    }
    if (!ReportAnyErrors(" Write Reg", wID, retval, error)) {
      Serial.println(" Success");
    } else {
      Serial.println();
    }
    wReg++;   // get to the next reg
  }
}

//=======================================================================================
uint8_t getServoByte(uint8_t id, uint8_t reg) {
  uint8_t val;
  uint8_t dxl_error = 0;                          // Dynamixel error
  int dxl_comm_result;

  if (!IsValidServo(id)) {
    Serial.println("GSB not valid servo");
    return 0xff;
  }
  dynamixel::PortHandler *portHandler = portHandlers[g_servo_protocol[id].b.port];

  if (g_servo_protocol[id].b.protocol == SERVO_PROTOCOL1) {
    dxl_comm_result = packetHandler1->read1ByteTxRx(portHandler, id, reg, &val, &dxl_error);
  } else {
    dxl_comm_result = packetHandler2->read1ByteTxRx(portHandler, id, reg, &val, &dxl_error);
  }
  return (dxl_comm_result == COMM_SUCCESS) ? val : 0xff;
}

//=======================================================================================
void SetBaudRate()
{
  word wBaud;

  if (!FGetNextCmdNum(&wBaud))
    return;    // no parameters so bail.
  Serial.print("Setting Baud to: ");
  Serial.println(wBaud);

  for (uint8_t i = 0; i < COUNT_PORTHANDLERS; i++) {
    if (!portHandlers[i]->setBaudRate(wBaud)) {
      Serial.print("Failed to change the Port ");
      Serial.print(port_handler_numbers[i], DEC);
      Serial.print("Baud to: ");
      Serial.println(wBaud, DEC);
    }
  }

  Serial.println("Doing new Servo Scan");
  FindServos();
}

//=================================================================================
// Lets initialize our memory usage code, to get an idea of how much has been
// used
register uint8_t * stack_ptr asm("sp");
extern char end asm("end");

uint32_t g_end_stack_pointer;
uint32_t g_start_heap_pointer;

void initMemoryUsageTest()
{
  // Guess on start of stack. // probably using less than 100 bytes of stack space...
  g_end_stack_pointer = ((uint32_t)stack_ptr + 100) & 0xfffff000;

  // get the start of the heap ponter
  g_start_heap_pointer = (uint32_t)&end;

  // Print out some memory information
  Serial.printf("starting Heap info: start: %x current: %x\n", g_start_heap_pointer, (uint32_t)_sbrk(0));
  Serial.printf("Start Stack info: end: %x current: %x\n", g_end_stack_pointer, (uint32_t)stack_ptr);
  Serial.println("Try to init memory");
  Serial.flush(); // make sure it has chance to write out.
  uint8_t *sp_minus = stack_ptr - 10;  // leave a little slop
  for (uint8_t *p = (uint8_t*)_sbrk(0); p < sp_minus; p++) *p = 0xff; // init to ff
  Serial.println("After init memory");
}

//=================================================================================
void printMemoryUsage()
{
  uint8_t *current_heap_ptr = (uint8_t*)_sbrk(0);
  Serial.printf("Heap ptr: %x  Usage: %d\n", (uint32_t)current_heap_ptr,
                 (uint32_t)current_heap_ptr - g_start_heap_pointer);

  // stack info
  uint8_t *sp_minus = stack_ptr - 10;  // leave a little slop
  uint8_t *p = current_heap_ptr;

  // try to find out how far the stack has been used
  while ((p < sp_minus) && (*p == 0xff)) p++;
  Serial.printf("Stack Max: %x, usage: %d\n", p, g_end_stack_pointer - (uint32_t)p);
  Serial.printf("Estimated unused memory: %d\n", (uint32_t)(p - current_heap_ptr));
}

There is an Init function and a print memory usage function that I am trying out, which tries to init memory to 0xff and then scans looking for first place not 0xff:

Run with current develop code (After last nights merge)...


CM9.04 Servo Test program
starting Heap info: start: 20003458 current: 20003694
Start Stack info: end: 20005000 current: 20004fc8
Try to init memory
After init memory
Get Packet Handlers
Get Port Handler 1 20003698
    Call Open Port
    Set Baud Rate: 1000000
Get Port Handler 3 20003738
    Call Open Port
    Set Baud Rate: 1000000

Search for all servos
Begin Searching on Port: 1
  Begin Protocol 1: 1.00
................................................................
................................................................
................................................................
.............................................................  Done
  Begin Protocol 2: 2.00
  Done
Begin Searching on Port: 3
  Begin Protocol 1: 1.00
................................................................
................................................................
................................................................
.............................................................  Done
  Begin Protocol 2: 2.00
    1, Model:4241 - 2047
    2, Model:4242 - 2998
    3, Model:4243 - 2048
  Done
PrintServo Voltage called
Servo: 1 Voltage in 10ths: 121
PrintServo Voltage called
Heap ptr: 200040e0  Usage: 3208
Stack Max: 20004698, usage: 2408
Estimated unused memory: 1464
0 - All Servos off
1 - All Servos center
2 - Set Servo position [<Servo>] <Position> [<Speed>]
3 - Set Servo Angle
4 - Get Servo Positions
5 - Find All Servos
6 - Set Servo return delay time
8 - Set ID: <old> <new>
9 - Print Servo Values
b - Baud <new baud>
t - Toggle track Servos
h - hold [<sn>]
f - free [<sn>]
r - Reboot [<sn>]
w - write <servo> <reg> <val> (<val2>...)

:
Cmd: h
PrintServo Voltage called
Heap ptr: 200040e0  Usage: 3208
Stack Max: 20004698, usage: 2408
Estimated unused memory: 1464
0 - All Servos off
...

Now if you run it with my updated Dynamixel SDK code, which I was talking about on different thread.
I extracted the code and then created new branch with the changes: https://github.com/KurtE/OpenCM9.04/tree/dynamixel_sdk_update_memory_reduction

Then run similar run, where I just enter my command: H which does a write to enable torque. The memory output with updated code...


:
Cmd: h
PrintServo Voltage called
Heap ptr: 200037d4  Usage: 892
Stack Max: 20004c10, usage: 1008
Estimated unused memory: 5180

Let me know what you think... Still probably needs some more sophistication, but maybe at least gives some hints...

OpusK commented 5 years ago

@KurtE ,

void printMemoryUsage()
{
  uint8_t *current_heap_ptr = (uint8_t*)_sbrk(0);
  Serial.printf("Heap ptr: %x  Usage: %d\n", (uint32_t)current_heap_ptr,
                 (uint32_t)current_heap_ptr - g_start_heap_pointer);

  // stack info
  uint8_t *sp_minus = stack_ptr - 10;  // leave a little slop
  uint8_t *p = current_heap_ptr;

  // try to find out how far the stack has been used
  while ((p < sp_minus) && (*p == 0xff)) p++;
  Serial.printf("Stack Max: %x, usage: %d\n", p, g_end_stack_pointer - (uint32_t)p);
  Serial.printf("Estimated unused memory: %d\n", (uint32_t)(p - current_heap_ptr));
}

The function you created seems to be very useful. Looking at your issue, we have planned to add a function that will return the current memory usage to OpenCR and OpenCM. It's probably similar to the way you used it.

Heap ptr: 200037d4 Usage: 892 Stack Max: 20004c10, usage: 1008 Estimated unused memory: 5180

I think this result is very meaningful. However, with respect to the SDK, @kijongGil's opinion is important, so we will need to wait until he comes back and confirms.

There seems to be quite different memory management methods in environments with a lot of resources and very few resources. However, if the API is not changed, it will not affect existing users, so it will be possible to optimize the internal code (even if it is divided into define processing)

KurtE commented 5 years ago

Hi @OpusK,

Thanks, With this stuff, I think it would also be good to look at how much memory we are using in global variables as well. So will try to add another print in memory test functions.

Example if I compile the example b_Blink_LED it says Global variables use 9700 bytes of dynamic memory (almost half). Question is can we cut that down more? Example Serial1, Serial2, Serial3 all have static buffers for TX, RX of 128 bytes so maybe 768 bytes there... I know on the Teensy, the code was setup such that each SerialX object was in it's own source file, such that only if the user used something of that Serial object, would it be included by the linker. But I know there are tricks that need to be done to make it work, as the SerialEvent code would bring it in.... ...

As a comparison, if I compile blink on a Teensy LC global variables take up 2048 out of 8k (M0 processor) or if I comple for the T3.2 it takes up 3436 out of 64K (M3 processor).

As for my test case in previous post. Yes, I totally understand this is more for @kijongGil and part of my other issue, that I put up against the library. https://github.com/ROBOTIS-GIT/DynamixelSDK/issues/258

I did not change any of the APIs in this comparison. What I did do was to again change the underlying code, such that if I did things like read or write 1, 2, 4 bytes, I did not malloc the TX buffer, but instead built it on the stack (Passed in pointer to buffer to use). I know from the sizes that for 1, 2 bytes no need for space for stuffing, for 4 bytes, I added an extra byte to buffer in case of stuffing.

Currently I have separate code that does the stuffing here, while it is directly outputting the data to the buffer. Now that I have a more optimal stuffing function, may simply want to call it. However in those cases, I would like it to not realloc buffer, but know that there was enough space allocated in the first place. Also wondering if it would be too slimy to have the addStuffing call not call realloc if the memory did not come from heap... That is if it is in the range: &end and _sbrk(0)

Again more for other issue. But again would like to get rid of realloc altogether as it is playing with fire. Example: suppose you call malloc(32) bytes and you get txBuffer pointer, and then later call realloc(38), and some other things were allocated in the space right after where txBuffer was. realloc will allocate memory in different location in heap and copy your 32 bytes into it, and then return the new pointer to you AND release the old memory buffer. But then you return from this call, and the caller continues to use the OLD location and tries to write that data out, and then it frees the old memory, which typically will cause the heap to be corrupted. But again more for the other issue.

KurtE commented 5 years ago

@OpusK,

Not sure who in this list, but thought I would do a quick test to get an idea of where all of the space in the data section is... So I again compiled the example blink. This time I used the default Arduino one, only edited to change which pin number. This one does not output text to Serial.

The compiler still said:

Global variables use 9700 bytes of dynamic memory.

So I found the objdump program, I then edited the file, extracted the areas above and below data section, used grep to find lines with < then edited again removed a few bad lines, changed space to , and imported into excel, where I then used function to convert hex values to decimal, and then subtract the line below from the line, to get an idea of which items are taking space... sort of like:

C:\Users\kurte\AppData\Local\Arduino15\packages\OpenCR\tools\opencr_gcc\5.4.0-2016q2\arm-none-eabi\bin\objdump.exe -D Blink.ino.elf > foo
subl foo 
grep "<" foo > data_sections.csv
subl data_sections.csv

Then to show the data here, I exported again to CSV file, changed the ,'s into | and ... Here is the data...

Address	Name	Address DEC	Size
20000000	_sdata	536870912	4
20000004	SystemCoreClock	536870916	4
20000008	huart_inst	536870920	12
20000014	USBD_CDC_CfgFSDesc	536870932	68
20000058	USBD_CDC	536871000	56
20000090	USBD_CDC_DeviceQualifierDesc	536871056	12
2000009c	USBD_CDC_OtherSpeedCfgDesc	536871068	68
200000e0	USBD_CDC_CfgHSDesc	536871136	68
20000124	JUMP_BOOT_STR	536871204	4
20000128	h_cdc_tx_timer	536871208	4
2000012c	LineCoding	536871212	8
20000134	USBD_CDC_fops	536871220	16
20000144	VCP_Desc	536871236	28
20000160	USBD_StringSerial	536871264	28
2000017c	impure_data	536871292	96
200001dc	_impure_ptr	536871388	4
200001e0	lconv	536871392	56
20000218	_sbss	536871448	4
2000021c	object.8541	536871452	24
20000234	heap_end.5918	536871476	4
20000238	DataVar	536871480	2
2000023a	IsInit	536871482	2
2000023c	VirtAddVarTab	536871484	1024
2000063c	I2cHandle	536872508	120
200006b4	is_dxl_port	536872628	4
200006b8	is_uart_mode	536872632	4
200006bc	drv_uart_rx_buf_tail	536872636	12
200006c8	drv_uart_num	536872648	4
200006cc	drv_uart_rx_buf	536872652	768
200009cc	sw_timer_mcros	536873420	4
200009d0	swtimer_tbl	536873424	160
20000a70	excute.8929	536873584	2
20000a72	sw_timer_handle_index	536873586	2
20000a74	sw_timer_counter	536873588	4
20000a78	IwdgHandle	536873592	16
20000a88	hw_pin_config	536873608	30
20000aa6	ifalt.8894	536873638	1
20000aa7	is_opened	536873639	1
20000aa8	rxd_length	536873640	4
20000aac	rxd_BufPtrIn	536873644	4
20000ab0	UserTxBufPtrIn	536873648	4
20000ab4	usb_cdc_bitrate	536873652	4
20000ab8	rxd_BufPtrOut	536873656	4
20000abc	is_tx_full	536873660	1
20000abd	is_reopen	536873661	3
20000ac0	UserTxBufPtrOut	536873664	4
20000ac4	CDC_Reset_Status	536873668	4
20000ac8	remotewakeupon	536873672	4
20000acc	cfgidx.8899	536873676	4
20000ad0	uwTick	536873680	4
20000ad4	serial3_tx_buffer	536873684	128
20000b54	serial1_tx_buffer	536873812	128
20000bd4	Serial	536873940	36
20000bf8	Serial1	536873976	48
20000c28	Serial2	536874024	48
20000c58	Serial3	536874072	48
20000c88	serial2_tx_buffer	536874120	128
20000d08	__malloc_sbrk_start	536874248	4
20000d0c	__malloc_free_list	536874252	4
20000d10	USBD_Device	536874256	548
20000f34	hADC1	536874804	48
20000f64	exti_data	536874852	312
2000109c	hOC2	536875164	112
2000110c	hTIM4	536875276	60
20001148	hTIM2	536875336	60
20001184	hOC4	536875396	112
200011f4	hOC1	536875508	112
20001264	pwm_init	536875620	32
20001284	hTIM1	536875652	60
200012c0	hTIM3	536875712	60
200012fc	pwm_period	536875772	120
20001374	hOC3	536875892	112
200013e4	pwm_freq	536876004	120
2000145c	hspi2	536876124	88
200014b4	spi_dma	536876212	32
200014d4	hspi1	536876244	88
2000152c	hDrvTim	536876332	352
2000168c	hdma_rx	536876684	168
20001734	huart	536876852	192
200017f4	hdma_tx	536877044	168
2000189c	rxd_buffer	536877212	512
20001a9c	BuffLength	536877724	4
20001aa0	UserTxBufferForUSB	536877728	512
20001ca0	UserTxBuffer	536878240	512
20001ea0	UserRxBuffer	536878752	512
200020a0	hpcd	536879264	1056
200024c0	USBD_StrDesc	536880320	256
200025c0	pFlash	536880576	32
200025e0	errno	536880608	4
200025e4	end-0x4	536880612	-536880612

KurtE commented 5 years ago

Follow on to above - I start to look for some low hanging fruit, especially larger entries, like: VirtAddVarTab (1024 bytes) Which is used only in hw\drivers\drv_eeprom.c
Defined as: static uint16_t VirtAddVarTab[NB_OF_VAR] where NB_OF_VAR is 512. Only thing that writes to this is:

int drv_eeprom_init()
{
  uint16_t i;

  for( i=0; i<NB_OF_VAR; i++ )
  {
    VirtAddVarTab[i] = i;
  }
...

So it simply is init as 0,1,2... 0x1ff

Two solutions. Could change it like:

static const uint16_t VirtAddVarTab[NB_OF_VAR] = 
{
  0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
  0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f,
  0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f,
...  
  0x1e0, 0x1e1, 0x1e2, 0x1e3, 0x1e4, 0x1e5, 0x1e6, 0x1e7, 0x1e8, 0x1e9, 0x1ea, 0x1eb, 0x1ec, 0x1ed, 0x1ee, 0x1ef,
  0x1f0, 0x1f1, 0x1f2, 0x1f3, 0x1f4, 0x1f5, 0x1f6, 0x1f7, 0x1f8, 0x1f9, 0x1fa, 0x1fb, 0x1fc, 0x1fd, 0x1fe, 0x1ff
};

Which I tried (#ifdef the for loop I showed above). Compiles fine and sure enough data usage drops by 1024 bytes, but code size increases by the static data.

Option 2: Could define something like:

#define VIRTADDVARTAB(index)   (index)

Could make this conditional. where as an option you could still define the variable and then define it as: #define VIRTADDVARTAB(index) VirtAddVarTab[index]

Then change the 9 places in this file, like:

          if (( *(__IO uint16_t*)(PAGE0_BASE_ADDRESS + 6)) == VirtAddVarTab[varidx])

to

          if (( *(__IO uint16_t*)(PAGE0_BASE_ADDRESS + 6)) == VIRTADDVARTAB(varidx))

This would remove the data and probably not grow the code... Maybe even shrink it...

Looks like USB is using at least 3.5K or memory.

Each Serial1/2/3 uses at least 256 byte RX buffer 128 byte TX buffer, 48 byte object... Wonder to they all need to be that large. Example on Teensy, the defaults for Serial objects Serial1(TX=64, RX=64), Serial2(40, 64), Serial3(40, 64), so for example if we reduce their sizes to 64 and 64, again we save (256*3) data bytes...

Of course there are more radical changes, where hopefully you can setup the code, such that if a sketch does not use any of some main feature, than the code and data objects associated with it are not brought into the binary. Probably more than I want to chew ;)

OpusK commented 5 years ago

Changing the buffer size or building only the necessary Serials (including SerialEvent processing) can affect existing examples. However, this is a policy issue, so internal discussion is necessary.

First, regarding the buffer size, I think we can reduce the size if there is no problem with DXL communication.

Second, as far as I can see, Arduino's official boards have created all of their Serial classes globally, so we seem to have done so too. If we need this, it will be enough to add developer options.

int drv_eeprom_init()

In this regard, I agree. Perhaps the first designer wanted to create an API that could scale the EEPROM.

KurtE commented 5 years ago

As you mentioned, having Serial1, 2, 3 optionally is a little more work and can be a bit tricky.

The issue is how to do it without impacting user programs. With a build, where all of the compiled objects are put into a library (archive). During the link only those objects which are referenced are included in the binary. That is if anything within that compiled unit is included...

So since Serial1, Serial2 and Serial3 as well as their buffers is included in variant.cpp and obviously there are things referenced in this file, all of these objects are defined.

So for example on Teensy builds, Each of the SerialX objects are defined in their own source file. Note: On teensy, actually there is underlying C code for each of these object so there is Serial1.c Serial2.c... and later there are wrapper classes, each of these wrappers are again in their own source file. But that is besides the point.

So for example if nothing calls anything within Serial2, than in theory the Serial2 object and currently it's TX buffer... The RX buffer is a 2 dimensional array as part of lower object... But this could change as well.

However there is the complication of the function:

void serialEventRun(void)
{
  if (Serial.available()) serialEvent();
  if (Serial1.available()) serialEvent1();
  if (Serial2.available()) serialEvent2();
  if (Serial3.available()) serialEvent3();
}

Which calls all of these... And brings in all of the objects...

So there are some hacks that you could do like:

HardwareSerial *SerialObjects[4] = {NULL, NULL, NULL, NULL}; 

void serialEventRun(void)
{
  if (SerialObjects[0] && SerialObjects[0]->available()) serialEvent();
  if (SerialObjects[1] && SerialObjects[1]->available()) serialEvent1();
  if (SerialObjects[2] && SerialObjects[2]->available()) serialEvent2();
  if (SerialObjects[3] && SerialObjects[3]->available()) serialEvent3();
}

Actually in this case would probably always init the [0] to Serial object, or not make that change on that line. For the others than you might change the functions like Serial1.begin(), to then update this object list to point to it's object...

Again can be tricky. Especially if you also want to try to get all of the internal objects DRV...

OpusK commented 5 years ago

@KurtE ,

Thank you for your good suggestion. With serialEvent() it will be a solution, but it will be difficult to apply because there are Serial1,2,3 classes that are used in existing TB3 and other examples.

However, it is a good idea to reduce the default buffer size of the Serial class (about 64 bytes) Of course, the DXL port still needs a large buffer, so this should be fixed. Also, in the case of the LDS example, a large buffer is required because the amount of LDS data is large. To solve this problem, we can add an API that can change the pointer to a buffer created by the user.

For EEPROM, it would be better to change it to const.

What do you think?

OpusK commented 5 years ago

This issue has been closed as there weren't recent activities. Please feel free to reopen this thread if there's any opinion to throw. Thanks.

KurtE commented 5 years ago

@OpusK - makes sense to close this one.

This can be an ongoing type of thing, that every so often someone should take a look at the sizes of things that are part of the system and see if there are any low hanging fruit.

Examples of Serial buffers and the like. With only 3 serial objects, maybe not as critical, but for example some Teensy boards have up to 8 serial devices... An in their case for the T4, I did do some stuff similar to mentioned above. The SerialX objects each go into their own source file, along with their own buffers and their own default SerialEventX function.

The main loop code that did stuff like if (Serial2.available()) SerialEvent2(); code instead ran off a function pointer array, and if slot(2) had a function pointer would it get called... And the default SerialEvent2 object removes itself from the list... SerialX.begin() adds itself to the list. So only if someone calls Serial2.begin() will the code for Serial2.available() be called and if the user did not provide their own copy of SerialEvent2 to overwrite the weak version, then the first time some character comes in the code to call this, it will disable this checking... Not perfect, as someone could do a Serial2.begin(), and maybe actually never uses Serial2, so the main loop will continue to call Serial2.available()...

OpusK commented 5 years ago

@KurtE

This issue reopened.

Currently, I have other things to deal with, so after I finish them first, I will look back at this issue. Thank you for your contribution :)

KurtE commented 2 months ago

Sort of old and who knows what applies any more so, going to go ahead and close it out

ROBOTIS-GIT / OpenCM9.04

Can we detect how much stack space and heap memory is used by a sketch? #54