From 674e7c210a3f18644ea706006958888cb34f49d7 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 17:36:08 +0200 Subject: [PATCH 1/9] apps-sandbox/dotbot: replace with the dotbot-next app Breaking change: dotbot-sandbox-.bin is now the app built on the wheel speed loop, the pose estimator and onboard waypoint steering. The PD heading loop on drv/control_loop no longer runs in the sandbox, so the pure-pursuit define goes with it. AI-assisted: Claude Opus 5.5 --- apps-sandbox/applications.emProject | 26 +- apps-sandbox/dotbot-next/README.md | 143 ---- apps-sandbox/dotbot-next/main.c | 1083 ------------------------- apps-sandbox/dotbot/README.md | 144 +++- apps-sandbox/dotbot/main.c | 1172 +++++++++++++++++++++------ sandbox-dotbot-v2.emProject | 2 +- sandbox-dotbot-v3.emProject | 2 +- sandbox-nrf5340dk.emProject | 2 +- 8 files changed, 1083 insertions(+), 1491 deletions(-) delete mode 100644 apps-sandbox/dotbot-next/README.md delete mode 100644 apps-sandbox/dotbot-next/main.c diff --git a/apps-sandbox/applications.emProject b/apps-sandbox/applications.emProject index 8f2c779e..05171e02 100644 --- a/apps-sandbox/applications.emProject +++ b/apps-sandbox/applications.emProject @@ -25,35 +25,11 @@ - - - - - - - - - - - - - - - - - - - diff --git a/apps-sandbox/dotbot-next/README.md b/apps-sandbox/dotbot-next/README.md deleted file mode 100644 index a96a4da8..00000000 --- a/apps-sandbox/dotbot-next/README.md +++ /dev/null @@ -1,143 +0,0 @@ -# DotBot control application, rebuilt - -Successor to `apps-sandbox/dotbot`, built up one layer at a time rather than -edited in place. It stays joined, polls the position the secure side solves, -samples the wheel encoders, runs a per-wheel speed loop and a pose estimator, -advertises in the same format as `apps-sandbox/dotbot`, and accepts direct motor -commands and wheel velocity commands. That is all it does. - -It exists for two reasons. It is an instrument for measuring the plant and the -position source before any control layer sits on top of them, and it is the base -those layers get added to, one at a time, each testable against only the layers -below it. - -## What it does not do - -No steering law and no waypoint sequencing. Those are added later, deliberately, -and each one has to earn its place against a measurement taken with this build. - -The pose estimator (`drv/pose_estimator` in DotBot-libs) tracks the wheel-axle -midpoint and the heading. It predicts on every tick from the encoders and takes -each new solve as a position-only measurement of the photodiode, a lever arm ahead -of the axle. It acquires its initial heading passively, from a chain of -consistent solves once the robot has moved, so nothing has to drive a calibration -manoeuvre first. A robot moved by hand with its wheels still is recognised within -three fixes: the estimator drops its pose, the advertisement falls back to the -last solve with the unknown heading, and the heading comes back once the robot -moves. Its constants are provisional until measured on the floor. - -## Driving - -Three drive modes, each with exactly one writer of the motors: - -| Mode | Entered by | Motors | -|---|---|---| -| idle | boot, `CONTROL_MODE`, the command timeout | the wheel loop at a zero setpoint: brakes a turning wheel, then lets it coast once it stands | -| raw | `CMD_MOVE_RAW` | the command's duty, the wheel loop off | -| velocity | `CMD_WHEEL_VELOCITY` | the wheel loop, toward per-wheel setpoints in mm/s clamped to ±700 | - -The wheel loop (`drv/wheel_control` in DotBot-libs) runs every 10 ms tick. A zero -setpoint shorts the motor while the wheel still turns, so a stop does not coast -on. A wheel held at 80 duty or more with no encoder counts for 500 ms is stalled: -its motor coasts until the setpoint changes or the drive stops, and the bench -records carry its duty as `-127`. The ±700 mm/s clamp keeps a count longer than -the QDEC's 128 us sample period. - -Commands arrive in the IPC interrupt and are applied on the next tick; a newer -command replaces one not yet applied. Only `CMD_MOVE_RAW` and -`CMD_WHEEL_VELOCITY` refresh the command timeout, so a host that keeps sending -other packets still stops a driving robot by going quiet on drive commands. - -## Structure - -**One periodic tick.** `TICK_MS` (10 ms) drives a single RTC0 channel and every -slower activity divides it down: the wheel loop and the estimator's predict run -every tick, position at 100 ms, command timeout at 200 ms. The advertisement -period follows the node's minimum TX interval, between 100 and 1000 ms, and is -500 ms while not joined. The alternative, one channel per period, uses all three -usable RTC0 channels and leaves nothing for the encoder sampling rate a velocity -loop needs. - -**The application is the position pipeline's clock.** `swarmit_keep_alive()` is -what runs the lighthouse solve and republishes it to shared data, so the rate that -call is made at *is* the position rate, and reading the position without having -just called it returns the previous solve. The two are deliberately adjacent in -`_position_poll()` and must stay that way. It also feeds the watchdog, so its -cadence is bounded above by the watchdog timeout as well. - -**Freshness comes from a sequence, not from the coordinates.** -`swarmit_localization_get_fix()` returns the position together with the sequence -number of the solve it came from. The secure side advances that sequence by one -per published solve, so an unchanged sequence means the same measurement read -twice. The alternative, comparing coordinates, cannot tell a re-read from a robot -that has genuinely not moved, and the estimator must not run an update twice on -one measurement. `swarmit_localization_get_position()` -still exists and still has its original signature; this application does not call -it. - -The tick callback only increments a counter. The main loop compares it against -what it has serviced, drops any backlog rather than replaying it, and records the -worst backlog seen. A late tick is more useful reported than replayed, and on a -bench instrument that number is data. - -**Advertisement is byte-identical** to the one `apps-sandbox/dotbot` emits, so -host-side parsing is unchanged. Heading and position are the estimator's while -it tracks; otherwise heading is the unknown-value sentinel `-1000` and position is -the last solve. Fields this application does not own carry unknown values: -waypoints and waypoint index are zero, control mode is manual. Encoder counts are totals since the previous -advertisement rather than since the previous control step, which is the same field -carrying the only meaning available here. - -**Bench telemetry** is a second frame behind `DB_BENCH_TELEMETRY`, sent after -each advertisement. It carries every 10 ms wheel step since the previous frame -(credited counts, duty with `-128` for braked and `-127` for stalled, setpoints in -units of 10 mm/s), every new solve since the -previous frame (tick, fix sequence, raw coordinates), the encoder totals since -boot, the drive mode and the worst tick backlog. Steps and solves beyond what one -frame holds (24 and 4) are dropped oldest first and the drop is counted, and the -totals make a lost frame cost resolution but not distance. The frame is -deliberately absent from `protocol_data_type_t`: it must not exist in a shipped -target, so it claims value 13 by local convention only. Do not register that -value in the shared enum without moving this first. - -## Behaviour carried over deliberately - -Rewriting rather than editing loses corrections nobody wrote down. These were -taken from `apps-sandbox/dotbot` and `apps/dotbot` on purpose, and the reasoning -is recorded here so the next rewrite does not have to rediscover them. - -| Behaviour | Why it is here | -|---|---| -| `volatile` on anything an interrupt writes | The tick counter is written in the RTC callback and read in the main loop. Without it the compiler may cache the read and the loop never wakes. | -| `swarmit_keep_alive()` immediately before reading the position | It performs the solve and the republish, so it sets the position rate rather than merely keeping the application resident. Calling it less often than the position is read means reading the same solve twice. | -| Forwarding the SPIM4 interrupt to `swarmit_localization_handle_isr()` | That is how lighthouse sweeps are captured. A sandbox app that does not forward it gets no solves at all. | -| Coordinates above 100000 mm mean no solve | The secure side bounds-checks against the same figure and does not publish a solve outside it, so this is a second line rather than the only one. It stays because the bound is the only thing standing between a garbage solve and a position. | -| Encoder reads are destructive | `db_qdec_read_and_clear` empties the counter, so two consumers reading it steal counts from each other. Here there is one consumer and a running total; a second consumer needs a non-destructive sampling layer, not a second call. | -| RGB LED and QDEC behind board `#ifdef`s | Not every board in the family carries them. The flags are hardware presence, not features. | - -## Behaviour deliberately changed - -| Change | Reason | -|---|---| -| The command timeout is unconditional | In `apps-sandbox/dotbot` it is skipped in automatic mode, so an autonomously driving robot has no deadman at all. This application has no autonomous mode, so silence always means stop, and the exemption should not be reintroduced without a replacement. | -| Timeout arithmetic uses a masked difference | `db_timer_ticks()` returns a 24-bit counter that wraps every 512 s. A plain `now > then + delay` comparison is false for the entire pass after a wrap, so a robot whose last command arrived just before the rollover keeps its last commanded speed. | -| No displacement gate on incoming fixes | The gate in `apps-sandbox/dotbot` is anchored on the last accepted fix and only an accepted fix moves the anchor, so once the anchor is stale by more than the threshold, every fix that could correct it is rejected. Rejecting outliers belongs where the uncertainty is tracked, not against a self-referential anchor. | -| Freshness read from a sequence, not from the coordinates | `apps-sandbox/dotbot` can only compare coordinate values, which reads a stationary robot as having no new fix and a re-read of one solve as a measurement in its own right. | -| The device id is not read | It was retrieved at startup and never used. | - -## Build - -``` -SEGGER_DIR="" BUILD_TARGET=sandbox-dotbot-v3 BUILD_CONFIG=Debug make dotbot-next -``` - -It links against `apps-sandbox/cmse_implib.a`, the secure gateway import library -the swarmit bootloader build produces. That blob and the bootloader on the robot -are one unit: the veneer addresses it names move whenever the set of gateway -functions changes, so a bot flashed with a bootloader older than the blob will -call the wrong entry point. Updating the blob means a cabled reflash of the -bootloader, and a rebuild of every application in `apps-sandbox/`, not only this -one. - -Add `DB_BENCH_TELEMETRY` to the project's preprocessor definitions to enable the -second frame. Leave it off for anything that is not a bench run. diff --git a/apps-sandbox/dotbot-next/main.c b/apps-sandbox/dotbot-next/main.c deleted file mode 100644 index 8ee72c04..00000000 --- a/apps-sandbox/dotbot-next/main.c +++ /dev/null @@ -1,1083 +0,0 @@ -/** - * @file - * @defgroup project_dotbot_next DotBot control application, rebuilt - * @ingroup projects - * @brief Sandboxed DotBot app: keepalive, position, encoders, telemetry, - * direct motor commands, the per-wheel speed loop, the pose estimator and - * steering along a batch of waypoints. - * - * @copyright Inria, 2026 - */ - -#include -#include -#include -#include -#include -#include -// Include BSP headers -#include "board.h" -#include "board_config.h" -#include "geometry.h" -#include "gpio.h" -#include "motors.h" -#include "pose_estimator.h" -#include "protocol.h" -#include "qdec.h" -#include "rgbled_pwm.h" -#include "steering.h" -#include "timer.h" -#include "wheel_control.h" - -//=========================== defines ========================================== - -#define TIMER_DEV (0) -#define QDEC_LEFT (0) ///< Left wheel QDEC peripheral index -#define QDEC_RIGHT (1) ///< Right wheel QDEC peripheral index - -/// One periodic tick drives everything; slower activities divide it down. -#define TICK_MS (10U) -#define TICKS_PER_POSITION (10U) ///< 100 ms, and the rate a new solve is published at -#define TICKS_PER_TIMEOUT (20U) ///< 200 ms - -/// Adverts take this share of the node's transmit slots, one per minimum TX -/// interval, and the rest is left for the net core's STATUS frame -#define ADVERT_TX_SHARE_PERCENT (50U) -#define ADVERT_PERIOD_MIN_MS (100U) ///< Floor, however short the interval -#define ADVERT_PERIOD_MAX_MS (1000U) ///< Ceiling, however long the interval -#define ADVERT_PERIOD_DEF_MS (500U) ///< While not joined, when the interval reads 0 - -_Static_assert(TICK_MS == DB_WHEEL_CONTROL_TICK_MS, "the wheel loop's dt assumes this tick"); -_Static_assert(TICK_MS == DB_POSE_ESTIMATOR_TICK_MS, "the estimator's timeout assumes this tick"); -_Static_assert(TICK_MS == DB_STEERING_TICK_MS, "the steering's timeouts assume this tick"); -_Static_assert(DB_STEERING_PERIOD_TICKS == TICKS_PER_POSITION, "steering runs once per position poll, after it"); -_Static_assert((int)DB_STEERING_FAIL_NO_HEADING == (int)DB_WAYPOINTS_FAIL_NO_HEADING && (int)DB_STEERING_FAIL_SETTLE == (int)DB_WAYPOINTS_FAIL_SETTLE, - "the advertisement carries the steering's fail reason as is"); -_Static_assert(DB_STEERING_MAX_POINTS == DB_MAX_WAYPOINTS, "a batch fills the steering"); - -/// Largest wheel speed a command may set, in mm/s. A count then takes 135 us, -/// just over the QDEC's default 128 us sample period. -#define WHEEL_SPEED_MAX_MM_S (700) - -/// Room for the largest command this app accepts, header byte included: a full -/// waypoint batch, threshold and count first, then its trailer and headings -#define RX_MAILBOX_BYTES (1U + sizeof(uint16_t) + 1U + DB_MAX_WAYPOINTS * (sizeof(protocol_lh2_location_t) + sizeof(int16_t)) + sizeof(protocol_lh2_waypoints_trailer_t)) - -/// Cruise speeds a max speed command may set, mm/s -#define MAX_SPEED_MIN_MM_S (20U) -#define MAX_SPEED_MAX_MM_S (700U) - -#define DB_BUFFER_MAX_BYTES (255U) -#define TIMEOUT_STOP_TICKS (17000U) ///< ~500 ms of RTC ticks without a command - -/// NRF_RTC->COUNTER is 24 bits at 32768 Hz, so it wraps every 512 s and a plain -/// `now > then + delay` comparison is false for the whole pass after a wrap. -#define DB_RTC_COUNTER_MASK (0x00FFFFFFU) - -/// Coordinates above this are the secure side reporting no usable solve. -#define POSITION_INVALID_MM (100000U) - -/// The advertisement's "unknown" heading, sent while the estimator is not tracking -#define DIRECTION_INVALID (-1000) - -#if defined(DB_BENCH_TELEMETRY) || defined(DB_BENCH_TRACE) -/// Duty the bench records carry for a braked motor, outside [-100, 100] -#define PWM_BRAKED (INT8_MIN) -/// Duty the bench records carry for a wheel the loop has stalled, outside [-100, 100] -#define PWM_STALLED (INT8_MIN + 1) -#endif - -#if defined(DB_BENCH_TELEMETRY) -/// Bench-only frame type; keep it out of protocol_data_type_t, where 13 is unused. -#define DB_PROTOCOL_BENCH_TELEMETRY (13) -#define TELEMETRY_STEPS (24U) ///< Steps one frame carries, the newest kept -#define TELEMETRY_FIXES (4U) ///< New solves one frame carries, the newest kept -#endif - -/// Who writes the motors. Exactly one writer per mode. -typedef enum { - DRIVE_IDLE, ///< Nothing commanded; the wheel loop at zero brakes a turning wheel, else coasts - DRIVE_RAW, ///< MOVE_RAW writes duty directly and the wheel loop is off - DRIVE_VELOCITY, ///< The wheel loop is the only writer - DRIVE_WAYPOINT, ///< The steering sets the wheel loop's setpoints, or holds the motors braked -} drive_mode_t; - -typedef struct { - uint32_t x; ///< X coordinate in mm - uint32_t y; ///< Y coordinate in mm -} position_2d_t; -_Static_assert(sizeof(position_2d_t) == 8, "must match the bootloader's layout"); -_Static_assert(offsetof(position_2d_t, y) == 4, "must match the bootloader's layout"); - -typedef struct { - uint8_t radio_buffer[DB_BUFFER_MAX_BYTES]; - uint32_t ts_last_packet_received; ///< RTC ticks at the last command received - position_2d_t position; ///< Last solve accepted from the secure side - uint32_t fix_sequence; ///< Sequence of the last solve seen; 0 before the first - bool has_position; ///< False until the first in-bounds solve - uint32_t encoder_total_left; ///< Counts since boot; wraps, and deltas are taken modulo 2^32 - uint32_t encoder_total_right; ///< Counts since boot; wraps, and deltas are taken modulo 2^32 - uint32_t double_total_left; ///< Double transitions since boot, already credited in the totals - uint32_t double_total_right; ///< Double transitions since boot, already credited in the totals - drive_mode_t drive_mode; ///< Which writer owns the motors - int8_t pwm_left; ///< Last commanded duty, reported back for telemetry - int8_t pwm_right; ///< Last commanded duty, reported back for telemetry - bool brake_left; ///< Left motor shorted, its duty ignored - bool brake_right; ///< Right motor shorted, its duty ignored - uint32_t max_tick_backlog; ///< Worst number of ticks the main loop fell behind -} bench_vars_t; - -/// One consumer's position in the encoder totals. Each consumer keeps its own, -/// so reading counts does not take them from anybody else. -typedef struct { - uint32_t left; ///< Left total at this consumer's last read - uint32_t right; ///< Right total at this consumer's last read -} encoder_cursor_t; - -//============================= swarmit ======================================== - -typedef void (*ipc_isr_cb_t)(const uint8_t *, size_t); - -// Swarmit NSC callable functions -void swarmit_keep_alive(void); -void swarmit_send_raw_data(const uint8_t *packet, uint8_t length); -void swarmit_ipc_isr(ipc_isr_cb_t cb); -uint32_t swarmit_localization_get_fix(position_2d_t *position); -void swarmit_get_battery_level(uint16_t *battery_level); -void swarmit_localization_handle_isr(void); -uint32_t swarmit_get_min_tx_interval_us(void); - -//=========================== variables ======================================== - -static bench_vars_t _vars = { 0 }; - -/// The advertisement's own cursor into the encoder totals. -static encoder_cursor_t _advertisement_encoders = { 0 }; - -static volatile uint32_t _tick_count = 0; ///< Written by the tick callback only -static uint32_t _tick_serviced = 0; ///< Read and written by the main loop only -static uint32_t _tick_position = 0; ///< Tick of the last position poll, main loop only -static uint32_t _tick_timeout = 0; ///< Tick of the last timeout check, main loop only -static uint32_t _tick_advert = 0; ///< Tick of the last advertisement, main loop only -static uint32_t _advert_period = 0; ///< Ticks between advertisements, re-derived at each one -static uint32_t _tick_wheel = 0; ///< Tick of the last wheel step, main loop only - -/// The wheel loop's own cursor into the encoder totals -static encoder_cursor_t _wheel_encoders = { 0 }; - -/// Feedforward from an untethered open-loop duty sweep of one v3 on the office -/// carpet (breakaway 37-45, rolling at 32 + 0.092..0.101 duty per mm/s for -/// either wheel and direction); ki from closed-loop holds, kp from step -/// responses. Full duty, and no slew limit short of it. -static const db_wheel_control_conf_t _wheel_conf = { - .kp = 0.52f, - .ki = 5.2f, - .u_breakaway = 44.0f, - .kick_ramp = 0.5f, - .u_run = 32.0f, - .k_run = 0.097f, - .i_zone = 38.0f, - .pwm_max = 100.0f, - .pwm_slew_per_tick = 100.0f, - .stall_pwm = 80.0f, - .stall_ms = 500U, -}; -static db_wheel_control_t _wheel_left; -static db_wheel_control_t _wheel_right; - -static const db_steering_conf_t _steering_conf = { - .lever_mm = DB_LH2_LEVER_ARM_EFFECTIVE, - .v_max_mm_s = DB_STEERING_V_MAX_MM_S, - .approach_per_s = DB_STEERING_APPROACH_PER_S, - .runon_s = DB_STEERING_RUNON_S, - .spin_mm_s = DB_STEERING_SPIN_MM_S, - .spin_min_mm_s = DB_STEERING_SPIN_MIN_MM_S, - .heading_kp = DB_STEERING_HEADING_KP, - .heading_kd = DB_STEERING_HEADING_KD, - .align_enter_deg = DB_STEERING_ALIGN_ENTER_DEG, - .align_exit_deg = DB_STEERING_ALIGN_EXIT_DEG, - .full_speed_deg = DB_STEERING_FULL_SPEED_DEG, - .final_tol_deg = DB_STEERING_FINAL_TOL_DEG, - .near_mm = DB_STEERING_NEAR_MM, - .bearing_min_mm = DB_STEERING_BEARING_MIN_MM, - .lookahead_s = DB_STEERING_LOOKAHEAD_S, - .arrival_min_mm = DB_STEERING_ARRIVAL_MIN_MM, - .precise_min_mm = DB_STEERING_PRECISE_MIN_MM, - .pass_mm = DB_STEERING_PASS_MM, - .creep_mm_s = DB_STEERING_CREEP_MM_S, - .settle_skip_ticks = DB_STEERING_SETTLE_SKIP_TICKS, - .settle_fixes = DB_STEERING_SETTLE_FIXES, - .settle_ticks = DB_STEERING_SETTLE_TICKS, - .settle_nudges = DB_STEERING_SETTLE_NUDGES, - .nudge_ticks = DB_STEERING_NUDGE_TICKS, - .no_heading_turn_ticks = DB_STEERING_NO_HEADING_TURN_TICKS, - .no_heading_ticks = DB_STEERING_NO_HEADING_TICKS, - .turn_ticks = DB_STEERING_TURN_TICKS, - .progress_ticks = DB_STEERING_PROGRESS_TICKS, - .progress_mm = DB_STEERING_PROGRESS_MM, - .hold_ticks = DB_STEERING_HOLD_TICKS, - .recover = DB_STEERING_RECOVER_DRIVE, - .recover_mm = DB_STEERING_RECOVER_MM, - .recover_mm_s = DB_STEERING_RECOVER_MM_S, - .bounds_mm = { 0, 0, 10000.0f, 10000.0f }, // the LH2 calibration's validity rectangle - .bounds_margin_mm = DB_STEERING_BOUNDS_MARGIN_MM, -}; -#if defined(DB_BENCH_TELEMETRY) -/// The same with the spin recovery, selected by a bench command -static const db_steering_conf_t _steering_conf_spin = { - .lever_mm = DB_LH2_LEVER_ARM_EFFECTIVE, - .v_max_mm_s = DB_STEERING_V_MAX_MM_S, - .approach_per_s = DB_STEERING_APPROACH_PER_S, - .runon_s = DB_STEERING_RUNON_S, - .spin_mm_s = DB_STEERING_SPIN_MM_S, - .spin_min_mm_s = DB_STEERING_SPIN_MIN_MM_S, - .heading_kp = DB_STEERING_HEADING_KP, - .heading_kd = DB_STEERING_HEADING_KD, - .align_enter_deg = DB_STEERING_ALIGN_ENTER_DEG, - .align_exit_deg = DB_STEERING_ALIGN_EXIT_DEG, - .full_speed_deg = DB_STEERING_FULL_SPEED_DEG, - .final_tol_deg = DB_STEERING_FINAL_TOL_DEG, - .near_mm = DB_STEERING_NEAR_MM, - .bearing_min_mm = DB_STEERING_BEARING_MIN_MM, - .lookahead_s = DB_STEERING_LOOKAHEAD_S, - .arrival_min_mm = DB_STEERING_ARRIVAL_MIN_MM, - .precise_min_mm = DB_STEERING_PRECISE_MIN_MM, - .pass_mm = DB_STEERING_PASS_MM, - .creep_mm_s = DB_STEERING_CREEP_MM_S, - .settle_skip_ticks = DB_STEERING_SETTLE_SKIP_TICKS, - .settle_fixes = DB_STEERING_SETTLE_FIXES, - .settle_ticks = DB_STEERING_SETTLE_TICKS, - .settle_nudges = DB_STEERING_SETTLE_NUDGES, - .nudge_ticks = DB_STEERING_NUDGE_TICKS, - .no_heading_turn_ticks = DB_STEERING_NO_HEADING_TURN_TICKS, - .no_heading_ticks = DB_STEERING_NO_HEADING_TICKS, - .turn_ticks = DB_STEERING_TURN_TICKS, - .progress_ticks = DB_STEERING_PROGRESS_TICKS, - .progress_mm = DB_STEERING_PROGRESS_MM, - .hold_ticks = DB_STEERING_HOLD_TICKS, - .recover = DB_STEERING_RECOVER_SPIN, - .recover_mm = DB_STEERING_RECOVER_MM, - .recover_mm_s = DB_STEERING_RECOVER_MM_S, - .bounds_mm = { 0, 0, 10000.0f, 10000.0f }, // the LH2 calibration's validity rectangle - .bounds_margin_mm = DB_STEERING_BOUNDS_MARGIN_MM, -}; -#endif -/// Read over the debugger for its state and failure reason -__attribute__((used)) static db_steering_t _steering; -static uint32_t _tick_steering = 0; ///< Tick of the last steering step, main loop only -static bool _steering_brake = false; ///< The steering holds the motors braked -static uint8_t _batch_id = 0; ///< Of the last batch accepted, 0 for none -static protocol_waypoints_abort_t _abort_reason = DB_WAYPOINTS_ABORT_STOP; ///< What stopped the last batch - -static const db_pose_estimator_conf_t _estimator_conf = { - .lever_mm = DB_LH2_LEVER_ARM_EFFECTIVE, - .lever_angle_deg = DB_LH2_LEVER_ANGLE, - .r_pos_mm2 = DB_POSE_ESTIMATOR_R_POS_MM2, - .q_pos_mm2_per_mm = DB_POSE_ESTIMATOR_Q_POS_MM2_PER_MM, - .q_heading_roll_deg2_per_mm = DB_POSE_ESTIMATOR_Q_HEADING_ROLL_DEG2_PER_MM, - .q_heading_turn_deg2_per_mm = DB_POSE_ESTIMATOR_Q_HEADING_TURN_DEG2_PER_MM, - .turn_speed_ref_mm_s = DB_POSE_ESTIMATOR_TURN_SPEED_REF_MM_S, - .gate = DB_POSE_ESTIMATOR_GATE, - .fix_age_ticks = DB_POSE_ESTIMATOR_FIX_AGE_TICKS, - .timeout_ticks = DB_POSE_ESTIMATOR_TIMEOUT_TICKS, - .seed_fixes = DB_POSE_ESTIMATOR_SEED_FIXES, - .seed_tolerance_mm = DB_POSE_ESTIMATOR_SEED_TOLERANCE_MM, - .acquire_mm = DB_POSE_ESTIMATOR_ACQUIRE_MM, - .kidnap_fixes = DB_POSE_ESTIMATOR_KIDNAP_FIXES, - .kidnap_still_mm = DB_POSE_ESTIMATOR_KIDNAP_STILL_MM, - .kidnap_settle_ticks = DB_POSE_ESTIMATOR_KIDNAP_SETTLE_TICKS, - .still_mm_s = DB_POSE_ESTIMATOR_STILL_MM_S, - .reanchor_mm = DB_POSE_ESTIMATOR_REANCHOR_MM, - .reanchor_heading_var_deg2 = DB_POSE_ESTIMATOR_REANCHOR_HEADING_VAR_DEG2, - .q_pos_slip_mm2_per_mm_s = DB_POSE_ESTIMATOR_Q_POS_SLIP_MM2_PER_MM_S, - .q_heading_slip_deg2_per_mm_s = DB_POSE_ESTIMATOR_Q_HEADING_SLIP_DEG2_PER_MM_S, - .slip_deadband_mm_s = DB_POSE_ESTIMATOR_SLIP_DEADBAND_MM_S, - .speed_tau_ms = DB_POSE_ESTIMATOR_SPEED_TAU_MS, -}; -/// Read over the debugger for its counters and covariance -__attribute__((used)) static db_pose_estimator_t _estimator; -static encoder_cursor_t _estimator_encoders = { 0 }; ///< The estimator's own cursor into the encoder totals -static uint32_t _tick_estimator = 0; ///< Tick of the last predict, main loop only - -/// Commands arrive in the IPC interrupt and are applied on the next tick, so -/// the main loop is the only writer of the drive state and the motors. -static uint8_t _rx_buffer[RX_MAILBOX_BYTES]; -static size_t _rx_length = 0; -static volatile bool _rx_pending = false; - -#if defined(DB_BENCH_TRACE) -/// One tick while anything drives the motors, read back over the debugger -typedef struct __attribute__((packed)) { - uint32_t tick; ///< Serviced tick - int16_t setpoint_left; ///< mm/s - int16_t setpoint_right; ///< mm/s - int16_t counts_left; ///< Credited counts over this step - int16_t counts_right; ///< Credited counts over this step - int8_t pwm_left; ///< Duty written, PWM_BRAKED while braked, PWM_STALLED while stalled - int8_t pwm_right; ///< Duty written, PWM_BRAKED while braked, PWM_STALLED while stalled - uint8_t elapsed; ///< Ticks this step covered - uint8_t mode; ///< drive_mode_t at this step -} wheel_trace_t; - -#define TRACE_LENGTH (1000U) ///< 10 s of steps -#define TRACE_TAIL_TICKS (100U) ///< Keep recording this long after the loop stops - -__attribute__((used)) static wheel_trace_t _trace[TRACE_LENGTH]; -__attribute__((used)) static uint32_t _trace_count = 0; -static uint32_t _trace_tail = 0; - -/// Every new solve the secure side publishes, in a ring, whatever the drive -/// mode: fix rate and jitter come from the sequence against the tick -typedef struct __attribute__((packed)) { - uint32_t tick; ///< Serviced tick the solve was read on - uint32_t sequence; ///< Fix sequence - uint32_t x; ///< mm, as reported, before the bounds check - uint32_t y; ///< mm, as reported, before the bounds check -} fix_trace_t; - -#define FIX_TRACE_LENGTH (3000U) ///< 5 minutes at 10 Hz - -__attribute__((used)) static fix_trace_t _fix_trace[FIX_TRACE_LENGTH]; -__attribute__((used)) static uint32_t _fix_trace_count = 0; ///< Total written; the ring index is this modulo the length -#endif - -#if defined(DB_BENCH_TELEMETRY) -/// One wheel step, as the telemetry frame carries it -typedef struct __attribute__((packed)) { - int8_t counts_left; ///< Credited counts over this step, saturated - int8_t counts_right; ///< Credited counts over this step, saturated - int8_t pwm_left; ///< Duty written, PWM_BRAKED while braked, PWM_STALLED while stalled - int8_t pwm_right; ///< Duty written, PWM_BRAKED while braked, PWM_STALLED while stalled - int8_t setpoint_left; ///< In units of 10 mm/s - int8_t setpoint_right; ///< In units of 10 mm/s -} telemetry_step_t; - -/// One new solve, as the telemetry frame carries it -typedef struct __attribute__((packed)) { - uint16_t tick; ///< Low 16 bits of the serviced tick the solve was read on - uint16_t sequence; ///< Low 16 bits of the fix sequence - uint16_t x; ///< mm, before the bounds check, saturated - uint16_t y; ///< mm, before the bounds check, saturated -} telemetry_fix_t; - -static telemetry_step_t _telemetry_steps[TELEMETRY_STEPS]; -static uint32_t _telemetry_step_count = 0; ///< Steps recorded; the ring index is this modulo the length -static uint32_t _telemetry_step_sent = 0; ///< Step count at the last frame -static uint32_t _telemetry_step_tick = 0; ///< Serviced tick of the newest step -static telemetry_fix_t _telemetry_fixes[TELEMETRY_FIXES]; -static uint32_t _telemetry_fix_count = 0; ///< Solves recorded; the ring index is this modulo the length -static uint32_t _telemetry_fix_sent = 0; ///< Solve count at the last frame -#endif - -#ifdef DB_RGB_LED_PWM_RED_PORT -static const db_rgbled_pwm_conf_t _rgbled_pwm_conf = { - .pwm = 1, - .pins = { - { .port = DB_RGB_LED_PWM_RED_PORT, .pin = DB_RGB_LED_PWM_RED_PIN }, - { .port = DB_RGB_LED_PWM_GREEN_PORT, .pin = DB_RGB_LED_PWM_GREEN_PIN }, - { .port = DB_RGB_LED_PWM_BLUE_PORT, .pin = DB_RGB_LED_PWM_BLUE_PIN }, - } -}; -#endif - -#ifdef DB_QDEC_LEFT_A_PORT -static const qdec_conf_t _qdec_left_conf = { - .pin_a = &db_qdec_left_a_pin, - .pin_b = &db_qdec_left_b_pin, -}; - -static const qdec_conf_t _qdec_right_conf = { - .pin_a = &db_qdec_right_a_pin, - .pin_b = &db_qdec_right_b_pin, -}; -#endif - -//=========================== prototypes ======================================= - -static void _tick(void); -static void _service_tick(uint32_t tick); -static void _encoders_init(void); -static void _encoders_accumulate(void); -static void _encoders_delta(encoder_cursor_t *cursor, int32_t *left, int32_t *right); -static void _position_poll(void); -static void _timeout_check(void); -static void _advertise(void); -static uint32_t _advert_period_ticks(void); -static void _set_motors(int16_t left, int16_t right, bool brake_left, bool brake_right); -static void _rx_process(void); -static void _drive_stop(void); -static void _note_abort(protocol_waypoints_abort_t reason); -static void _wheel_service(uint32_t tick); -static void _estimator_service(uint32_t tick); -static void _steering_service(uint32_t tick); -static void _steering_poll(void); -static void _steering_apply(const db_steering_output_t *out); -static void _steering_pose(db_steering_pose_t *pose); -static void _enter_drive_mode(drive_mode_t mode); - -/// Elapsed rather than a multiple, since the main loop drops its backlog and -/// can step over any given tick. -static inline bool _due(uint32_t *last, uint32_t tick, uint32_t period) { - if (tick - *last < period) { - return false; - } - *last = tick; - return true; -} - -static inline uint32_t _ticks_since(uint32_t then) { - return (db_timer_ticks(TIMER_DEV) - then) & DB_RTC_COUNTER_MASK; -} - -//=========================== callbacks ======================================== - -/// The newest command replaces one the main loop has not applied yet -static void _rx_data_callback(const uint8_t *pkt, size_t len) { - if (len == 0 || len > sizeof(_rx_buffer)) { - return; - } - if (pkt[0] == DB_PROTOCOL_CMD_MOVE_RAW || pkt[0] == DB_PROTOCOL_CMD_WHEEL_VELOCITY || pkt[0] == DB_PROTOCOL_LH2_WAYPOINTS) { - _vars.ts_last_packet_received = db_timer_ticks(TIMER_DEV); - } - memcpy(_rx_buffer, pkt, len); - _rx_length = len; - __DMB(); // the buffer is complete before the flag says so - _rx_pending = true; -} - -//=========================== main ============================================= - -int main(void) { - db_board_init(); -#ifdef DB_RGB_LED_PWM_RED_PORT - db_rgbled_pwm_init(&_rgbled_pwm_conf); -#endif - db_motors_init(); - _encoders_init(); - db_wheel_control_init(&_wheel_left, &_wheel_conf); - db_wheel_control_init(&_wheel_right, &_wheel_conf); - db_pose_estimator_init(&_estimator, &_estimator_conf); - db_steering_init(&_steering, &_steering_conf); - db_gpio_init(&db_led1, DB_GPIO_OUT); - - _advert_period = _advert_period_ticks(); - db_timer_init(TIMER_DEV); - db_timer_set_periodic_ms(TIMER_DEV, 0, TICK_MS, &_tick); - - while (1) { - __WFE(); - - uint32_t now = _tick_count; - uint32_t missed = now - _tick_serviced; - if (missed == 0) { - continue; - } - // The backlog is dropped, not replayed; the worst case is telemetered - if (missed - 1 > _vars.max_tick_backlog) { - _vars.max_tick_backlog = missed - 1; - } - _tick_serviced = now; - _service_tick(now); - } -} - -//=========================== private functions ================================ - -static void _tick(void) { - _tick_count++; -} - -static void _service_tick(uint32_t tick) { - _rx_process(); - _encoders_accumulate(); - _wheel_service(tick); - _estimator_service(tick); - - if (_due(&_tick_position, tick, TICKS_PER_POSITION)) { - _position_poll(); - } - if (_due(&_tick_steering, tick, DB_STEERING_PERIOD_TICKS)) { - _steering_service(tick); - } - _steering_poll(); - if (_due(&_tick_timeout, tick, TICKS_PER_TIMEOUT)) { - _timeout_check(); - } - if (_due(&_tick_advert, tick, _advert_period)) { - _advert_period = _advert_period_ticks(); - _advertise(); - } -} - -static void _rx_process(void) { - if (!_rx_pending) { - return; - } - // Masked so the IPC interrupt cannot replace the buffer mid-copy - uint32_t primask = __get_PRIMASK(); - __disable_irq(); - __DMB(); // read the buffer only after seeing the flag - uint8_t packet[RX_MAILBOX_BYTES]; - size_t length = _rx_length; - memcpy(packet, _rx_buffer, length); - __DMB(); // the copy is complete before the flag frees the buffer - _rx_pending = false; - __set_PRIMASK(primask); - - const uint8_t *payload = &packet[1]; - switch (packet[0]) { - case DB_PROTOCOL_CMD_MOVE_RAW: - { - if (length < 1 + sizeof(protocol_move_raw_command_t)) { - break; - } - protocol_move_raw_command_t command; - memcpy(&command, payload, sizeof(command)); - _note_abort(DB_WAYPOINTS_ABORT_DIRECT); - _enter_drive_mode(DRIVE_RAW); - _set_motors((int16_t)(100 * ((float)command.left_y / INT8_MAX)), (int16_t)(100 * ((float)command.right_y / INT8_MAX)), false, false); - } break; - case DB_PROTOCOL_CMD_WHEEL_VELOCITY: - { - if (length < 1 + sizeof(protocol_wheel_velocity_command_t)) { - break; - } - protocol_wheel_velocity_command_t command; - memcpy(&command, payload, sizeof(command)); - if (_vars.drive_mode != DRIVE_VELOCITY) { - _note_abort(DB_WAYPOINTS_ABORT_DIRECT); - _enter_drive_mode(DRIVE_VELOCITY); - } - int16_t left = command.left_mm_s; - int16_t right = command.right_mm_s; - left = (left > WHEEL_SPEED_MAX_MM_S) ? WHEEL_SPEED_MAX_MM_S : ((left < -WHEEL_SPEED_MAX_MM_S) ? -WHEEL_SPEED_MAX_MM_S : left); - right = (right > WHEEL_SPEED_MAX_MM_S) ? WHEEL_SPEED_MAX_MM_S : ((right < -WHEEL_SPEED_MAX_MM_S) ? -WHEEL_SPEED_MAX_MM_S : right); - db_wheel_control_set_setpoint(&_wheel_left, left); - db_wheel_control_set_setpoint(&_wheel_right, right); - } break; - case DB_PROTOCOL_CMD_RGB_LED: - { -#ifdef DB_RGB_LED_PWM_RED_PORT - if (length < 1 + sizeof(protocol_rgbled_command_t)) { - break; - } - protocol_rgbled_command_t command; - memcpy(&command, payload, sizeof(command)); - db_rgbled_pwm_set_color(command.r, command.g, command.b); -#endif - } break; - case DB_PROTOCOL_LH2_WAYPOINTS: - { - db_steering_path_t path; - uint8_t batch_id; - if (!db_steering_path_from_wire(payload, length - 1, &path, &batch_id)) { - break; - } -#if defined(DB_BENCH_TELEMETRY) - // Bench only: a threshold of 0xFFFF drops the estimator's pose, as a - // kidnap does, to exercise the steering's heading recovery; 0xFFFE - // does the same with the spin recovery, until the next waypoint - if (path.threshold_mm >= (float)(UINT16_MAX - 1)) { - _steering.conf = (path.threshold_mm == (float)UINT16_MAX) ? &_steering_conf : &_steering_conf_spin; - db_pose_estimator_init(&_estimator, &_estimator_conf); - break; - } - _steering.conf = &_steering_conf; -#endif - // A resent batch the robot already has, its advertisement not yet heard - if (batch_id != 0 && batch_id == _batch_id) { - break; - } - _batch_id = batch_id; - if (path.count == 0) { - _note_abort(DB_WAYPOINTS_ABORT_STOP); - _drive_stop(); - break; - } - if (_vars.drive_mode != DRIVE_WAYPOINT) { - _enter_drive_mode(DRIVE_WAYPOINT); - } - _steering_brake = false; - db_steering_set_path(&_steering, &path); - } break; - case DB_PROTOCOL_CMD_MAX_SPEED: - { - if (length < 1 + sizeof(protocol_max_speed_command_t)) { - break; - } - protocol_max_speed_command_t command; - memcpy(&command, payload, sizeof(command)); - uint16_t v = command.max_speed_mm_s; - if (v != 0) { - v = (v < MAX_SPEED_MIN_MM_S) ? MAX_SPEED_MIN_MM_S : ((v > MAX_SPEED_MAX_MM_S) ? MAX_SPEED_MAX_MM_S : v); - } - db_steering_set_max_speed(&_steering, (float)v); - } break; - case DB_PROTOCOL_CONTROL_MODE: - _note_abort(DB_WAYPOINTS_ABORT_CONTROL_MODE); - _drive_stop(); - break; - default: - break; - } -} - -/// Records what stops a batch in progress; later commands leave the reason as it is -static void _note_abort(protocol_waypoints_abort_t reason) { - if (db_steering_active(&_steering)) { - _abort_reason = reason; - } -} - -/// Brakes both motors; from the next tick the wheel loop releases each one once -/// its wheel stands -static void _drive_stop(void) { - _enter_drive_mode(DRIVE_IDLE); - _set_motors(0, 0, true, true); -} - -/// Hands the motors to a new writer: the wheel loop starts from zero and the -/// steering drops its target unless it is the new writer -static void _enter_drive_mode(drive_mode_t mode) { -#if defined(DB_BENCH_TRACE) - if (_vars.drive_mode == DRIVE_IDLE && mode != DRIVE_IDLE) { - _trace_count = 0; - } -#endif - if (mode != DRIVE_WAYPOINT) { - db_steering_stop(&_steering); - } - _steering_brake = false; - _vars.drive_mode = mode; - db_wheel_control_reset(&_wheel_left); - db_wheel_control_reset(&_wheel_right); -} - -#if defined(DB_BENCH_TELEMETRY) -static inline int8_t _saturate_i8(int32_t value) { - return (value > INT8_MAX) ? INT8_MAX : ((value < INT8_MIN) ? INT8_MIN : (int8_t)value); -} -#endif - -#if defined(DB_BENCH_TELEMETRY) || defined(DB_BENCH_TRACE) -static inline int8_t _pwm_recorded(int8_t pwm, bool brake, bool stalled) { - return brake ? PWM_BRAKED : (stalled ? PWM_STALLED : pwm); -} -#endif - -/// Runs on every tick so the cursor never lags, and writes the motors only -/// while the loop owns them: driving by velocity, and after a stop, where its -/// zero setpoints brake the wheels until they stand -static void _wheel_service(uint32_t tick) { - uint32_t elapsed = tick - _tick_wheel; - _tick_wheel = tick; - int32_t left; - int32_t right; - _encoders_delta(&_wheel_encoders, &left, &right); - - if (_steering_brake) { - _set_motors(0, 0, true, true); - } else if (_vars.drive_mode != DRIVE_RAW) { - int8_t pwm_left = db_wheel_control_step(&_wheel_left, left, elapsed); - int8_t pwm_right = db_wheel_control_step(&_wheel_right, right, elapsed); - _set_motors(pwm_left, pwm_right, _wheel_left.brake, _wheel_right.brake); - } - -#if defined(DB_BENCH_TELEMETRY) - _telemetry_steps[_telemetry_step_count % TELEMETRY_STEPS] = (telemetry_step_t){ - .counts_left = _saturate_i8(left), - .counts_right = _saturate_i8(right), - .pwm_left = _pwm_recorded(_vars.pwm_left, _vars.brake_left, _wheel_left.stalled), - .pwm_right = _pwm_recorded(_vars.pwm_right, _vars.brake_right, _wheel_right.stalled), - .setpoint_left = _saturate_i8((int32_t)_wheel_left.setpoint / 10), - .setpoint_right = _saturate_i8((int32_t)_wheel_right.setpoint / 10), - }; - _telemetry_step_count++; - _telemetry_step_tick = tick; -#endif - -#if defined(DB_BENCH_TRACE) - if (_vars.drive_mode != DRIVE_IDLE) { - _trace_tail = TRACE_TAIL_TICKS; - } else if (_trace_tail > 0) { - _trace_tail--; - } else { - return; - } - if (_trace_count < TRACE_LENGTH) { - _trace[_trace_count++] = (wheel_trace_t){ - .tick = tick, - .setpoint_left = (int16_t)_wheel_left.setpoint, - .setpoint_right = (int16_t)_wheel_right.setpoint, - .counts_left = (int16_t)left, - .counts_right = (int16_t)right, - .pwm_left = _pwm_recorded(_vars.pwm_left, _vars.brake_left, _wheel_left.stalled), - .pwm_right = _pwm_recorded(_vars.pwm_right, _vars.brake_right, _wheel_right.stalled), - .elapsed = (uint8_t)elapsed, - .mode = (uint8_t)_vars.drive_mode, - }; - } -#endif -} - -/// Every tick, whatever drives the motors, so the pose follows any motion -static void _estimator_service(uint32_t tick) { - uint32_t elapsed = tick - _tick_estimator; - _tick_estimator = tick; - int32_t left; - int32_t right; - _encoders_delta(&_estimator_encoders, &left, &right); - db_pose_estimator_predict(&_estimator, left, right, elapsed); -} - -static db_steering_pose_status_t _steering_pose_status(db_pose_estimator_status_t status) { - switch (status) { - case DB_POSE_ESTIMATOR_TRACKING: - return DB_STEERING_POSE_TRACKING; - case DB_POSE_ESTIMATOR_LOST: - return DB_STEERING_POSE_LOST; - default: - return DB_STEERING_POSE_SEEDING; - } -} - -static void _steering_pose(db_steering_pose_t *pose) { - pose->status = _steering_pose_status(_estimator.status); - pose->x_mm = _estimator.x; - pose->y_mm = _estimator.y; - pose->heading_deg = _estimator.theta * 180.0f / (float)M_PI; -} - -/// A brake from the steering holds both motors shorted until it asks otherwise -static void _steering_apply(const db_steering_output_t *out) { - if (out->brake) { - if (!_steering_brake) { - db_wheel_control_reset(&_wheel_left); - db_wheel_control_reset(&_wheel_right); - _steering_brake = true; - } - return; - } - _steering_brake = false; - // Past the wheel limit, both wheels give up the excess, so the turn is kept - float left = out->left_mm_s; - float right = out->right_mm_s; - float excess = fmaxf(fabsf(left), fabsf(right)) - WHEEL_SPEED_MAX_MM_S; - if (excess > 0) { - float shift = (left + right >= 0) ? excess : -excess; - left -= shift; - right -= shift; - } - left = fmaxf(-WHEEL_SPEED_MAX_MM_S, fminf(WHEEL_SPEED_MAX_MM_S, left)); - right = fmaxf(-WHEEL_SPEED_MAX_MM_S, fminf(WHEEL_SPEED_MAX_MM_S, right)); - db_wheel_control_set_setpoint(&_wheel_left, left); - db_wheel_control_set_setpoint(&_wheel_right, right); -} - -/// Once per position poll, right after it, while steering owns the wheel loop -static void _steering_service(uint32_t tick) { - static uint32_t last = 0; - uint32_t elapsed = tick - last; - last = tick; - if (_vars.drive_mode != DRIVE_WAYPOINT) { - return; - } - db_steering_pose_t pose; - _steering_pose(&pose); - db_steering_output_t out; - db_steering_step(&_steering, &pose, elapsed, &out); - _steering_apply(&out); -} - -/// Every tick, for the stops of a precise arrival that fall between steps -static void _steering_poll(void) { - if (_vars.drive_mode != DRIVE_WAYPOINT) { - return; - } - db_steering_pose_t pose; - _steering_pose(&pose); - db_steering_output_t out; - if (db_steering_poll(&_steering, &pose, &out)) { - _steering_apply(&out); - } -} - -/// From the node's minimum TX interval, so a gateway on another schedule changes -/// the rate within one period -static uint32_t _advert_period_ticks(void) { - uint32_t min_tx_interval_us = swarmit_get_min_tx_interval_us(); - uint32_t period_ms = ADVERT_PERIOD_DEF_MS; - if (min_tx_interval_us > 0) { - period_ms = (min_tx_interval_us / 1000U) * 100U / ADVERT_TX_SHARE_PERCENT; - if (period_ms < ADVERT_PERIOD_MIN_MS) { - period_ms = ADVERT_PERIOD_MIN_MS; - } else if (period_ms > ADVERT_PERIOD_MAX_MS) { - period_ms = ADVERT_PERIOD_MAX_MS; - } - } -#if defined(DB_BENCH_TELEMETRY) - // Each advert is two frames - period_ms *= 2; -#endif - return period_ms / TICK_MS; -} - -static void _encoders_init(void) { -#ifdef DB_QDEC_LEFT_A_PORT - db_qdec_init(QDEC_LEFT, &_qdec_left_conf, NULL, NULL); - db_qdec_init(QDEC_RIGHT, &_qdec_right_conf, NULL, NULL); -#endif -} - -/// The hardware read is destructive, so the tick drains it into totals that are -/// never cleared; consumers take deltas against their own cursor. -static void _encoders_accumulate(void) { -#ifdef DB_QDEC_LEFT_A_PORT - uint32_t dbl_left; - uint32_t dbl_right; - int32_t acc_left = db_qdec_read_and_clear_dbl(QDEC_LEFT, &dbl_left); - int32_t acc_right = db_qdec_read_and_clear_dbl(QDEC_RIGHT, &dbl_right); - _vars.encoder_total_left += (uint32_t)db_wheel_control_counts(acc_left, dbl_left); - _vars.encoder_total_right += (uint32_t)db_wheel_control_counts(acc_right, dbl_right); - _vars.double_total_left += dbl_left; - _vars.double_total_right += dbl_right; -#endif -} - -/// Counts since this cursor last read, leaving the totals for other consumers. -static void _encoders_delta(encoder_cursor_t *cursor, int32_t *left, int32_t *right) { - uint32_t total_left = _vars.encoder_total_left; - uint32_t total_right = _vars.encoder_total_right; - *left = (int32_t)(total_left - cursor->left); - *right = (int32_t)(total_right - cursor->right); - cursor->left = total_left; - cursor->right = total_right; -} - -/// swarmit_keep_alive() runs the solve; call it immediately before reading the fix. -static void _position_poll(void) { - swarmit_keep_alive(); - - position_2d_t solve = { 0 }; - uint32_t sequence = swarmit_localization_get_fix(&solve); - - // An unchanged sequence is the previous solve read a second time - if (sequence == _vars.fix_sequence) { - return; - } - _vars.fix_sequence = sequence; - -#if defined(DB_BENCH_TRACE) - _fix_trace[_fix_trace_count % FIX_TRACE_LENGTH] = (fix_trace_t){ - .tick = _tick_serviced, - .sequence = sequence, - .x = solve.x, - .y = solve.y, - }; - _fix_trace_count++; -#endif -#if defined(DB_BENCH_TELEMETRY) - _telemetry_fixes[_telemetry_fix_count % TELEMETRY_FIXES] = (telemetry_fix_t){ - .tick = (uint16_t)_tick_serviced, - .sequence = (uint16_t)sequence, - .x = (solve.x > UINT16_MAX) ? UINT16_MAX : (uint16_t)solve.x, - .y = (solve.y > UINT16_MAX) ? UINT16_MAX : (uint16_t)solve.y, - }; - _telemetry_fix_count++; -#endif - - if (solve.x > POSITION_INVALID_MM || solve.y > POSITION_INVALID_MM) { - return; - } - _vars.position = solve; - _vars.has_position = true; - db_pose_estimator_update(&_estimator, (float)solve.x, (float)solve.y); - db_steering_fix(&_steering, (float)solve.x, (float)solve.y); -} - -/// Raw and velocity driving both stop when the host goes silent. A waypoint -/// needs no resending: the steering stops on arrival, on losing its pose and -/// on its own timeouts. -static void _timeout_check(void) { - if (_vars.drive_mode != DRIVE_IDLE && _vars.drive_mode != DRIVE_WAYPOINT && _ticks_since(_vars.ts_last_packet_received) > TIMEOUT_STOP_TICKS) { - _drive_stop(); - } -} - -static void _set_motors(int16_t left, int16_t right, bool brake_left, bool brake_right) { - db_motors_set_pwm_brake(left, right, brake_left, brake_right); - _vars.pwm_left = brake_left ? 0 : (int8_t)left; - _vars.pwm_right = brake_right ? 0 : (int8_t)right; - _vars.brake_left = brake_left; - _vars.brake_right = brake_right; -} - -static void _put(uint8_t *buf, size_t *length, const void *value, size_t size) { - memcpy(&buf[*length], value, size); - *length += size; -} - -#if defined(DB_BENCH_TELEMETRY) -static inline uint8_t _saturate_u8(uint32_t value) { - return (value > UINT8_MAX) ? UINT8_MAX : (uint8_t)value; -} - -/// Every step and every new solve since the previous frame, newest kept when -/// there are more than a frame holds; resets the worst tick backlog it reports. -/// Layout: type, newest step tick (u32), steps carried, steps dropped, backlog, -/// drive mode in the low nibble with the steering state in the high one, -/// encoder totals (i32 x 2), solves carried, solves dropped, then the steps -/// oldest first, then the solves oldest first, then the estimator: status, the -/// last gated fix's squared distance x 10 (u16, saturated), and its kidnap and -/// re-anchor counts (u8 each, wrapping), then the steering's point index and -/// corrections made (u8 each). -static void _send_bench_telemetry(void) { - size_t length = 0; - uint8_t *buf = _vars.radio_buffer; - - uint32_t steps = _telemetry_step_count - _telemetry_step_sent; - uint32_t steps_out = (steps > TELEMETRY_STEPS) ? TELEMETRY_STEPS : steps; - uint32_t fixes = _telemetry_fix_count - _telemetry_fix_sent; - uint32_t fixes_out = (fixes > TELEMETRY_FIXES) ? TELEMETRY_FIXES : fixes; - - buf[length++] = DB_PROTOCOL_BENCH_TELEMETRY; - _put(buf, &length, &_telemetry_step_tick, sizeof(_telemetry_step_tick)); - buf[length++] = (uint8_t)steps_out; - buf[length++] = _saturate_u8(steps - steps_out); - buf[length++] = _saturate_u8(_vars.max_tick_backlog); - _vars.max_tick_backlog = 0; - buf[length++] = (uint8_t)(_vars.drive_mode | (_steering.state << 4)); - _put(buf, &length, &_vars.encoder_total_left, sizeof(_vars.encoder_total_left)); - _put(buf, &length, &_vars.encoder_total_right, sizeof(_vars.encoder_total_right)); - buf[length++] = (uint8_t)fixes_out; - buf[length++] = _saturate_u8(fixes - fixes_out); - - for (uint32_t i = _telemetry_step_count - steps_out; i != _telemetry_step_count; i++) { - _put(buf, &length, &_telemetry_steps[i % TELEMETRY_STEPS], sizeof(telemetry_step_t)); - } - for (uint32_t i = _telemetry_fix_count - fixes_out; i != _telemetry_fix_count; i++) { - _put(buf, &length, &_telemetry_fixes[i % TELEMETRY_FIXES], sizeof(telemetry_fix_t)); - } - _telemetry_step_sent = _telemetry_step_count; - _telemetry_fix_sent = _telemetry_fix_count; - - buf[length++] = (uint8_t)_estimator.status; - float d2 = _estimator.last_d2 * 10.0f; - uint16_t d2x10 = (d2 >= (float)UINT16_MAX) ? UINT16_MAX : (uint16_t)d2; - _put(buf, &length, &d2x10, sizeof(d2x10)); - buf[length++] = (uint8_t)_estimator.kidnaps; - buf[length++] = (uint8_t)_estimator.reanchors; - buf[length++] = _steering.index; - buf[length++] = (uint8_t)_steering.nudges; - - swarmit_send_raw_data(buf, (uint8_t)length); -} -#endif - -/// The batch's completion and why, for the advertisement -static void _waypoints_status(protocol_waypoints_report_t *report) { - switch (_steering.completion) { - case DB_STEERING_DONE_IN_PROGRESS: - report->status = DB_WAYPOINTS_IN_PROGRESS; - break; - case DB_STEERING_DONE_ARRIVED: - report->status = DB_WAYPOINTS_ARRIVED; - break; - case DB_STEERING_DONE_FAILED: - report->status = DB_WAYPOINTS_FAILED; - report->reason = (uint8_t)_steering.fail; - break; - case DB_STEERING_DONE_ABORTED: - report->status = DB_WAYPOINTS_ABORTED; - report->reason = (uint8_t)_abort_reason; - break; - default: - report->status = DB_WAYPOINTS_NONE; - break; - } -} - -/// Layout of DB_PROTOCOL_DOTBOT_ADVERTISEMENT, then the waypoint report; the -/// fields up to the waypoint index are those of apps-sandbox/dotbot. -/// Fields this app does not own carry their unknown-value sentinels. -static void _advertise(void) { - db_gpio_toggle(&db_led1); - - size_t length = 0; - uint8_t *buf = _vars.radio_buffer; - - buf[length++] = DB_PROTOCOL_DOTBOT_ADVERTISEMENT; - buf[length++] = 0xff; // calibrated bitmask, unknown - - int16_t direction = DIRECTION_INVALID; - float heading; - if (db_pose_estimator_heading_deg(&_estimator, &heading)) { - direction = (int16_t)lroundf(heading); - } - _put(buf, &length, &direction, sizeof(direction)); - - // The photodiode position: the estimator's while it tracks, else the last solve - protocol_lh2_location_t position = { - .x = _vars.has_position ? _vars.position.x : 0, - .y = _vars.has_position ? _vars.position.y : 0, - }; - float sensor_x; - float sensor_y; - if (db_pose_estimator_sensor(&_estimator, &sensor_x, &sensor_y) && sensor_x >= 0 && sensor_y >= 0) { - position.x = (uint32_t)lroundf(sensor_x); - position.y = (uint32_t)lroundf(sensor_y); - } - _put(buf, &length, &position, sizeof(position)); - - uint16_t battery_level = 0; - swarmit_get_battery_level(&battery_level); - _put(buf, &length, &battery_level, sizeof(battery_level)); - - buf[length++] = (uint8_t)_vars.pwm_left; - buf[length++] = (uint8_t)_vars.pwm_right; - buf[length++] = (uint8_t)(db_steering_active(&_steering) ? ControlAuto : ControlManual); - - int32_t encoder_left; - int32_t encoder_right; - _encoders_delta(&_advertisement_encoders, &encoder_left, &encoder_right); - _put(buf, &length, &encoder_left, sizeof(encoder_left)); - _put(buf, &length, &encoder_right, sizeof(encoder_right)); - - // The point being driven to, and its index; the count once arrived - uint32_t waypoint_x = 0; - uint32_t waypoint_y = 0; - if (_steering.state != DB_STEERING_IDLE) { - waypoint_x = (uint32_t)lroundf(_steering.target.x_mm); - waypoint_y = (uint32_t)lroundf(_steering.target.y_mm); - } - _put(buf, &length, &waypoint_x, sizeof(waypoint_x)); - _put(buf, &length, &waypoint_y, sizeof(waypoint_y)); - buf[length++] = _steering.index; - - protocol_waypoints_report_t report = { - .batch_id = _batch_id, - .max_speed_10mm = (uint8_t)lroundf(_steering.v_max_mm_s / 10.0f), - .axle_x = DB_AXLE_UNKNOWN, - .axle_y = DB_AXLE_UNKNOWN, - }; - _waypoints_status(&report); - if (_estimator.status == DB_POSE_ESTIMATOR_TRACKING && _estimator.x >= 0 && _estimator.y >= 0 && _estimator.x < DB_AXLE_UNKNOWN && _estimator.y < DB_AXLE_UNKNOWN) { - report.axle_x = (uint16_t)lroundf(_estimator.x); - report.axle_y = (uint16_t)lroundf(_estimator.y); - } - _put(buf, &length, &report, sizeof(report)); - - swarmit_send_raw_data(buf, (uint8_t)length); - -#if defined(DB_BENCH_TELEMETRY) - _send_bench_telemetry(); -#endif -} - -void IPC_IRQHandler(void) { - swarmit_ipc_isr(_rx_data_callback); -} - -void SPIM4_IRQHandler(void) { - swarmit_localization_handle_isr(); -} diff --git a/apps-sandbox/dotbot/README.md b/apps-sandbox/dotbot/README.md index 1358f038..a2906070 100644 --- a/apps-sandbox/dotbot/README.md +++ b/apps-sandbox/dotbot/README.md @@ -1,15 +1,139 @@ # DotBot control application -This application allows the DotBot to be controlled remotely either from -- a joystick or nrf52 compatible board running a firmware that sends compatible - commands _move_ or _rgbled_ -- a computer running [the dotbot-controller tool](https://github.com/DotBots/PyDotBot) -and with a nRF52840-DK connected to it and used as gateway. The nRF52840-DK must run the -`03app_dotbot_gateway` firmware -- the buttons on the nRF52840-DK gatewaty itself +The DotBot app that runs in the SwarmIT sandbox, built up one layer at a time +rather than edited in place from the app it replaced. It stays joined, polls the +position the secure side solves, samples the wheel encoders, runs a per-wheel +speed loop and a pose estimator, steers along a batch of waypoints +(`drv/steering` in DotBot-libs), advertises in the standard DotBot format plus a +waypoint report, and accepts direct motor commands and wheel velocity commands. -
+Each layer was added only once the layers below it were measured, so the app +doubles as the instrument for measuring the plant and the position source. -![DotBot demo](../../doc/sphinx/_static/images/03app_dotbot.gif) +## Estimator -
+The pose estimator (`drv/pose_estimator` in DotBot-libs) tracks the wheel-axle +midpoint and the heading. It predicts on every tick from the encoders and takes +each new solve as a position-only measurement of the photodiode, a lever arm ahead +of the axle. It acquires its initial heading passively, from a chain of +consistent solves once the robot has moved, so nothing has to drive a calibration +manoeuvre first. A robot moved by hand with its wheels still is recognised within +three fixes: the estimator drops its pose, the advertisement falls back to the +last solve with the unknown heading, and the heading comes back once the robot +moves. Its constants are provisional until measured on the floor. + +## Driving + +Three drive modes, each with exactly one writer of the motors: + +| Mode | Entered by | Motors | +|---|---|---| +| idle | boot, `CONTROL_MODE`, the command timeout | the wheel loop at a zero setpoint: brakes a turning wheel, then lets it coast once it stands | +| raw | `CMD_MOVE_RAW` | the command's duty, the wheel loop off | +| velocity | `CMD_WHEEL_VELOCITY` | the wheel loop, toward per-wheel setpoints in mm/s clamped to ±700 | + +The wheel loop (`drv/wheel_control` in DotBot-libs) runs every 10 ms tick. A zero +setpoint shorts the motor while the wheel still turns, so a stop does not coast +on. A wheel held at 80 duty or more with no encoder counts for 500 ms is stalled: +its motor coasts until the setpoint changes or the drive stops, and the bench +records carry its duty as `-127`. The ±700 mm/s clamp keeps a count longer than +the QDEC's 128 us sample period. + +Commands arrive in the IPC interrupt and are applied on the next tick; a newer +command replaces one not yet applied. Only `CMD_MOVE_RAW` and +`CMD_WHEEL_VELOCITY` refresh the command timeout, so a host that keeps sending +other packets still stops a driving robot by going quiet on drive commands. + +## Structure + +**One periodic tick.** `TICK_MS` (10 ms) drives a single RTC0 channel and every +slower activity divides it down: the wheel loop and the estimator's predict run +every tick, position at 100 ms, command timeout at 200 ms. The advertisement +period follows the node's minimum TX interval, between 100 and 1000 ms, and is +500 ms while not joined. The alternative, one channel per period, uses all three +usable RTC0 channels and leaves nothing for the encoder sampling rate a velocity +loop needs. + +**The application is the position pipeline's clock.** `swarmit_keep_alive()` is +what runs the lighthouse solve and republishes it to shared data, so the rate that +call is made at *is* the position rate, and reading the position without having +just called it returns the previous solve. The two are deliberately adjacent in +`_position_poll()` and must stay that way. It also feeds the watchdog, so its +cadence is bounded above by the watchdog timeout as well. + +**Freshness comes from a sequence, not from the coordinates.** +`swarmit_localization_get_fix()` returns the position together with the sequence +number of the solve it came from. The secure side advances that sequence by one +per published solve, so an unchanged sequence means the same measurement read +twice. The alternative, comparing coordinates, cannot tell a re-read from a robot +that has genuinely not moved, and the estimator must not run an update twice on +one measurement. `swarmit_localization_get_position()` +still exists and still has its original signature; this application does not call +it. + +The tick callback only increments a counter. The main loop compares it against +what it has serviced, drops any backlog rather than replaying it, and records the +worst backlog seen. A late tick is more useful reported than replayed, and on a +bench instrument that number is data. + +**Advertisement** fields are those of the standard DotBot advertisement, so +host-side parsing is unchanged. Heading and position are the estimator's while +it tracks; otherwise heading is the unknown-value sentinel `-1000` and position is +the last solve. Fields this application does not own carry unknown values: +waypoints and waypoint index are zero, control mode is manual. Encoder counts are totals since the previous +advertisement rather than since the previous control step, which is the same field +carrying the only meaning available here. + +**Bench telemetry** is a second frame behind `DB_BENCH_TELEMETRY`, sent after +each advertisement. It carries every 10 ms wheel step since the previous frame +(credited counts, duty with `-128` for braked and `-127` for stalled, setpoints in +units of 10 mm/s), every new solve since the +previous frame (tick, fix sequence, raw coordinates), the encoder totals since +boot, the drive mode and the worst tick backlog. Steps and solves beyond what one +frame holds (24 and 4) are dropped oldest first and the drop is counted, and the +totals make a lost frame cost resolution but not distance. The frame is +deliberately absent from `protocol_data_type_t`: it must not exist in a shipped +target, so it claims value 13 by local convention only. Do not register that +value in the shared enum without moving this first. + +## Behaviour carried over deliberately + +Rewriting rather than editing loses corrections nobody wrote down. These were +taken from the previous sandbox app and `apps/dotbot` on purpose, and the reasoning +is recorded here so the next rewrite does not have to rediscover them. + +| Behaviour | Why it is here | +|---|---| +| `volatile` on anything an interrupt writes | The tick counter is written in the RTC callback and read in the main loop. Without it the compiler may cache the read and the loop never wakes. | +| `swarmit_keep_alive()` immediately before reading the position | It performs the solve and the republish, so it sets the position rate rather than merely keeping the application resident. Calling it less often than the position is read means reading the same solve twice. | +| Forwarding the SPIM4 interrupt to `swarmit_localization_handle_isr()` | That is how lighthouse sweeps are captured. A sandbox app that does not forward it gets no solves at all. | +| Coordinates above 100000 mm mean no solve | The secure side bounds-checks against the same figure and does not publish a solve outside it, so this is a second line rather than the only one. It stays because the bound is the only thing standing between a garbage solve and a position. | +| Encoder reads are destructive | `db_qdec_read_and_clear` empties the counter, so two consumers reading it steal counts from each other. Here there is one consumer and a running total; a second consumer needs a non-destructive sampling layer, not a second call. | +| RGB LED and QDEC behind board `#ifdef`s | Not every board in the family carries them. The flags are hardware presence, not features. | + +## Behaviour deliberately changed + +| Change | Reason | +|---|---| +| The command timeout is unconditional | In the previous sandbox app it was skipped in automatic mode, so an autonomously driving robot has no deadman at all. This application has no autonomous mode, so silence always means stop, and the exemption should not be reintroduced without a replacement. | +| Timeout arithmetic uses a masked difference | `db_timer_ticks()` returns a 24-bit counter that wraps every 512 s. A plain `now > then + delay` comparison is false for the entire pass after a wrap, so a robot whose last command arrived just before the rollover keeps its last commanded speed. | +| No displacement gate on incoming fixes | The gate in the previous sandbox app (and still in `apps/dotbot`) is anchored on the last accepted fix and only an accepted fix moves the anchor, so once the anchor is stale by more than the threshold, every fix that could correct it is rejected. Rejecting outliers belongs where the uncertainty is tracked, not against a self-referential anchor. | +| Freshness read from a sequence, not from the coordinates | The previous sandbox app could only compare coordinate values, which reads a stationary robot as having no new fix and a re-read of one solve as a measurement in its own right. | +| The device id is not read | It was retrieved at startup and never used. | + +## Build + +``` +SEGGER_DIR="" BUILD_TARGET=sandbox-dotbot-v3 BUILD_CONFIG=Debug make dotbot +``` + +It links against `apps-sandbox/cmse_implib.a`, the secure gateway import library +the swarmit bootloader build produces. That blob and the bootloader on the robot +are one unit: the veneer addresses it names move whenever the set of gateway +functions changes, so a bot flashed with a bootloader older than the blob will +call the wrong entry point. Updating the blob means a cabled reflash of the +bootloader, and a rebuild of every application in `apps-sandbox/`, not only this +one. + +Add `DB_BENCH_TELEMETRY` to the project's preprocessor definitions to enable the +second frame. Leave it off for anything that is not a bench run. diff --git a/apps-sandbox/dotbot/main.c b/apps-sandbox/dotbot/main.c index fa56f6a8..84420f7f 100644 --- a/apps-sandbox/dotbot/main.c +++ b/apps-sandbox/dotbot/main.c @@ -1,93 +1,376 @@ /** * @file - * @defgroup project_dotbot DotBot application + * @defgroup project_sandbox_dotbot DotBot control application * @ingroup projects - * @brief This is the radio-controlled DotBot app + * @brief Sandboxed DotBot app: keepalive, position, encoders, telemetry, + * direct motor commands, the per-wheel speed loop, the pose estimator and + * steering along a batch of waypoints. * - * The remote control can be either a keyboard, a joystick or buttons on the gateway - * itself - * - * @author Said Alvarado-Marin - * @author Alexandre Abadie - * @copyright Inria, 2022 + * @copyright Inria, 2026 */ +#include #include +#include +#include #include -#include #include -#include // Include BSP headers #include "board.h" #include "board_config.h" -#include "device.h" +#include "geometry.h" #include "gpio.h" -#include "protocol.h" #include "motors.h" +#include "pose_estimator.h" +#include "protocol.h" #include "qdec.h" #include "rgbled_pwm.h" +#include "steering.h" #include "timer.h" -#include "control_loop.h" +#include "wheel_control.h" //=========================== defines ========================================== -#define DB_RADIO_FREQ (8U) ///< Set the frequency to 2408 MHz -#define RADIO_APP (DotBot) ///< DotBot Radio App -#define TIMER_DEV (0) -#define QDEC_LEFT (0) ///< Left wheel QDEC peripheral index -#define QDEC_RIGHT (1) ///< Right wheel QDEC peripheral index -#define DB_POSITION_UPDATE_DELAY_MS (100U) ///< 100ms delay between each LH2 position updates +#define TIMER_DEV (0) +#define QDEC_LEFT (0) ///< Left wheel QDEC peripheral index +#define QDEC_RIGHT (1) ///< Right wheel QDEC peripheral index + +/// One periodic tick drives everything; slower activities divide it down. +#define TICK_MS (10U) +#define TICKS_PER_POSITION (10U) ///< 100 ms, and the rate a new solve is published at +#define TICKS_PER_TIMEOUT (20U) ///< 200 ms + /// Adverts take this share of the node's transmit slots, one per minimum TX /// interval, and the rest is left for the net core's STATUS frame -#define ADVERT_TX_SHARE_PERCENT (50U) -#define ADVERT_PERIOD_MIN_MS (100U) ///< Floor, and the advert timer's period -#define ADVERT_PERIOD_MAX_MS (1000U) ///< Ceiling, however long the interval -#define ADVERT_PERIOD_DEF_MS (500U) ///< While not joined, when the interval reads 0 -#define DB_TIMEOUT_CHECK_DELAY_MS (200U) ///< 200ms delay between each timeout delay check -#define TIMEOUT_CHECK_DELAY_TICKS (17000) ///< ~500 ms delay between packet received timeout checks -#define DB_BUFFER_MAX_BYTES (255U) ///< Max bytes in UART receive buffer -#define DB_LH2_OUTLIER_THRESHOLD (500U) ///< Max allowed displacement (mm) between two consecutive LH2 fixes +#define ADVERT_TX_SHARE_PERCENT (50U) +#define ADVERT_PERIOD_MIN_MS (100U) ///< Floor, however short the interval +#define ADVERT_PERIOD_MAX_MS (1000U) ///< Ceiling, however long the interval +#define ADVERT_PERIOD_DEF_MS (500U) ///< While not joined, when the interval reads 0 + +_Static_assert(TICK_MS == DB_WHEEL_CONTROL_TICK_MS, "the wheel loop's dt assumes this tick"); +_Static_assert(TICK_MS == DB_POSE_ESTIMATOR_TICK_MS, "the estimator's timeout assumes this tick"); +_Static_assert(TICK_MS == DB_STEERING_TICK_MS, "the steering's timeouts assume this tick"); +_Static_assert(DB_STEERING_PERIOD_TICKS == TICKS_PER_POSITION, "steering runs once per position poll, after it"); +_Static_assert((int)DB_STEERING_FAIL_NO_HEADING == (int)DB_WAYPOINTS_FAIL_NO_HEADING && (int)DB_STEERING_FAIL_SETTLE == (int)DB_WAYPOINTS_FAIL_SETTLE, + "the advertisement carries the steering's fail reason as is"); +_Static_assert(DB_STEERING_MAX_POINTS == DB_MAX_WAYPOINTS, "a batch fills the steering"); + +/// Largest wheel speed a command may set, in mm/s. A count then takes 135 us, +/// just over the QDEC's default 128 us sample period. +#define WHEEL_SPEED_MAX_MM_S (700) + +/// Room for the largest command this app accepts, header byte included: a full +/// waypoint batch, threshold and count first, then its trailer and headings +#define RX_MAILBOX_BYTES (1U + sizeof(uint16_t) + 1U + DB_MAX_WAYPOINTS * (sizeof(protocol_lh2_location_t) + sizeof(int16_t)) + sizeof(protocol_lh2_waypoints_trailer_t)) + +/// Cruise speeds a max speed command may set, mm/s +#define MAX_SPEED_MIN_MM_S (20U) +#define MAX_SPEED_MAX_MM_S (700U) + +#define DB_BUFFER_MAX_BYTES (255U) +#define TIMEOUT_STOP_TICKS (17000U) ///< ~500 ms of RTC ticks without a command + +/// NRF_RTC->COUNTER is 24 bits at 32768 Hz, so it wraps every 512 s and a plain +/// `now > then + delay` comparison is false for the whole pass after a wrap. +#define DB_RTC_COUNTER_MASK (0x00FFFFFFU) + +/// Coordinates above this are the secure side reporting no usable solve. +#define POSITION_INVALID_MM (100000U) + +/// The advertisement's "unknown" heading, sent while the estimator is not tracking +#define DIRECTION_INVALID (-1000) + +#if defined(DB_BENCH_TELEMETRY) || defined(DB_BENCH_TRACE) +/// Duty the bench records carry for a braked motor, outside [-100, 100] +#define PWM_BRAKED (INT8_MIN) +/// Duty the bench records carry for a wheel the loop has stalled, outside [-100, 100] +#define PWM_STALLED (INT8_MIN + 1) +#endif + +#if defined(DB_BENCH_TELEMETRY) +/// Bench-only frame type; keep it out of protocol_data_type_t, where 13 is unused. +#define DB_PROTOCOL_BENCH_TELEMETRY (13) +#define TELEMETRY_STEPS (24U) ///< Steps one frame carries, the newest kept +#define TELEMETRY_FIXES (4U) ///< New solves one frame carries, the newest kept +#endif + +/// Who writes the motors. Exactly one writer per mode. +typedef enum { + DRIVE_IDLE, ///< Nothing commanded; the wheel loop at zero brakes a turning wheel, else coasts + DRIVE_RAW, ///< MOVE_RAW writes duty directly and the wheel loop is off + DRIVE_VELOCITY, ///< The wheel loop is the only writer + DRIVE_WAYPOINT, ///< The steering sets the wheel loop's setpoints, or holds the motors braked +} drive_mode_t; typedef struct { uint32_t x; ///< X coordinate in mm uint32_t y; ///< Y coordinate in mm } position_2d_t; +_Static_assert(sizeof(position_2d_t) == 8, "must match the bootloader's layout"); +_Static_assert(offsetof(position_2d_t, y) == 4, "must match the bootloader's layout"); + +typedef struct { + uint8_t radio_buffer[DB_BUFFER_MAX_BYTES]; + uint32_t ts_last_packet_received; ///< RTC ticks at the last command received + position_2d_t position; ///< Last solve accepted from the secure side + uint32_t fix_sequence; ///< Sequence of the last solve seen; 0 before the first + bool has_position; ///< False until the first in-bounds solve + uint32_t encoder_total_left; ///< Counts since boot; wraps, and deltas are taken modulo 2^32 + uint32_t encoder_total_right; ///< Counts since boot; wraps, and deltas are taken modulo 2^32 + uint32_t double_total_left; ///< Double transitions since boot, already credited in the totals + uint32_t double_total_right; ///< Double transitions since boot, already credited in the totals + drive_mode_t drive_mode; ///< Which writer owns the motors + int8_t pwm_left; ///< Last commanded duty, reported back for telemetry + int8_t pwm_right; ///< Last commanded duty, reported back for telemetry + bool brake_left; ///< Left motor shorted, its duty ignored + bool brake_right; ///< Right motor shorted, its duty ignored + uint32_t max_tick_backlog; ///< Worst number of ticks the main loop fell behind +} bench_vars_t; +/// One consumer's position in the encoder totals. Each consumer keeps its own, +/// so reading counts does not take them from anybody else. typedef struct { - uint32_t ts_last_packet_received; ///< Last timestamp in microseconds a control packet was received - uint8_t radio_buffer[DB_BUFFER_MAX_BYTES]; ///< Internal buffer that contains the command to send (from buttons) - position_2d_t last_position; ///< Last computed LH2 location received - protocol_control_mode_t control_mode; ///< Remote control mode - protocol_lh2_waypoints_t waypoints; ///< List of waypoints - volatile bool update_control_loop; ///< Whether the control loop need an update - volatile bool advertize; ///< Whether an advertize packet should be sent - volatile bool update_position; ///< Whether position must be updated - uint64_t device_id; ///< Device ID of the DotBot -} dotbot_vars_t; + uint32_t left; ///< Left total at this consumer's last read + uint32_t right; ///< Right total at this consumer's last read +} encoder_cursor_t; //============================= swarmit ======================================== typedef void (*ipc_isr_cb_t)(const uint8_t *, size_t); -// Swarmit NSC callbable functions -void swarmit_keep_alive(void); - -void swarmit_send_raw_data(const uint8_t *packet, uint8_t length); -void swarmit_ipc_isr(ipc_isr_cb_t cb); - -void swarmit_localization_get_position(position_2d_t *position); +// Swarmit NSC callable functions +void swarmit_keep_alive(void); +void swarmit_send_raw_data(const uint8_t *packet, uint8_t length); +void swarmit_ipc_isr(ipc_isr_cb_t cb); +uint32_t swarmit_localization_get_fix(position_2d_t *position); void swarmit_get_battery_level(uint16_t *battery_level); void swarmit_localization_handle_isr(void); uint32_t swarmit_get_min_tx_interval_us(void); //=========================== variables ======================================== -static dotbot_vars_t _dotbot_vars = { 0 }; -static robot_control_t _control_vars = { 0 }; -static void *_control_ctx = NULL; +static bench_vars_t _vars = { 0 }; + +/// The advertisement's own cursor into the encoder totals. +static encoder_cursor_t _advertisement_encoders = { 0 }; + +static volatile uint32_t _tick_count = 0; ///< Written by the tick callback only +static uint32_t _tick_serviced = 0; ///< Read and written by the main loop only +static uint32_t _tick_position = 0; ///< Tick of the last position poll, main loop only +static uint32_t _tick_timeout = 0; ///< Tick of the last timeout check, main loop only +static uint32_t _tick_advert = 0; ///< Tick of the last advertisement, main loop only +static uint32_t _advert_period = 0; ///< Ticks between advertisements, re-derived at each one +static uint32_t _tick_wheel = 0; ///< Tick of the last wheel step, main loop only + +/// The wheel loop's own cursor into the encoder totals +static encoder_cursor_t _wheel_encoders = { 0 }; + +/// Feedforward from an untethered open-loop duty sweep of one v3 on the office +/// carpet (breakaway 37-45, rolling at 32 + 0.092..0.101 duty per mm/s for +/// either wheel and direction); ki from closed-loop holds, kp from step +/// responses. Full duty, and no slew limit short of it. +static const db_wheel_control_conf_t _wheel_conf = { + .kp = 0.52f, + .ki = 5.2f, + .u_breakaway = 44.0f, + .kick_ramp = 0.5f, + .u_run = 32.0f, + .k_run = 0.097f, + .i_zone = 38.0f, + .pwm_max = 100.0f, + .pwm_slew_per_tick = 100.0f, + .stall_pwm = 80.0f, + .stall_ms = 500U, +}; +static db_wheel_control_t _wheel_left; +static db_wheel_control_t _wheel_right; + +static const db_steering_conf_t _steering_conf = { + .lever_mm = DB_LH2_LEVER_ARM_EFFECTIVE, + .v_max_mm_s = DB_STEERING_V_MAX_MM_S, + .approach_per_s = DB_STEERING_APPROACH_PER_S, + .runon_s = DB_STEERING_RUNON_S, + .spin_mm_s = DB_STEERING_SPIN_MM_S, + .spin_min_mm_s = DB_STEERING_SPIN_MIN_MM_S, + .heading_kp = DB_STEERING_HEADING_KP, + .heading_kd = DB_STEERING_HEADING_KD, + .align_enter_deg = DB_STEERING_ALIGN_ENTER_DEG, + .align_exit_deg = DB_STEERING_ALIGN_EXIT_DEG, + .full_speed_deg = DB_STEERING_FULL_SPEED_DEG, + .final_tol_deg = DB_STEERING_FINAL_TOL_DEG, + .near_mm = DB_STEERING_NEAR_MM, + .bearing_min_mm = DB_STEERING_BEARING_MIN_MM, + .lookahead_s = DB_STEERING_LOOKAHEAD_S, + .arrival_min_mm = DB_STEERING_ARRIVAL_MIN_MM, + .precise_min_mm = DB_STEERING_PRECISE_MIN_MM, + .pass_mm = DB_STEERING_PASS_MM, + .creep_mm_s = DB_STEERING_CREEP_MM_S, + .settle_skip_ticks = DB_STEERING_SETTLE_SKIP_TICKS, + .settle_fixes = DB_STEERING_SETTLE_FIXES, + .settle_ticks = DB_STEERING_SETTLE_TICKS, + .settle_nudges = DB_STEERING_SETTLE_NUDGES, + .nudge_ticks = DB_STEERING_NUDGE_TICKS, + .no_heading_turn_ticks = DB_STEERING_NO_HEADING_TURN_TICKS, + .no_heading_ticks = DB_STEERING_NO_HEADING_TICKS, + .turn_ticks = DB_STEERING_TURN_TICKS, + .progress_ticks = DB_STEERING_PROGRESS_TICKS, + .progress_mm = DB_STEERING_PROGRESS_MM, + .hold_ticks = DB_STEERING_HOLD_TICKS, + .recover = DB_STEERING_RECOVER_DRIVE, + .recover_mm = DB_STEERING_RECOVER_MM, + .recover_mm_s = DB_STEERING_RECOVER_MM_S, + .bounds_mm = { 0, 0, 10000.0f, 10000.0f }, // the LH2 calibration's validity rectangle + .bounds_margin_mm = DB_STEERING_BOUNDS_MARGIN_MM, +}; +#if defined(DB_BENCH_TELEMETRY) +/// The same with the spin recovery, selected by a bench command +static const db_steering_conf_t _steering_conf_spin = { + .lever_mm = DB_LH2_LEVER_ARM_EFFECTIVE, + .v_max_mm_s = DB_STEERING_V_MAX_MM_S, + .approach_per_s = DB_STEERING_APPROACH_PER_S, + .runon_s = DB_STEERING_RUNON_S, + .spin_mm_s = DB_STEERING_SPIN_MM_S, + .spin_min_mm_s = DB_STEERING_SPIN_MIN_MM_S, + .heading_kp = DB_STEERING_HEADING_KP, + .heading_kd = DB_STEERING_HEADING_KD, + .align_enter_deg = DB_STEERING_ALIGN_ENTER_DEG, + .align_exit_deg = DB_STEERING_ALIGN_EXIT_DEG, + .full_speed_deg = DB_STEERING_FULL_SPEED_DEG, + .final_tol_deg = DB_STEERING_FINAL_TOL_DEG, + .near_mm = DB_STEERING_NEAR_MM, + .bearing_min_mm = DB_STEERING_BEARING_MIN_MM, + .lookahead_s = DB_STEERING_LOOKAHEAD_S, + .arrival_min_mm = DB_STEERING_ARRIVAL_MIN_MM, + .precise_min_mm = DB_STEERING_PRECISE_MIN_MM, + .pass_mm = DB_STEERING_PASS_MM, + .creep_mm_s = DB_STEERING_CREEP_MM_S, + .settle_skip_ticks = DB_STEERING_SETTLE_SKIP_TICKS, + .settle_fixes = DB_STEERING_SETTLE_FIXES, + .settle_ticks = DB_STEERING_SETTLE_TICKS, + .settle_nudges = DB_STEERING_SETTLE_NUDGES, + .nudge_ticks = DB_STEERING_NUDGE_TICKS, + .no_heading_turn_ticks = DB_STEERING_NO_HEADING_TURN_TICKS, + .no_heading_ticks = DB_STEERING_NO_HEADING_TICKS, + .turn_ticks = DB_STEERING_TURN_TICKS, + .progress_ticks = DB_STEERING_PROGRESS_TICKS, + .progress_mm = DB_STEERING_PROGRESS_MM, + .hold_ticks = DB_STEERING_HOLD_TICKS, + .recover = DB_STEERING_RECOVER_SPIN, + .recover_mm = DB_STEERING_RECOVER_MM, + .recover_mm_s = DB_STEERING_RECOVER_MM_S, + .bounds_mm = { 0, 0, 10000.0f, 10000.0f }, // the LH2 calibration's validity rectangle + .bounds_margin_mm = DB_STEERING_BOUNDS_MARGIN_MM, +}; +#endif +/// Read over the debugger for its state and failure reason +__attribute__((used)) static db_steering_t _steering; +static uint32_t _tick_steering = 0; ///< Tick of the last steering step, main loop only +static bool _steering_brake = false; ///< The steering holds the motors braked +static uint8_t _batch_id = 0; ///< Of the last batch accepted, 0 for none +static protocol_waypoints_abort_t _abort_reason = DB_WAYPOINTS_ABORT_STOP; ///< What stopped the last batch + +static const db_pose_estimator_conf_t _estimator_conf = { + .lever_mm = DB_LH2_LEVER_ARM_EFFECTIVE, + .lever_angle_deg = DB_LH2_LEVER_ANGLE, + .r_pos_mm2 = DB_POSE_ESTIMATOR_R_POS_MM2, + .q_pos_mm2_per_mm = DB_POSE_ESTIMATOR_Q_POS_MM2_PER_MM, + .q_heading_roll_deg2_per_mm = DB_POSE_ESTIMATOR_Q_HEADING_ROLL_DEG2_PER_MM, + .q_heading_turn_deg2_per_mm = DB_POSE_ESTIMATOR_Q_HEADING_TURN_DEG2_PER_MM, + .turn_speed_ref_mm_s = DB_POSE_ESTIMATOR_TURN_SPEED_REF_MM_S, + .gate = DB_POSE_ESTIMATOR_GATE, + .fix_age_ticks = DB_POSE_ESTIMATOR_FIX_AGE_TICKS, + .timeout_ticks = DB_POSE_ESTIMATOR_TIMEOUT_TICKS, + .seed_fixes = DB_POSE_ESTIMATOR_SEED_FIXES, + .seed_tolerance_mm = DB_POSE_ESTIMATOR_SEED_TOLERANCE_MM, + .acquire_mm = DB_POSE_ESTIMATOR_ACQUIRE_MM, + .kidnap_fixes = DB_POSE_ESTIMATOR_KIDNAP_FIXES, + .kidnap_still_mm = DB_POSE_ESTIMATOR_KIDNAP_STILL_MM, + .kidnap_settle_ticks = DB_POSE_ESTIMATOR_KIDNAP_SETTLE_TICKS, + .still_mm_s = DB_POSE_ESTIMATOR_STILL_MM_S, + .reanchor_mm = DB_POSE_ESTIMATOR_REANCHOR_MM, + .reanchor_heading_var_deg2 = DB_POSE_ESTIMATOR_REANCHOR_HEADING_VAR_DEG2, + .q_pos_slip_mm2_per_mm_s = DB_POSE_ESTIMATOR_Q_POS_SLIP_MM2_PER_MM_S, + .q_heading_slip_deg2_per_mm_s = DB_POSE_ESTIMATOR_Q_HEADING_SLIP_DEG2_PER_MM_S, + .slip_deadband_mm_s = DB_POSE_ESTIMATOR_SLIP_DEADBAND_MM_S, + .speed_tau_ms = DB_POSE_ESTIMATOR_SPEED_TAU_MS, +}; +/// Read over the debugger for its counters and covariance +__attribute__((used)) static db_pose_estimator_t _estimator; +static encoder_cursor_t _estimator_encoders = { 0 }; ///< The estimator's own cursor into the encoder totals +static uint32_t _tick_estimator = 0; ///< Tick of the last predict, main loop only + +/// Commands arrive in the IPC interrupt and are applied on the next tick, so +/// the main loop is the only writer of the drive state and the motors. +static uint8_t _rx_buffer[RX_MAILBOX_BYTES]; +static size_t _rx_length = 0; +static volatile bool _rx_pending = false; + +#if defined(DB_BENCH_TRACE) +/// One tick while anything drives the motors, read back over the debugger +typedef struct __attribute__((packed)) { + uint32_t tick; ///< Serviced tick + int16_t setpoint_left; ///< mm/s + int16_t setpoint_right; ///< mm/s + int16_t counts_left; ///< Credited counts over this step + int16_t counts_right; ///< Credited counts over this step + int8_t pwm_left; ///< Duty written, PWM_BRAKED while braked, PWM_STALLED while stalled + int8_t pwm_right; ///< Duty written, PWM_BRAKED while braked, PWM_STALLED while stalled + uint8_t elapsed; ///< Ticks this step covered + uint8_t mode; ///< drive_mode_t at this step +} wheel_trace_t; + +#define TRACE_LENGTH (1000U) ///< 10 s of steps +#define TRACE_TAIL_TICKS (100U) ///< Keep recording this long after the loop stops + +__attribute__((used)) static wheel_trace_t _trace[TRACE_LENGTH]; +__attribute__((used)) static uint32_t _trace_count = 0; +static uint32_t _trace_tail = 0; + +/// Every new solve the secure side publishes, in a ring, whatever the drive +/// mode: fix rate and jitter come from the sequence against the tick +typedef struct __attribute__((packed)) { + uint32_t tick; ///< Serviced tick the solve was read on + uint32_t sequence; ///< Fix sequence + uint32_t x; ///< mm, as reported, before the bounds check + uint32_t y; ///< mm, as reported, before the bounds check +} fix_trace_t; + +#define FIX_TRACE_LENGTH (3000U) ///< 5 minutes at 10 Hz + +__attribute__((used)) static fix_trace_t _fix_trace[FIX_TRACE_LENGTH]; +__attribute__((used)) static uint32_t _fix_trace_count = 0; ///< Total written; the ring index is this modulo the length +#endif + +#if defined(DB_BENCH_TELEMETRY) +/// One wheel step, as the telemetry frame carries it +typedef struct __attribute__((packed)) { + int8_t counts_left; ///< Credited counts over this step, saturated + int8_t counts_right; ///< Credited counts over this step, saturated + int8_t pwm_left; ///< Duty written, PWM_BRAKED while braked, PWM_STALLED while stalled + int8_t pwm_right; ///< Duty written, PWM_BRAKED while braked, PWM_STALLED while stalled + int8_t setpoint_left; ///< In units of 10 mm/s + int8_t setpoint_right; ///< In units of 10 mm/s +} telemetry_step_t; -#ifdef DB_RGB_LED_PWM_RED_PORT // Only available on DotBot v2 -static const db_rgbled_pwm_conf_t rgbled_pwm_conf = { +/// One new solve, as the telemetry frame carries it +typedef struct __attribute__((packed)) { + uint16_t tick; ///< Low 16 bits of the serviced tick the solve was read on + uint16_t sequence; ///< Low 16 bits of the fix sequence + uint16_t x; ///< mm, before the bounds check, saturated + uint16_t y; ///< mm, before the bounds check, saturated +} telemetry_fix_t; + +static telemetry_step_t _telemetry_steps[TELEMETRY_STEPS]; +static uint32_t _telemetry_step_count = 0; ///< Steps recorded; the ring index is this modulo the length +static uint32_t _telemetry_step_sent = 0; ///< Step count at the last frame +static uint32_t _telemetry_step_tick = 0; ///< Serviced tick of the newest step +static telemetry_fix_t _telemetry_fixes[TELEMETRY_FIXES]; +static uint32_t _telemetry_fix_count = 0; ///< Solves recorded; the ring index is this modulo the length +static uint32_t _telemetry_fix_sent = 0; ///< Solve count at the last frame +#endif + +#ifdef DB_RGB_LED_PWM_RED_PORT +static const db_rgbled_pwm_conf_t _rgbled_pwm_conf = { .pwm = 1, .pins = { { .port = DB_RGB_LED_PWM_RED_PORT, .pin = DB_RGB_LED_PWM_RED_PIN }, @@ -97,7 +380,7 @@ static const db_rgbled_pwm_conf_t rgbled_pwm_conf = { }; #endif -#ifdef DB_QDEC_LEFT_A_PORT // Boards that carry wheel quadrature encoders +#ifdef DB_QDEC_LEFT_A_PORT static const qdec_conf_t _qdec_left_conf = { .pin_a = &db_qdec_left_a_pin, .pin_b = &db_qdec_left_b_pin, @@ -109,196 +392,436 @@ static const qdec_conf_t _qdec_right_conf = { }; #endif -static volatile uint32_t _advert_period = ADVERT_PERIOD_DEF_MS; ///< Written by the main loop -static volatile uint32_t _advert_elapsed_ms = 0; ///< Advert timer callback only - //=========================== prototypes ======================================= +static void _tick(void); +static void _service_tick(uint32_t tick); +static void _encoders_init(void); +static void _encoders_accumulate(void); +static void _encoders_delta(encoder_cursor_t *cursor, int32_t *left, int32_t *right); +static void _position_poll(void); static void _timeout_check(void); static void _advertise(void); -static uint32_t _advert_period_ms(void); -static void _update_control_loop(void); -static void _position_update(void); -static void _encoders_init(void); -static void _encoders_read(int32_t *left, int32_t *right); +static uint32_t _advert_period_ticks(void); +static void _set_motors(int16_t left, int16_t right, bool brake_left, bool brake_right); +static void _rx_process(void); +static void _drive_stop(void); +static void _note_abort(protocol_waypoints_abort_t reason); +static void _wheel_service(uint32_t tick); +static void _estimator_service(uint32_t tick); +static void _steering_service(uint32_t tick); +static void _steering_poll(void); +static void _steering_apply(const db_steering_output_t *out); +static void _steering_pose(db_steering_pose_t *pose); +static void _enter_drive_mode(drive_mode_t mode); + +/// Elapsed rather than a multiple, since the main loop drops its backlog and +/// can step over any given tick. +static inline bool _due(uint32_t *last, uint32_t tick, uint32_t period) { + if (tick - *last < period) { + return false; + } + *last = tick; + return true; +} + +static inline uint32_t _ticks_since(uint32_t then) { + return (db_timer_ticks(TIMER_DEV) - then) & DB_RTC_COUNTER_MASK; +} //=========================== callbacks ======================================== +/// The newest command replaces one the main loop has not applied yet static void _rx_data_callback(const uint8_t *pkt, size_t len) { - (void)len; + if (len == 0 || len > sizeof(_rx_buffer)) { + return; + } + if (pkt[0] == DB_PROTOCOL_CMD_MOVE_RAW || pkt[0] == DB_PROTOCOL_CMD_WHEEL_VELOCITY || pkt[0] == DB_PROTOCOL_LH2_WAYPOINTS) { + _vars.ts_last_packet_received = db_timer_ticks(TIMER_DEV); + } + memcpy(_rx_buffer, pkt, len); + _rx_length = len; + __DMB(); // the buffer is complete before the flag says so + _rx_pending = true; +} + +//=========================== main ============================================= + +int main(void) { + db_board_init(); +#ifdef DB_RGB_LED_PWM_RED_PORT + db_rgbled_pwm_init(&_rgbled_pwm_conf); +#endif + db_motors_init(); + _encoders_init(); + db_wheel_control_init(&_wheel_left, &_wheel_conf); + db_wheel_control_init(&_wheel_right, &_wheel_conf); + db_pose_estimator_init(&_estimator, &_estimator_conf); + db_steering_init(&_steering, &_steering_conf); + db_gpio_init(&db_led1, DB_GPIO_OUT); + + _advert_period = _advert_period_ticks(); + db_timer_init(TIMER_DEV); + db_timer_set_periodic_ms(TIMER_DEV, 0, TICK_MS, &_tick); + + while (1) { + __WFE(); + + uint32_t now = _tick_count; + uint32_t missed = now - _tick_serviced; + if (missed == 0) { + continue; + } + // The backlog is dropped, not replayed; the worst case is telemetered + if (missed - 1 > _vars.max_tick_backlog) { + _vars.max_tick_backlog = missed - 1; + } + _tick_serviced = now; + _service_tick(now); + } +} - _dotbot_vars.ts_last_packet_received = db_timer_ticks(TIMER_DEV); - uint8_t *cmd_ptr = (uint8_t *)pkt; - // parse received packet and update the motors' speeds - switch ((uint8_t)*cmd_ptr++) { +//=========================== private functions ================================ + +static void _tick(void) { + _tick_count++; +} + +static void _service_tick(uint32_t tick) { + _rx_process(); + _encoders_accumulate(); + _wheel_service(tick); + _estimator_service(tick); + + if (_due(&_tick_position, tick, TICKS_PER_POSITION)) { + _position_poll(); + } + if (_due(&_tick_steering, tick, DB_STEERING_PERIOD_TICKS)) { + _steering_service(tick); + } + _steering_poll(); + if (_due(&_tick_timeout, tick, TICKS_PER_TIMEOUT)) { + _timeout_check(); + } + if (_due(&_tick_advert, tick, _advert_period)) { + _advert_period = _advert_period_ticks(); + _advertise(); + } +} + +static void _rx_process(void) { + if (!_rx_pending) { + return; + } + // Masked so the IPC interrupt cannot replace the buffer mid-copy + uint32_t primask = __get_PRIMASK(); + __disable_irq(); + __DMB(); // read the buffer only after seeing the flag + uint8_t packet[RX_MAILBOX_BYTES]; + size_t length = _rx_length; + memcpy(packet, _rx_buffer, length); + __DMB(); // the copy is complete before the flag frees the buffer + _rx_pending = false; + __set_PRIMASK(primask); + + const uint8_t *payload = &packet[1]; + switch (packet[0]) { case DB_PROTOCOL_CMD_MOVE_RAW: { - protocol_move_raw_command_t *command = (protocol_move_raw_command_t *)cmd_ptr; - int16_t left = (int16_t)(100 * ((float)command->left_y / INT8_MAX)); - int16_t right = (int16_t)(100 * ((float)command->right_y / INT8_MAX)); - _control_vars.pwm_left = left; - _control_vars.pwm_right = right; - db_motors_set_pwm(left, right); + if (length < 1 + sizeof(protocol_move_raw_command_t)) { + break; + } + protocol_move_raw_command_t command; + memcpy(&command, payload, sizeof(command)); + _note_abort(DB_WAYPOINTS_ABORT_DIRECT); + _enter_drive_mode(DRIVE_RAW); + _set_motors((int16_t)(100 * ((float)command.left_y / INT8_MAX)), (int16_t)(100 * ((float)command.right_y / INT8_MAX)), false, false); + } break; + case DB_PROTOCOL_CMD_WHEEL_VELOCITY: + { + if (length < 1 + sizeof(protocol_wheel_velocity_command_t)) { + break; + } + protocol_wheel_velocity_command_t command; + memcpy(&command, payload, sizeof(command)); + if (_vars.drive_mode != DRIVE_VELOCITY) { + _note_abort(DB_WAYPOINTS_ABORT_DIRECT); + _enter_drive_mode(DRIVE_VELOCITY); + } + int16_t left = command.left_mm_s; + int16_t right = command.right_mm_s; + left = (left > WHEEL_SPEED_MAX_MM_S) ? WHEEL_SPEED_MAX_MM_S : ((left < -WHEEL_SPEED_MAX_MM_S) ? -WHEEL_SPEED_MAX_MM_S : left); + right = (right > WHEEL_SPEED_MAX_MM_S) ? WHEEL_SPEED_MAX_MM_S : ((right < -WHEEL_SPEED_MAX_MM_S) ? -WHEEL_SPEED_MAX_MM_S : right); + db_wheel_control_set_setpoint(&_wheel_left, left); + db_wheel_control_set_setpoint(&_wheel_right, right); } break; case DB_PROTOCOL_CMD_RGB_LED: { - protocol_rgbled_command_t *command = (protocol_rgbled_command_t *)cmd_ptr; - db_rgbled_pwm_set_color(command->r, command->g, command->b); +#ifdef DB_RGB_LED_PWM_RED_PORT + if (length < 1 + sizeof(protocol_rgbled_command_t)) { + break; + } + protocol_rgbled_command_t command; + memcpy(&command, payload, sizeof(command)); + db_rgbled_pwm_set_color(command.r, command.g, command.b); +#endif } break; - case DB_PROTOCOL_CONTROL_MODE: - db_motors_set_pwm(0, 0); - break; case DB_PROTOCOL_LH2_WAYPOINTS: { - _dotbot_vars.control_mode = ControlManual; - uint16_t threshold = 0; - memcpy(&threshold, cmd_ptr, sizeof(uint16_t)); - cmd_ptr += sizeof(uint16_t); - uint8_t count = (uint8_t)*cmd_ptr++; - if (count > DB_MAX_WAYPOINTS) { - count = DB_MAX_WAYPOINTS; + db_steering_path_t path; + uint8_t batch_id; + if (!db_steering_path_from_wire(payload, length - 1, &path, &batch_id)) { + break; } - memcpy(&_dotbot_vars.waypoints.points, cmd_ptr, count * sizeof(protocol_lh2_location_t)); - coordinate_t waypoints[DB_MAX_WAYPOINTS]; - for (uint8_t i = 0; i < count; i++) { - waypoints[i].x = _dotbot_vars.waypoints.points[i].x; - waypoints[i].y = _dotbot_vars.waypoints.points[i].y; +#if defined(DB_BENCH_TELEMETRY) + // Bench only: a threshold of 0xFFFF drops the estimator's pose, as a + // kidnap does, to exercise the steering's heading recovery; 0xFFFE + // does the same with the spin recovery, until the next waypoint + if (path.threshold_mm >= (float)(UINT16_MAX - 1)) { + _steering.conf = (path.threshold_mm == (float)UINT16_MAX) ? &_steering_conf : &_steering_conf_spin; + db_pose_estimator_init(&_estimator, &_estimator_conf); + break; } - control_loop_set_waypoints(_control_ctx, waypoints, count, (uint32_t)threshold); - // Drain whatever the wheels accumulated while idle, so the first - // control step of the new batch integrates only its own motion. - _encoders_read(&_control_vars.encoder_left, &_control_vars.encoder_right); - _control_vars.encoder_left = 0; - _control_vars.encoder_right = 0; - if (count > 0) { - _dotbot_vars.control_mode = ControlAuto; - } else { - db_motors_set_pwm(0, 0); - _dotbot_vars.control_mode = ControlManual; + _steering.conf = &_steering_conf; +#endif + // A resent batch the robot already has, its advertisement not yet heard + if (batch_id != 0 && batch_id == _batch_id) { + break; + } + _batch_id = batch_id; + if (path.count == 0) { + _note_abort(DB_WAYPOINTS_ABORT_STOP); + _drive_stop(); + break; + } + if (_vars.drive_mode != DRIVE_WAYPOINT) { + _enter_drive_mode(DRIVE_WAYPOINT); } + _steering_brake = false; + db_steering_set_path(&_steering, &path); } break; + case DB_PROTOCOL_CMD_MAX_SPEED: + { + if (length < 1 + sizeof(protocol_max_speed_command_t)) { + break; + } + protocol_max_speed_command_t command; + memcpy(&command, payload, sizeof(command)); + uint16_t v = command.max_speed_mm_s; + if (v != 0) { + v = (v < MAX_SPEED_MIN_MM_S) ? MAX_SPEED_MIN_MM_S : ((v > MAX_SPEED_MAX_MM_S) ? MAX_SPEED_MAX_MM_S : v); + } + db_steering_set_max_speed(&_steering, (float)v); + } break; + case DB_PROTOCOL_CONTROL_MODE: + _note_abort(DB_WAYPOINTS_ABORT_CONTROL_MODE); + _drive_stop(); + break; default: break; } } -//=========================== main ============================================= +/// Records what stops a batch in progress; later commands leave the reason as it is +static void _note_abort(protocol_waypoints_abort_t reason) { + if (db_steering_active(&_steering)) { + _abort_reason = reason; + } +} -int main(void) { - db_board_init(); -#ifdef DB_RGB_LED_PWM_RED_PORT - db_rgbled_pwm_init(&rgbled_pwm_conf); +/// Brakes both motors; from the next tick the wheel loop releases each one once +/// its wheel stands +static void _drive_stop(void) { + _enter_drive_mode(DRIVE_IDLE); + _set_motors(0, 0, true, true); +} + +/// Hands the motors to a new writer: the wheel loop starts from zero and the +/// steering drops its target unless it is the new writer +static void _enter_drive_mode(drive_mode_t mode) { +#if defined(DB_BENCH_TRACE) + if (_vars.drive_mode == DRIVE_IDLE && mode != DRIVE_IDLE) { + _trace_count = 0; + } #endif - db_motors_init(); - _encoders_init(); - db_gpio_init(&db_led1, DB_GPIO_OUT); - _control_ctx = control_loop_alloc(); + if (mode != DRIVE_WAYPOINT) { + db_steering_stop(&_steering); + } + _steering_brake = false; + _vars.drive_mode = mode; + db_wheel_control_reset(&_wheel_left); + db_wheel_control_reset(&_wheel_right); +} - // Set an invalid heading since the value is unknown on startup. - // Control loop is stopped - _control_vars.direction = DB_DIRECTION_INVALID; - _dotbot_vars.update_control_loop = false; - _dotbot_vars.advertize = false; - _dotbot_vars.update_position = false; +#if defined(DB_BENCH_TELEMETRY) +static inline int8_t _saturate_i8(int32_t value) { + return (value > INT8_MAX) ? INT8_MAX : ((value < INT8_MIN) ? INT8_MIN : (int8_t)value); +} +#endif - // Retrieve the device id once at startup - _dotbot_vars.device_id = db_device_id(); +#if defined(DB_BENCH_TELEMETRY) || defined(DB_BENCH_TRACE) +static inline int8_t _pwm_recorded(int8_t pwm, bool brake, bool stalled) { + return brake ? PWM_BRAKED : (stalled ? PWM_STALLED : pwm); +} +#endif - db_timer_init(TIMER_DEV); - db_timer_set_periodic_ms(TIMER_DEV, 0, DB_TIMEOUT_CHECK_DELAY_MS, &_timeout_check); - db_timer_set_periodic_ms(TIMER_DEV, 1, DB_POSITION_UPDATE_DELAY_MS, &_position_update); - db_timer_set_periodic_ms(TIMER_DEV, 2, ADVERT_PERIOD_MIN_MS, &_advertise); +/// Runs on every tick so the cursor never lags, and writes the motors only +/// while the loop owns them: driving by velocity, and after a stop, where its +/// zero setpoints brake the wheels until they stand +static void _wheel_service(uint32_t tick) { + uint32_t elapsed = tick - _tick_wheel; + _tick_wheel = tick; + int32_t left; + int32_t right; + _encoders_delta(&_wheel_encoders, &left, &right); - while (1) { - __WFE(); + if (_steering_brake) { + _set_motors(0, 0, true, true); + } else if (_vars.drive_mode != DRIVE_RAW) { + int8_t pwm_left = db_wheel_control_step(&_wheel_left, left, elapsed); + int8_t pwm_right = db_wheel_control_step(&_wheel_right, right, elapsed); + _set_motors(pwm_left, pwm_right, _wheel_left.brake, _wheel_right.brake); + } - if (_dotbot_vars.update_position) { - _dotbot_vars.update_position = false; - swarmit_keep_alive(); - swarmit_localization_get_position(&_dotbot_vars.last_position); +#if defined(DB_BENCH_TELEMETRY) + _telemetry_steps[_telemetry_step_count % TELEMETRY_STEPS] = (telemetry_step_t){ + .counts_left = _saturate_i8(left), + .counts_right = _saturate_i8(right), + .pwm_left = _pwm_recorded(_vars.pwm_left, _vars.brake_left, _wheel_left.stalled), + .pwm_right = _pwm_recorded(_vars.pwm_right, _vars.brake_right, _wheel_right.stalled), + .setpoint_left = _saturate_i8((int32_t)_wheel_left.setpoint / 10), + .setpoint_right = _saturate_i8((int32_t)_wheel_right.setpoint / 10), + }; + _telemetry_step_count++; + _telemetry_step_tick = tick; +#endif - if (_dotbot_vars.last_position.x > 100000 || _dotbot_vars.last_position.y > 100000) { - // Invalid coordinates, do not update direction and upload position - continue; - } +#if defined(DB_BENCH_TRACE) + if (_vars.drive_mode != DRIVE_IDLE) { + _trace_tail = TRACE_TAIL_TICKS; + } else if (_trace_tail > 0) { + _trace_tail--; + } else { + return; + } + if (_trace_count < TRACE_LENGTH) { + _trace[_trace_count++] = (wheel_trace_t){ + .tick = tick, + .setpoint_left = (int16_t)_wheel_left.setpoint, + .setpoint_right = (int16_t)_wheel_right.setpoint, + .counts_left = (int16_t)left, + .counts_right = (int16_t)right, + .pwm_left = _pwm_recorded(_vars.pwm_left, _vars.brake_left, _wheel_left.stalled), + .pwm_right = _pwm_recorded(_vars.pwm_right, _vars.brake_right, _wheel_right.stalled), + .elapsed = (uint8_t)elapsed, + .mode = (uint8_t)_vars.drive_mode, + }; + } +#endif +} - coordinate_t location = { - .x = _dotbot_vars.last_position.x, - .y = _dotbot_vars.last_position.y, - }; - coordinate_t last_location = { .x = _control_vars.pos_x, .y = _control_vars.pos_y }; - float dlx = (float)location.x - (float)last_location.x; - float dly = (float)location.y - (float)last_location.y; - if (_control_vars.pos_x != 0 && _control_vars.pos_y != 0 && - sqrtf(dlx * dlx + dly * dly) > DB_LH2_OUTLIER_THRESHOLD) { - continue; - } - int16_t angle = _control_vars.direction; - if (compute_angle(&last_location, &location, &angle)) { - _control_vars.direction = angle; - _control_vars.pos_x = location.x; - _control_vars.pos_y = location.y; - } - _dotbot_vars.update_control_loop = (_dotbot_vars.control_mode == ControlAuto); - } +/// Every tick, whatever drives the motors, so the pose follows any motion +static void _estimator_service(uint32_t tick) { + uint32_t elapsed = tick - _tick_estimator; + _tick_estimator = tick; + int32_t left; + int32_t right; + _encoders_delta(&_estimator_encoders, &left, &right); + db_pose_estimator_predict(&_estimator, left, right, elapsed); +} - if (_dotbot_vars.update_control_loop) { - _update_control_loop(); - _dotbot_vars.update_control_loop = false; - } +static db_steering_pose_status_t _steering_pose_status(db_pose_estimator_status_t status) { + switch (status) { + case DB_POSE_ESTIMATOR_TRACKING: + return DB_STEERING_POSE_TRACKING; + case DB_POSE_ESTIMATOR_LOST: + return DB_STEERING_POSE_LOST; + default: + return DB_STEERING_POSE_SEEDING; + } +} + +static void _steering_pose(db_steering_pose_t *pose) { + pose->status = _steering_pose_status(_estimator.status); + pose->x_mm = _estimator.x; + pose->y_mm = _estimator.y; + pose->heading_deg = _estimator.theta * 180.0f / (float)M_PI; +} - if (_dotbot_vars.advertize) { - size_t length = 0; - _dotbot_vars.radio_buffer[length++] = DB_PROTOCOL_DOTBOT_ADVERTISEMENT; - // calibrated bitmask hard-coded 0xff: secure side doesn't expose per-LH calibration state via NSC yet. - _dotbot_vars.radio_buffer[length++] = 0xff; - memcpy(&_dotbot_vars.radio_buffer[length], &_control_vars.direction, sizeof(int16_t)); - length += sizeof(int16_t); - protocol_lh2_location_t position = { - .x = _control_vars.pos_x, - .y = _control_vars.pos_y, - }; - memcpy(&_dotbot_vars.radio_buffer[length], &position, sizeof(protocol_lh2_location_t)); - length += sizeof(protocol_lh2_location_t); - uint16_t battery_level = 0; - swarmit_get_battery_level(&battery_level); - memcpy(&_dotbot_vars.radio_buffer[length], &battery_level, sizeof(uint16_t)); - length += sizeof(uint16_t); - memcpy(&_dotbot_vars.radio_buffer[length++], &_control_vars.pwm_left, sizeof(int8_t)); - memcpy(&_dotbot_vars.radio_buffer[length++], &_control_vars.pwm_right, sizeof(int8_t)); - memcpy(&_dotbot_vars.radio_buffer[length++], &_dotbot_vars.control_mode, sizeof(uint8_t)); - memcpy(&_dotbot_vars.radio_buffer[length], &_control_vars.encoder_left, sizeof(int32_t)); - length += sizeof(int32_t); - memcpy(&_dotbot_vars.radio_buffer[length], &_control_vars.encoder_right, sizeof(int32_t)); - length += sizeof(int32_t); - memcpy(&_dotbot_vars.radio_buffer[length], &_control_vars.waypoint_x, sizeof(uint32_t)); - length += sizeof(uint32_t); - memcpy(&_dotbot_vars.radio_buffer[length], &_control_vars.waypoint_y, sizeof(uint32_t)); - length += sizeof(uint32_t); - memcpy(&_dotbot_vars.radio_buffer[length++], &_control_vars.waypoint_idx, sizeof(uint8_t)); - swarmit_send_raw_data(_dotbot_vars.radio_buffer, length); - _dotbot_vars.advertize = false; - _advert_period = _advert_period_ms(); +/// A brake from the steering holds both motors shorted until it asks otherwise +static void _steering_apply(const db_steering_output_t *out) { + if (out->brake) { + if (!_steering_brake) { + db_wheel_control_reset(&_wheel_left); + db_wheel_control_reset(&_wheel_right); + _steering_brake = true; } + return; + } + _steering_brake = false; + // Past the wheel limit, both wheels give up the excess, so the turn is kept + float left = out->left_mm_s; + float right = out->right_mm_s; + float excess = fmaxf(fabsf(left), fabsf(right)) - WHEEL_SPEED_MAX_MM_S; + if (excess > 0) { + float shift = (left + right >= 0) ? excess : -excess; + left -= shift; + right -= shift; } + left = fmaxf(-WHEEL_SPEED_MAX_MM_S, fminf(WHEEL_SPEED_MAX_MM_S, left)); + right = fmaxf(-WHEEL_SPEED_MAX_MM_S, fminf(WHEEL_SPEED_MAX_MM_S, right)); + db_wheel_control_set_setpoint(&_wheel_left, left); + db_wheel_control_set_setpoint(&_wheel_right, right); } -//=========================== private functions ================================ +/// Once per position poll, right after it, while steering owns the wheel loop +static void _steering_service(uint32_t tick) { + static uint32_t last = 0; + uint32_t elapsed = tick - last; + last = tick; + if (_vars.drive_mode != DRIVE_WAYPOINT) { + return; + } + db_steering_pose_t pose; + _steering_pose(&pose); + db_steering_output_t out; + db_steering_step(&_steering, &pose, elapsed, &out); + _steering_apply(&out); +} -static void _update_control_loop(void) { - _encoders_read(&_control_vars.encoder_left, &_control_vars.encoder_right); - update_control(&_control_vars, _control_ctx); - db_motors_set_pwm(_control_vars.pwm_left, _control_vars.pwm_right); +/// Every tick, for the stops of a precise arrival that fall between steps +static void _steering_poll(void) { + if (_vars.drive_mode != DRIVE_WAYPOINT) { + return; + } + db_steering_pose_t pose; + _steering_pose(&pose); + db_steering_output_t out; + if (db_steering_poll(&_steering, &pose, &out)) { + _steering_apply(&out); + } +} - if (_control_vars.all_done) { - _dotbot_vars.control_mode = ControlManual; - _control_vars.encoder_left = 0; - _control_vars.encoder_right = 0; +/// From the node's minimum TX interval, so a gateway on another schedule changes +/// the rate within one period +static uint32_t _advert_period_ticks(void) { + uint32_t min_tx_interval_us = swarmit_get_min_tx_interval_us(); + uint32_t period_ms = ADVERT_PERIOD_DEF_MS; + if (min_tx_interval_us > 0) { + period_ms = (min_tx_interval_us / 1000U) * 100U / ADVERT_TX_SHARE_PERCENT; + if (period_ms < ADVERT_PERIOD_MIN_MS) { + period_ms = ADVERT_PERIOD_MIN_MS; + } else if (period_ms > ADVERT_PERIOD_MAX_MS) { + period_ms = ADVERT_PERIOD_MAX_MS; + } } +#if defined(DB_BENCH_TELEMETRY) + // Each advert is two frames + period_ms *= 2; +#endif + return period_ms / TICK_MS; } -// Wheel odometry. On a board without quadrature encoders the counts stay at -// zero, which the control loop reads as "unavailable". static void _encoders_init(void) { #ifdef DB_QDEC_LEFT_A_PORT db_qdec_init(QDEC_LEFT, &_qdec_left_conf, NULL, NULL); @@ -306,54 +829,249 @@ static void _encoders_init(void) { #endif } -static void _encoders_read(int32_t *left, int32_t *right) { +/// The hardware read is destructive, so the tick drains it into totals that are +/// never cleared; consumers take deltas against their own cursor. +static void _encoders_accumulate(void) { #ifdef DB_QDEC_LEFT_A_PORT - *left = db_qdec_read_and_clear(QDEC_LEFT); - *right = db_qdec_read_and_clear(QDEC_RIGHT); -#else - *left = 0; - *right = 0; + uint32_t dbl_left; + uint32_t dbl_right; + int32_t acc_left = db_qdec_read_and_clear_dbl(QDEC_LEFT, &dbl_left); + int32_t acc_right = db_qdec_read_and_clear_dbl(QDEC_RIGHT, &dbl_right); + _vars.encoder_total_left += (uint32_t)db_wheel_control_counts(acc_left, dbl_left); + _vars.encoder_total_right += (uint32_t)db_wheel_control_counts(acc_right, dbl_right); + _vars.double_total_left += dbl_left; + _vars.double_total_right += dbl_right; #endif } -static void _timeout_check(void) { - uint32_t ticks = db_timer_ticks(TIMER_DEV); - if (_dotbot_vars.control_mode != ControlAuto && ticks > _dotbot_vars.ts_last_packet_received + TIMEOUT_CHECK_DELAY_TICKS) { - db_motors_set_pwm(0, 0); - } +/// Counts since this cursor last read, leaving the totals for other consumers. +static void _encoders_delta(encoder_cursor_t *cursor, int32_t *left, int32_t *right) { + uint32_t total_left = _vars.encoder_total_left; + uint32_t total_right = _vars.encoder_total_right; + *left = (int32_t)(total_left - cursor->left); + *right = (int32_t)(total_right - cursor->right); + cursor->left = total_left; + cursor->right = total_right; } -/// Runs at the floor period and flags an advert once the derived period has -/// passed. The period is read from the budget in the main loop, not here. -static void _advertise(void) { - _advert_elapsed_ms += ADVERT_PERIOD_MIN_MS; - if (_advert_elapsed_ms < _advert_period) { +/// swarmit_keep_alive() runs the solve; call it immediately before reading the fix. +static void _position_poll(void) { + swarmit_keep_alive(); + + position_2d_t solve = { 0 }; + uint32_t sequence = swarmit_localization_get_fix(&solve); + + // An unchanged sequence is the previous solve read a second time + if (sequence == _vars.fix_sequence) { return; } - _advert_elapsed_ms = 0; - db_gpio_toggle(&db_led1); - _dotbot_vars.advertize = true; + _vars.fix_sequence = sequence; + +#if defined(DB_BENCH_TRACE) + _fix_trace[_fix_trace_count % FIX_TRACE_LENGTH] = (fix_trace_t){ + .tick = _tick_serviced, + .sequence = sequence, + .x = solve.x, + .y = solve.y, + }; + _fix_trace_count++; +#endif +#if defined(DB_BENCH_TELEMETRY) + _telemetry_fixes[_telemetry_fix_count % TELEMETRY_FIXES] = (telemetry_fix_t){ + .tick = (uint16_t)_tick_serviced, + .sequence = (uint16_t)sequence, + .x = (solve.x > UINT16_MAX) ? UINT16_MAX : (uint16_t)solve.x, + .y = (solve.y > UINT16_MAX) ? UINT16_MAX : (uint16_t)solve.y, + }; + _telemetry_fix_count++; +#endif + + if (solve.x > POSITION_INVALID_MM || solve.y > POSITION_INVALID_MM) { + return; + } + _vars.position = solve; + _vars.has_position = true; + db_pose_estimator_update(&_estimator, (float)solve.x, (float)solve.y); + db_steering_fix(&_steering, (float)solve.x, (float)solve.y); } -/// Derived from the node's minimum TX interval after every advert, so a -/// gateway on another schedule changes the rate within one period -static uint32_t _advert_period_ms(void) { - uint32_t min_tx_interval_us = swarmit_get_min_tx_interval_us(); - if (min_tx_interval_us == 0) { - return ADVERT_PERIOD_DEF_MS; +/// Raw and velocity driving both stop when the host goes silent. A waypoint +/// needs no resending: the steering stops on arrival, on losing its pose and +/// on its own timeouts. +static void _timeout_check(void) { + if (_vars.drive_mode != DRIVE_IDLE && _vars.drive_mode != DRIVE_WAYPOINT && _ticks_since(_vars.ts_last_packet_received) > TIMEOUT_STOP_TICKS) { + _drive_stop(); } - uint32_t period_ms = (min_tx_interval_us / 1000U) * 100U / ADVERT_TX_SHARE_PERCENT; - if (period_ms < ADVERT_PERIOD_MIN_MS) { - return ADVERT_PERIOD_MIN_MS; +} + +static void _set_motors(int16_t left, int16_t right, bool brake_left, bool brake_right) { + db_motors_set_pwm_brake(left, right, brake_left, brake_right); + _vars.pwm_left = brake_left ? 0 : (int8_t)left; + _vars.pwm_right = brake_right ? 0 : (int8_t)right; + _vars.brake_left = brake_left; + _vars.brake_right = brake_right; +} + +static void _put(uint8_t *buf, size_t *length, const void *value, size_t size) { + memcpy(&buf[*length], value, size); + *length += size; +} + +#if defined(DB_BENCH_TELEMETRY) +static inline uint8_t _saturate_u8(uint32_t value) { + return (value > UINT8_MAX) ? UINT8_MAX : (uint8_t)value; +} + +/// Every step and every new solve since the previous frame, newest kept when +/// there are more than a frame holds; resets the worst tick backlog it reports. +/// Layout: type, newest step tick (u32), steps carried, steps dropped, backlog, +/// drive mode in the low nibble with the steering state in the high one, +/// encoder totals (i32 x 2), solves carried, solves dropped, then the steps +/// oldest first, then the solves oldest first, then the estimator: status, the +/// last gated fix's squared distance x 10 (u16, saturated), and its kidnap and +/// re-anchor counts (u8 each, wrapping), then the steering's point index and +/// corrections made (u8 each). +static void _send_bench_telemetry(void) { + size_t length = 0; + uint8_t *buf = _vars.radio_buffer; + + uint32_t steps = _telemetry_step_count - _telemetry_step_sent; + uint32_t steps_out = (steps > TELEMETRY_STEPS) ? TELEMETRY_STEPS : steps; + uint32_t fixes = _telemetry_fix_count - _telemetry_fix_sent; + uint32_t fixes_out = (fixes > TELEMETRY_FIXES) ? TELEMETRY_FIXES : fixes; + + buf[length++] = DB_PROTOCOL_BENCH_TELEMETRY; + _put(buf, &length, &_telemetry_step_tick, sizeof(_telemetry_step_tick)); + buf[length++] = (uint8_t)steps_out; + buf[length++] = _saturate_u8(steps - steps_out); + buf[length++] = _saturate_u8(_vars.max_tick_backlog); + _vars.max_tick_backlog = 0; + buf[length++] = (uint8_t)(_vars.drive_mode | (_steering.state << 4)); + _put(buf, &length, &_vars.encoder_total_left, sizeof(_vars.encoder_total_left)); + _put(buf, &length, &_vars.encoder_total_right, sizeof(_vars.encoder_total_right)); + buf[length++] = (uint8_t)fixes_out; + buf[length++] = _saturate_u8(fixes - fixes_out); + + for (uint32_t i = _telemetry_step_count - steps_out; i != _telemetry_step_count; i++) { + _put(buf, &length, &_telemetry_steps[i % TELEMETRY_STEPS], sizeof(telemetry_step_t)); } - if (period_ms > ADVERT_PERIOD_MAX_MS) { - return ADVERT_PERIOD_MAX_MS; + for (uint32_t i = _telemetry_fix_count - fixes_out; i != _telemetry_fix_count; i++) { + _put(buf, &length, &_telemetry_fixes[i % TELEMETRY_FIXES], sizeof(telemetry_fix_t)); } - return period_ms; + _telemetry_step_sent = _telemetry_step_count; + _telemetry_fix_sent = _telemetry_fix_count; + + buf[length++] = (uint8_t)_estimator.status; + float d2 = _estimator.last_d2 * 10.0f; + uint16_t d2x10 = (d2 >= (float)UINT16_MAX) ? UINT16_MAX : (uint16_t)d2; + _put(buf, &length, &d2x10, sizeof(d2x10)); + buf[length++] = (uint8_t)_estimator.kidnaps; + buf[length++] = (uint8_t)_estimator.reanchors; + buf[length++] = _steering.index; + buf[length++] = (uint8_t)_steering.nudges; + + swarmit_send_raw_data(buf, (uint8_t)length); } +#endif -static void _position_update(void) { - _dotbot_vars.update_position = true; +/// The batch's completion and why, for the advertisement +static void _waypoints_status(protocol_waypoints_report_t *report) { + switch (_steering.completion) { + case DB_STEERING_DONE_IN_PROGRESS: + report->status = DB_WAYPOINTS_IN_PROGRESS; + break; + case DB_STEERING_DONE_ARRIVED: + report->status = DB_WAYPOINTS_ARRIVED; + break; + case DB_STEERING_DONE_FAILED: + report->status = DB_WAYPOINTS_FAILED; + report->reason = (uint8_t)_steering.fail; + break; + case DB_STEERING_DONE_ABORTED: + report->status = DB_WAYPOINTS_ABORTED; + report->reason = (uint8_t)_abort_reason; + break; + default: + report->status = DB_WAYPOINTS_NONE; + break; + } +} + +/// Layout of DB_PROTOCOL_DOTBOT_ADVERTISEMENT, then the waypoint report; the +/// fields up to the waypoint index are the standard DotBot advertisement. +/// Fields this app does not own carry their unknown-value sentinels. +static void _advertise(void) { + db_gpio_toggle(&db_led1); + + size_t length = 0; + uint8_t *buf = _vars.radio_buffer; + + buf[length++] = DB_PROTOCOL_DOTBOT_ADVERTISEMENT; + buf[length++] = 0xff; // calibrated bitmask, unknown + + int16_t direction = DIRECTION_INVALID; + float heading; + if (db_pose_estimator_heading_deg(&_estimator, &heading)) { + direction = (int16_t)lroundf(heading); + } + _put(buf, &length, &direction, sizeof(direction)); + + // The photodiode position: the estimator's while it tracks, else the last solve + protocol_lh2_location_t position = { + .x = _vars.has_position ? _vars.position.x : 0, + .y = _vars.has_position ? _vars.position.y : 0, + }; + float sensor_x; + float sensor_y; + if (db_pose_estimator_sensor(&_estimator, &sensor_x, &sensor_y) && sensor_x >= 0 && sensor_y >= 0) { + position.x = (uint32_t)lroundf(sensor_x); + position.y = (uint32_t)lroundf(sensor_y); + } + _put(buf, &length, &position, sizeof(position)); + + uint16_t battery_level = 0; + swarmit_get_battery_level(&battery_level); + _put(buf, &length, &battery_level, sizeof(battery_level)); + + buf[length++] = (uint8_t)_vars.pwm_left; + buf[length++] = (uint8_t)_vars.pwm_right; + buf[length++] = (uint8_t)(db_steering_active(&_steering) ? ControlAuto : ControlManual); + + int32_t encoder_left; + int32_t encoder_right; + _encoders_delta(&_advertisement_encoders, &encoder_left, &encoder_right); + _put(buf, &length, &encoder_left, sizeof(encoder_left)); + _put(buf, &length, &encoder_right, sizeof(encoder_right)); + + // The point being driven to, and its index; the count once arrived + uint32_t waypoint_x = 0; + uint32_t waypoint_y = 0; + if (_steering.state != DB_STEERING_IDLE) { + waypoint_x = (uint32_t)lroundf(_steering.target.x_mm); + waypoint_y = (uint32_t)lroundf(_steering.target.y_mm); + } + _put(buf, &length, &waypoint_x, sizeof(waypoint_x)); + _put(buf, &length, &waypoint_y, sizeof(waypoint_y)); + buf[length++] = _steering.index; + + protocol_waypoints_report_t report = { + .batch_id = _batch_id, + .max_speed_10mm = (uint8_t)lroundf(_steering.v_max_mm_s / 10.0f), + .axle_x = DB_AXLE_UNKNOWN, + .axle_y = DB_AXLE_UNKNOWN, + }; + _waypoints_status(&report); + if (_estimator.status == DB_POSE_ESTIMATOR_TRACKING && _estimator.x >= 0 && _estimator.y >= 0 && _estimator.x < DB_AXLE_UNKNOWN && _estimator.y < DB_AXLE_UNKNOWN) { + report.axle_x = (uint16_t)lroundf(_estimator.x); + report.axle_y = (uint16_t)lroundf(_estimator.y); + } + _put(buf, &length, &report, sizeof(report)); + + swarmit_send_raw_data(buf, (uint8_t)length); + +#if defined(DB_BENCH_TELEMETRY) + _send_bench_telemetry(); +#endif } void IPC_IRQHandler(void) { diff --git a/sandbox-dotbot-v2.emProject b/sandbox-dotbot-v2.emProject index 284d237d..030200b9 100644 --- a/sandbox-dotbot-v2.emProject +++ b/sandbox-dotbot-v2.emProject @@ -28,7 +28,7 @@ build_output_file_name="$(OutDir)/$(ProjectName)-$(BuildTarget)$(EXE)" build_treat_warnings_as_errors="Yes" c_additional_options="-Wno-strict-prototypes" - c_preprocessor_definitions="ARM_MATH_ARMV8MML;NRF5340_XXAA;NRF_APPLICATION;__NRF_FAMILY;CONFIG_NFCT_PINS_AS_GPIOS;FLASH_PLACEMENT=1;BOARD_DOTBOT_V2;OTA_USE_CRYPTO;NRF_TRUSTZONE_NONSECURE;USE_SWARMIT;DOTBOT_CONTROL_LOOP_USE_PURE_PURSUIT" + c_preprocessor_definitions="ARM_MATH_ARMV8MML;NRF5340_XXAA;NRF_APPLICATION;__NRF_FAMILY;CONFIG_NFCT_PINS_AS_GPIOS;FLASH_PLACEMENT=1;BOARD_DOTBOT_V2;OTA_USE_CRYPTO;NRF_TRUSTZONE_NONSECURE;USE_SWARMIT" c_user_include_directories="$(SolutionDir)/../dotbot-libs/bsp;$(SolutionDir)/../dotbot-libs/crypto;$(SolutionDir)/../dotbot-libs/drv;$(PackagesDir)/nRF/Device/Include;$(PackagesDir)/CMSIS_5/CMSIS/Core/Include" clang_machine_outliner="Yes" compiler_color_diagnostics="Yes" diff --git a/sandbox-dotbot-v3.emProject b/sandbox-dotbot-v3.emProject index c8fbc187..44f7e0d1 100644 --- a/sandbox-dotbot-v3.emProject +++ b/sandbox-dotbot-v3.emProject @@ -28,7 +28,7 @@ build_output_file_name="$(OutDir)/$(ProjectName)-$(BuildTarget)$(EXE)" build_treat_warnings_as_errors="Yes" c_additional_options="-Wno-strict-prototypes" - c_preprocessor_definitions="ARM_MATH_ARMV8MML;NRF5340_XXAA;NRF_APPLICATION;__NRF_FAMILY;CONFIG_NFCT_PINS_AS_GPIOS;FLASH_PLACEMENT=1;BOARD_DOTBOT_V3;OTA_USE_CRYPTO;NRF_TRUSTZONE_NONSECURE;USE_SWARMIT;DOTBOT_CONTROL_LOOP_USE_PURE_PURSUIT" + c_preprocessor_definitions="ARM_MATH_ARMV8MML;NRF5340_XXAA;NRF_APPLICATION;__NRF_FAMILY;CONFIG_NFCT_PINS_AS_GPIOS;FLASH_PLACEMENT=1;BOARD_DOTBOT_V3;OTA_USE_CRYPTO;NRF_TRUSTZONE_NONSECURE;USE_SWARMIT" c_user_include_directories="$(SolutionDir)/../dotbot-libs/bsp;$(SolutionDir)/../dotbot-libs/crypto;$(SolutionDir)/../dotbot-libs/drv;$(PackagesDir)/nRF/Device/Include;$(PackagesDir)/CMSIS_5/CMSIS/Core/Include" clang_machine_outliner="Yes" compiler_color_diagnostics="Yes" diff --git a/sandbox-nrf5340dk.emProject b/sandbox-nrf5340dk.emProject index 9ff60556..ba8356b7 100644 --- a/sandbox-nrf5340dk.emProject +++ b/sandbox-nrf5340dk.emProject @@ -28,7 +28,7 @@ build_output_file_name="$(OutDir)/$(ProjectName)-$(BuildTarget)$(EXE)" build_treat_warnings_as_errors="Yes" c_additional_options="-Wno-strict-prototypes" - c_preprocessor_definitions="ARM_MATH_ARMV8MML;NRF5340_XXAA;NRF_APPLICATION;__NRF_FAMILY;CONFIG_NFCT_PINS_AS_GPIOS;FLASH_PLACEMENT=1;BOARD_NRF5340DK;OTA_USE_CRYPTO;NRF_TRUSTZONE_NONSECURE;USE_SWARMIT;DOTBOT_CONTROL_LOOP_USE_PURE_PURSUIT" + c_preprocessor_definitions="ARM_MATH_ARMV8MML;NRF5340_XXAA;NRF_APPLICATION;__NRF_FAMILY;CONFIG_NFCT_PINS_AS_GPIOS;FLASH_PLACEMENT=1;BOARD_NRF5340DK;OTA_USE_CRYPTO;NRF_TRUSTZONE_NONSECURE;USE_SWARMIT" c_user_include_directories="$(SolutionDir)/../dotbot-libs/bsp;$(SolutionDir)/../dotbot-libs/crypto;$(SolutionDir)/../dotbot-libs/drv;$(PackagesDir)/nRF/Device/Include;$(PackagesDir)/CMSIS_5/CMSIS/Core/Include" clang_machine_outliner="Yes" compiler_color_diagnostics="Yes" From 73ec310a94c8fd1fb2757ce4f13b27a908a9d041 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 17:40:59 +0200 Subject: [PATCH 2/9] dotbot-libs: bump to the odometric goal on the wheel speed loop AI-assisted: Claude Opus 5.5 --- dotbot-libs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dotbot-libs b/dotbot-libs index 739d2858..58a3f059 160000 --- a/dotbot-libs +++ b/dotbot-libs @@ -1 +1 @@ -Subproject commit 739d285877439f4f93def1bbb4b4cc0498630ba3 +Subproject commit 58a3f059981c9db986e534e8922a3465b23520ea From b8938ab1dce656b1dfc4df5f430fa2bbf5b5aa51 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 17:40:59 +0200 Subject: [PATCH 3/9] apps-sandbox/move: drive the square on the wheel speed loop AI-assisted: Claude Opus 5.5 --- apps-sandbox/applications.emProject | 2 +- apps-sandbox/move/README.md | 6 +- apps-sandbox/move/main.c | 168 ++++++++++++++++++++++++---- 3 files changed, 154 insertions(+), 22 deletions(-) diff --git a/apps-sandbox/applications.emProject b/apps-sandbox/applications.emProject index 05171e02..cc82c2c9 100644 --- a/apps-sandbox/applications.emProject +++ b/apps-sandbox/applications.emProject @@ -100,7 +100,7 @@ diff --git a/apps-sandbox/move/README.md b/apps-sandbox/move/README.md index 4d12a795..91f95f55 100644 --- a/apps-sandbox/move/README.md +++ b/apps-sandbox/move/README.md @@ -1 +1,5 @@ -# High-level move controls for the DotBot +# Drive a square + +Drives a 200 mm square on the per-wheel speed loop, then blinks. Each side and +each corner is an odometric goal: the wheels run until the encoders say the +distance is done, then brake to a stand. No position fix is used. diff --git a/apps-sandbox/move/main.c b/apps-sandbox/move/main.c index c7d10723..24e534fc 100644 --- a/apps-sandbox/move/main.c +++ b/apps-sandbox/move/main.c @@ -2,44 +2,172 @@ * @file * @defgroup swarmit_move Move application on top of SwarmIT * @ingroup swarmit - * @brief This application uses the move API to make the robot move + * @brief Drive a square on the wheel speed loop, then blink * - * @author Alexandre Abadie - * @copyright Inria, 2024 + * Each side and each corner is an odometric goal: the wheels run on the speed + * loop until the encoders say the distance is done, then brake to a stand. + * + * @copyright Inria, 2026 */ #include -#include "move.h" -#include "timer.h" -#include "gpio.h" +#include +#include +#include + +#include "board.h" #include "board_config.h" +#include "gpio.h" +#include "motors.h" +#include "qdec.h" +#include "timer.h" +#include "wheel_control.h" //=========================== swarmit ========================================== void swarmit_keep_alive(void); void swarmit_localization_handle_isr(void); +//=========================== defines ========================================== + +#define TIMER_DEV (0) +#define QDEC_LEFT (0) +#define QDEC_RIGHT (1) +#define TICKS_PER_KEEP_ALIVE (20U) ///< 200 ms +#define TICKS_PER_BLINK (25U) ///< 250 ms, once the square is done +#define PAUSE_TICKS (30U) ///< 300 ms standing between two moves +#define SIDE_MM (200.0f) +#define STRAIGHT_MM_S (150.0f) +#define TURN_MM_S (100.0f) + +typedef struct { + float distance_mm; ///< straight move, used when angle_deg is zero + float angle_deg; ///< turn in place, positive clockwise +} move_t; + +//=========================== variables ======================================== + +static const qdec_conf_t _qdec_left = { + .pin_a = &db_qdec_left_a_pin, + .pin_b = &db_qdec_left_b_pin, +}; + +static const qdec_conf_t _qdec_right = { + .pin_a = &db_qdec_right_a_pin, + .pin_b = &db_qdec_right_b_pin, +}; + +/// Keep in step with the gains of apps-sandbox/dotbot +static const db_wheel_control_conf_t _wheel_conf = { + .kp = 0.52f, + .ki = 5.2f, + .u_breakaway = 44.0f, + .kick_ramp = 0.5f, + .u_run = 32.0f, + .k_run = 0.097f, + .i_zone = 38.0f, + .pwm_max = 100.0f, + .pwm_slew_per_tick = 100.0f, + .stall_pwm = 80.0f, + .stall_ms = 500U, +}; + +static const move_t _moves[] = { + { SIDE_MM, 0 }, + { 0, 90 }, + { SIDE_MM, 0 }, + { 0, 90 }, + { SIDE_MM, 0 }, + { 0, 90 }, + { SIDE_MM, 0 }, + { 0, 90 }, +}; + +static db_wheel_control_t _wheel_left; +static db_wheel_control_t _wheel_right; +static db_wheel_goal_t _goal; +static volatile uint32_t _tick_count = 0; + +//=========================== callbacks ======================================== + +static void _tick(void) { + _tick_count++; +} + +//=========================== private ========================================== + +static void _move_start(const move_t *move) { + if (move->angle_deg != 0) { + db_wheel_goal_turn(&_goal, move->angle_deg, TURN_MM_S); + } else { + db_wheel_goal_straight(&_goal, move->distance_mm, STRAIGHT_MM_S); + } +} + +/// One tick of the speed loop; true once the goal is done and both wheels stand +static bool _move_step(void) { + uint32_t dbl_left; + uint32_t dbl_right; + int32_t left = db_wheel_control_counts(db_qdec_read_and_clear_dbl(QDEC_LEFT, &dbl_left), dbl_left); + int32_t right = db_wheel_control_counts(db_qdec_read_and_clear_dbl(QDEC_RIGHT, &dbl_right), dbl_right); + float setpoint_left; + float setpoint_right; + bool driving = db_wheel_goal_step(&_goal, left, right, &setpoint_left, &setpoint_right); + db_wheel_control_set_setpoint(&_wheel_left, setpoint_left); + db_wheel_control_set_setpoint(&_wheel_right, setpoint_right); + int8_t pwm_left = db_wheel_control_step(&_wheel_left, left, 1); + int8_t pwm_right = db_wheel_control_step(&_wheel_right, right, 1); + db_motors_set_pwm_brake(pwm_left, pwm_right, _wheel_left.brake, _wheel_right.brake); + return !driving && !_wheel_left.brake && !_wheel_right.brake; +} + //=========================== main ============================================= int main(void) { - db_timer_init(1); - db_timer_set_periodic_ms(1, 0, 200, &swarmit_keep_alive); - + db_board_init(); db_gpio_init(&db_led1, DB_GPIO_OUT); + db_motors_init(); + db_qdec_init(QDEC_LEFT, &_qdec_left, NULL, NULL); + db_qdec_init(QDEC_RIGHT, &_qdec_right, NULL, NULL); + db_wheel_control_init(&_wheel_left, &_wheel_conf); + db_wheel_control_init(&_wheel_right, &_wheel_conf); + db_timer_init(TIMER_DEV); + db_timer_set_periodic_ms(TIMER_DEV, 0, DB_WHEEL_CONTROL_TICK_MS, &_tick); - db_move_init(); - db_move_straight(200, 60); - db_move_rotate(90, 60); - db_move_straight(200, 60); - db_move_rotate(90, 60); - db_move_straight(200, 60); - db_move_rotate(90, 60); - db_move_straight(200, 60); - db_move_rotate(90, 60); + size_t next = 0; + uint32_t serviced = 0; + uint32_t standing = 0; + bool done = false; + _move_start(&_moves[next]); while (1) { - db_gpio_toggle(&db_led1); - db_timer_delay_ms(1, 250); + __WFE(); + while (serviced != _tick_count) { + serviced++; + if (serviced % TICKS_PER_KEEP_ALIVE == 0) { + swarmit_keep_alive(); + } + if (done) { + if (serviced % TICKS_PER_BLINK == 0) { + db_gpio_toggle(&db_led1); + } + continue; + } + if (!_move_step()) { + standing = 0; + continue; + } + if (++standing < PAUSE_TICKS) { + continue; + } + standing = 0; + if (++next >= sizeof(_moves) / sizeof(_moves[0])) { + done = true; + db_motors_coast(); + continue; + } + _move_start(&_moves[next]); + } } } From c89a9c95964f8a943b5557e87c2a60e661b08c8a Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 17:40:59 +0200 Subject: [PATCH 4/9] dotbot-libs: bump to drv/move removed AI-assisted: Claude Opus 5.5 --- dotbot-libs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dotbot-libs b/dotbot-libs index 58a3f059..13e66431 160000 --- a/dotbot-libs +++ b/dotbot-libs @@ -1 +1 @@ -Subproject commit 58a3f059981c9db986e534e8922a3465b23520ea +Subproject commit 13e66431e6a4089fd6ae18643365b3d1e8921895 From eaa5a00355d7a8974be4c3a74bd82ce7f3acf3da Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 17:41:43 +0200 Subject: [PATCH 5/9] agents: describe the sandbox dotbot app after the cut-over AI-assisted: Claude Opus 5.5 --- AGENTS.md | 45 +++++++++++++++++++++++++++------------------ 1 file changed, 27 insertions(+), 18 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index ded3cb4f..ed893411 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -13,14 +13,15 @@ fork/PR flow) come from the agentic workspace this repo is checked out into. / `dotbot_gateway_lr` (nRF DK as a radio gateway), `sailbot`, `freebot`, `xgo`, `lh2_calibration` (the LH2 calibration firmware), `lh2_mini_mote_*`, `nrf5340_net` (network-core image), `log_dump`. -- **`apps-sandbox/`** - the same robot apps built as **TrustZone non-secure +- **`apps-sandbox/`** - robot apps built as **TrustZone non-secure images** that run *inside* the SwarmIT sandbox (`dotbot`, `dotbot-simple`, `move`, `motors`, `rgbled`, `spin`). They link against `cmse_implib.a` (the Non-Secure-Callable import lib produced by `swarmit`). Absorbed from the old `dotbot-swarmit` repo. - **`dotbot-libs/`** - submodule (`DotBots/DotBot-libs`): BSP + drivers. The - **control loop math lives here** (`dotbot-libs/drv/control_loop/`), shared by - bare and sandbox builds. `swarmit` pins the *same* `dotbot-libs`, so the + **control loop math lives here**: the bare app's `drv/control_loop/`, the + sandbox app's `drv/wheel_control/`, `drv/pose_estimator/` and `drv/steering/`. + `swarmit` pins the *same* `dotbot-libs`, so the `control_loop.c` you read here is byte-identical to the copy under `swarmit/dotbot-libs/`. - **`*.emProject`** - one SES solution per target/board: `dotbot-v1/v2/v3`, @@ -35,6 +36,14 @@ spans three repos. It is a **two-tier layered control loop**: a fast loop on the bot, a slow loop on the Python host. Getting the tiers and rates wrong is the root of most "why doesn't the robot go where I told it" confusion. +**Two apps, two loops.** The bare `apps/dotbot` runs the PD heading loop in +`dotbot-libs/drv/control_loop/` described below. The sandbox `apps-sandbox/dotbot` +does not use `control_loop` at all: it runs a 10 ms per-wheel speed loop +(`drv/wheel_control`), a pose estimator on the encoders and LH2 +(`drv/pose_estimator`) and onboard waypoint steering (`drv/steering`), and adds a +waypoint report to its advertisement. Its `README.md` is the reference for it; +the steering-law and AUTO/MANUAL sections below are the bare app's. + ### Who closes which loop - **Fast inner loop - ON THE BOT (this repo).** The bot computes its own LH2 @@ -54,13 +63,13 @@ keyboard driving of a single bot. | Rate | Bare (`apps/dotbot/main.c`) | Sandbox (`apps-sandbox/dotbot/main.c`) | |---|---|---| -| Inner control step (`update_control` + motor write) | **~250 ms** - gated on the LH2 trigger, `5 * DB_LH2_UPDATE_DELAY_MS` (50) on RTC ch1 (`main.c:42,206,252`) | **~100 ms** - `DB_POSITION_UPDATE_DELAY_MS` (100) on RTC ch1 (`main.c:36,178,211`) | -| On-bot LH2 position refresh | ~250 ms (same trigger; computed app-side via `db_lh2_calculate_position`) | ~100 ms (read from the secure side via NSC `swarmit_localization_get_position`; the bot can't touch LH2 directly) | -| App advertisement (position/telemetry UP the radio) | **500 ms** - `DB_ADVERTIZEMENT_DELAY_MS` (RTC ch2) | **500 ms** - same constant | +| Inner control step | **~250 ms** `update_control` - gated on the LH2 trigger, `5 * DB_LH2_UPDATE_DELAY_MS` (50) on RTC ch1 | **10 ms** wheel speed loop and estimator predict (`TICK_MS`); steering every 100 ms | +| On-bot LH2 position refresh | ~250 ms (same trigger; computed app-side via `db_lh2_calculate_position`) | ~100 ms (read from the secure side via NSC `swarmit_localization_get_fix`, with its fix sequence; the bot can't touch LH2 directly) | +| App advertisement (position/telemetry UP the radio) | **500 ms** - `DB_ADVERTIZEMENT_DELAY_MS` (RTC ch2) | **100-1000 ms**, from the node's minimum TX interval (500 ms while not joined) | | Manual-command deadman (stops motors if no packet) | ~520 ms (`17000` RTC ticks / 32768) | ~520 ms | | swarmit netcore STATUS frame | n/a | **~1 s** - `mr_timer_hf_set_periodic_us(..., 1000000, _send_status)` in `swarmit/device/network_core/Source/main.c` | -The control step is **event-gated on a fresh valid LH2 fix and runs in AUTO mode +In the bare app the control step is **event-gated on a fresh valid LH2 fix and runs in AUTO mode only** - so the real rate is "at most once per trigger," skipped on a missed fix. Both bare and sandbox use the RTC (`dotbot-libs/bsp/nrf/timer.c`), not a high-frequency timer, for these periods. @@ -76,14 +85,13 @@ In `dotbot-libs/drv/control_loop/control_loop.c`, `update_control()`. It is an IMU. Two compile-time variants change *only* the target point, not the rate or the PD math: -- **`DOTBOT_CONTROL_LOOP_USE_PURE_PURSUIT`** - defined in the **sandbox** - `.emProject`s. Aims at a lookahead point `1.5 * threshold` along the current - segment instead of the raw next waypoint -> smoother curves. +- **`DOTBOT_CONTROL_LOOP_USE_PURE_PURSUIT`** - aims at a lookahead point + `1.5 * threshold` along the current segment instead of the raw next waypoint. + Not defined in any firmware build. - **`DOTBOT_CONTROL_LOOP_USE_EKF`** - a 3-state `[x,y,theta]` EKF fusing encoder odometry + LH2. **Not enabled in any firmware** - only in PyDotBot's host-side - sim build (`PyDotBot/utils/control_loop`, default off). The sandbox app has no - QDEC/encoders at all, so even if enabled its predict step would get zero - odometry. Bare dotbot enables **neither** -> plain PD toward the raw waypoint. + sim build (`PyDotBot/utils/control_loop`, default off). Bare dotbot enables + **neither** -> plain PD toward the raw waypoint. ### AUTO vs MANUAL mode (what the host polls) @@ -144,9 +152,12 @@ PyDotBot's `AGENTS.md`. ### Key files - `apps/dotbot/main.c` - bare app: timers, `radio_callback`, `_update_control_loop`. -- `apps-sandbox/dotbot/main.c` - sandbox app: NSC position read, `swarmit_keep_alive`. -- `dotbot-libs/drv/control_loop/control_loop.c` + `control_loop.h` - the PD / - pure-pursuit / EKF math (shared, byte-identical across checkouts). +- `apps-sandbox/dotbot/main.c` + `README.md` - sandbox app: the tick scheduler, + NSC fix read, `swarmit_keep_alive`, the wheel loop, estimator and steering. +- `dotbot-libs/drv/control_loop/control_loop.c` + `control_loop.h` - the bare + app's PD / pure-pursuit / EKF math, also built host-side by PyDotBot's simulator. +- `dotbot-libs/drv/wheel_control/`, `drv/pose_estimator/`, `drv/steering/` - the + sandbox app's layers, each with a host test (`make test` in DotBot-libs). - `swarmit/device/network_core/Source/main.c` - the `_send_status` (0x10) path. ## Build / run / test @@ -198,8 +209,6 @@ table - note e.g. **dotbot-v2 is an nRF5340**, not nRF52833; the truth is each - Don't add work to the LH2/control ISRs or the timer callbacks - the inner loop is timing-sensitive; latency there desyncs the control step. -- Don't enable `DOTBOT_CONTROL_LOOP_USE_EKF` in a firmware build expecting it to - work in the sandbox (no encoders there). - Don't "fix" the two-namespace position split here unilaterally - it's a cross-repo (PyDotBot + swarmit adapter) decision; see the control-loop section. - Don't run `make docker` locally (CI-only; slow under QEMU). From c962699fb0dab47d83666dad846f797a6b23138e Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 18:04:08 +0200 Subject: [PATCH 6/9] apps-sandbox/move: step over the missed ticks, stop the square on a stall AI-assisted: Claude Opus 5.5 --- apps-sandbox/move/README.md | 4 +- apps-sandbox/move/main.c | 82 ++++++++++++++++++++++--------------- 2 files changed, 51 insertions(+), 35 deletions(-) diff --git a/apps-sandbox/move/README.md b/apps-sandbox/move/README.md index 91f95f55..239076f4 100644 --- a/apps-sandbox/move/README.md +++ b/apps-sandbox/move/README.md @@ -2,4 +2,6 @@ Drives a 200 mm square on the per-wheel speed loop, then blinks. Each side and each corner is an odometric goal: the wheels run until the encoders say the -distance is done, then brake to a stand. No position fix is used. +distance is done, then brake to a stand. No position fix is used. A stalled +wheel (held at high duty without turning, against an obstacle) ends the square +there. diff --git a/apps-sandbox/move/main.c b/apps-sandbox/move/main.c index 24e534fc..60825463 100644 --- a/apps-sandbox/move/main.c +++ b/apps-sandbox/move/main.c @@ -87,6 +87,7 @@ static db_wheel_control_t _wheel_left; static db_wheel_control_t _wheel_right; static db_wheel_goal_t _goal; static volatile uint32_t _tick_count = 0; +static bool _stalled = false; ///< a wheel stalled, the square is over //=========================== callbacks ======================================== @@ -104,19 +105,24 @@ static void _move_start(const move_t *move) { } } -/// One tick of the speed loop; true once the goal is done and both wheels stand -static bool _move_step(void) { +/// One step of the speed loop over the ticks elapsed since the previous one; +/// true once the goal is done and both wheels stand. A stalled wheel ends the goal. +static bool _move_step(uint32_t elapsed) { uint32_t dbl_left; uint32_t dbl_right; int32_t left = db_wheel_control_counts(db_qdec_read_and_clear_dbl(QDEC_LEFT, &dbl_left), dbl_left); int32_t right = db_wheel_control_counts(db_qdec_read_and_clear_dbl(QDEC_RIGHT, &dbl_right), dbl_right); - float setpoint_left; - float setpoint_right; - bool driving = db_wheel_goal_step(&_goal, left, right, &setpoint_left, &setpoint_right); + if (_wheel_left.stalled || _wheel_right.stalled) { + _stalled = true; + db_wheel_goal_start(&_goal, 0, 0, 0); + } + float setpoint_left; + float setpoint_right; + bool driving = db_wheel_goal_step(&_goal, left, right, &setpoint_left, &setpoint_right); db_wheel_control_set_setpoint(&_wheel_left, setpoint_left); db_wheel_control_set_setpoint(&_wheel_right, setpoint_right); - int8_t pwm_left = db_wheel_control_step(&_wheel_left, left, 1); - int8_t pwm_right = db_wheel_control_step(&_wheel_right, right, 1); + int8_t pwm_left = db_wheel_control_step(&_wheel_left, left, elapsed); + int8_t pwm_right = db_wheel_control_step(&_wheel_right, right, elapsed); db_motors_set_pwm_brake(pwm_left, pwm_right, _wheel_left.brake, _wheel_right.brake); return !driving && !_wheel_left.brake && !_wheel_right.brake; } @@ -134,40 +140,48 @@ int main(void) { db_timer_init(TIMER_DEV); db_timer_set_periodic_ms(TIMER_DEV, 0, DB_WHEEL_CONTROL_TICK_MS, &_tick); - size_t next = 0; - uint32_t serviced = 0; - uint32_t standing = 0; - bool done = false; + size_t next = 0; + uint32_t serviced = 0; + uint32_t keep_alive = 0; + uint32_t blink = 0; + uint32_t standing = 0; + bool done = false; _move_start(&_moves[next]); while (1) { __WFE(); - while (serviced != _tick_count) { - serviced++; - if (serviced % TICKS_PER_KEEP_ALIVE == 0) { - swarmit_keep_alive(); - } - if (done) { - if (serviced % TICKS_PER_BLINK == 0) { - db_gpio_toggle(&db_led1); - } - continue; - } - if (!_move_step()) { - standing = 0; - continue; - } - if (++standing < PAUSE_TICKS) { - continue; + uint32_t now = _tick_count; + uint32_t elapsed = now - serviced; + if (elapsed == 0) { + continue; + } + serviced = now; + if (now - keep_alive >= TICKS_PER_KEEP_ALIVE) { + keep_alive = now; + swarmit_keep_alive(); + } + if (done) { + if (now - blink >= TICKS_PER_BLINK) { + blink = now; + db_gpio_toggle(&db_led1); } + continue; + } + if (!_move_step(elapsed)) { standing = 0; - if (++next >= sizeof(_moves) / sizeof(_moves[0])) { - done = true; - db_motors_coast(); - continue; - } - _move_start(&_moves[next]); + continue; + } + standing += elapsed; + if (standing < PAUSE_TICKS) { + continue; + } + standing = 0; + if (_stalled || ++next >= sizeof(_moves) / sizeof(_moves[0])) { + done = true; + db_motors_coast(); + continue; } + _move_start(&_moves[next]); } } From 4fc02cc2f5671cdbac58f9068c296bae407cdf23 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 18:04:08 +0200 Subject: [PATCH 7/9] apps-sandbox/dotbot: describe the waypoint mode and its command timeout AI-assisted: Claude Opus 5.5 --- apps-sandbox/dotbot/README.md | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/apps-sandbox/dotbot/README.md b/apps-sandbox/dotbot/README.md index a2906070..71395e29 100644 --- a/apps-sandbox/dotbot/README.md +++ b/apps-sandbox/dotbot/README.md @@ -24,13 +24,14 @@ moves. Its constants are provisional until measured on the floor. ## Driving -Three drive modes, each with exactly one writer of the motors: +Four drive modes, each with exactly one writer of the motors: | Mode | Entered by | Motors | |---|---|---| | idle | boot, `CONTROL_MODE`, the command timeout | the wheel loop at a zero setpoint: brakes a turning wheel, then lets it coast once it stands | | raw | `CMD_MOVE_RAW` | the command's duty, the wheel loop off | | velocity | `CMD_WHEEL_VELOCITY` | the wheel loop, toward per-wheel setpoints in mm/s clamped to ±700 | +| waypoint | a non-empty `LH2_WAYPOINTS` batch | the steering, through the wheel loop's setpoints, or both motors braked while it holds | The wheel loop (`drv/wheel_control` in DotBot-libs) runs every 10 ms tick. A zero setpoint shorts the motor while the wheel still turns, so a stop does not coast @@ -40,9 +41,13 @@ records carry its duty as `-127`. The ±700 mm/s clamp keeps a count longer than the QDEC's 128 us sample period. Commands arrive in the IPC interrupt and are applied on the next tick; a newer -command replaces one not yet applied. Only `CMD_MOVE_RAW` and -`CMD_WHEEL_VELOCITY` refresh the command timeout, so a host that keeps sending -other packets still stops a driving robot by going quiet on drive commands. +command replaces one not yet applied. Only `CMD_MOVE_RAW`, `CMD_WHEEL_VELOCITY` +and `LH2_WAYPOINTS` refresh the command timeout, so a host that keeps sending +other packets still stops a raw or velocity drive by going quiet on drive +commands. A waypoint batch is not under the command timeout: it needs no +resending, and the steering stops it on arrival, on losing its heading and on +its own turn, progress, hold and settle timeouts. An empty batch, +`CONTROL_MODE`, or a raw or velocity command stops it. ## Structure @@ -79,8 +84,9 @@ bench instrument that number is data. **Advertisement** fields are those of the standard DotBot advertisement, so host-side parsing is unchanged. Heading and position are the estimator's while it tracks; otherwise heading is the unknown-value sentinel `-1000` and position is -the last solve. Fields this application does not own carry unknown values: -waypoints and waypoint index are zero, control mode is manual. Encoder counts are totals since the previous +the last solve. The calibration bitmask is unknown (`0xff`). Control mode is +automatic while a batch is active, and the waypoint fields give the point being +driven to, followed by the waypoint report. Encoder counts are totals since the previous advertisement rather than since the previous control step, which is the same field carrying the only meaning available here. @@ -115,7 +121,7 @@ is recorded here so the next rewrite does not have to rediscover them. | Change | Reason | |---|---| -| The command timeout is unconditional | In the previous sandbox app it was skipped in automatic mode, so an autonomously driving robot has no deadman at all. This application has no autonomous mode, so silence always means stop, and the exemption should not be reintroduced without a replacement. | +| The command timeout covers every host-driven mode | In the previous sandbox app it was skipped in automatic mode, so an autonomously driving robot had no deadman at all. Here only a waypoint batch is exempt, and it has a replacement: the steering ends the batch on its own timeouts and on losing its heading. | | Timeout arithmetic uses a masked difference | `db_timer_ticks()` returns a 24-bit counter that wraps every 512 s. A plain `now > then + delay` comparison is false for the entire pass after a wrap, so a robot whose last command arrived just before the rollover keeps its last commanded speed. | | No displacement gate on incoming fixes | The gate in the previous sandbox app (and still in `apps/dotbot`) is anchored on the last accepted fix and only an accepted fix moves the anchor, so once the anchor is stale by more than the threshold, every fix that could correct it is rejected. Rejecting outliers belongs where the uncertainty is tracked, not against a self-referential anchor. | | Freshness read from a sequence, not from the coordinates | The previous sandbox app could only compare coordinate values, which reads a stationary robot as having no new fix and a re-read of one solve as a measurement in its own right. | From 13432192c3f2787a9d9039840022079f385037fa Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Thu, 24 Sep 2026 18:04:25 +0200 Subject: [PATCH 8/9] dotbot-libs: bump to the wheel control sample stopping on a stall AI-assisted: Claude Opus 5.5 --- dotbot-libs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dotbot-libs b/dotbot-libs index 13e66431..c11eb270 160000 --- a/dotbot-libs +++ b/dotbot-libs @@ -1 +1 @@ -Subproject commit 13e66431e6a4089fd6ae18643365b3d1e8921895 +Subproject commit c11eb27016e9a5605fb16d60c8442958965b0378 From ac159858acd16f85f355c016d70f0b6574c98878 Mon Sep 17 00:00:00 2001 From: Geovane Fedrecheski Date: Fri, 25 Sep 2026 12:21:58 +0200 Subject: [PATCH 9/9] dotbot-libs: bump to the cut-over merge on main AI-assisted: Claude Opus 5.5 --- dotbot-libs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dotbot-libs b/dotbot-libs index c11eb270..c1ff6ca5 160000 --- a/dotbot-libs +++ b/dotbot-libs @@ -1 +1 @@ -Subproject commit c11eb27016e9a5605fb16d60c8442958965b0378 +Subproject commit c1ff6ca5a828ebf26bb2d9d97e799abb17e03867