From 32980c4052e6fb8f41fb4394f56452281ab91794 Mon Sep 17 00:00:00 2001 From: Ryan Howard Date: Mon, 22 Apr 2024 14:53:18 -0400 Subject: [PATCH] feat(sensors): enable logging both pressure sensors on the 8/96 (#761) * hook up a second pressure sensor buffer for the 8/96 * add new sensorid so we can address a 'both' case when we want to do one move and report both pressure sensors * make sure we have a case for 'sensor_report' in brushed motor handler * kill the recording of both sensors when a move finishes * format * send message to both queues * fix the defines for 8 and 96 channel * add an arg to the push script to make the sensors push easier * adjust buffer sizes to get them to fit in the ram --- include/bootloader/core/ids.h | 3 ++ include/can/core/ids.hpp | 2 + .../brushed_motor_interrupt_handler.hpp | 2 + .../stepper_motor/motor_interrupt_handler.hpp | 54 +++++++++++++------ pipettes/core/CMakeLists.txt | 9 ++-- pipettes/core/sensor_tasks.cpp | 13 ++++- push | 36 +++++++++---- 7 files changed, 87 insertions(+), 32 deletions(-) diff --git a/include/bootloader/core/ids.h b/include/bootloader/core/ids.h index e51d6cbb6..b3c7ae1b9 100644 --- a/include/bootloader/core/ids.h +++ b/include/bootloader/core/ids.h @@ -194,6 +194,8 @@ typedef enum { typedef enum { can_sensorid_s0 = 0x0, can_sensorid_s1 = 0x1, + can_sensorid_unused = 0x2, + can_sensorid_both = 0x3, } CANSensorId; /** Links sensor threshold triggers to pins. */ @@ -238,6 +240,7 @@ typedef enum { can_movestopcondition_stall = 0x10, can_movestopcondition_ignore_stalls = 0x20, can_movestopcondition_limit_switch_backoff = 0x40, + can_movestopcondition_sensor_report = 0x80, } CANMoveStopCondition; /** A bit field of the arbitration id parts. */ diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp index 2c5dd4169..e27d99b3d 100644 --- a/include/can/core/ids.hpp +++ b/include/can/core/ids.hpp @@ -208,6 +208,7 @@ enum class SensorId { S0 = 0x0, S1 = 0x1, UNUSED = 0x2, + BOTH = 0x3, }; /** Links sensor threshold triggers to pins. */ @@ -252,6 +253,7 @@ enum class MoveStopCondition { stall = 0x10, ignore_stalls = 0x20, limit_switch_backoff = 0x40, + sensor_report = 0x80, }; /** High-level type of pipette. */ diff --git a/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp b/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp index 7b5a04c63..634249d8c 100644 --- a/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp +++ b/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp @@ -121,6 +121,7 @@ class BrushedMotorInterruptHandler { true, AckMessageId::complete_without_condition); } break; + case MoveStopCondition::sensor_report: case MoveStopCondition::ignore_stalls: case MoveStopCondition::limit_switch_backoff: case MoveStopCondition::sync_line: @@ -301,6 +302,7 @@ class BrushedMotorInterruptHandler { case MoveStopCondition::sync_line: case MoveStopCondition::ignore_stalls: case MoveStopCondition::limit_switch_backoff: + case MoveStopCondition::sensor_report: // this is an unused move stop condition for the brushed motor // just return with no condition // TODO creat can bus error messages and send that instead diff --git a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp index 61fce36d2..2fe3a8bc5 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -379,7 +379,20 @@ class MotorInterruptHandler { */ return tick_count < buffered_move.duration; } - +#ifdef USE_PRESSURE_MOVE + auto send_bind_message(can::ids::SensorId sensor, uint8_t binding) -> void { + auto msg = can::messages::BindSensorOutputRequest{ + .message_index = buffered_move.message_index, + .sensor = can::ids::SensorType::pressure, + .sensor_id = sensor, + .binding = binding}; + if (sensor == can::ids::SensorId::S0) { + send_to_pressure_sensor_queue_rear(msg); + } else { + send_to_pressure_sensor_queue_front(msg); + } + } +#endif void update_move() { _has_active_move = move_queue.try_read_isr(&buffered_move); if (_has_active_move) { @@ -388,13 +401,13 @@ class MotorInterruptHandler { hardware.get_encoder_pulses(); #ifdef USE_PRESSURE_MOVE if (buffered_move.sensor_id != can::ids::SensorId::UNUSED) { - auto msg = can::messages::BindSensorOutputRequest{ - .message_index = buffered_move.message_index, - .sensor = can::ids::SensorType::pressure, - .sensor_id = buffered_move.sensor_id, - .binding = static_cast(0x3) // sync and report - }; - send_to_pressure_sensor_queue(msg); + auto binding = static_cast(0x3); // sync and report + if (buffered_move.sensor_id == can::ids::SensorId::BOTH) { + send_bind_message(can::ids::SensorId::S0, binding); + send_bind_message(can::ids::SensorId::S1, binding); + } else { + send_bind_message(buffered_move.sensor_id, binding); + } } #endif } @@ -486,13 +499,14 @@ class MotorInterruptHandler { build_and_send_ack(ack_msg_id); #ifdef USE_PRESSURE_MOVE if (buffered_move.sensor_id != can::ids::SensorId::UNUSED) { - auto stop_msg = can::messages::BindSensorOutputRequest{ - .message_index = buffered_move.message_index, - .sensor = can::ids::SensorType::pressure, - .sensor_id = buffered_move.sensor_id, - .binding = - static_cast(can::ids::SensorOutputBinding::sync)}; - send_to_pressure_sensor_queue(stop_msg); + auto binding = + static_cast(can::ids::SensorOutputBinding::sync); + if (buffered_move.sensor_id == can::ids::SensorId::BOTH) { + send_bind_message(can::ids::SensorId::S0, binding); + send_bind_message(can::ids::SensorId::S1, binding); + } else { + send_bind_message(buffered_move.sensor_id, binding); + } } #endif set_buffered_move(MotorMoveMessage{}); @@ -638,11 +652,17 @@ class MotorInterruptHandler { static_cast(position_tracker >> 31)); } #ifdef USE_PRESSURE_MOVE - void send_to_pressure_sensor_queue( + void send_to_pressure_sensor_queue_rear( can::messages::BindSensorOutputRequest& m) { std::ignore = sensor_tasks::get_queues() .pressure_sensor_queue_rear->try_write_isr(m); - // if (!success) {this->cancel_and_clear_moves();} + } + void send_to_pressure_sensor_queue_front( + can::messages::BindSensorOutputRequest& m) { + // send to both queues, they will handle their own gating based on + // sensor id + std::ignore = sensor_tasks::get_queues() + .pressure_sensor_queue_front->try_write_isr(m); } #endif uint64_t tick_count = 0x0; diff --git a/pipettes/core/CMakeLists.txt b/pipettes/core/CMakeLists.txt index 0a22909c0..f95c9d85c 100644 --- a/pipettes/core/CMakeLists.txt +++ b/pipettes/core/CMakeLists.txt @@ -28,7 +28,8 @@ function(target_pipettes_core_multi TARGET REVISION) target_pipettes_core_common(${TARGET} ${REVISION}) target_compile_definitions(${TARGET} PUBLIC PIPETTE_TYPE_DEFINE=EIGHT_CHANNEL) if(${USE_PRESSURE_MOVE}) - target_compile_definitions(${TARGET} PUBLIC P_BUFF_SIZE=2800) + target_compile_definitions(${TARGET} PUBLIC P_BUFF_SIZE=1800) + target_compile_definitions(${TARGET} PUBLIC USE_TWO_BUFFERS=true) endif() target_sources(${TARGET} PUBLIC ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/can_task_low_throughput.cpp) @@ -38,7 +39,8 @@ function(target_pipettes_core_96 TARGET REVISION) target_pipettes_core_common(${TARGET} ${REVISION}) target_compile_definitions(${TARGET} PUBLIC PIPETTE_TYPE_DEFINE=NINETY_SIX_CHANNEL) if(${USE_PRESSURE_MOVE}) - target_compile_definitions(${TARGET} PUBLIC P_BUFF_SIZE=1800) + target_compile_definitions(${TARGET} PUBLIC P_BUFF_SIZE=900) + target_compile_definitions(${TARGET} PUBLIC USE_TWO_BUFFERS=true) endif() target_sources(${TARGET} PUBLIC ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/can_task_high_throughput.cpp) @@ -48,7 +50,8 @@ function(target_pipettes_core_384 TARGET REVISION) target_pipettes_core_common(${TARGET} ${REVISION}) target_compile_definitions(${TARGET} PUBLIC PIPETTE_TYPE_DEFINE=THREE_EIGHTY_FOUR_CHANNEL) if(${USE_PRESSURE_MOVE}) - target_compile_definitions(${TARGET} PUBLIC P_BUFF_SIZE=1800) + target_compile_definitions(${TARGET} PUBLIC P_BUFF_SIZE=1500) + target_compile_definitions(${TARGET} PUBLIC USE_TWO_BUFFERS=true) endif() target_sources(${TARGET} PUBLIC ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/can_task_high_throughput.cpp) diff --git a/pipettes/core/sensor_tasks.cpp b/pipettes/core/sensor_tasks.cpp index e5b59956b..bb46b4e21 100644 --- a/pipettes/core/sensor_tasks.cpp +++ b/pipettes/core/sensor_tasks.cpp @@ -7,6 +7,9 @@ static auto tasks = sensor_tasks::Tasks{}; static auto queue_client = sensor_tasks::QueueClient{}; static std::array p_buff; +#ifdef USE_TWO_BUFFERS +static std::array p_buff_front; +#endif static auto eeprom_task_builder = freertos_task::TaskStarter<512, eeprom::task::EEPromTask>{}; @@ -140,7 +143,15 @@ void sensor_tasks::start_tasks( auto& pressure_sensor_task_front = pressure_sensor_task_builder_front.start( 5, "pressure sensor s1", secondary_pressure_i2c_client, secondary_pressure_i2c_poller, queues, sensor_hardware_secondary, - sensor_version, p_buff); + sensor_version, +#ifdef USE_TWO_BUFFERS + p_buff_front); +#else + // we don't want to build a second buffer for the single channel, but if + // we dont pass in the correct sized array here, compilation fails + // this doesn't matter though cause single channels will never call this + p_buff); +#endif auto& capacitive_sensor_task_rear = capacitive_sensor_task_builder_rear.start( 5, "capacitive sensor s0", i2c3_task_client, i2c3_poller_client, diff --git a/push b/push index 5a35197ca..87a0c761a 100755 --- a/push +++ b/push @@ -59,8 +59,12 @@ def _build_fw(zip_path, apps_path): for fname in os.listdir(apps_path): zf.write(os.path.join(apps_path, fname), fname) -def _transfer_firmware(host, repo_path, scp, ssh): - apps_path = os.path.join(repo_path, 'dist', 'applications') +def _transfer_firmware(host, repo_path, scp, ssh, sensors): + dist_dir = "dist" + if sensors: + dist_dir = dist_dir+"-sensors" + apps_path = os.path.join(repo_path, 'dist-sensor', 'applications') + apps_path = os.path.join(repo_path, 'dist-sensor', 'applications') with _controlled_tempdir() as td: local_zip_path = os.path.join(td, 'fw.zip') robot_zip_path = '/tmp/fw.zip' @@ -69,9 +73,14 @@ def _transfer_firmware(host, repo_path, scp, ssh): _ssh(ssh, host, 'unzip -o {zip_path} -d /usr/lib/firmware/'.format(zip_path=robot_zip_path)) _ssh(ssh, host, 'rm {zip_path}'.format(zip_path=robot_zip_path)) -def _prep_firmware(repo_path, cmake): - _cmd([cmake, '--build', '--preset=firmware-g4', '--target', 'firmware-applications'], cwd=repo_path) - _cmd([cmake, '--install', './build-cross', '--component', 'Applications'], cwd=repo_path) +def _prep_firmware(repo_path, cmake, sensors): + preset = "firmware-g4" + working_dir = "./build-cross" + if sensors: + preset = preset+"-sensors" + working_dir = working_dir+"-sensors" + _cmd([cmake, '--build', f'--preset={preset}', '--target', 'firmware-applications'], cwd=repo_path) + _cmd([cmake, '--install', f'{working_dir}/', '--component', 'Applications'], cwd=repo_path) @contextmanager def _prep_robot(host, ssh): @@ -96,19 +105,19 @@ def _find_utils(): def _restart_robot(host, ssh): _ssh(ssh, host, 'nohup systemctl restart opentrons-robot-server &') -def _do_push(host, repo_path, build, restart): +def _do_push(host, repo_path, build, restart, sensors): ssh, scp, cmake = _find_utils() if build: - _prep_firmware(repo_path, cmake) + _prep_firmware(repo_path, cmake, sensors) with _prep_robot(host, ssh): - _transfer_firmware(host, repo_path, scp, ssh) + _transfer_firmware(host, repo_path, scp, ssh, sensors) if restart: _restart_robot(host, ssh) -def push(host, repo_path=None, build=True, restart=True): +def push(host, repo_path=None, build=True, restart=True, sensors=False): repo = repo_path or os.dirname(__file__) try: - _do_push(host, repo, build, restart) + _do_push(host, repo, build, restart, sensors) return 0 except subprocess.CalledProcessError as e: print( @@ -125,7 +134,7 @@ def _push_from_argparse(args): if args.key: _SSH_EXTRA_OPTS.append('-i') _SSH_EXTRA_OPTS.append(args.key) - return push(args.host, os.path.abspath(args.repo_path), not args.no_build, not args.no_restart) + return push(args.host, os.path.abspath(args.repo_path), not args.no_build, not args.no_restart, args.sensors) def _arg_parser(parent=None): parents = [] @@ -156,6 +165,11 @@ def _arg_parser(parent=None): type=str, help='Private SSH key to use' ) + parser.add_argument( + '--sensors', + action='store_true', + help='Private SSH key to use' + ) return parser def _main():