added temeprature sensor publisher - to test

Author: JPGC04

pio_workspace/lib/power_custom/power_micro_ros.cpp

@@ -2,11 +2,14 @@
 
 rcl_subscription_t propulsion_microseconds_subscriber;
 std_msgs__msg__Int16MultiArray propulsion_microseconds_msg;
+
+rcl_publisher_t power_board_temperature_publisher;
+std_msgs__msg__Float32 power_board_temperature_msg;
+
 rclc_executor_t executor;
 rclc_support_t support;
 rcl_allocator_t allocator;
 rcl_node_t node;
-rcl_timer_t timer;
 
 void micro_ros_init() {
     set_microros_transports();
@@ -30,6 +33,15 @@ void micro_ros_init() {
     propulsion_microseconds_msg.data.size = 8;
     propulsion_microseconds_msg.data.capacity = 8;
 
+    // create publisher
+  RCCHECK(rclc_publisher_init_default(
+    &power_board_temperature_publisher,
+    &node,
+    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
+    "/power/board/temperature"));
+
+    power_board_temperature_msg.data = 0.0;
+
     // Create executor
     RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
     RCCHECK(rclc_executor_add_subscription(&executor, &propulsion_microseconds_subscriber, &propulsion_microseconds_msg, &propulsion_microseconds_subscription_callback, ON_NEW_DATA));
@@ -45,6 +57,8 @@ void error_loop() {
     free(propulsion_microseconds_msg.data.data);
   }
 
+  power_board_temperature_msg.data = 0.0;
+
   int error = 0;
 
   while(error <= 20) {
@@ -57,6 +71,11 @@ void error_loop() {
   digitalWrite(LED_PIN, HIGH);
 }
 
+void power_board_temperature_publish(float power_board_temperature) {
+  power_board_temperature_msg.data = power_board_temperature;
+  RCSOFTCHECK(rcl_publish(&power_board_temperature_publisher, &power_board_temperature_msg, NULL));
+}
+
 void propulsion_microseconds_subscription_callback(const void * msgin) {
     const std_msgs__msg__Int16MultiArray * propulsion_microseconds_msg = (const std_msgs__msg__Int16MultiArray *)msgin;
 

pio_workspace/lib/power_custom/power_micro_ros.h

@@ -8,16 +8,20 @@
 #include <rclc/executor.h>
 
 #include <std_msgs/msg/int16_multi_array.h>
+#include <std_msgs/msg/float32.h>
 
 #define LED_PIN 13
 
 extern rcl_subscription_t propulsion_microseconds_subscriber;
 extern std_msgs__msg__Int16MultiArray propulsion_microseconds_msg;
+
+extern rcl_publisher_t power_board_temperature_publisher;
+extern std_msgs__msg__Float32 power_board_temperature_msg;
+
 extern rclc_executor_t executor;
 extern rclc_support_t support;
 extern rcl_allocator_t allocator;
 extern rcl_node_t node;
-extern rcl_timer_t timer;
 
 #define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
 #define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
@@ -28,4 +32,6 @@ void micro_ros_init();
 void spin_micro_ros();
 void error_loop();
 
+void power_board_temperature_publish(float power_board_temperature);
+
 void propulsion_microseconds_subscription_callback(const void * msgin);

pio_workspace/src/power_main.cpp

@@ -6,6 +6,9 @@
 #include "power_micro_ros.h"
 
 #define LED_PIN 13
+#define TEMP_SENSE 23
+
+TMP36 temperature_sensor(TEMP_SENSE, 3.3);
 
 void power_setup() {
     pinMode(LED_PIN, OUTPUT);
@@ -14,13 +17,17 @@ void power_setup() {
     initThrusters();
 
     micro_ros_init();
+
+    temperature_sensor.begin();
 
     delay(2000);
 }
 
 void power_loop() {
     updateThrusters(microseconds);
 
+    power_board_temperature_publish(temperature_sensor.readTemperature());
+
     spin_micro_ros();
 
     delay(10);