/* Correction du TP 2 (partie 3, ROS) -- abonnement, timer, reconnexion. * * Reprend le projet du TP 1 (memes reglages CubeMX, memoire, micro-ROS) ; * ajoute I2C2 PB10/PB11 Standard Mode pour le TMP102. * La correction finale (etape 3) est donnee : les etapes 1 et 2 en sont des * sous-ensembles. */ /* ===== USER CODE BEGIN Includes ============================================= */ #include #include #include #include #include #include #include #include #include #include /* USER CODE END Includes */ /* ===== USER CODE BEGIN PD =================================================== */ #define POSTE_DOMAIN_ID 11u /* 10 + numero du poste ; JAMAIS 77 */ #define TMP102_ADDR (0x48u << 1) #define EXEC_HANDLES 2u /* abonnement leds + timer */ /* Appel dont l'echec est sans consequence (destruction d'une session deja * morte) : rcl marque ses fonctions warn_unused_result, un (void) ne suffit * pas a GCC. */ #define IGNORE_RET(call) do { rcl_ret_t ignored_ = (call); (void)ignored_; } while (0) /* USER CODE END PD */ /* ===== USER CODE BEGIN 0 ==================================================== */ bool cubemx_transport_open(struct uxrCustomTransport *transport); bool cubemx_transport_close(struct uxrCustomTransport *transport); size_t cubemx_transport_write(struct uxrCustomTransport *transport, const uint8_t *buf, size_t len, uint8_t *err); size_t cubemx_transport_read(struct uxrCustomTransport *transport, uint8_t *buf, size_t len, int timeout, uint8_t *err); void *microros_allocate(size_t size, void *state); void microros_deallocate(void *pointer, void *state); void *microros_reallocate(void *pointer, size_t size, void *state); void *microros_zero_allocate(size_t number_of_elements, size_t size_of_element, void *state); typedef enum { WAITING_AGENT, CREATING, CONNECTED, DESTROYING } link_state_t; static rcl_allocator_t allocator; static rclc_support_t support; static rcl_node_t node; static rcl_subscription_t sub_leds; static rcl_publisher_t pub_temp; static rcl_timer_t timer_temp; static rclc_executor_t executor; static std_msgs__msg__UInt8 msg_leds; static sensor_msgs__msg__Temperature msg_temp; static char frame[] = "ratbot_lab_board"; static void set_led(GPIO_TypeDef *port, uint16_t pin, uint32_t on) { HAL_GPIO_WritePin(port, pin, on ? GPIO_PIN_SET : GPIO_PIN_RESET); } /* Etape 1 : un bit par LED. LED_4 sert d'indicateur de liaison : seuls les * bits 0 a 2 sont appliques tant qu'elle est utilisee ainsi. */ static void leds_cb(const void *msgin) { const std_msgs__msg__UInt8 *m = (const std_msgs__msg__UInt8 *)msgin; set_led(LED_1_GPIO_Port, LED_1_Pin, m->data & 0x1u); set_led(LED_2_GPIO_Port, LED_2_Pin, m->data & 0x2u); set_led(LED_3_GPIO_Port, LED_3_Pin, m->data & 0x4u); } static int tmp102_temp_mC(int32_t *out) { uint8_t b[2]; int32_t raw; if (HAL_I2C_Mem_Read(&hi2c2, TMP102_ADDR, 0x00u, I2C_MEMADD_SIZE_8BIT, b, 2u, 5u) != HAL_OK) { return 0; } raw = (int32_t)((((uint16_t)b[0] << 8) | b[1]) >> 4); if (raw & 0x800) { raw -= 4096; } *out = (raw * 625) / 10; return 1; } /* Etape 2 : mesure horodatee, une fois par seconde. */ static void temp_cb(rcl_timer_t *timer, int64_t last_call_time) { int32_t t_mC; int64_t ns; (void)last_call_time; if ((timer == NULL) || !tmp102_temp_mC(&t_mC)) { return; } ns = rmw_uros_epoch_nanos(); msg_temp.header.stamp.sec = (int32_t)(ns / 1000000000); msg_temp.header.stamp.nanosec = (uint32_t)(ns % 1000000000); msg_temp.temperature = (double)t_mC / 1000.0; msg_temp.variance = 0.0; if (rcl_publish(&pub_temp, &msg_temp, NULL) != RCL_RET_OK) { /* rien : la surveillance de l'agent decidera s'il faut tout recreer */ } } /* Cree tout ; renvoie false au premier echec (le reste n'est pas cree). */ static bool create_entities(void) { rcl_init_options_t opts = rcl_get_zero_initialized_init_options(); allocator = rcl_get_default_allocator(); if (rcl_init_options_init(&opts, allocator) != RCL_RET_OK) { return false; } if (rcl_init_options_set_domain_id(&opts, POSTE_DOMAIN_ID) != RCL_RET_OK) { return false; } if (rclc_support_init_with_options(&support, 0, NULL, &opts, &allocator) != RCL_RET_OK) { return false; } if (rclc_node_init_default(&node, "ratbot_lab", "tp", &support) != RCL_RET_OK) { return false; } (void)rmw_uros_sync_session(1000); if (rclc_subscription_init_default(&sub_leds, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, UInt8), "leds") != RCL_RET_OK) { return false; } if (rclc_publisher_init_default(&pub_temp, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Temperature), "temperature") != RCL_RET_OK) { return false; } if (rclc_timer_init_default(&timer_temp, &support, RCL_MS_TO_NS(1000), temp_cb) != RCL_RET_OK) { return false; } executor = rclc_executor_get_zero_initialized_executor(); if (rclc_executor_init(&executor, &support.context, EXEC_HANDLES, &allocator) != RCL_RET_OK) { return false; } if (rclc_executor_add_subscription(&executor, &sub_leds, &msg_leds, &leds_cb, ON_NEW_DATA) != RCL_RET_OK) { return false; } if (rclc_executor_add_timer(&executor, &timer_temp) != RCL_RET_OK) { return false; } return true; } /* Detruit tout. Les fonctions fini acceptent des entites jamais creees * (initialisees a zero) et renvoient alors une erreur, ignoree ici. */ static void destroy_entities(void) { rmw_context_t *ctx = rcl_context_get_rmw_context(&support.context); if (ctx != NULL) /* NULL si la creation a echoue avant le support */ { (void)rmw_uros_set_context_entity_destroy_session_timeout(ctx, 0); } IGNORE_RET(rcl_publisher_fini(&pub_temp, &node)); IGNORE_RET(rcl_subscription_fini(&sub_leds, &node)); IGNORE_RET(rcl_timer_fini(&timer_temp)); IGNORE_RET(rclc_executor_fini(&executor)); IGNORE_RET(rcl_node_fini(&node)); IGNORE_RET(rclc_support_fini(&support)); } /* USER CODE END 0 */ void StartDefaultTask(void *argument) { link_state_t state = WAITING_AGENT; uint32_t last_ping = 0u; (void)argument; rmw_uros_set_custom_transport(true, (void *)&huart5, cubemx_transport_open, cubemx_transport_close, cubemx_transport_write, cubemx_transport_read); allocator = rcutils_get_zero_initialized_allocator(); allocator.allocate = microros_allocate; allocator.deallocate = microros_deallocate; allocator.reallocate = microros_reallocate; allocator.zero_allocate = microros_zero_allocate; if (!rcutils_set_default_allocator(&allocator)) { Error_Handler(); } /* Etape 2 : memoire de la chaine fournie par nous */ msg_temp.header.frame_id.data = frame; msg_temp.header.frame_id.size = strlen(frame); msg_temp.header.frame_id.capacity = sizeof frame; for (;;) { switch (state) { case WAITING_AGENT: HAL_GPIO_TogglePin(LED_4_GPIO_Port, LED_4_Pin); state = (rmw_uros_ping_agent(100, 1) == RMW_RET_OK) ? CREATING : WAITING_AGENT; if (state == WAITING_AGENT) { osDelay(500); } break; case CREATING: if (create_entities()) { HAL_GPIO_WritePin(LED_4_GPIO_Port, LED_4_Pin, GPIO_PIN_SET); last_ping = osKernelGetTickCount(); state = CONNECTED; } else { state = DESTROYING; } break; case CONNECTED: (void)rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10)); if (osKernelGetTickCount() - last_ping >= 1000u) { last_ping = osKernelGetTickCount(); if (rmw_uros_ping_agent(100, 1) != RMW_RET_OK) { state = DESTROYING; } } break; case DESTROYING: default: destroy_entities(); HAL_GPIO_WritePin(LED_4_GPIO_Port, LED_4_Pin, GPIO_PIN_RESET); state = WAITING_AGENT; break; } } } /* Reponses attendues aux questions : * 1. data 5 = 0b0101 : LED_1 et LED_3 ; data 10 = 0b1010 : LED_2 et * LED_4 (LED_4 n'est pas appliquee dans cette correction, voir * leds_cb). * 2. Les octets arrivent (le DMA les recoit, l'agent les envoie), mais * personne ne traite les messages : aucun rappel, les LED ne changent * pas. * 3. Oui : l'executeur tourne toutes les 10 ms au plus (spin_some, 10 ms), * largement au-dessus de 50 Hz. Au-dela de la capacite (liaison 115200, * historique de 4 messages), des messages sont perdus. * 4. Oui, a quelques millisecondes pres. Sans synchronisation, * rmw_uros_epoch_nanos() renvoie le temps depuis le demarrage de la * carte (ou 0) : des dates en 1970, incomparables aux autres mesures. * 5. Environ 0,4 ms a 100 kHz : negligeable, mais pendant ce temps aucun * autre rappel ne s'execute. Un capteur lent (ou un HAL_Delay dans un * rappel) retarderait d'autant les commandes recues. * 6. Jusqu'a ~0,5 s de ping en attente, plus la creation des entites (une * a deux secondes a 115200) : la cadence du ping d'attente et le nombre * d'entites. * 7. L'agent qui revient ne connait plus rien de la session : toutes les * entites cote carte font reference a une session morte. Les detruire * et les recreer est le seul etat sur. * 8. En bonne logique, revenir a un etat sur (eteintes) : une commande * perimee ne doit pas rester appliquee. Cette correction les laisse en * l'etat -- c'est l'objet du troisieme exercice. */