AMiRo-OS

#include <cstdint>

#define BL_CALLBACK_TABLE_ADDR  (0x08000000 + 0x01C0)

#define BL_MAGIC_NUMBER         ((uint32_t)0xFF669900u)

#define SHUTDOWN_NONE             0

#define SHUTDOWN_TRANSPORTATION   1

#define SHUTDOWN_DEEPSLEEP        2

#define SHUTDOWN_HIBERNATE        3

#define SHUTDOWN_RESTART          4

#define SHUTDOWN_HANDLE_REQUEST   5

#include <ch.hpp>

#include <amiro/util/util.h>

#include <global.hpp>

#include <exti.hpp>

#include <chprintf.h>

#include <shell.h>

#include "linefollow.hpp"

#include "amiro_map.hpp"

using namespace chibios_rt;

Global global;

struct blVersion_t {

  const uint8_t identifier;

  const uint8_t major;

  const uint8_t minor;

  const uint8_t patch;

} __attribute__((packed));

void systemShutdown() {

  types::kinematic k;

  uint8_t i;

//  // make sure we assert SYS_PD_N to delay shutdown until we're done.

//  boardRequestShutdown();

    // stop the user thread

  global.userThread.requestTerminate();

  global.userThread.wait();

  k.x = 0x00u;

  k.w_z = 0x00u;

  // stop wheels

  global.robot.setTargetSpeed(k);

  global.robot.terminate();

  for (i = 0x00; i < global.vcnl4020.size(); i++) {

    global.vcnl4020[i].requestTerminate();

    global.vcnl4020[i].wait();

  }

  global.ina219.requestTerminate();

  global.ina219.wait();

  global.hmc5883l.requestTerminate();

  global.hmc5883l.wait();

  global.l3g4200d.requestTerminate();

  global.l3g4200d.wait();

  global.motorcontrol.requestTerminate();

  global.motorcontrol.wait();

  global.odometry.requestTerminate();

  global.odometry.wait();

  // stop I²C

  for (i = 0; i < global.V_I2C2.size(); ++i)

    global.V_I2C2[i].stop();

  global.HW_I2C2.stop();

  global.lis331dlh.requestTerminate();

  global.lis331dlh.wait();

  global.lis331dlh.configure(&global.accel_sleep_config);

//  global.lis331dlh.start(NORMALPRIO +4);

//  boardWriteIoPower(0);

//  boardStandby();

  return;

}

//void (*shellcmd_t)(BaseSequentialStream *chp, int argc, char *argv[]);

void shellRequestShutdown(BaseSequentialStream *chp, int argc, char *argv[]) {

  chprintf(chp, "shellRequestShutdown\n");

  /* if nor argument was given, print some help text */

  if (argc == 0 || strcmp(argv[0], "help") == 0) {

    chprintf(chp, "\tUSAGE:\n");

    chprintf(chp, "> shutdown <type>\n");

    chprintf(chp, "\n");

    chprintf(chp, "\ttype\n");

    chprintf(chp, "The type of shutdown to perform.\n");

    chprintf(chp, "Choose one of the following types:\n");

    chprintf(chp, "  transportation - Ultra low-power mode with all wakeups disabled.\n");

    chprintf(chp, "                   The robot can not be charged.\n");

    chprintf(chp, "  deepsleep      - Ultra low-power mode with several wakeups enabled.\n");

    chprintf(chp, "                   The robot can only be charged via the power plug.\n");

    chprintf(chp, "  hibernate      - Medium low-power mode, but with full charging capabilities.\n");

    chprintf(chp, "  restart        - Performs a system restart.\n");

    chprintf(chp, "Alternatively, you can use the shortcuts 't', 'd', 'h', and 'r' respectively.");

    chprintf(chp, "\n");

    return;

  }

  if (strcmp(argv[0],"transportation") == 0 || strcmp(argv[0],"t") == 0) {

    shutdown_now = SHUTDOWN_TRANSPORTATION;

    chprintf(chp, "shutdown to transportation mode initialized\n");

  } else if (strcmp(argv[0],"deepsleep") == 0 || strcmp(argv[0],"d") == 0) {

    shutdown_now = SHUTDOWN_DEEPSLEEP;

    chprintf(chp, "shutdown to deepsleep mode initialized\n");

  } else if (strcmp(argv[0],"hibernate") == 0 || strcmp(argv[0],"h") == 0) {

    shutdown_now = SHUTDOWN_HIBERNATE;

    chprintf(chp, "shutdown to hibernate mode initialized\n");

  } else if (strcmp(argv[0],"restart") == 0 || strcmp(argv[0],"r") == 0) {

    chprintf(chp, "restart initialized\n");

    shutdown_now = SHUTDOWN_RESTART;

  } else {

    chprintf(chp, "ERROR: unknown argument!\n");

    shutdown_now = SHUTDOWN_NONE;

  }

  return;

}

void shellRequestWakeup(BaseSequentialStream *chp, int argc, char *argv[]) {

  int i;

  chprintf(chp, "shellRequestWakeup\n");

  for (i = 0x00u; i < argc; i++)

    chprintf(chp, "%s\n", argv[i]);

  boardWakeup();

}

void shellRequestGetMemoryData(BaseSequentialStream *chp, int argc, char *argv[]) {

  enum Type {HEX, U8, U16, U32, S8, S16, S32};

  chprintf(chp, "shellRequestReadData\n");

  if (argc < 2 || strcmp(argv[0],"help") == 0)

  {

    chprintf(chp, "Usage: %s\n","get_memory_data <type> <start> [<count>]");

    chprintf(chp, "\n");

    chprintf(chp, "\ttype\n");

    chprintf(chp, "The data type as which to interpret the data.\n");

    chprintf(chp, "Choose one of the following types:\n");

    chprintf(chp, "  hex - one byte as hexadecimal value\n");

    chprintf(chp, "  u8  - unsigned integer (8 bit)\n");

    chprintf(chp, "  u16 - unsigned integer (16 bit)\n");

    chprintf(chp, "  u32 - unsigned integer (32 bit)\n");

    chprintf(chp, "  s8  - signed integer (8 bit)\n");

    chprintf(chp, "  s16 - signed integer (16 bit)\n");

    chprintf(chp, "  s32 - signed integer (32 bit)\n");

    chprintf(chp, "\tstart\n");

    chprintf(chp, "The first byte to read from the memory.\n");

    chprintf(chp, "\tcount [default = 1]\n");

    chprintf(chp, "The number of elements to read.\n");

    chprintf(chp, "\n");

    chprintf(chp, "\tNOTE\n");

    chprintf(chp, "Type conversions of this function might fail.\n");

    chprintf(chp, "If so, use type=hex and convert by hand.\n");

    chprintf(chp, "\n");

    return;

  }

  uint8_t type_size = 0;

  Type type = HEX;

  if (strcmp(argv[0],"hex") == 0) {

    type_size = sizeof(unsigned char);

    type = HEX;

  } else if(strcmp(argv[0],"u8") == 0) {

    type_size = sizeof(uint8_t);

    type = U8;

  } else if(strcmp(argv[0],"u16") == 0) {

    type_size = sizeof(uint16_t);

    type = U16;

  } else if(strcmp(argv[0],"u32") == 0) {

    type_size = sizeof(uint32_t);

    type = U32;

  } else if(strcmp(argv[0],"s8") == 0) {

    type_size = sizeof(int8_t);

    type = S8;

  } else if(strcmp(argv[0],"s16") == 0) {

    type_size = sizeof(int16_t);

    type = S16;

  } else if(strcmp(argv[0],"s32") == 0) {

    type_size = sizeof(int32_t);

    type = S32;

  } else {

    chprintf(chp, "First argument invalid. Use 'get_memory_data help' for help.\n");

    return;

  }

  unsigned int start_byte = atoi(argv[1]);

  unsigned int num_elements = 1;

  if (argc >= 3)

    num_elements = atoi(argv[2]);

  const size_t eeprom_size = EEPROM::getsize(&global.at24c01);

  uint8_t buffer[eeprom_size];

  if (start_byte + (type_size * num_elements) > eeprom_size) {

    num_elements = (eeprom_size - start_byte) / type_size;

    chprintf(chp, "Warning: request exceeds eeprom size -> limiting to %u values.\n", num_elements);

  }

  chFileStreamSeek((BaseFileStream*)&global.at24c01, start_byte);

  // Work around, because stm32f1 cannot read a single byte

  if (type_size*num_elements < 2)

    type_size = 2;

  uint32_t bytes_read = chSequentialStreamRead((BaseFileStream*)&global.at24c01, buffer, type_size*num_elements);

  if (bytes_read != type_size*num_elements)

    chprintf(chp, "Warning: %u of %u requested bytes were read.\n", bytes_read, type_size*num_elements);

  for (unsigned int i = 0; i < num_elements; ++i) {

    switch (type) {

      case HEX:

        chprintf(chp, "%02X ", buffer[i]);

        break;

      case U8:

        chprintf(chp, "%03u ", ((uint8_t*)buffer)[i]);

        break;

      case U16:

        chprintf(chp, "%05u ", ((uint16_t*)buffer)[i]);

        break;

      case U32:

        chprintf(chp, "%010u ", ((uint32_t*)buffer)[i]);

        break;

      case S8:

        chprintf(chp, "%+03d ", ((int8_t*)buffer)[i]);

        break;

      case S16:

        chprintf(chp, "%+05d ", ((int16_t*)buffer)[i]);

        break;

      case S32:

        chprintf(chp, "%+010d ", ((int32_t*)buffer)[i]);

        break;

      default:

        break;

    }

  }

  chprintf(chp, "\n");

  return;

}

void shellRequestSetLights(BaseSequentialStream *chp, int argc, char *argv[]) {

  if (argc < 2 || argc == 3 ||strcmp(argv[0],"help") == 0) {

    chprintf(chp, "\tUSAGE:\n");

    chprintf(chp, "> set_lights <led mask> <white/red> [<green> <blue>]\n");

    chprintf(chp, "\n");

    chprintf(chp, "\tled mask\n");

    chprintf(chp, "The LEDs to be set.\n");

    chprintf(chp, "You can set multiple LEDs at once by adding the following values:\n");

    chprintf(chp, "  0x01 - rear left LED (SSW)\n");

    chprintf(chp, "  0x02 - left rear LED (WSW)\n");

    chprintf(chp, "  0x04 - left front LED (WNW)\n");

    chprintf(chp, "  0x08 - front left LED (NNW)\n");

    chprintf(chp, "  0x10 - front right LED (NNE)\n");

    chprintf(chp, "  0x20 - right front LED (ENE)\n");

    chprintf(chp, "  0x40 - right rear LED (ESE)\n");

    chprintf(chp, "  0x80 - rear right LED (SSE)\n");

    chprintf(chp, "\twhite/red\n");

    chprintf(chp, "If no optional argument is given, this arguments sets the white value of the selected LEDs.\n");

    chprintf(chp, "Otherwise this arguments sets the red color channel value.\n");

    chprintf(chp, "\tgreen\n");

    chprintf(chp, "Sets the green color channel value.\n");

    chprintf(chp, "\tblue\n");

    chprintf(chp, "Sets the blue color channel value.\n");

    chprintf(chp, "\n");

    chprintf(chp, "\tExample:\n");

    chprintf(chp, "This line will set the two most left and two most right LEDs to bright cyan.\n");

    chprintf(chp, "> set_lights 0x66 0 255 255\n");

    chprintf(chp, "\n");

    return;

  }

  int arg_mask = strtol(argv[0], NULL, 0);

  int red = strtol(argv[1], NULL, 0);

  int green = red;

  int blue = red;

  if (argc >= 4) {

    green = strtol(argv[2], NULL, 0);

    blue = strtol(argv[3], NULL, 0);

  }

  Color color(red, green, blue);

  if (arg_mask & 0x01) {

    global.robot.setLightColor(constants::LightRing::LED_SSW, color);