Let a tower set its encoder scale, and send no speed after a failed homing

This commit is contained in:
pgdalmeida 2026-09-28 18:21:37 +02:00
parent 049c0c5f0e
commit ab9fa3dbb0
Signed by: pedro.almeida
GPG Key ID: D4A6C394DF13F1D7
17 changed files with 152 additions and 7 deletions

View File

@ -193,6 +193,10 @@ enc_deviation = 1000
sw_mode = 0 sw_mode = 0
; Is this axis's encoder aligned to XACTUAL and usable for hold and DIAG? ; Is this axis's encoder aligned to XACTUAL and usable for hold and DIAG?
has_encoder = true has_encoder = true
; Microsteps per encoder count (ENC_CONST). Leave it out for the default, 12.8.
; A drivetrain whose encoder differs must declare it: wrong by 2%, the counter
; and the encoder drift apart as the axis turns and homing takes that for a stop.
;enc_const = 12.5
[Camera.rgb] [Camera.rgb]
; The serial of the physical camera fitted to this tower. A USB DEV_ id, or a ; The serial of the physical camera fitted to this tower. A USB DEV_ id, or a

View File

@ -84,6 +84,7 @@ Parsed and validated by `ConfigLoader` ([src/core/Config.cpp](../src/core/Config
| `Motor.<axis>` | `homing_speed_deg_s` / `center_speed_deg_s` | float | — | Endstop-search and park speeds, also in °/s. **hardware.ini.** Both are needed *before* the first home, so they convert through the declared scale (`steps_per_rev / travel_deg`), not the homed one | | `Motor.<axis>` | `homing_speed_deg_s` / `center_speed_deg_s` | float | — | Endstop-search and park speeds, also in °/s. **hardware.ini.** Both are needed *before* the first home, so they convert through the declared scale (`steps_per_rev / travel_deg`), not the homed one |
| `Motor.<axis>` | `steps_per_rev` | int | — | Expected full mechanical travel, **in counts**. **hardware.ini.** The firmware's only guard against a phantom endstop | | `Motor.<axis>` | `steps_per_rev` | int | — | Expected full mechanical travel, **in counts**. **hardware.ini.** The firmware's only guard against a phantom endstop |
| `Motor.<axis>` | `enc_deviation` | int | — | \|XACTUAL−XENC\| that means "position untrustworthy". **hardware.ini** | | `Motor.<axis>` | `enc_deviation` | int | — | \|XACTUAL−XENC\| that means "position untrustworthy". **hardware.ini** |
| `Motor.<axis>` | `enc_const` | float | 12.8 | Microsteps per encoder count (the TMC's `ENC_CONST`), up to 4 decimals. **hardware.ini**, optional: only a drivetrain whose encoder differs declares it. Wrong by a few percent, the step counter and encoder drift apart as the axis turns, and homing takes the drift for a stop (staeffelsberg: 12.5, from its old firmware) |
| `Motor.<axis>` | `sw_mode` / `has_encoder` | int / bool | — | TMC `SW_MODE` (physical endstop switches on REFL/REFR) and whether this axis's encoder is aligned and usable. **hardware.ini** | | `Motor.<axis>` | `sw_mode` / `has_encoder` | int / bool | — | TMC `SW_MODE` (physical endstop switches on REFL/REFR) and whether this axis's encoder is aligned and usable. **hardware.ini** |
| `Scan` | `grid_file` | string | — | CSV of `yaw_deg,pitch_deg` waypoints; empty → generate | | `Scan` | `grid_file` | string | — | CSV of `yaw_deg,pitch_deg` waypoints; empty → generate |
| `Scan` | `yaw_intervals` | int | `56` | Generated yaw positions across `[yaw_min_deg, yaw_max_deg]` | | `Scan` | `yaw_intervals` | int | `56` | Generated yaw positions across `[yaw_min_deg, yaw_max_deg]` |

View File

@ -77,6 +77,10 @@ bash survey-legacy-tower --firmware # pauses the old service ~20 s; changes
- its stored north calibration (`y`); - its stored north calibration (`y`);
- its TMC registers (`i`), with the run current worked out for the usual - its TMC registers (`i`), with the run current worked out for the usual
sense resistors. sense resistors.
- the old firmware's source, from the newest build in the Arduino cache:
its register table (encoder scale `R3A`, current `R0B`/`R10`, encoder limit
`R3D`) and homing speed. The running firmware cannot report these: the
registers are write-only, and it answers `i` for them with 0.
The report is saved as `~/legacy-survey-<date>.txt`. The report is saved as `~/legacy-survey-<date>.txt`.
@ -894,6 +898,12 @@ Answer the installer's ambient sensor question with `dht11` and the fan
question with yes, and choose `loom = legacy`. The firmware is then built with question with yes, and choose `loom = legacy`. The firmware is then built with
the DHT11 and fan (see configuration.md, "The legacy towers"). the DHT11 and fan (see configuration.md, "The legacy towers").
**The encoder scale.** Take it from the survey's "old firmware's source":
`R3A` is `ENC_CONST`, the whole part in the high 16 bits and 1/10000ths in the
low 16, so `0x000C1388` is 12.5. If it is not 12.8, put it in the answers file
as `yaw.enc_const=12.5` (the installer never asks for it). On staeffelsberg,
2026-09-28, it was missing: homing took the 2% drift for a stop after 131°.
Check it **on site** after the first flash: Check it **on site** after the first flash:
1. The fan runs at full speed from power-up. 1. The fan runs at full speed from power-up.
2. It drops once readings come in, if the box is below 25 °C. 2. It drops once readings come in, if the box is below 25 °C.

View File

@ -34,6 +34,9 @@ need_root() { [[ $EUID -eq 0 ]] || die "$1 needs root - re-run with sudo"; }
# drivetrain that no probe can read. # drivetrain that no probe can read.
KIT_AXIS_KEYS=(r_sense_ohm run_current_ma homing_current_ma travel_deg steps_per_rev KIT_AXIS_KEYS=(r_sense_ohm run_current_ma homing_current_ma travel_deg steps_per_rev
sw_mode enc_deviation has_encoder) sw_mode enc_deviation has_encoder)
# Keys with a firmware default that fits most towers: written only when given,
# never a TODO. enc_const: a legacy tower's encoder scale (staeffelsberg 12.5).
KIT_AXIS_OPTIONAL=(enc_const)
# What to measure, per key - the text of each `; TODO`. # What to measure, per key - the text of each `; TODO`.
kit_axis_todo() { kit_axis_todo() {
@ -46,6 +49,7 @@ kit_axis_todo() {
sw_mode) echo "TMC SW_MODE. 0 = no physical switches (StallGuard homing); set the bits if this axis has REFL/REFR wired" ;; sw_mode) echo "TMC SW_MODE. 0 = no physical switches (StallGuard homing); set the bits if this axis has REFL/REFR wired" ;;
enc_deviation) echo "|XACTUAL - XENC| that means 'position untrustworthy'. Measure the free-running gap on THIS axis and set ~4-5x it (tower 121: gap ~200, set 1000). Re-measure whenever the homing current changes" ;; enc_deviation) echo "|XACTUAL - XENC| that means 'position untrustworthy'. Measure the free-running gap on THIS axis and set ~4-5x it (tower 121: gap ~200, set 1000). Re-measure whenever the homing current changes" ;;
has_encoder) echo "true if an encoder is fitted to this axis and aligned to XACTUAL (usable for hold and DIAG), else false" ;; has_encoder) echo "true if an encoder is fitted to this axis and aligned to XACTUAL (usable for hold and DIAG), else false" ;;
enc_const) echo "microsteps per encoder count (the TMC's ENC_CONST); leave out for the default 12.8. A legacy tower's own firmware names it (staeffelsberg: 12.5)" ;;
esac esac
} }
@ -421,6 +425,9 @@ EOF
kv_or_todo "$key" "${MOTOR[$ax.$key]:-}" "$(kit_axis_todo "$key")" kv_or_todo "$key" "${MOTOR[$ax.$key]:-}" "$(kit_axis_todo "$key")"
[[ $key == steps_per_rev ]] && center_note "${MOTOR[$ax.$key]:-}" "$ax" [[ $key == steps_per_rev ]] && center_note "${MOTOR[$ax.$key]:-}" "$ax"
done done
for key in "${KIT_AXIS_OPTIONAL[@]}"; do
[[ -n ${MOTOR[$ax.$key]:-} ]] && printf '%-18s = %s\n' "$key" "${MOTOR[$ax.$key]}"
done
cat <<'EOF' cat <<'EOF'
; Speeds are DEGREES PER SECOND, not VMAX: a register value means different ; Speeds are DEGREES PER SECOND, not VMAX: a register value means different
; things on different gear ratios, so it is not portable between towers. These ; things on different gear ratios, so it is not portable between towers. These
@ -494,7 +501,7 @@ Wiring ([Pins], which the firmware is built from):
only the lines for what is declared only the lines for what is declared
--pin KEY=N one pin at a time instead (yaw_cs=1, spi_sck=2, ...) --pin KEY=N one pin at a time instead (yaw_cs=1, spi_sck=2, ...)
The drivetrain, per axis - anything not given is written as a ; TODO: The drivetrain, per axis - anything not given is written as a ; TODO:
--motor AXIS.KEY=VALUE KEY one of: ${KIT_AXIS_KEYS[*]} --motor AXIS.KEY=VALUE KEY one of: ${KIT_AXIS_KEYS[*]} ${KIT_AXIS_OPTIONAL[*]}
--r-sense, --run-current, --homing-current, --travel-deg, --steps-per-rev, --r-sense, --run-current, --homing-current, --travel-deg, --steps-per-rev,
--sw-mode the same, for the FIRST axis in --axes --sw-mode the same, for the FIRST axis in --axes
Other: Other:
@ -567,8 +574,8 @@ kit_main() {
for key in "${!MOTOR[@]}"; do for key in "${!MOTOR[@]}"; do
local ax="${key%%.*}" k="${key#*.}" ok=0 known local ax="${key%%.*}" k="${key#*.}" ok=0 known
[[ ",${AXES// /}," == *",$ax,"* ]] || die "--motor $key: $ax is not in --axes $AXES" [[ ",${AXES// /}," == *",$ax,"* ]] || die "--motor $key: $ax is not in --axes $AXES"
for known in "${KIT_AXIS_KEYS[@]}"; do [[ $k == "$known" ]] && ok=1; done for known in "${KIT_AXIS_KEYS[@]}" "${KIT_AXIS_OPTIONAL[@]}"; do [[ $k == "$known" ]] && ok=1; done
[[ $ok -eq 1 ]] || die "--motor $key: unknown key (one of: ${KIT_AXIS_KEYS[*]})" [[ $ok -eq 1 ]] || die "--motor $key: unknown key (one of: ${KIT_AXIS_KEYS[*]} ${KIT_AXIS_OPTIONAL[*]})"
done done
local board; board="$(probe_board)" local board; board="$(probe_board)"

View File

@ -115,6 +115,10 @@ struct AxisDrivetrainConfig {
// The firmware holds none of these once it asks for them. // The firmware holds none of these once it asks for them.
long steps_per_rev = 0; // expected full mechanical travel, in counts long steps_per_rev = 0; // expected full mechanical travel, in counts
long enc_deviation = 0; // |XACTUAL-XENC| that means "position untrustworthy" long enc_deviation = 0; // |XACTUAL-XENC| that means "position untrustworthy"
// Microsteps per encoder count (the TMC's ENC_CONST). 0 = undeclared: the
// firmware keeps its default, 12.8. Only a drivetrain whose encoder differs
// declares it - staeffelsberg's legacy firmware used 12.5.
double enc_const = 0.0;
// Wiring: TMC SW_MODE (physical endstop switches on REFL/REFR), and whether // Wiring: TMC SW_MODE (physical endstop switches on REFL/REFR), and whether
// this axis's encoder is aligned to XACTUAL and usable for hold and DIAG. // this axis's encoder is aligned to XACTUAL and usable for hold and DIAG.

View File

@ -36,6 +36,7 @@ struct DumpAxis {
// config handshake, which is a version skew, not a value of zero. // config handshake, which is a version skew, not a value of zero.
long steps_per_rev = -1; long steps_per_rev = -1;
long enc_deviation = -1; long enc_deviation = -1;
long enc_const = -1; // ENC_CONST as the driver holds it
long homing_speed = -1; long homing_speed = -1;
long center_speed = -1; long center_speed = -1;
long sw_mode = -1; long sw_mode = -1;

View File

@ -68,6 +68,12 @@ struct AxisConfigSource {
bool have_wiring = false; bool have_wiring = false;
unsigned sw_mode = 0; unsigned sw_mode = 0;
bool has_encoder = true; bool has_encoder = true;
// Optional: the encoder scale as the ENC_CONST register value (see
// encConstRegister). Never in a NEED line - the firmware has a default - so
// it is sent first, unasked, when declared.
bool have_enc_const = false;
long enc_const = 0;
}; };
struct FirmwareConfigPlan { struct FirmwareConfigPlan {
@ -90,4 +96,10 @@ struct FirmwareConfigPlan {
// existing CENTER and MARGIN commands use and hand-typed sessions rely on them. // existing CENTER and MARGIN commands use and hand-typed sessions rely on them.
FirmwareConfigPlan planFirmwareConfig(const NeedRequest& need, const AxisConfigSource& src); FirmwareConfigPlan planFirmwareConfig(const NeedRequest& need, const AxisConfigSource& src);
// Microsteps per encoder count as the TMC5160 holds it in decimal mode
// (ENCMODE bit 10): the whole part in the high 16 bits, the fraction in
// 1/10000ths in the low 16. 12.5 -> 0x000C1388. nullopt for anything that
// format cannot hold exactly: zero or less, 32768 or more, or a fifth decimal.
std::optional<long> encConstRegister(double microsteps_per_count);
} // namespace fgc } // namespace fgc

View File

@ -145,6 +145,7 @@ public:
" soft_margin=" + val("margin", "5000") + " soft_margin=" + val("margin", "5000") +
" steps_per_rev=" + val("steps_per_rev", "0") + " steps_per_rev=" + val("steps_per_rev", "0") +
" enc_dev=" + val("enc_deviation", "0") + " enc_dev=" + val("enc_deviation", "0") +
" enc_const=" + val("enc_const", "794432") +
" homing_speed=" + val("homing_speed", "0") + " homing_speed=" + val("homing_speed", "0") +
" center_speed=" + val("center_speed", "0") + " center_speed=" + val("center_speed", "0") +
" sw_mode=" + val("sw_mode", "0") + " sw_mode=" + val("sw_mode", "0") +

View File

@ -609,6 +609,14 @@ if [[ ! -r $HW ]]; then
ask_value "$ax.$key" " $ax $key" "$pat" ask_value "$ax.$key" " $ax $key" "$pat"
[[ -n $REPLY_VAL ]] && KIT_ARGS+=(--motor "$ax.$key=$REPLY_VAL") [[ -n $REPLY_VAL ]] && KIT_ARGS+=(--motor "$ax.$key=$REPLY_VAL")
done done
# Optional keys are taken from the answers file only (yaw.enc_const=12.5):
# asking every install about a default nearly every tower keeps is noise.
for key in "${KIT_AXIS_OPTIONAL[@]}"; do
[[ -n ${ANS[$ax.$key]:-} ]] || continue
[[ ${ANS[$ax.$key]} =~ ^[0-9]+(\.[0-9]{1,4})?$ ]] || die "bad value for '$ax.$key': ${ANS[$ax.$key]}"
say "$ax $key = ${ANS[$ax.$key]} (from the answers)"
KIT_ARGS+=(--motor "$ax.$key=${ANS[$ax.$key]}")
done
done done
"$KIT/install.sh" "${KIT_ARGS[@]}" > "$WORK/hardware.ini" 2> "$WORK/kit.err" || { cat "$WORK/kit.err" >&2; die "the kit could not generate hardware.ini"; } "$KIT/install.sh" "${KIT_ARGS[@]}" > "$WORK/hardware.ini" 2> "$WORK/kit.err" || { cat "$WORK/kit.err" >&2; die "the kit could not generate hardware.ini"; }
HW_NEW="$WORK/hardware.ini" HW_NEW="$WORK/hardware.ini"

View File

@ -93,6 +93,24 @@ for c in /opt/fire_gimbal_control/config.ini "$HOME"/*/config.ini "$HOME"/*/*/co
grep -vE '^[[:space:]]*(;|#|$)' "$c" | sed -E 's/^(mqtt_pw[[:space:]]*=).*/\1 *****/' | sed 's/^/ /' grep -vE '^[[:space:]]*(;|#|$)' "$c" | sed -E 's/^(mqtt_pw[[:space:]]*=).*/\1 *****/' | sed 's/^/ /'
done done
hr "the old firmware's source"
# What the running firmware cannot report: ENC_CONST, GLOBALSCALER and IHOLD_IRUN
# are write-only on the TMC5160, and the old sketch answers `i` for them from its
# own copy, which reads 0. Its register table is in its source - the newest build
# in the Arduino IDE's cache is most likely the one flashed.
sketch="$(ls -t "$HOME"/.cache/arduino/sketches/*/sketch/*.ino.cpp 2>/dev/null |
xargs -r grep -l chipCS_enc 2>/dev/null | head -1)"
if [[ -z $sketch ]]; then
echo "no build of the old sketch in ~/.cache/arduino; look for FWT_firmware*.ino by hand"
else
echo "newest build: $sketch ($(date -r "$sketch" '+%Y-%m-%d %H:%M'))"
echo " source: $(sed -n 's/^#line 1 "\(.*\)"/\1/p' "$sketch" | head -1)"
grep -hE '^#define (V_ENC_INIT|ONE_ROUND_STEPS)|gear_red_var *=|one_round_in_steps *=' "$sketch" | sed 's/^/ /'
# ENC_CONST is decimal: 0x000C1388 is 12 + 0x1388/10000 = 12.5 microsteps per count.
grep -hE '^#define R(0B|10|3A|3D|26|27|28) ' "$(dirname "$sketch")"/TMC5160.h 2>/dev/null | sed 's/^/ /'
echo " (R3A = ENC_CONST: hardware.ini [Motor.yaw] enc_const = whole + low 16 bits/10000)"
fi
[[ $FIRMWARE -eq 1 ]] || { hr "firmware"; echo "not asked (run with --firmware)"; echo; echo "saved: $OUT"; exit 0; } [[ $FIRMWARE -eq 1 ]] || { hr "firmware"; echo "not asked (run with --firmware)"; echo; echo "saved: $OUT"; exit 0; }
hr "the old firmware" hr "the old firmware"

View File

@ -1227,7 +1227,16 @@ struct Application::Impl {
// Production move speeds for subsequent MOVE commands, converted from the // Production move speeds for subsequent MOVE commands, converted from the
// declared deg/s. Sent here rather than earlier because deriveCountsPerDeg() // declared deg/s. Sent here rather than earlier because deriveCountsPerDeg()
// above has just refreshed the scale this conversion depends on. // above has just refreshed the scale this conversion depends on.
//
// Only after a successful home. A failed homing leaves the driver in
// velocity mode, where firmware before 2026-09-28 wrote SPEED straight to
// VMAX: on staeffelsberg that turned the axis on its own until it jammed on
// its stop and ground there. The firmware now refuses that too; this keeps
// towers still running the old build safe.
if (homed)
sendProductionSpeeds(); sendProductionSpeeds();
else
LOG_WARN << "production speed not sent: the gimbal is not homed";
LOG_INFO << "Gimbal init finished"; LOG_INFO << "Gimbal init finished";
} }
@ -1286,6 +1295,10 @@ struct Application::Impl {
s.steps_per_rev = d.steps_per_rev; s.steps_per_rev = d.steps_per_rev;
s.have_deviation = d.enc_deviation > 0; s.have_deviation = d.enc_deviation > 0;
s.enc_deviation = d.enc_deviation; s.enc_deviation = d.enc_deviation;
if (const auto r = encConstRegister(d.enc_const)) {
s.have_enc_const = true;
s.enc_const = *r;
}
s.have_wiring = d.wiring_declared; s.have_wiring = d.wiring_declared;
s.sw_mode = d.sw_mode; s.sw_mode = d.sw_mode;

View File

@ -1,4 +1,5 @@
#include "fgc/Config.h" #include "fgc/Config.h"
#include "fgc/FirmwareConfig.h"
#include "fgc/Logger.h" #include "fgc/Logger.h"
#include "fgc/Paths.h" #include "fgc/Paths.h"
@ -512,6 +513,7 @@ AppConfig ConfigLoader::fromMap(const std::map<std::string, std::string>& kv) {
d.center_speed_deg_s = getDouble(kv, pre + "center_speed_deg_s", d.center_speed_deg_s); d.center_speed_deg_s = getDouble(kv, pre + "center_speed_deg_s", d.center_speed_deg_s);
d.steps_per_rev = getInt(kv, pre + "steps_per_rev", static_cast<int>(d.steps_per_rev)); d.steps_per_rev = getInt(kv, pre + "steps_per_rev", static_cast<int>(d.steps_per_rev));
d.enc_deviation = getInt(kv, pre + "enc_deviation", static_cast<int>(d.enc_deviation)); d.enc_deviation = getInt(kv, pre + "enc_deviation", static_cast<int>(d.enc_deviation));
d.enc_const = getDouble(kv, pre + "enc_const", d.enc_const);
d.wiring_declared = kv.count(pre + "sw_mode") || kv.count(pre + "has_encoder"); d.wiring_declared = kv.count(pre + "sw_mode") || kv.count(pre + "has_encoder");
d.sw_mode = static_cast<unsigned>(getInt(kv, pre + "sw_mode", static_cast<int>(d.sw_mode))); d.sw_mode = static_cast<unsigned>(getInt(kv, pre + "sw_mode", static_cast<int>(d.sw_mode)));
d.has_encoder = getBool(kv, pre + "has_encoder", d.has_encoder); d.has_encoder = getBool(kv, pre + "has_encoder", d.has_encoder);
@ -567,6 +569,10 @@ AppConfig ConfigLoader::fromMap(const std::map<std::string, std::string>& kv) {
throw std::runtime_error(sec + ".travel_deg must be within (0, 720]; the " throw std::runtime_error(sec + ".travel_deg must be within (0, 720]; the "
"direction of travel is carried by the sign of " "direction of travel is carried by the sign of "
"counts_per_deg, not by this angle"); "counts_per_deg, not by this angle");
if (d.enc_const != 0.0 && !encConstRegister(d.enc_const))
throw std::runtime_error(sec + ".enc_const must be between 0 and 32768 with at "
"most 4 decimals (microsteps per encoder count; "
"leave it out for the default 12.8)");
}; };
checkDrivetrain("yaw", cfg.drivetrain.yaw); checkDrivetrain("yaw", cfg.drivetrain.yaw);
checkDrivetrain("pitch", cfg.drivetrain.pitch); checkDrivetrain("pitch", cfg.drivetrain.pitch);

View File

@ -182,6 +182,7 @@ DumpData parseDump(const std::string& block) {
ax.homing_irun = asInt(get(m, "irun_home", "-1")); ax.homing_irun = asInt(get(m, "irun_home", "-1"));
ax.steps_per_rev = asInt(get(m, "steps_per_rev", "-1")); ax.steps_per_rev = asInt(get(m, "steps_per_rev", "-1"));
ax.enc_deviation = asInt(get(m, "enc_dev", "-1")); ax.enc_deviation = asInt(get(m, "enc_dev", "-1"));
ax.enc_const = asInt(get(m, "enc_const", "-1"));
ax.homing_speed = asInt(get(m, "homing_speed", "-1")); ax.homing_speed = asInt(get(m, "homing_speed", "-1"));
ax.center_speed = asInt(get(m, "center_speed", "-1")); ax.center_speed = asInt(get(m, "center_speed", "-1"));
ax.sw_mode = asInt(get(m, "sw_mode", "-1")); ax.sw_mode = asInt(get(m, "sw_mode", "-1"));

View File

@ -1,5 +1,7 @@
#include "fgc/FirmwareConfig.h" #include "fgc/FirmwareConfig.h"
#include <algorithm>
#include <cmath>
#include <sstream> #include <sstream>
namespace fgc { namespace fgc {
@ -34,6 +36,7 @@ Answer answerFor(const std::string& key, const AxisConfigSource& s) {
if (key == "sw_mode") return {s.have_wiring, static_cast<long long>(s.sw_mode)}; if (key == "sw_mode") return {s.have_wiring, static_cast<long long>(s.sw_mode)};
if (key == "has_encoder") return {s.have_wiring, s.has_encoder ? 1 : 0}; if (key == "has_encoder") return {s.have_wiring, s.has_encoder ? 1 : 0};
if (key == "enc_const") return {s.have_enc_const, s.enc_const};
return {}; // not in the vocabulary return {}; // not in the vocabulary
} }
@ -43,6 +46,7 @@ bool known(const std::string& key) {
"global_scaler", "run_irun", "homing_irun", "center_count", "center", "global_scaler", "run_irun", "homing_irun", "center_count", "center",
"soft_margin", "margin", "steps_per_rev", "enc_deviation", "soft_margin", "margin", "steps_per_rev", "enc_deviation",
"normal_speed", "homing_speed", "center_speed", "sw_mode", "has_encoder", "normal_speed", "homing_speed", "center_speed", "sw_mode", "has_encoder",
"enc_const",
}; };
for (const char* k : kKeys) for (const char* k : kKeys)
if (key == k) return true; if (key == k) return true;
@ -72,7 +76,14 @@ FirmwareConfigPlan planFirmwareConfig(const NeedRequest& need, const AxisConfigS
FirmwareConfigPlan plan; FirmwareConfigPlan plan;
const std::string axis(1, need.axis); const std::string axis(1, need.axis);
for (const auto& key : need.keys) { // The encoder scale goes first, asked for or not: it is optional, so no NEED
// names it, and the firmware writes it to the driver with the rest of the
// set only if it has arrived by the time the last required key does.
std::vector<std::string> keys = need.keys;
if (src.have_enc_const && std::find(keys.begin(), keys.end(), "enc_const") == keys.end())
keys.insert(keys.begin(), "enc_const");
for (const auto& key : keys) {
if (!known(key)) { if (!known(key)) {
// The firmware wants something this host has never heard of, which // The firmware wants something this host has never heard of, which
// means it is newer. Naming it is the whole diagnostic. // means it is newer. Naming it is the whole diagnostic.
@ -89,4 +100,14 @@ FirmwareConfigPlan planFirmwareConfig(const NeedRequest& need, const AxisConfigS
return plan; return plan;
} }
std::optional<long> encConstRegister(double v) {
if (!(v > 0.0) || v >= 32768.0) return std::nullopt;
const double scaled = v * 10000.0;
const double rounded = std::round(scaled);
if (std::fabs(scaled - rounded) > 1e-6) return std::nullopt;
const long whole = static_cast<long>(rounded) / 10000;
const long frac = static_cast<long>(rounded) % 10000;
return (whole << 16) | frac;
}
} // namespace fgc } // namespace fgc

View File

@ -1,4 +1,5 @@
#include "fgc/TestRunner.h" #include "fgc/TestRunner.h"
#include "fgc/FirmwareConfig.h"
#include "fgc/sensors/McuEnvSensor.h" #include "fgc/sensors/McuEnvSensor.h"
@ -834,6 +835,7 @@ long axisField(const DumpData& d, char code, long DumpAxis::*field) {
} }
long echoSteps(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::steps_per_rev); } long echoSteps(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::steps_per_rev); }
long echoDev(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::enc_deviation); } long echoDev(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::enc_deviation); }
long echoEncConst(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::enc_const); }
long echoSwMode(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::sw_mode); } long echoSwMode(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::sw_mode); }
long echoHomingSpeed(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::homing_speed); } long echoHomingSpeed(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::homing_speed); }
long echoCenterSpeed(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::center_speed); } long echoCenterSpeed(const DumpData& d, char c) { return axisField(d, c, &DumpAxis::center_speed); }
@ -970,6 +972,8 @@ void TestRunner::testHwMotor(TestReport& r) {
const DeclaredValue vals[] = { const DeclaredValue vals[] = {
{"steps_per_rev", cfg.steps_per_rev > 0, cfg.steps_per_rev, echoSteps(d, code)}, {"steps_per_rev", cfg.steps_per_rev > 0, cfg.steps_per_rev, echoSteps(d, code)},
{"enc_deviation", cfg.enc_deviation > 0, cfg.enc_deviation, echoDev(d, code)}, {"enc_deviation", cfg.enc_deviation > 0, cfg.enc_deviation, echoDev(d, code)},
{"enc_const", encConstRegister(cfg.enc_const).has_value(),
encConstRegister(cfg.enc_const).value_or(0), echoEncConst(d, code)},
{"sw_mode", cfg.wiring_declared, (long)cfg.sw_mode, echoSwMode(d, code)}, {"sw_mode", cfg.wiring_declared, (long)cfg.sw_mode, echoSwMode(d, code)},
{"homing_speed", cfg.homing_speed_deg_s > 0, {"homing_speed", cfg.homing_speed_deg_s > 0,
speedVmax(cfg.homing_speed_deg_s, cfg, map), echoHomingSpeed(d, code)}, speedVmax(cfg.homing_speed_deg_s, cfg, map), echoHomingSpeed(d, code)},

View File

@ -172,8 +172,8 @@ TEST_CASE("the host-owned drivetrain values are read back from DUMP") {
"DUMP BEGIN build=abc uptime=1 mcusr=0x01 free_ram=4096\n" "DUMP BEGIN build=abc uptime=1 mcusr=0x01 free_ram=4096\n"
"DUMP Y state=2 hsub=0 enabled=1 lim_neg=0 lim_pos=1000000 hold_target=0 speed=39738 " "DUMP Y state=2 hsub=0 enabled=1 lim_neg=0 lim_pos=1000000 hold_target=0 speed=39738 "
"center=500000 soft_margin=5000 gs=80 irun=31 irun_home=7 steps_per_rev=1210778 " "center=500000 soft_margin=5000 gs=80 irun=31 irun_home=7 steps_per_rev=1210778 "
"enc_dev=1500 homing_speed=9958 center_speed=29873 sw_mode=0 cfg_rx=0xFFE " "enc_dev=1500 enc_const=791432 homing_speed=9958 center_speed=29873 sw_mode=0 "
"hold_corr=0 hold_peak=0 hold_cumul=0 has_encoder=1\n" "cfg_rx=0xFFE hold_corr=0 hold_peak=0 hold_cumul=0 has_encoder=1\n"
"DUMP END\n"; "DUMP END\n";
auto d = fgc::parseDump(text); auto d = fgc::parseDump(text);
REQUIRE(d.valid); REQUIRE(d.valid);
@ -181,6 +181,7 @@ TEST_CASE("the host-owned drivetrain values are read back from DUMP") {
const auto& a = d.axes[0]; const auto& a = d.axes[0];
CHECK(a.steps_per_rev == 1210778); CHECK(a.steps_per_rev == 1210778);
CHECK(a.enc_deviation == 1500); CHECK(a.enc_deviation == 1500);
CHECK(a.enc_const == 791432);
CHECK(a.homing_speed == 9958); CHECK(a.homing_speed == 9958);
CHECK(a.center_speed == 29873); CHECK(a.center_speed == 29873);
CHECK(a.sw_mode == 0); CHECK(a.sw_mode == 0);

View File

@ -303,3 +303,36 @@ TEST_CASE("the mock leaves NEEDCFG per axis, as the firmware does") {
CHECK(d.find("DUMP Y state=1") != std::string::npos); CHECK(d.find("DUMP Y state=1") != std::string::npos);
CHECK(d.find("DUMP P state=5") != std::string::npos); CHECK(d.find("DUMP P state=5") != std::string::npos);
} }
// ---- the encoder scale (ENC_CONST), optional per drivetrain ----
TEST_CASE("the encoder scale converts to the driver's decimal format, exactly or not at all") {
CHECK(encConstRegister(12.8) == 0x000C1F40L); // the firmware default
CHECK(encConstRegister(12.5) == 0x000C1388L); // staeffelsberg's legacy firmware
CHECK(encConstRegister(1.0) == 0x00010000L);
CHECK(encConstRegister(0.0001) == 0x00000001L);
CHECK_FALSE(encConstRegister(0.0));
CHECK_FALSE(encConstRegister(-12.5)); // a reversed encoder is not modelled
CHECK_FALSE(encConstRegister(32768.0));
CHECK_FALSE(encConstRegister(12.34565)); // a fifth decimal the driver cannot hold
}
TEST_CASE("a declared encoder scale is sent first, asked for or not") {
auto src = iotaYaw();
src.have_enc_const = true;
src.enc_const = 0x000C1388L;
const auto need = parseNeedLine("NEED Y steps_per_rev global_scaler");
REQUIRE(need);
const auto p = planFirmwareConfig(*need, src);
REQUIRE(p.sets.size() == 3);
CHECK(p.sets[0] == "SET Y enc_const 791432");
CHECK(p.complete());
}
TEST_CASE("an undeclared encoder scale is never sent, so the firmware keeps its default") {
const auto need = parseNeedLine("NEED Y steps_per_rev global_scaler");
REQUIRE(need);
const auto p = planFirmwareConfig(*need, iotaYaw());
CHECK(p.sets.size() == 2);
for (const auto& s : p.sets) CHECK(s.find("enc_const") == std::string::npos);
}