|
#!/usr/bin/env python3 |
|
"""Extract old/new AP_Motors6DOF mixer functions from git and emit a C++ comparison harness.""" |
|
|
|
from __future__ import annotations |
|
|
|
import hashlib |
|
import os |
|
import subprocess |
|
import textwrap |
|
|
|
REPO = "/home/will/git/ardupilot" |
|
OUT_DIR = "/tmp/sub-mixer-equivalence" |
|
SRC_PATH = "libraries/AP_Motors/AP_Motors6DOF.cpp" |
|
|
|
|
|
def git(*args: str) -> str: |
|
return subprocess.check_output(["git", "-C", REPO, *args], text=True) |
|
|
|
|
|
def extract_function(src: str, qualname: str) -> str: |
|
needle = f"void {qualname}(" |
|
start = src.find(needle) |
|
if start < 0: |
|
raise SystemExit(f"could not find {qualname}") |
|
# include preceding comment block if present |
|
comment = src.rfind("\n//", 0, start) |
|
if comment >= 0 and src[comment:start].count("\n") <= 8: |
|
# only keep comments immediately above the function |
|
header = src[comment + 1 : start] |
|
if all(line.startswith("//") or line.strip() == "" for line in header.splitlines()): |
|
start = comment + 1 |
|
i = src.find("{", start) |
|
depth = 0 |
|
for j in range(i, len(src)): |
|
if src[j] == "{": |
|
depth += 1 |
|
elif src[j] == "}": |
|
depth -= 1 |
|
if depth == 0: |
|
return src[start : j + 1] |
|
raise SystemExit(f"unbalanced braces for {qualname}") |
|
|
|
|
|
def sha256(text: str) -> str: |
|
return hashlib.sha256(text.encode()).hexdigest() |
|
|
|
|
|
def rewrite_methods(body: str) -> str: |
|
return body.replace("void AP_Motors6DOF::", "void ") |
|
|
|
|
|
OLD_SRC = git("show", f"HEAD^:{SRC_PATH}") |
|
NEW_SRC = git("show", f"HEAD:{SRC_PATH}") |
|
OLD_FUNCS = [ |
|
extract_function(OLD_SRC, "AP_Motors6DOF::output_armed_stabilizing"), |
|
extract_function(OLD_SRC, "AP_Motors6DOF::output_armed_stabilizing_vectored"), |
|
extract_function(OLD_SRC, "AP_Motors6DOF::output_armed_stabilizing_vectored_6dof"), |
|
] |
|
NEW_FUNC = extract_function(NEW_SRC, "AP_Motors6DOF::output_armed_stabilizing") |
|
|
|
HEAD = git("rev-parse", "HEAD").strip() |
|
PARENT = git("rev-parse", "HEAD^").strip() |
|
HEAD_SUBJECT = git("log", "-1", "--format=%s", "HEAD").strip() |
|
|
|
old_text = "\n\n".join(OLD_FUNCS) + "\n" |
|
new_text = NEW_FUNC + "\n" |
|
|
|
hashes = { |
|
"old_output_armed_stabilizing": sha256(OLD_FUNCS[0]), |
|
"old_vectored": sha256(OLD_FUNCS[1]), |
|
"old_vectored_6dof": sha256(OLD_FUNCS[2]), |
|
"new_output_armed_stabilizing": sha256(NEW_FUNC), |
|
} |
|
|
|
with open(os.path.join(OUT_DIR, "extracted_old.cpp"), "w") as f: |
|
f.write(old_text) |
|
with open(os.path.join(OUT_DIR, "extracted_new.cpp"), "w") as f: |
|
f.write(new_text) |
|
with open(os.path.join(OUT_DIR, "source_hashes.txt"), "w") as f: |
|
f.write(f"HEAD {HEAD} {HEAD_SUBJECT}\n") |
|
f.write(f"PARENT {PARENT}\n") |
|
for k, v in hashes.items(): |
|
f.write(f"{k} {v}\n") |
|
|
|
old_methods = rewrite_methods(old_text) |
|
new_methods = rewrite_methods(new_text) |
|
|
|
cpp = f'''// Auto-generated mixer equivalence proof. |
|
// Old functions extracted from {PARENT}:{SRC_PATH} |
|
// New function extracted from {HEAD}:{SRC_PATH} |
|
// |
|
// SHA256 of extracted text: |
|
// old output_armed_stabilizing {hashes["old_output_armed_stabilizing"]} |
|
// old output_armed_stabilizing_vectored {hashes["old_vectored"]} |
|
// old output_armed_stabilizing_vectored_6dof {hashes["old_vectored_6dof"]} |
|
// new output_armed_stabilizing {hashes["new_output_armed_stabilizing"]} |
|
|
|
#include <cmath> |
|
#include <cstdint> |
|
#include <cstdio> |
|
#include <cstring> |
|
#include <random> |
|
#include <string> |
|
#include <vector> |
|
|
|
#define AP_MOTORS_MAX_NUM_MOTORS 12 |
|
#define AP_BATTERY_ENABLED 0 |
|
|
|
static inline bool is_zero(const float x) {{ |
|
return fabsf(x) < 1.1920928955078125e-07f; // FLT_EPSILON |
|
}} |
|
|
|
static inline float constrain_float(float amt, float low, float high) {{ |
|
if (std::isnan(amt)) {{ |
|
return (low + high) / 2.0f; |
|
}} |
|
if (amt < low) {{ |
|
return low; |
|
}} |
|
if (amt > high) {{ |
|
return high; |
|
}} |
|
return amt; |
|
}} |
|
|
|
enum sub_frame_t {{ |
|
SUB_FRAME_BLUEROV1 = 0, |
|
SUB_FRAME_VECTORED = 1, |
|
SUB_FRAME_VECTORED_6DOF = 2, |
|
SUB_FRAME_VECTORED_6DOF_90DEG = 3, |
|
SUB_FRAME_SIMPLEROV_3 = 4, |
|
SUB_FRAME_SIMPLEROV_4 = 5, |
|
SUB_FRAME_SIMPLEROV_5 = 6, |
|
SUB_FRAME_CUSTOM = 7 |
|
}}; |
|
|
|
struct Limits {{ |
|
bool roll, pitch, yaw, throttle_lower, throttle_upper; |
|
void set_all(bool flag) {{ |
|
roll = pitch = yaw = throttle_lower = throttle_upper = flag; |
|
}} |
|
bool operator==(const Limits &o) const {{ |
|
return roll == o.roll && pitch == o.pitch && yaw == o.yaw && |
|
throttle_lower == o.throttle_lower && throttle_upper == o.throttle_upper; |
|
}} |
|
}}; |
|
|
|
struct MixerState {{ |
|
float _roll_in, _roll_in_ff; |
|
float _pitch_in, _pitch_in_ff; |
|
float _yaw_in, _yaw_in_ff; |
|
float _forward_in; |
|
float _lateral_in; |
|
float _throttle_bidir; |
|
float _throttle_thrust_max; |
|
float _forwardVerticalCouplingFactor; |
|
float _max_throttle; |
|
float _output_limited; |
|
int _active_frame_class; |
|
bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]; |
|
float _roll_factor[AP_MOTORS_MAX_NUM_MOTORS]; |
|
float _pitch_factor[AP_MOTORS_MAX_NUM_MOTORS]; |
|
float _yaw_factor[AP_MOTORS_MAX_NUM_MOTORS]; |
|
float _throttle_factor[AP_MOTORS_MAX_NUM_MOTORS]; |
|
float _forward_factor[AP_MOTORS_MAX_NUM_MOTORS]; |
|
float _lateral_factor[AP_MOTORS_MAX_NUM_MOTORS]; |
|
int8_t _motor_reverse[AP_MOTORS_MAX_NUM_MOTORS]; |
|
float _thrust_rpyt_out[AP_MOTORS_MAX_NUM_MOTORS]; |
|
Limits limit; |
|
|
|
float get_throttle_bidirectional() const {{ |
|
return constrain_float(_throttle_bidir, -1.0f, 1.0f); |
|
}} |
|
}}; |
|
|
|
struct OldMixer : MixerState {{ |
|
{textwrap.indent(old_methods, " ")} |
|
}}; |
|
|
|
struct NewMixer : MixerState {{ |
|
{textwrap.indent(new_methods, " ")} |
|
}}; |
|
|
|
static_assert(sizeof(OldMixer) == sizeof(NewMixer), "mixer state layout mismatch"); |
|
|
|
struct MotorDef {{ |
|
int num; |
|
float roll, pitch, yaw, thr, fwd, lat; |
|
}}; |
|
|
|
static void apply_layout(MixerState &s, const MotorDef *defs, int n) {{ |
|
for (int i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {{ |
|
s.motor_enabled[i] = false; |
|
s._roll_factor[i] = s._pitch_factor[i] = s._yaw_factor[i] = 0; |
|
s._throttle_factor[i] = s._forward_factor[i] = s._lateral_factor[i] = 0; |
|
s._motor_reverse[i] = 1; |
|
s._thrust_rpyt_out[i] = 0; |
|
}} |
|
for (int i = 0; i < n; i++) {{ |
|
const int m = defs[i].num; |
|
s.motor_enabled[m] = true; |
|
s._roll_factor[m] = defs[i].roll; |
|
s._pitch_factor[m] = defs[i].pitch; |
|
s._yaw_factor[m] = defs[i].yaw; |
|
s._throttle_factor[m] = defs[i].thr; |
|
s._forward_factor[m] = defs[i].fwd; |
|
s._lateral_factor[m] = defs[i].lat; |
|
s._motor_reverse[m] = 1; |
|
}} |
|
}} |
|
|
|
// Layouts copied from AP_Motors6DOF::setup_motors() |
|
static const MotorDef LAYOUT_BLUEROV1[] = {{ |
|
{{0, 0, 0, -1, 0, 1, 0}}, |
|
{{1, 0, 0, 1, 0, 1, 0}}, |
|
{{2, -0.5f, 0.5f, 0, 0.45f, 0, 0}}, |
|
{{3, 0.5f, 0.5f, 0, 0.45f, 0, 0}}, |
|
{{4, 0, -1, 0, 1, 0, 0}}, |
|
{{5, -0.25f, 0, 0, 0, 0, 1}}, |
|
}}; |
|
static const MotorDef LAYOUT_VECTORED[] = {{ |
|
{{0, 0, 0, 1, 0, -1, 1}}, |
|
{{1, 0, 0, -1, 0, -1, -1}}, |
|
{{2, 0, 0, -1, 0, 1, 1}}, |
|
{{3, 0, 0, 1, 0, 1, -1}}, |
|
{{4, 1, 0, 0, -1, 0, 0}}, |
|
{{5, -1, 0, 0, -1, 0, 0}}, |
|
}}; |
|
static const MotorDef LAYOUT_VECTORED_6DOF[] = {{ |
|
{{0, 0, 0, 1, 0, -1, 1}}, |
|
{{1, 0, 0, -1, 0, -1, -1}}, |
|
{{2, 0, 0, -1, 0, 1, 1}}, |
|
{{3, 0, 0, 1, 0, 1, -1}}, |
|
{{4, 1, -1, 0, -1, 0, 0}}, |
|
{{5, -1, -1, 0, -1, 0, 0}}, |
|
{{6, 1, 1, 0, -1, 0, 0}}, |
|
{{7, -1, 1, 0, -1, 0, 0}}, |
|
}}; |
|
static const MotorDef LAYOUT_VECTORED_6DOF_90DEG[] = {{ |
|
{{0, 1, 1, 0, 1, 0, 0}}, |
|
{{1, 0, 0, 1, 0, 1, 0}}, |
|
{{2, 1, -1, 0, 1, 0, 0}}, |
|
{{3, 0, 0, 0, 0, 0, 1}}, |
|
{{4, 0, 0, 0, 0, 0, 1}}, |
|
{{5, -1, 1, 0, 1, 0, 0}}, |
|
{{6, 0, 0, -1, 0, 1, 0}}, |
|
{{7, -1, -1, 0, 1, 0, 0}}, |
|
}}; |
|
static const MotorDef LAYOUT_SIMPLEROV_3[] = {{ |
|
{{0, 0, 0, -1, 0, 1, 0}}, |
|
{{1, 0, 0, 1, 0, 1, 0}}, |
|
{{2, 0, 0, 0, -1, 0, 0}}, |
|
}}; |
|
static const MotorDef LAYOUT_DEFAULT[] = {{ |
|
{{0, 0, 0, -1, 0, 1, 0}}, |
|
{{1, 0, 0, 1, 0, 1, 0}}, |
|
{{2, 1, 0, 0, -1, 0, 0}}, |
|
{{3, -1, 0, 0, -1, 0, 0}}, |
|
{{4, 0, 0, 0, 0, 0, 1}}, |
|
}}; |
|
|
|
struct FrameCase {{ |
|
const char *name; |
|
sub_frame_t frame; |
|
const MotorDef *defs; |
|
int n; |
|
}}; |
|
|
|
static const FrameCase FRAMES[] = {{ |
|
{{"BLUEROV1", SUB_FRAME_BLUEROV1, LAYOUT_BLUEROV1, 6}}, |
|
{{"VECTORED", SUB_FRAME_VECTORED, LAYOUT_VECTORED, 6}}, |
|
{{"VECTORED_6DOF", SUB_FRAME_VECTORED_6DOF, LAYOUT_VECTORED_6DOF, 8}}, |
|
{{"VECTORED_6DOF_90DEG", SUB_FRAME_VECTORED_6DOF_90DEG, LAYOUT_VECTORED_6DOF_90DEG, 8}}, |
|
{{"SIMPLEROV_3", SUB_FRAME_SIMPLEROV_3, LAYOUT_SIMPLEROV_3, 3}}, |
|
{{"DEFAULT", SUB_FRAME_SIMPLEROV_4, LAYOUT_DEFAULT, 5}}, |
|
}}; |
|
|
|
static uint32_t fbits(float x) {{ |
|
uint32_t u; |
|
static_assert(sizeof(float) == 4, "float must be 32-bit"); |
|
std::memcpy(&u, &x, 4); |
|
return u; |
|
}} |
|
|
|
static void set_inputs(MixerState &s, float roll, float pitch, float yaw, |
|
float throttle, float forward, float lateral, |
|
float roll_ff = 0, float pitch_ff = 0, float yaw_ff = 0, |
|
float thr_max = 1.0f, float fv_k = 1.0f, |
|
float max_thr = 1.0f, float out_lim = 1.0f) {{ |
|
s._roll_in = roll; s._roll_in_ff = roll_ff; |
|
s._pitch_in = pitch; s._pitch_in_ff = pitch_ff; |
|
s._yaw_in = yaw; s._yaw_in_ff = yaw_ff; |
|
s._throttle_bidir = throttle; |
|
s._forward_in = forward; |
|
s._lateral_in = lateral; |
|
s._throttle_thrust_max = thr_max; |
|
s._forwardVerticalCouplingFactor = fv_k; |
|
s._max_throttle = max_thr; |
|
s._output_limited = out_lim; |
|
}} |
|
|
|
static bool compare(const OldMixer &a, const NewMixer &b, std::string &why) {{ |
|
if (!(a.limit == b.limit)) {{ |
|
why = "limit flags differ"; |
|
return false; |
|
}} |
|
if (a._output_limited != b._output_limited && |
|
fbits(a._output_limited) != fbits(b._output_limited)) {{ |
|
why = "_output_limited differs"; |
|
return false; |
|
}} |
|
for (int i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {{ |
|
if (fbits(a._thrust_rpyt_out[i]) != fbits(b._thrust_rpyt_out[i])) {{ |
|
char buf[256]; |
|
std::snprintf(buf, sizeof(buf), |
|
"motor %d bits differ old=%08x (%a) new=%08x (%a)", |
|
i, fbits(a._thrust_rpyt_out[i]), a._thrust_rpyt_out[i], |
|
fbits(b._thrust_rpyt_out[i]), b._thrust_rpyt_out[i]); |
|
why = buf; |
|
return false; |
|
}} |
|
}} |
|
return true; |
|
}} |
|
|
|
static void print_outputs(FILE *fp, const char *tag, const MixerState &s) {{ |
|
std::fprintf(fp, " %s motors:", tag); |
|
for (int i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {{ |
|
if (!s.motor_enabled[i]) {{ |
|
continue; |
|
}} |
|
std::fprintf(fp, " M%d=%a", i + 1, s._thrust_rpyt_out[i]); |
|
}} |
|
std::fprintf(fp, " lim_thr=[%d,%d]\\n", |
|
(int)s.limit.throttle_lower, (int)s.limit.throttle_upper); |
|
}} |
|
|
|
int main() {{ |
|
std::fprintf(stdout, "AP_Motors6DOF mixer equivalence proof\\n"); |
|
std::fprintf(stdout, "old: %s\\n", "{PARENT}"); |
|
std::fprintf(stdout, "new: %s (%s)\\n", "{HEAD}", "{HEAD_SUBJECT}"); |
|
std::fprintf(stdout, "comparison: bit-identical IEEE-754 float32 bits of _thrust_rpyt_out[12]\\n"); |
|
std::fprintf(stdout, " plus throttle limit flags and _output_limited\\n\\n"); |
|
|
|
uint64_t total = 0; |
|
uint64_t failed = 0; |
|
std::string why; |
|
|
|
// Deterministic unit-axis and saturation cases on every stock frame. |
|
struct NamedInput {{ |
|
const char *name; |
|
float roll, pitch, yaw, thr, fwd, lat; |
|
float thr_max, fv_k, max_thr, out_lim; |
|
}}; |
|
const NamedInput named[] = {{ |
|
{{"zeros", 0, 0, 0, 0, 0, 0, 1, 1, 1, 1}}, |
|
{{"roll+", 1, 0, 0, 0, 0, 0, 1, 1, 1, 1}}, |
|
{{"roll-", -1, 0, 0, 0, 0, 0, 1, 1, 1, 1}}, |
|
{{"pitch+", 0, 1, 0, 0, 0, 0, 1, 1, 1, 1}}, |
|
{{"yaw+", 0, 0, 1, 0, 0, 0, 1, 1, 1, 1}}, |
|
{{"throttle+", 0, 0, 0, 1, 0, 0, 1, 1, 1, 1}}, |
|
{{"throttle-", 0, 0, 0, -1, 0, 0, 1, 1, 1, 1}}, |
|
{{"forward+", 0, 0, 0, 0, 1, 0, 1, 1, 1, 1}}, |
|
{{"forward-", 0, 0, 0, 0, -1, 0, 1, 1, 1, 1}}, |
|
{{"lateral+", 0, 0, 0, 0, 0, 1, 1, 1, 1, 1}}, |
|
{{"fwd+thr coupling", 0, 0, 0, 0.8f, 0.8f, 0, 1, 1.2f, 1, 1}}, |
|
{{"fwd-thr coupling", 0, 0, 0, -0.8f, -0.8f, 0, 1, 1.2f, 1, 1}}, |
|
{{"all-axes saturate", 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}}, |
|
{{"throttle over max", 0, 0, 0, 1.5f, 0, 0, 0.6f, 1, 1, 1}}, |
|
{{"throttle under min", 0, 0, 0, -1.5f, 0, 0, 0.6f, 1, 1, 1}}, |
|
{{"6dof max_throttle 0.5", 0, 0, 0, 1, 0, 0, 1, 1, 0.5f, 1}}, |
|
{{"output_limited 0.5", 0, 0, 0, 0.7f, 0.4f, 0.2f, 1, 1, 1, 0.5f}}, |
|
{{"near is_zero forward", 0, 0, 0, 0.5f, 1.0e-7f, 0, 1, 1.2f, 1, 1}}, |
|
{{"ff terms", 0.2f, -0.3f, 0.4f, 0.1f, -0.2f, 0.3f, 1, 1, 1, 1}}, |
|
}}; |
|
|
|
std::fprintf(stdout, "## Named cases (stock frames)\\n"); |
|
for (const auto &fr : FRAMES) {{ |
|
for (const auto &c : named) {{ |
|
OldMixer oldm{{}}; |
|
NewMixer newm{{}}; |
|
apply_layout(oldm, fr.defs, fr.n); |
|
apply_layout(newm, fr.defs, fr.n); |
|
oldm._active_frame_class = fr.frame; |
|
newm._active_frame_class = fr.frame; |
|
set_inputs(oldm, c.roll, c.pitch, c.yaw, c.thr, c.fwd, c.lat, |
|
0, 0, 0, c.thr_max, c.fv_k, c.max_thr, c.out_lim); |
|
set_inputs(newm, c.roll, c.pitch, c.yaw, c.thr, c.fwd, c.lat, |
|
0, 0, 0, c.thr_max, c.fv_k, c.max_thr, c.out_lim); |
|
// feed-forward for the ff case |
|
if (std::strcmp(c.name, "ff terms") == 0) {{ |
|
oldm._roll_in_ff = 0.15f; newm._roll_in_ff = 0.15f; |
|
oldm._pitch_in_ff = -0.10f; newm._pitch_in_ff = -0.10f; |
|
oldm._yaw_in_ff = 0.05f; newm._yaw_in_ff = 0.05f; |
|
}} |
|
oldm.output_armed_stabilizing(); |
|
newm.output_armed_stabilizing(); |
|
total++; |
|
if (!compare(oldm, newm, why)) {{ |
|
failed++; |
|
std::fprintf(stdout, "FAIL %s / %s: %s\\n", fr.name, c.name, why.c_str()); |
|
}} else if (std::strcmp(c.name, "zeros") == 0 || |
|
std::strcmp(c.name, "all-axes saturate") == 0 || |
|
std::strcmp(c.name, "fwd+thr coupling") == 0) {{ |
|
std::fprintf(stdout, "OK %s / %s\\n", fr.name, c.name); |
|
print_outputs(stdout, "old", oldm); |
|
print_outputs(stdout, "new", newm); |
|
}} |
|
}} |
|
}} |
|
|
|
// Random fuzz: stock layouts + random inputs, plus random factor matrices. |
|
std::mt19937 rng(0x6D697865); // 'mixe' |
|
std::uniform_real_distribution<float> unit(-1.5f, 1.5f); |
|
std::uniform_real_distribution<float> pos(0.05f, 1.0f); |
|
std::uniform_real_distribution<float> kfac(0.0f, 1.5f); |
|
std::uniform_int_distribution<int> coin(0, 1); |
|
|
|
const int kFuzzPerFrame = 200000; |
|
std::fprintf(stdout, "\\n## Random fuzz (%d cases per stock frame)\\n", kFuzzPerFrame); |
|
for (const auto &fr : FRAMES) {{ |
|
uint64_t frame_fail = 0; |
|
for (int n = 0; n < kFuzzPerFrame; n++) {{ |
|
OldMixer oldm{{}}; |
|
NewMixer newm{{}}; |
|
apply_layout(oldm, fr.defs, fr.n); |
|
apply_layout(newm, fr.defs, fr.n); |
|
oldm._active_frame_class = fr.frame; |
|
newm._active_frame_class = fr.frame; |
|
const float r = unit(rng), p = unit(rng), y = unit(rng); |
|
const float t = unit(rng), f = unit(rng), l = unit(rng); |
|
const float rff = unit(rng) * 0.3f, pff = unit(rng) * 0.3f, yff = unit(rng) * 0.3f; |
|
const float tmax = pos(rng); |
|
const float fv = kfac(rng); |
|
const float mt = pos(rng); |
|
const float ol = pos(rng); |
|
set_inputs(oldm, r, p, y, t, f, l, rff, pff, yff, tmax, fv, mt, ol); |
|
set_inputs(newm, r, p, y, t, f, l, rff, pff, yff, tmax, fv, mt, ol); |
|
if (coin(rng)) {{ |
|
for (int i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {{ |
|
if (oldm.motor_enabled[i]) {{ |
|
const int8_t rev = coin(rng) ? 1 : -1; |
|
oldm._motor_reverse[i] = rev; |
|
newm._motor_reverse[i] = rev; |
|
}} |
|
}} |
|
}} |
|
oldm.output_armed_stabilizing(); |
|
newm.output_armed_stabilizing(); |
|
total++; |
|
if (!compare(oldm, newm, why)) {{ |
|
failed++; |
|
frame_fail++; |
|
if (frame_fail <= 3) {{ |
|
std::fprintf(stdout, "FAIL %s fuzz #%d: %s\\n", fr.name, n, why.c_str()); |
|
}} |
|
}} |
|
}} |
|
std::fprintf(stdout, "%s: %d/%d bit-identical\\n", |
|
fr.name, kFuzzPerFrame - (int)frame_fail, kFuzzPerFrame); |
|
}} |
|
|
|
const int kRandomLayout = 200000; |
|
std::fprintf(stdout, "\\n## Random factor matrices (%d cases, all 3 mixer branches)\\n", kRandomLayout); |
|
const sub_frame_t branches[] = {{ |
|
SUB_FRAME_BLUEROV1, SUB_FRAME_VECTORED, SUB_FRAME_VECTORED_6DOF |
|
}}; |
|
const char *branch_names[] = {{"generic", "vectored", "vectored_6dof"}}; |
|
for (int b = 0; b < 3; b++) {{ |
|
uint64_t bf = 0; |
|
for (int n = 0; n < kRandomLayout; n++) {{ |
|
OldMixer oldm{{}}; |
|
NewMixer newm{{}}; |
|
oldm._active_frame_class = branches[b]; |
|
newm._active_frame_class = branches[b]; |
|
for (int i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {{ |
|
const bool en = (i < 8) ? true : (coin(rng) == 1); |
|
oldm.motor_enabled[i] = newm.motor_enabled[i] = en; |
|
const float rf = unit(rng), pf = unit(rng), yf = unit(rng); |
|
const float tf = unit(rng), ff = unit(rng), lf = unit(rng); |
|
const int8_t rev = coin(rng) ? 1 : -1; |
|
oldm._roll_factor[i] = newm._roll_factor[i] = rf; |
|
oldm._pitch_factor[i] = newm._pitch_factor[i] = pf; |
|
oldm._yaw_factor[i] = newm._yaw_factor[i] = yf; |
|
oldm._throttle_factor[i] = newm._throttle_factor[i] = tf; |
|
oldm._forward_factor[i] = newm._forward_factor[i] = ff; |
|
oldm._lateral_factor[i] = newm._lateral_factor[i] = lf; |
|
oldm._motor_reverse[i] = newm._motor_reverse[i] = rev; |
|
oldm._thrust_rpyt_out[i] = newm._thrust_rpyt_out[i] = 0; |
|
}} |
|
set_inputs(oldm, unit(rng), unit(rng), unit(rng), unit(rng), unit(rng), unit(rng), |
|
unit(rng)*0.3f, unit(rng)*0.3f, unit(rng)*0.3f, |
|
pos(rng), kfac(rng), pos(rng), pos(rng)); |
|
newm._roll_in = oldm._roll_in; newm._roll_in_ff = oldm._roll_in_ff; |
|
newm._pitch_in = oldm._pitch_in; newm._pitch_in_ff = oldm._pitch_in_ff; |
|
newm._yaw_in = oldm._yaw_in; newm._yaw_in_ff = oldm._yaw_in_ff; |
|
newm._throttle_bidir = oldm._throttle_bidir; |
|
newm._forward_in = oldm._forward_in; |
|
newm._lateral_in = oldm._lateral_in; |
|
newm._throttle_thrust_max = oldm._throttle_thrust_max; |
|
newm._forwardVerticalCouplingFactor = oldm._forwardVerticalCouplingFactor; |
|
newm._max_throttle = oldm._max_throttle; |
|
newm._output_limited = oldm._output_limited; |
|
oldm.output_armed_stabilizing(); |
|
newm.output_armed_stabilizing(); |
|
total++; |
|
if (!compare(oldm, newm, why)) {{ |
|
failed++; |
|
bf++; |
|
if (bf <= 3) {{ |
|
std::fprintf(stdout, "FAIL %s random-layout #%d: %s\\n", |
|
branch_names[b], n, why.c_str()); |
|
}} |
|
}} |
|
}} |
|
std::fprintf(stdout, "%s: %d/%d bit-identical\\n", |
|
branch_names[b], kRandomLayout - (int)bf, kRandomLayout); |
|
}} |
|
|
|
std::fprintf(stdout, "\\n## Result\\n"); |
|
std::fprintf(stdout, "compared %llu cases, failed %llu\\n", |
|
(unsigned long long)total, (unsigned long long)failed); |
|
if (failed == 0) {{ |
|
std::fprintf(stdout, "PASS: old and new mixers are bit-identical on every compared output.\\n"); |
|
return 0; |
|
}} |
|
std::fprintf(stdout, "FAIL: mixers diverged.\\n"); |
|
return 1; |
|
}} |
|
''' |
|
|
|
out_cpp = os.path.join(OUT_DIR, "proof.cpp") |
|
with open(out_cpp, "w") as f: |
|
f.write(cpp) |
|
print(f"wrote {out_cpp}") |
|
for k, v in hashes.items(): |
|
print(f"{k} {v}") |
|
print(f"HEAD {HEAD}") |
|
print(f"PARENT {PARENT}") |
