accelerometer on stm32f4-discovery, with trace cut / jumper, is sendi…

…ng at 1 khz, hooray

CMakeLists.txt

@@ -14,7 +14,7 @@ endfunction()
 set(SYSTEM "SYSTEM" CACHE STRING "generic-posix")
 message("system: ${SYSTEM}")
 
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -Wall -g -O2")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -Wall -O2")
 
 # set up the toolchain for this system
 include(systems/${SYSTEM}/toolchain.cmake)

README.md

@@ -3,6 +3,10 @@ a free, portable, minimalist RTPS implementation
 
 extremely high awesome factor
 
+## WARNING WARNING WARNING
+This code is extremely experimental. It is an abomination. Approximately everything is expected to change rapidly as features are added and cleaned up.
+## WARNING WARNING WARNING
+
 To build it, just type "make" and it will spin CMake up a few times to build freertps for a few different systems.
 
 Then, to try it out on the STM32F7-Discovery board, you can do this (providing that you have the latest source (not release) of OpenOCD, and have cherry-picked a few patches floating around the mailing list to add support for STM32F7:

apps/imu/imu.c

@@ -43,22 +43,22 @@ typedef struct
 
 void timer_cb()
 {
-  float xyz[3];
+  static float xyz[3];
   if (!imu_poll_accels(xyz))
   {
     printf("woah! couldn't poll the imu!\r\n");
     return;
   }
-  printf("imu: [%+8.3f, %+8.3f, %+8.3f]\r\n",
-         xyz[0], xyz[1], xyz[2]);
+  //printf("imu: [%+8.3f, %+8.3f, %+8.3f]\r\n",
+  //       xyz[0], xyz[1], xyz[2]);
   if (!g_pub)
     return;
   static char __attribute__((aligned(4))) msg[1024] = {0};
   std_interfaces__header_t *header = (std_interfaces__header_t *)msg;
   header->stamp.sec = 1234;
   header->stamp.nanosec = 5678;
   static const char *frame_id = "imu_frame12";
-  header->frame_id_len_ = strlen(frame_id);
+  header->frame_id_len_ = strlen(frame_id) + 1;
   memcpy(header->frame_id, frame_id, header->frame_id_len_);
   static sensor_interfaces__imu_t imu;
   static int pub_count = 0;
@@ -70,20 +70,20 @@ void timer_cb()
   imu.angular_velocity.x = 5;
   imu.angular_velocity.y = 6;
   imu.angular_velocity.z = 7;
-  imu.linear_acceleration.x = 8;
-  imu.linear_acceleration.x = 9;
-  imu.linear_acceleration.x = 10;
+  imu.linear_acceleration.x = xyz[0];
+  imu.linear_acceleration.y = xyz[1];
+  imu.linear_acceleration.z = xyz[2];
   for (int i = 0; i < 9; i++)
   {
     imu.orientation_covariance[i] = 11 + i;
     imu.angular_velocity_covariance[i] = 20 + i;
     imu.linear_acceleration_covariance[i] = 29 + i;
   }
-  uint8_t *wpos = (uint8_t *)(header->frame_id) + header->frame_id_len_ + 1;
+  uint8_t *wpos = (uint8_t *)(header->frame_id) + header->frame_id_len_; // + 1;
   memcpy(wpos, &imu, sizeof(imu));
   uint32_t header_len = (wpos - (uint8_t *)msg);
   uint32_t cdr_len = sizeof(imu) + header_len;
-  printf("cdr len = %d header_len = %d\n", cdr_len, header_len);
+  //printf("cdr len = %d header_len = %d\n", (int)cdr_len, (int)header_len);
   freertps_publish(g_pub, (uint8_t *)msg, cdr_len);
   /*
   static char __attribute__((aligned(4))) msg[256] = {0};
@@ -98,7 +98,7 @@ void timer_cb()
 int main(int argc, char **argv)
 {
   imu_init();
-  freertps_timer_set_freq(1, timer_cb);
+  freertps_timer_set_freq(1000, timer_cb);
 
   printf("hello, world!\r\n");
   freertps_system_init();

systems/stm32f4_disco-metal/imu.c

@@ -92,21 +92,23 @@ static void accel_txrx(const uint8_t start_reg,
 
 bool imu_poll_accels(float *xyz)
 {
-  printf("\n\nimu_poll_accels\r\n");
   uint8_t raw_read[6];
   accel_txrx(0x28, 6, raw_read, NULL);
-  printf("raw read: %02x%02x  %02x%02x  %02x%02x\r\n",
-         raw_read[0], raw_read[1],
-         raw_read[2], raw_read[3],
-         raw_read[4], raw_read[5]);
   int16_t raw_accel[3];
   for (int i = 0; i < 3; i++)
   {
     raw_accel[i] = raw_read[i*2] | (raw_read[i*2+1] << 8);
     xyz[i] = raw_accel[i] / 16384.0f;
   }
+#ifdef VERBOSE_ACCEL
+  printf("\n\nimu_poll_accels\r\n");
+  printf("raw read: %02x%02x  %02x%02x  %02x%02x\r\n",
+         raw_read[0], raw_read[1],
+         raw_read[2], raw_read[3],
+         raw_read[4], raw_read[5]);
   printf("raw accel: [%d %d %d]\n",
          raw_accel[0], raw_accel[1], raw_accel[2]);
+#endif
   return true;
 }
 

tests/fake_imu/imu_listener.cpp

@@ -4,12 +4,17 @@
 
 void imu_cb(const sensor_interfaces::msg::Imu::ConstSharedPtr &msg)
 {
-  printf("imu rx\n");
-  printf("  orientation: [ %.3f %.3f %.3f %.3f ]\n",
+  printf(" accel: [%+6.3f %+6.3f %+6.3f]\n",
+         msg->linear_acceleration.x,
+         msg->linear_acceleration.y,
+         msg->linear_acceleration.z);
+  /*
+  printf("  orient: [ %.3f %.3f %.3f %.3f ]\n",
          msg->orientation.x,
          msg->orientation.y,
          msg->orientation.z,
          msg->orientation.w);
+  */
 }
 
 int main(int argc, char **argv)