diff --git a/gripper/core/tasks.cpp b/gripper/core/tasks.cpp index 42bd5d5ad..cd16770c6 100644 --- a/gripper/core/tasks.cpp +++ b/gripper/core/tasks.cpp @@ -160,7 +160,9 @@ void gripper_tasks::QueueClient::send_pressure_sensor_queue_front( void gripper_tasks::QueueClient::send_pressure_sensor_queue_rear( const sensors::utils::TaskMessage&) {} -void gripper_tasks::QueueClient::send_tip_notification_queue( +void gripper_tasks::QueueClient::send_tip_notification_queue_rear( + const sensors::tip_presence::TaskMessage&) {} +void gripper_tasks::QueueClient::send_tip_notification_queue_front( const sensors::tip_presence::TaskMessage&) {} /** diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp index d73bc66e7..9396c2c0a 100644 --- a/include/can/core/messages.hpp +++ b/include/can/core/messages.hpp @@ -1233,17 +1233,36 @@ struct BindSensorOutputResponse -> bool = default; }; -using TipStatusQueryRequest = Empty; +struct TipStatusQueryRequest : BaseMessage { + uint32_t message_index; + can::ids::SensorId sensor_id; + + template + static auto parse(Input body, Limit limit) -> TipStatusQueryRequest { + uint32_t msg_ind = 0; + uint8_t _id = 0; + + body = bit_utils::bytes_to_int(body, limit, msg_ind); + body = bit_utils::bytes_to_int(body, limit, _id); + return TipStatusQueryRequest{ + .message_index = msg_ind, + .sensor_id = static_cast(_id)}; + } + auto operator==(const TipStatusQueryRequest& other) const -> bool = default; +}; struct PushTipPresenceNotification : BaseMessage { uint32_t message_index; uint8_t ejector_flag_status; + can::ids::SensorId sensor_id{}; template auto serialize(Output body, Limit limit) const -> uint8_t { auto iter = bit_utils::int_to_bytes(message_index, body, limit); iter = bit_utils::int_to_bytes(ejector_flag_status, iter, limit); + iter = bit_utils::int_to_bytes(static_cast(sensor_id), iter, + limit); return iter - body; } diff --git a/include/gripper/core/tasks.hpp b/include/gripper/core/tasks.hpp index 637994c65..74d58a7ea 100644 --- a/include/gripper/core/tasks.hpp +++ b/include/gripper/core/tasks.hpp @@ -72,7 +72,9 @@ struct QueueClient : can::message_writer::MessageWriter { void send_pressure_sensor_queue_front(const sensors::utils::TaskMessage& m); void send_pressure_sensor_queue_rear(const sensors::utils::TaskMessage& m); - void send_tip_notification_queue( + void send_tip_notification_queue_rear( + const sensors::tip_presence::TaskMessage& m); + void send_tip_notification_queue_front( const sensors::tip_presence::TaskMessage& m); freertos_message_queue::FreeRTOSMessageQueue< diff --git a/include/pipettes/core/sensor_tasks.hpp b/include/pipettes/core/sensor_tasks.hpp index ff8660c66..f3c51e821 100644 --- a/include/pipettes/core/sensor_tasks.hpp +++ b/include/pipettes/core/sensor_tasks.hpp @@ -72,8 +72,11 @@ struct Tasks { freertos_message_queue::FreeRTOSMessageQueue>* pressure_sensor_task_front{nullptr}; sensors::tasks::TipPresenceNotificationTask< - freertos_message_queue::FreeRTOSMessageQueue>* tip_notification_task{ - nullptr}; + freertos_message_queue::FreeRTOSMessageQueue>* + tip_notification_task_rear{nullptr}; + sensors::tasks::TipPresenceNotificationTask< + freertos_message_queue::FreeRTOSMessageQueue>* + tip_notification_task_front{nullptr}; }; /** @@ -96,7 +99,10 @@ struct QueueClient : can::message_writer::MessageWriter { void send_pressure_sensor_queue_front(const sensors::utils::TaskMessage& m); - void send_tip_notification_queue( + void send_tip_notification_queue_rear( + const sensors::tip_presence::TaskMessage& m); + + void send_tip_notification_queue_front( const sensors::tip_presence::TaskMessage& m); freertos_message_queue::FreeRTOSMessageQueue* @@ -112,7 +118,11 @@ struct QueueClient : can::message_writer::MessageWriter { freertos_message_queue::FreeRTOSMessageQueue* pressure_sensor_queue_front{nullptr}; freertos_message_queue::FreeRTOSMessageQueue< - sensors::tip_presence::TaskMessage>* tip_notification_queue{nullptr}; + sensors::tip_presence::TaskMessage>* tip_notification_queue_rear{ + nullptr}; + freertos_message_queue::FreeRTOSMessageQueue< + sensors::tip_presence::TaskMessage>* tip_notification_queue_front{ + nullptr}; }; /** diff --git a/include/sensors/core/message_handlers/sensors.hpp b/include/sensors/core/message_handlers/sensors.hpp index b41632604..d06a88178 100644 --- a/include/sensors/core/message_handlers/sensors.hpp +++ b/include/sensors/core/message_handlers/sensors.hpp @@ -57,7 +57,15 @@ class SensorHandler { } void visit(const can::messages::TipStatusQueryRequest &m) { - client.send_tip_notification_queue(m); + switch (m.sensor_id) { + case can::ids::SensorId::S0: { + client.send_tip_notification_queue_rear(m); + break; + } + default: + client.send_tip_notification_queue_front(m); + break; + } } void send_to_queue(can::ids::SensorType type, can::ids::SensorId id, diff --git a/include/sensors/core/tasks/tip_presence_notification_task.hpp b/include/sensors/core/tasks/tip_presence_notification_task.hpp index 9e049bfab..0bde82df0 100644 --- a/include/sensors/core/tasks/tip_presence_notification_task.hpp +++ b/include/sensors/core/tasks/tip_presence_notification_task.hpp @@ -5,8 +5,9 @@ template class TipPresenceNotificationHandler { public: explicit TipPresenceNotificationHandler( - CanClient &can_client, sensors::hardware::SensorHardwareBase &hardware) - : can_client{can_client}, hardware{hardware} {} + CanClient &can_client, sensors::hardware::SensorHardwareBase &hardware, + const can::ids::SensorId &id) + : can_client{can_client}, hardware{hardware}, sensor_id{id} {} TipPresenceNotificationHandler(const TipPresenceNotificationHandler &) = delete; TipPresenceNotificationHandler(const TipPresenceNotificationHandler &&) = @@ -29,7 +30,13 @@ class TipPresenceNotificationHandler { can::messages::PushTipPresenceNotification{ .message_index = 0, .ejector_flag_status = - static_cast(hardware.check_tip_presence())}); + static_cast(hardware.check_tip_presence()), + .sensor_id = sensor_id, + }); + can_client.send_can_message(can::ids::NodeId::host, + can::messages::PushTipPresenceNotification{ + .message_index = 0, + }); } void visit(const can::messages::TipStatusQueryRequest &m) { @@ -38,12 +45,14 @@ class TipPresenceNotificationHandler { can::messages::PushTipPresenceNotification{ .message_index = m.message_index, .ejector_flag_status = - static_cast(hardware.check_tip_presence())}); + static_cast(hardware.check_tip_presence()), + .sensor_id = this->sensor_id}); } private: CanClient &can_client; sensors::hardware::SensorHardwareBase &hardware; + can::ids::SensorId sensor_id; }; /** @@ -56,7 +65,8 @@ class TipPresenceNotificationTask { public: using Messages = tip_presence::TaskMessage; using QueueType = QueueImpl; - TipPresenceNotificationTask(QueueType &queue) : queue{queue} {} + TipPresenceNotificationTask(QueueType &queue, can::ids::SensorId id) + : queue{queue}, sensor_id{id} {} TipPresenceNotificationTask(const TipPresenceNotificationTask &c) = delete; TipPresenceNotificationTask(const TipPresenceNotificationTask &&c) = delete; auto operator=(const TipPresenceNotificationTask &c) = delete; @@ -70,7 +80,8 @@ class TipPresenceNotificationTask { [[noreturn]] void operator()( CanClient *can_client, sensors::hardware::SensorHardwareBase *hardware) { - auto handler = TipPresenceNotificationHandler{*can_client, *hardware}; + auto handler = + TipPresenceNotificationHandler{*can_client, *hardware, sensor_id}; Messages message{}; for (;;) { if (queue.try_read(&message, queue.max_delay)) { @@ -83,6 +94,7 @@ class TipPresenceNotificationTask { private: QueueType &queue; + can::ids::SensorId sensor_id; }; } // namespace tasks diff --git a/pipettes/core/sensor_tasks.cpp b/pipettes/core/sensor_tasks.cpp index 484117a0e..e39aa480a 100644 --- a/pipettes/core/sensor_tasks.cpp +++ b/pipettes/core/sensor_tasks.cpp @@ -30,9 +30,13 @@ static auto pressure_sensor_task_builder_front = freertos_task::TaskStarter<512, sensors::tasks::PressureSensorTask, can::ids::SensorId>(can::ids::SensorId::S1); +static auto tip_notification_task_builder_rear = + freertos_task::TaskStarter<256, sensors::tasks::TipPresenceNotificationTask, + can::ids::SensorId>(can::ids::SensorId::S0); + static auto tip_notification_task_builder_front = - freertos_task::TaskStarter<256, - sensors::tasks::TipPresenceNotificationTask>{}; + freertos_task::TaskStarter<256, sensors::tasks::TipPresenceNotificationTask, + can::ids::SensorId>(can::ids::SensorId::S1); void sensor_tasks::start_tasks( sensor_tasks::CanWriterTask& can_writer, @@ -70,14 +74,14 @@ void sensor_tasks::start_tasks( capacitive_sensor_task_builder_rear.start( 5, "capacitive sensor s0", i2c3_task_client, i2c3_poller_client, sensor_hardware_primary, queues); - auto& tip_notification_task = tip_notification_task_builder_front.start( - 5, "tip notification", queues, sensor_hardware_primary); + auto& tip_notification_task_rear = tip_notification_task_builder_rear.start( + 5, "tip notification sensor s0", queues, sensor_hardware_primary); tasks.eeprom_task = &eeprom_task; tasks.environment_sensor_task = &environment_sensor_task; tasks.capacitive_sensor_task_rear = &capacitive_sensor_task_rear; tasks.pressure_sensor_task_rear = &pressure_sensor_task_rear; - tasks.tip_notification_task = &tip_notification_task; + tasks.tip_notification_task_rear = &tip_notification_task_rear; queues.set_queue(&can_writer.get_queue()); queues.eeprom_queue = &eeprom_task.get_queue(); @@ -85,7 +89,8 @@ void sensor_tasks::start_tasks( queues.capacitive_sensor_queue_rear = &capacitive_sensor_task_rear.get_queue(); queues.pressure_sensor_queue_rear = &pressure_sensor_task_rear.get_queue(); - queues.tip_notification_queue = &tip_notification_task.get_queue(); + queues.tip_notification_queue_rear = + &tip_notification_task_rear.get_queue(); } void sensor_tasks::start_tasks( @@ -134,14 +139,18 @@ void sensor_tasks::start_tasks( capacitive_sensor_task_builder_rear.start( 5, "capacitive sensor s0", i2c3_task_client, i2c3_poller_client, sensor_hardware_primary, queues, shared_cap_task); - auto& tip_notification_task = tip_notification_task_builder_front.start( - 5, "tip notification", queues, sensor_hardware_primary); + auto& tip_notification_task_rear = tip_notification_task_builder_rear.start( + 5, "tip notification sensor s0", queues, sensor_hardware_primary); + auto& tip_notification_task_front = + tip_notification_task_builder_front.start( + 5, "tip notification sensor s1", queues, sensor_hardware_secondary); tasks.eeprom_task = &eeprom_task; tasks.environment_sensor_task = &environment_sensor_task; tasks.pressure_sensor_task_rear = &pressure_sensor_task_rear; tasks.pressure_sensor_task_front = &pressure_sensor_task_front; - tasks.tip_notification_task = &tip_notification_task; + tasks.tip_notification_task_rear = &tip_notification_task_rear; + tasks.tip_notification_task_front = &tip_notification_task_front; tasks.capacitive_sensor_task_rear = &capacitive_sensor_task_rear; queues.set_queue(&can_writer.get_queue()); @@ -152,7 +161,10 @@ void sensor_tasks::start_tasks( queues.pressure_sensor_queue_rear = &pressure_sensor_task_rear.get_queue(); queues.pressure_sensor_queue_front = &pressure_sensor_task_front.get_queue(); - queues.tip_notification_queue = &tip_notification_task.get_queue(); + queues.tip_notification_queue_rear = + &tip_notification_task_rear.get_queue(); + queues.tip_notification_queue_front = + &tip_notification_task_front.get_queue(); if (shared_cap_task) { // There is only one cap sensor on the eight channel and so the "front" @@ -216,9 +228,14 @@ void sensor_tasks::QueueClient::send_pressure_sensor_queue_front( } } -void sensor_tasks::QueueClient::send_tip_notification_queue( +void sensor_tasks::QueueClient::send_tip_notification_queue_rear( + const sensors::tip_presence::TaskMessage& m) { + tip_notification_queue_rear->try_write(m); +} + +void sensor_tasks::QueueClient::send_tip_notification_queue_front( const sensors::tip_presence::TaskMessage& m) { - tip_notification_queue->try_write(m); + tip_notification_queue_front->try_write(m); } auto sensor_tasks::get_tasks() -> Tasks& { return tasks; } diff --git a/pipettes/firmware/main.cpp b/pipettes/firmware/main.cpp index 67e0303f5..e115c8dcc 100644 --- a/pipettes/firmware/main.cpp +++ b/pipettes/firmware/main.cpp @@ -143,13 +143,13 @@ static auto tail_accessor = extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { if (GPIO_Pin == tip_sense_gpio_primary.pin) { static_cast( - sensor_queue_client.tip_notification_queue->try_write_isr( + sensor_queue_client.tip_notification_queue_rear->try_write_isr( sensors::tip_presence::TipStatusChangeDetected{})); } else if (ok_for_secondary && GPIO_Pin == pins_for_sensor.secondary.value().tip_sense.value().pin) { static_cast( - sensor_queue_client.tip_notification_queue->try_write_isr( + sensor_queue_client.tip_notification_queue_front->try_write_isr( sensors::tip_presence::TipStatusChangeDetected{})); } } diff --git a/pipettes/firmware/utility_configurations.cpp b/pipettes/firmware/utility_configurations.cpp index 2bf5c7aba..1cbb11d67 100644 --- a/pipettes/firmware/utility_configurations.cpp +++ b/pipettes/firmware/utility_configurations.cpp @@ -121,7 +121,7 @@ auto utility_configs::sensor_configurations() gpio::PinConfig{ // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) .port = GPIOC, - .pin = GPIO_PIN_12, + .pin = GPIO_PIN_7, .active_setting = GPIO_PIN_RESET}}, .secondary = sensors::hardware::SensorHardwareConfiguration{ .sync_out = @@ -138,7 +138,7 @@ auto utility_configs::sensor_configurations() .tip_sense = gpio::PinConfig{ // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) .port = GPIOC, - .pin = GPIO_PIN_7, + .pin = GPIO_PIN_12, .active_setting = GPIO_PIN_RESET}}}; return pins; }