Jaime Lozano
Retrieves data from LSM6DSL sensors, filters and send to ROS
Dependencies: LSM6DSL kalman ros_lib_kinetic
Revision 0:1ad0b0cdf28f, committed 2018-03-28
- Comitter:
- Jaimelt
- Date:
- Wed Mar 28 19:42:58 2018 +0000
- Commit message:
- Initial commit
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM6DSL.lib Wed Mar 28 19:42:58 2018 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/ST/code/LSM6DSL/#20ccff7dd652
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/kalman.lib Wed Mar 28 19:42:58 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/cdonate/code/kalman/#8a460b0d6f09
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Mar 28 19:42:58 2018 +0000
@@ -0,0 +1,153 @@
+#include"mbed.h"
+#include <ros.h>
+#include "LSM6DSLSensor.h"
+#include <std_msgs/String.h>
+#include <sensor_msgs/Imu.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Pose.h>
+#include <nav_msgs/Path.h>
+#include <ros/time.h>
+
+
+/* Retrieve the composing elements of the expansion board */
+
+/* Interface definition */
+#ifdef TARGET_DISCO_L475VG_IOT01A
+static DevI2C devI2c(PB_11,PB_10);
+#else // X-Nucleo-IKS01A2 or SensorTile
+#ifdef TARGET_SENSOR_TILE
+#define SPI_TYPE_LPS22HB LPS22HBSensor::SPI3W
+#define SPI_TYPE_LSM6DSL LSM6DSLSensor::SPI3W
+SPI devSPI(PB_15, NC, PB_13); // 3-wires SPI on SensorTile
+static Serial ser(PC_12,PD_2); // Serial with SensorTile Cradle Exp. Board + Nucleo
+#define printf(...) ser.printf(__VA_ARGS__)
+#else // Nucleo-XXX + X-Nucleo-IKS01A2
+static DevI2C devI2c(D14,D15);
+#endif
+#endif
+
+/* Motion sensors */
+#ifdef TARGET_DISCO_L475VG_IOT01A
+static LSM6DSLSensor acc_gyro(&devI2c,LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW,PD_11); // low address
+#else // X-NUCLEO-IKS01A2 or SensorTile
+#if defined (TARGET_SENSOR_TILE)
+static LSM6DSLSensor acc_gyro(&devSPI,PB_12, NC, PA_2, SPI_TYPE_LSM6DSL);
+#else
+static LSM6DSLSensor acc_gyro(&devI2c,LSM6DSL_ACC_GYRO_I2C_ADDRESS_HIGH,D4,D5); // high address
+#endif
+#endif
+
+ros::NodeHandle nh;
+
+sensor_msgs::Imu imu_msg;
+ros::Publisher pub_imu("/imu", &imu_msg);
+
+geometry_msgs::PoseStamped pose_msg;
+ros::Publisher pub_pose("/pose", &pose_msg);
+
+nav_msgs::Path path_msg;
+ros::Publisher pub_path("/path", &path_msg);
+
+
+char frameid[] = "/imu";
+//char frameid_pose[] = "/pose";
+
+DigitalOut led = LED1;
+
+Timer t;
+
+float alpha = 0.1;
+float x_acc_old, y_acc_old;
+
+int main() {
+ int32_t axes[3];
+ int32_t angles[3];
+ float dx = 0, dy = 0, x_pos = 0, y_pos= 0, x_acc = 0, y_acc = 0, x_offset = 0, y_offset = 0;
+ float x_vel = 0;
+ float y_vel = 0;
+
+ float dt = 0.04;
+
+ //float yaw = ;
+
+ /* Init sensors with default params */
+ acc_gyro.init(NULL);
+
+ /* Enable sensors */
+ acc_gyro.enable_x();
+ acc_gyro.enable_g();
+
+ /*Offset calibration*/
+ for(int i = 0; i<200; i++){
+
+ acc_gyro.get_x_axes(axes);
+ acc_gyro.get_g_axes(angles);
+
+ x_offset += axes[0]/200;
+ y_offset += axes[1]/200;
+ wait_ms(10);
+ }
+
+ nh.initNode();
+ nh.advertise(pub_imu);
+ imu_msg.header.frame_id = frameid;
+ nh.advertise(pub_pose);
+ pose_msg.header.frame_id = frameid;
+ nh.advertise(pub_path);
+ path_msg.header.frame_id = frameid;
+
+
+
+ while (1) {
+
+ for(int i=0; i<20; i++) {
+
+ t.stop();
+ dt = t.read();
+ t.reset();
+ t.start();
+
+ acc_gyro.get_x_axes(axes);
+ acc_gyro.get_g_axes(angles);
+
+
+ x_acc = alpha * (axes[0]/104.383 - x_offset/104.383) + (1 - alpha) * x_acc;
+ y_vel = 0;
+
+ led = !led;
+
+ //Obtain pose from acceleration
+ dx = x_vel * dt + (dt*dt)/2.0*x_acc;//* cos(yaw);
+ dy = y_vel * dt + (dt*dt)/2.0*y_acc;;//* sin(yaw);
+
+ x_vel = x_vel + x_acc * dt;
+ y_vel = y_vel + y_acc * dt;
+ x_pos += dx;
+ y_pos += dy;
+
+ // nh.spinOnce();
+ }
+
+ imu_msg.header.stamp = nh.now();
+ pose_msg.header.stamp = nh.now();
+ path_msg.header.stamp = nh.now();
+
+ pose_msg.pose.position.x = x_pos;
+ pose_msg.pose.position.y = y_pos;
+ pub_pose.publish( &pose_msg );
+
+ imu_msg.linear_acceleration.x = x_acc;
+ imu_msg.linear_acceleration.y = y_acc;
+ imu_msg.linear_acceleration.z = axes[2]/104.383;
+ imu_msg.angular_velocity.x = angles[0];
+ imu_msg.angular_velocity.y = angles[1];
+ imu_msg.angular_velocity.z = angles[2];
+ pub_imu.publish( &imu_msg );
+
+ path_msg.poses[1].pose.position.x = x_pos;
+ path_msg.poses[1].pose.position.y = y_pos;
+ pub_path.publish( &path_msg);
+
+ nh.spinOnce();
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.lib Wed Mar 28 19:42:58 2018 +0000 @@ -0,0 +1,1 @@ +https://github.com/ARMmbed/mbed-os/#ca661f9d28526ca8f874b05432493a489c9671ea
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_kinetic.lib Wed Mar 28 19:42:58 2018 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f