Utilities broken out of the fast rate PR

Tools/scripts/pretty_diff_sym.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python
+
+'''
+This script intends to provide a pretty symbol size diff between two binaries.
+
+AP_FLAKE8_CLEAN
+'''
+
+import subprocess
+
+
+def run_nm(file):
+    """Run nm on the specified file and return the output."""
+    result = subprocess.run(['arm-none-eabi-nm', '-S', '-C', '--size-sort', file], capture_output=True, text=True)
+    if result.returncode != 0:
+        raise Exception(f"Error running nm on {file}: {result.stderr}")
+    return result.stdout
+
+
+def parse_nm_output(output):
+    """Parse the nm output and return a dictionary of symbols and their sizes."""
+    symbols = {}
+    for line in output.splitlines():
+        parts = line.split()
+        if len(parts) >= 4:
+            size = int(parts[1], 16)
+            symbol = parts[3]
+            symbols[symbol] = size
+    return symbols
+
+
+def compare_symbols(symbols1, symbols2):
+    """Compare the symbols between two dictionaries and return the size differences."""
+    diff = {}
+    all_symbols = set(symbols1.keys()).union(symbols2.keys())
+    for symbol in all_symbols:
+        size1 = symbols1.get(symbol, 0)
+        size2 = symbols2.get(symbol, 0)
+        if size1 != size2:
+            diff[symbol] = (size1, size2, size2 - size1)
+    return diff
+
+
+def main(file1, file2):
+    output1 = run_nm(file1)
+    output2 = run_nm(file2)
+
+    symbols1 = parse_nm_output(output1)
+    symbols2 = parse_nm_output(output2)
+
+    diff = compare_symbols(symbols1, symbols2)
+
+    print(f"Symbol size differences between {file1} and {file2}:")
+    total = 0
+    for symbol, (size1, size2, change) in diff.items():
+        print("%4d %s %s -> %s" % (change, symbol, size1, size2))
+        total += change
+    print(f"Total: {total}")
+
+
+if __name__ == "__main__":
+    import sys
+    if len(sys.argv) != 3:
+        print(f"Usage: {sys.argv[0]} <file1> <file2>")
+        sys.exit(1)
+
+    file1 = sys.argv[1]
+    file2 = sys.argv[2]
+
+    main(file1, file2)

libraries/AP_HAL/examples/CondMutex/CondMutex.cpp

@@ -0,0 +1,141 @@
+/*
+  test of HAL_BinarySemaphore
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/Semaphores.h>
+#include <AP_Math/AP_Math.h>
+
+#include <stdio.h>
+
+void setup();
+void loop();
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+class ProducerConsumerTest {
+public:
+    HAL_BinarySemaphore sem1;
+
+    void setup(void);
+    void producer(void);
+    void consumer(void);
+    void producer_tick(void);
+    void consumer_tick(void);
+    bool update();
+    void update_sent();
+    bool check() { return bsize>0; }
+
+    uint32_t ops, timeouts, sent, recv;
+    uint32_t bsize;
+    uint32_t last_print_us;
+    uint32_t last_sent_us;
+    uint32_t max_delayed_us;
+    HAL_Semaphore mtx;
+    uint32_t delay_count;
+};
+
+void ProducerConsumerTest::setup(void)
+{
+    hal.scheduler->thread_create(
+        FUNCTOR_BIND_MEMBER(&ProducerConsumerTest::producer, void), "producer", 2048, AP_HAL::Scheduler::PRIORITY_IO, 0);
+    hal.scheduler->thread_create(
+        FUNCTOR_BIND_MEMBER(&ProducerConsumerTest::consumer, void), "consumer", 2048, AP_HAL::Scheduler::PRIORITY_IO, 1);
+    ::printf("Setup threads\n");
+}
+
+void ProducerConsumerTest::consumer(void)
+{
+    while (true) {
+        consumer_tick();
+    }
+}
+
+void ProducerConsumerTest::consumer_tick(void)
+{
+    if (!check()) {
+        sem1.wait_blocking();
+    }
+    // we are avoiding wait_blocking() here to cope with double notifications
+    // however it also means that, pre-existing notifications will not
+    // be waited for, this means that we can exhaust the pending data
+    // and the wait_blocking() above will immediately return. This is why
+    // the availability of data must also be checked inside the lock
+    // it also means you have to go around the loop twice to get to a blocking state
+    // when going from some data to no data
+    if (!update()) {
+        // we thought we had a sample, but concurrency means we actually do not
+        // the pattern requires that we are able to exit early here without processing
+        // it should only ever happen two cycles at a time so is not a busy wait
+        return; 
+    }
+    hal.scheduler->delay_microseconds(100); // simluate processing delay
+}
+
+void ProducerConsumerTest::producer(void)
+{
+    while (true) {
+        // simulate fifo burst
+        producer_tick();
+        producer_tick();
+        producer_tick();
+        hal.scheduler->delay_microseconds(750);
+    }
+}
+
+void ProducerConsumerTest::producer_tick(void)
+{
+    update_sent();
+    sem1.signal();
+}
+
+bool ProducerConsumerTest::update()
+{
+    WITH_SEMAPHORE(mtx);
+    // see the comment in consumer_tick() as to why this is necessary
+    if (!check()) {
+        return false;
+    }
+    ops++;
+    recv++;
+    bsize--;
+    uint32_t now_us = AP_HAL::micros();
+    max_delayed_us = MAX(max_delayed_us, now_us - last_sent_us);
+    float dt = (now_us - last_print_us)*1.0e-6;
+    if (dt >= 1.0) {
+        last_print_us = now_us;
+        ::printf("tick %u %.3f ops/s, dt %uus missing %d max_delay %uus queue length %u\n",
+                 unsigned(AP_HAL::millis()),
+                 ops/dt,
+                 uint32_t(dt * 1.0e6 / ops),
+                 int32_t(sent) - int32_t(recv),
+                 max_delayed_us,
+                 bsize);
+        ops = 0;
+        max_delayed_us = 0;
+    }
+    return true;
+}
+
+void ProducerConsumerTest::update_sent()
+{
+    WITH_SEMAPHORE(mtx);
+    sent++;
+    bsize++;
+    last_sent_us = AP_HAL::micros();
+}
+
+static ProducerConsumerTest *ct;
+
+void setup(void)
+{
+    ct = new ProducerConsumerTest;
+    ct->setup();
+}
+
+void loop(void)
+{
+    hal.scheduler->delay(1000);
+}
+
+AP_HAL_MAIN();

libraries/AP_HAL/examples/CondMutex/wscript

@@ -0,0 +1,7 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+def build(bld):
+    bld.ap_example(
+        use='ap',
+    )

libraries/AP_Scripting/applets/switch_rates.lua

@@ -0,0 +1,176 @@
+--[[
+   rate switch utility. This helps to compare different tunes in-flight to compare performance
+--]]
+
+---@diagnostic disable: param-type-mismatch
+
+
+local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
+
+local PARAM_TABLE_KEY = 33
+local PARAM_TABLE_PREFIX = "RTSW_"
+local PARAM_BACKUP_TABLE_KEY = 34
+local PARAM_BACKUP_TABLE_PREFIX = "X_"
+
+local UPDATE_RATE_HZ = 4
+
+-- bind a parameter to a variable, old syntax to support older firmware
+function bind_param(name)
+   local p = Parameter()
+   if not p:init(name) then
+      return nil
+   end
+   return p
+end
+
+-- add a parameter and bind it to a variable
+function bind_add_param(name, idx, default_value)
+   assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
+   local p = bind_param(PARAM_TABLE_PREFIX .. name)
+   assert(p, string.format("could not find parameter %s", name))
+   return p
+end
+
+-- add a backup parameter with appropriate length and bind it to a variable
+function bind_add_backup_param(name, idx, default_value)
+   -- shorten pname to fit with X_ prefix
+   local short_name = string.sub(name, math.max(1, 1 + string.len(name) - (16 - string.len(PARAM_BACKUP_TABLE_PREFIX))))
+   assert(param:add_param(PARAM_BACKUP_TABLE_KEY, idx, short_name, default_value), string.format('could not add param %s', short_name))
+   local p = bind_param(PARAM_BACKUP_TABLE_PREFIX .. short_name)
+   assert(p, string.format("could not find parameter %s", PARAM_BACKUP_TABLE_PREFIX .. short_name))
+   return p
+end
+
+-- setup script specific parameters
+param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 5)
+
+--[[
+  // @Param: RTSW_ENABLE
+  // @DisplayName: parameter reversion enable
+  // @Description: Enable parameter reversion system
+  // @Values: 0:Disabled,1:Enabled
+  // @User: Standard
+--]]
+local PREV_ENABLE      = bind_add_param('ENABLE',         1, 0)
+
+--[[
+  // @Param: RTSW_RC_FUNC
+  // @DisplayName: param reversion RC function
+  // @Description: RCn_OPTION number to used to trigger parameter reversion
+  // @User: Standard
+--]]
+local PREV_RC_FUNC     = bind_add_param('RC_FUNC',       2, 300)
+
+-- params dictionary indexed by name
+local param_table = {
+   "ATC_ACCEL_P_MAX",
+   "ATC_ACCEL_R_MAX",
+   "ATC_ACCEL_Y_MAX",
+   "ATC_ANG_PIT_P",
+   "ATC_ANG_RLL_P",
+   "ATC_ANG_YAW_P",
+   "ATC_RAT_PIT_P",
+   "ATC_RAT_PIT_I",
+   "ATC_RAT_PIT_D",
+   "ATC_RAT_PIT_D_FF",
+   "ATC_RAT_PIT_FLTD",
+   "ATC_RAT_RLL_P",
+   "ATC_RAT_RLL_I",
+   "ATC_RAT_RLL_D",
+   "ATC_RAT_RLL_D_FF",
+   "ATC_RAT_RLL_FLTD",
+   "ATC_RAT_YAW_P",
+   "ATC_RAT_YAW_I",
+   "ATC_RAT_YAW_D",
+   "ATC_RAT_YAW_D_FF",
+   "ATC_RAT_YAW_FLTD",
+   "ATC_THR_G_BOOST",
+   "ACRO_RP_RATE_TC",
+   "ACRO_Y_RATE_TC",
+   "FSTRATE_ENABLE",
+   "FSTRATE_DIV",
+   "MOT_SPIN_MIN",
+   "MOT_SPIN_MAX",
+   "MOT_THST_EXPO",
+   "SERVO_DSHOT_RATE",
+}
+
+-- setup script specific parameters
+param:add_table(PARAM_BACKUP_TABLE_KEY, PARAM_BACKUP_TABLE_PREFIX, #param_table)
+
+local params = {}
+local prev_params = {}
+local param_count = 0
+
+if PREV_ENABLE:get() == 0 then
+   gcs:send_text(MAV_SEVERITY.NOTICE, string.format("Rate switch: disabled"))
+   return
+end
+
+local function add_param(pname)
+   local p = bind_param(pname)
+   if p then
+      params[pname] = p
+      param_count = param_count + 1
+      local px = bind_add_backup_param(pname, param_count, p:get())
+      if px then
+         prev_params[pname] = px
+      end
+   end
+end
+
+for _, pname in pairs(param_table) do
+   add_param(pname)
+end
+
+local function switch_parameters()
+   local count = 0
+   for pname, p1 in pairs(params) do
+      local p2 = prev_params[pname]
+      local v1 = p1:get()
+      local v2 = p2:get()
+      if v1 ~= v2 then
+         p1:set_and_save(v2)
+         p2:set_and_save(v1)
+         count = count + 1
+      end
+   end
+   return count
+end
+
+gcs:send_text(MAV_SEVERITY.INFO, string.format("Rate switch: stored %u parameters", param_count))
+
+local AuxSwitchPos = {LOW=0, MIDDLE=1, HIGH=2}
+local AuxSwitchPosNames = {"LOW", "MIDDLE", "HIGH"}
+
+-- start in LOW start
+local prev_pos = AuxSwitchPos.LOW
+
+-- main update function
+function update()
+   local sw_pos = rc:get_aux_cached(PREV_RC_FUNC:get())
+
+   if sw_pos ~= nil and sw_pos ~= prev_pos then
+      count = switch_parameters()
+      gcs:send_text(MAV_SEVERITY.INFO, string.format("Rate switch: %u parameters changed to %s", count, AuxSwitchPosNames[sw_pos + 1]))
+      prev_pos = sw_pos
+   end
+end
+
+-- wrapper around update(). This calls update() at 10Hz,
+-- and if update faults then an error is displayed, but the script is not
+-- stopped
+function protected_wrapper()
+  local success, err = pcall(update)
+  if not success then
+     gcs:send_text(MAV_SEVERITY.EMERGENCY, "Rate switch: internal Error: " .. err)
+     -- when we fault we run the update function again after 1s, slowing it
+     -- down a bit so we don't flood the console with errors
+     --return protected_wrapper, 1000
+     return
+  end
+  return protected_wrapper, 1000/UPDATE_RATE_HZ
+end
+
+-- start running update loop
+return protected_wrapper()