Let a tower set its encoder scale, and send no speed after a failed homing
This commit is contained in:
parent
049c0c5f0e
commit
ab9fa3dbb0
|
|
@ -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
|
||||||
|
|
|
||||||
|
|
@ -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]` |
|
||||||
|
|
|
||||||
|
|
@ -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.
|
||||||
|
|
|
||||||
|
|
@ -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)"
|
||||||
|
|
|
||||||
|
|
@ -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.
|
||||||
|
|
|
||||||
|
|
@ -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;
|
||||||
|
|
|
||||||
|
|
@ -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
|
||||||
|
|
|
||||||
|
|
@ -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") +
|
||||||
|
|
|
||||||
|
|
@ -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"
|
||||||
|
|
|
||||||
|
|
@ -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"
|
||||||
|
|
|
||||||
|
|
@ -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.
|
||||||
sendProductionSpeeds();
|
//
|
||||||
|
// 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();
|
||||||
|
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;
|
||||||
|
|
|
||||||
|
|
@ -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);
|
||||||
|
|
|
||||||
|
|
@ -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"));
|
||||||
|
|
|
||||||
|
|
@ -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
|
||||||
|
|
|
||||||
|
|
@ -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)},
|
||||||
|
|
|
||||||
|
|
@ -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);
|
||||||
|
|
|
||||||
|
|
@ -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);
|
||||||
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue