diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index f9479d285a1eca..be58742694ef8a 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -539,6 +539,10 @@ void AP_Periph_FW::update() networking_periph.update(); #endif +#if AP_PERIPH_VOLZ_ENABLED + volz_update(); +#endif // AP_PERIPH_VOLZ_ENABLED + #if (defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) && HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY == 8) || defined(HAL_PERIPH_ENABLE_NOTIFY) update_rainbow(); #endif diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 42bad265ab5356..ca34fc3eb5a186 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -46,6 +46,7 @@ #include #endif #include +#include #ifdef HAL_PERIPH_ENABLE_RELAY #ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT @@ -113,6 +114,22 @@ #undef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT #endif +#ifndef AP_PERIPH_VOLZ_ENABLED +#define AP_PERIPH_VOLZ_ENABLED 0 +#endif + +// support for sending the actuator_status_Volz DroneCAN message +#ifndef AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED +#define AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED AP_PERIPH_VOLZ_ENABLED && AP_SERVO_TELEM_ENABLED +#endif + +#if AP_PERIPH_VOLZ_ENABLED +#include +#if AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED +#include +#endif // AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED +#endif // AP_PERIPH_VOLZ_ENABLED + #include "Parameters.h" #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -334,7 +351,25 @@ class AP_Periph_FW { #if AP_KDECAN_ENABLED AP_KDECAN kdecan; #endif - + +#if AP_SERVO_TELEM_ENABLED + AP_Servo_Telem st; +#endif + +#if AP_PERIPH_VOLZ_ENABLED + void volz_init(); + void volz_update(); + struct { + uint32_t last_servo_telem_update_ms; + uint32_t last_actuator_status_send_ms; + uint8_t next_servo_to_send; + } volz; + AP_Volz_Protocol volz_protocol; + void volz_update_servo_telem(); +#if AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED + void send_com_volz_servo_ActuatorStatus(uint8_t id, const AP_Servo_Telem::TelemetryData &telem_data); +#endif // AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED +#endif #ifdef HAL_PERIPH_ENABLE_ESC_APD ESC_APD_Telem *apd_esc_telem[APD_ESC_INSTANCES]; void apd_esc_telem_update(); diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 3341a500568405..f4a72410e052a1 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -2,6 +2,10 @@ #include #include "AP_Periph.h" +#if AP_PERIPH_VOLZ_ENABLED +#include +#endif // AP_PERIPH_VOLZ_ENABLED + extern const AP_HAL::HAL &hal; #ifndef HAL_PERIPH_LED_BRIGHT_DEFAULT @@ -737,6 +741,12 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(imu, "INS", AP_InertialSensor), #endif +#if AP_PERIPH_VOLZ_ENABLED + // @Group: VOLZ_ + // @Path: ../libraries/AP_Volz/AP_Volz.cpp + GOBJECT(volz_protocol, "VOLZ_", AP_Volz_Protocol), +#endif // AP_PERIPH_VOLZ_ENABLED + AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index c2df5d27f0d5ea..41f229e4b62c6d 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -99,6 +99,8 @@ class Parameters { k_param_esc_extended_telem_rate, k_param_imu_sample_rate, k_param_imu, + k_param_volz_protocol, + k_param_volz_port, }; AP_Int16 format_version; diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp index 2d8df1079a8cfc..c7833a6b70d444 100644 --- a/Tools/AP_Periph/rc_out.cpp +++ b/Tools/AP_Periph/rc_out.cpp @@ -198,6 +198,7 @@ void AP_Periph_FW::sim_update_actuator(uint8_t actuator_id) } sim_actuator.last_send_ms = now_ms; +#if !AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED for (uint8_t i=0; i +#include +#endif // AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED + +#include + +extern const AP_HAL::HAL& hal; + +void AP_Periph_FW::volz_update() +{ + volz_protocol.update(); + + volz_update_servo_telem(); +} + +void AP_Periph_FW::volz_update_servo_telem() +{ + AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton(); + if (servo_telem == nullptr) { + return; + } + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - volz.last_servo_telem_update_ms > 100) { + servo_telem->update(); + } + volz.last_servo_telem_update_ms = now_ms; + + // send a servo CAN message every 10ms + if (now_ms - volz.last_actuator_status_send_ms < 10) { + return; + } + volz.last_actuator_status_send_ms = now_ms; + AP_Servo_Telem::TelemetryData telem_data; + bool found = false; + for (uint8_t first=volz.next_servo_to_send++; first != volz.next_servo_to_send; volz.next_servo_to_send++) { + if (!servo_telem->get_telem(volz.next_servo_to_send, telem_data)) { + continue; + } + if (telem_data.stale(now_ms)) { + continue; + } + found = true; + break; + } + if (!found) { + return; + } + +#if AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED + send_com_volz_servo_ActuatorStatus(volz.next_servo_to_send, telem_data); +#endif +} + +#if AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED +void AP_Periph_FW::send_com_volz_servo_ActuatorStatus(uint8_t id, const AP_Servo_Telem::TelemetryData &telem_data) +{ + com_volz_servo_ActuatorStatus actuator_status {}; + actuator_status.actuator_id = id; + actuator_status.actual_position = radians(telem_data.measured_position); + actuator_status.current = MIN(telem_data.current / 0.025, 255); + actuator_status.voltage = MIN(telem_data.voltage / 0.2, 255); + actuator_status.motor_pwm = 0; // actually power? + actuator_status.motor_temperature = MIN(telem_data.motor_temperature_cdeg + 50, 255); + + uint8_t buffer[COM_VOLZ_SERVO_ACTUATORSTATUS_MAX_SIZE]; + const uint16_t total_size = com_volz_servo_ActuatorStatus_encode(&actuator_status, buffer, !canfdout()); + canard_broadcast(COM_VOLZ_SERVO_ACTUATORSTATUS_SIGNATURE, + COM_VOLZ_SERVO_ACTUATORSTATUS_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); +} +#endif // AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED + +#endif // AP_PERIPH_VOLZ_ENABLED diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript index a65b269c32f32d..1ca13f978ffc08 100644 --- a/Tools/AP_Periph/wscript +++ b/Tools/AP_Periph/wscript @@ -84,6 +84,7 @@ def build(bld): 'GCS_MAVLink', 'AP_Relay', 'AP_MultiHeap', + 'AP_Servo_Telem', ] bld.ap_stlib( diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index b3382ee88370c2..e9dc54e79d7dd3 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -980,6 +980,20 @@ def configure_env(self, cfg, env): HAL_PERIPH_ENABLE_GPS = 1, ) +class sitl_periph_volz(sitl_periph): + def configure_env(self, cfg, env): + cfg.env.AP_PERIPH = 1 + super(sitl_periph_volz, self).configure_env(cfg, env) + env.DEFINES.update( + HAL_BUILD_AP_PERIPH = 1, + PERIPH_FW = 1, + CAN_APP_NODE_NAME = '"org.ardupilot.ap_periph_volz"', + APJ_BOARD_ID = 101, + + AP_PERIPH_VOLZ_ENABLED = 1, + SIM_VOLZ_ENABLED = 1, + ) + class sitl_periph_battmon(sitl_periph): def configure_env(self, cfg, env): cfg.env.AP_PERIPH = 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 3dd362bed181b3..f5dd80de04922f 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -8,6 +8,7 @@ import copy import math import os +import pathlib import signal from pymavlink import quaternion @@ -6759,6 +6760,89 @@ def tests1a(self): ]) return ret + def VolzPeriph(self): + '''test Volz support in AP_Periph''' + + self.progress("Building Volz peripheral") + volz_builddir = util.reltopdir('build-VolzPeriph') + board = 'sitl_periph_volz' + util.build_SITL( + 'bin/AP_Periph', + board=board, + clean=False, + configure=True, + debug=True, + extra_configure_args=[ + '--out', volz_builddir, + ], + extra_defines={ + "AP_SERVO_TELEM_ENABLED": 1, + "HAL_PERIPH_ENABLE_RC_OUT": 1, + "HAL_PWM_COUNT": 32, + "HAL_PERIPH_SHOW_SERIAL_MANAGER_PARAMS": 1, + }, + ) + + self.progress("Building Plane with AP_DRONECAN_VOLZ_FEEDBACK_ENABLED support") + volz_plane_builddir = util.reltopdir('build-VolzPlane') + util.build_SITL( + 'bin/arduplane', + clean=False, + configure=True, + debug=True, + extra_configure_args=[ + '--enable-Volz_DroneCAN', + '--out', volz_plane_builddir, + ], + # extra_defines={ + # 'AP_DRONECAN_VOLZ_FEEDBACK_ENABLED': 1, + # }, + ) + + self.progress("Running Volz peripheral") + volz_rundir = util.reltopdir('run-VolzPeriph') + if not os.path.exists(volz_rundir): + os.mkdir(volz_rundir) + binary_path = pathlib.Path(volz_builddir, board, 'bin', 'AP_Periph') + volz1 = util.start_SITL( + binary_path, + cwd=volz_rundir, + stdout_prefix="volz1", + supplementary=True, + gdb=self.gdb, + valgrind=self.valgrind, + customisations=[ + '-I', '1', + '--serial5=sim:volz', + ], + param_defaults={ + "VOLZ_MASK": 11, + "SIM_VOLZ_MASK": 11, + "SIM_VOLZ_ENA": 1, + "SERIAL5_PROTOCOL": 14, # 14 is Volz + }, + ) + self.expect_list_add(volz1) + + self.set_parameters({ + "CAN_P1_DRIVER": 1, + }) + + self.progress("Running Volz Plane") + + binary_path = pathlib.Path(volz_plane_builddir, 'sitl', 'bin', 'arduplane') + self.customise_SITL_commandline([], binary=binary_path) + + self.set_parameters({ + "CAN_D1_UC_SRV_BM": 11, + # "CAN_D1_UC_OPTION": 64, # himark message + "SIM_SPEEDUP": 1, + }) + self.delay_sim_time(100000) + + self.expect_list_remove(volz1) + util.pexpect_close(volz1) + def tests1b(self): return [ self.TerrainLoiter, @@ -6846,6 +6930,7 @@ def tests1b(self): self.BadRollChannelDefined, self.VolzMission, self.Volz, + self.VolzPeriph, ] def disabled_tests(self): diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py index 140afabf86343d..f2956a35e2e7be 100644 --- a/Tools/autotest/pysim/util.py +++ b/Tools/autotest/pysim/util.py @@ -346,6 +346,8 @@ def make_safe_filename(text): def valgrind_log_filepath(binary, model): + if model is None: + model = 'None' return make_safe_filename('%s-%s-valgrind.log' % (os.path.basename(binary), model,)) @@ -413,6 +415,7 @@ def flush(self): def start_SITL(binary, valgrind=False, callgrind=False, + cwd=None, gdb=False, gdb_no_tui=False, wipe=False, @@ -421,6 +424,7 @@ def start_SITL(binary, speedup=1, sim_rate_hz=None, defaults_filepath=[], + param_defaults=None, # dictionary unhide_parameters=False, gdbserver=False, breakpoints=[], @@ -431,11 +435,13 @@ def start_SITL(binary, supplementary=False, stdout_prefix=None): - if model is None and not supplementary: - raise ValueError("model must not be None") - """Launch a SITL instance.""" cmd = [] + # pexpect doesn't like pathlib: + if cwd is not None: + cwd = str(cwd) + if not isinstance(binary, str): + binary = str(binary) if (callgrind or valgrind) and os.path.exists('/usr/bin/valgrind'): # we specify a prefix for vgdb-pipe because on Vagrant virtual # machines the pipes are created on the mountpoint for the @@ -512,12 +518,20 @@ def start_SITL(binary, defaults_filepath = [defaults_filepath] defaults = [reltopdir(path) for path in defaults_filepath] + if param_defaults is not None: + text = "".join([f"{name} {value}\n" for (name, value) in param_defaults.items()]) + filepath = tempfile.NamedTemporaryFile(mode="w", delete=False) + print(text, file=filepath) + filepath.close() + defaults.append(str(filepath.name)) + if not supplementary: if wipe: cmd.append('-w') if home is not None: cmd.extend(['--home', home]) - cmd.extend(['--model', model]) + if model is not None: + cmd.extend(['--model', model]) if speedup is not None and speedup != 1: ntf = tempfile.NamedTemporaryFile(mode="w", delete=False) print(f"SIM_SPEEDUP {speedup}", file=ntf) @@ -557,7 +571,7 @@ def start_SITL(binary, runme = [os.path.join(autotest_dir, "run_in_terminal_window.sh"), 'mactest'] runme.extend(cmd) print(cmd) - out = subprocess.Popen(runme, stdout=subprocess.PIPE).communicate()[0] + out = subprocess.Popen(runme, stdout=subprocess.PIPE, cwd=cwd).communicate()[0] out = out.decode('utf-8') p = re.compile('tab 1 of window id (.*)') @@ -577,7 +591,7 @@ def start_SITL(binary, print("Cannot find %s process terminal" % binary) child = FakeMacOSXSpawn() elif gdb and not os.getenv('DISPLAY'): - subprocess.Popen(cmd) + subprocess.Popen(cmd, cwd=cwd) atexit.register(kill_screen_gdb) # we are expected to return a pexpect wrapped around the # stdout of the ArduPilot binary. Not going to happen until @@ -592,7 +606,7 @@ def start_SITL(binary, first = cmd[0] rest = cmd[1:] - child = pexpect.spawn(first, rest, logfile=pexpect_logfile, encoding=ENCODING, timeout=5) + child = pexpect.spawn(str(first), rest, logfile=pexpect_logfile, encoding=ENCODING, timeout=5, cwd=cwd) pexpect_autoclose(child) if gdb or lldb: # if we run GDB we do so in an xterm. "Waiting for diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h index a262f9fa13bb3b..8ec471d9d09779 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h @@ -93,7 +93,7 @@ #endif #ifndef AP_VOLZ_ENABLED -#define AP_VOLZ_ENABLED 0 +#define AP_VOLZ_ENABLED AP_PERIPH_VOLZ_ENABLED #endif #ifndef AP_ROBOTISSERVO_ENABLED diff --git a/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp b/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp index a5594ef89af4b0..7b5565367f4a1e 100644 --- a/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp +++ b/libraries/AP_Servo_Telem/AP_Servo_Telem.cpp @@ -31,6 +31,12 @@ AP_Servo_Telem::AP_Servo_Telem() _singleton = this; } +// return true if the data is stale +bool AP_Servo_Telem::TelemetryData::stale(uint32_t now_ms) const volatile +{ + return (last_update_ms == 0) || ((now_ms - last_update_ms) > 5000); +} + // return true if the requested types of data are available bool AP_Servo_Telem::TelemetryData::present(const uint16_t type_mask) const volatile { diff --git a/libraries/AP_Servo_Telem/AP_Servo_Telem.h b/libraries/AP_Servo_Telem/AP_Servo_Telem.h index 08c8a1fa8749e8..db32c5f16a5cbf 100644 --- a/libraries/AP_Servo_Telem/AP_Servo_Telem.h +++ b/libraries/AP_Servo_Telem/AP_Servo_Telem.h @@ -55,6 +55,7 @@ class AP_Servo_Telem { // return true if the requested types of data are available bool present(const uint16_t type_mask) const volatile; + bool stale(uint32_t now_ms) const volatile; }; // update at 10Hz to log telemetry diff --git a/libraries/SITL/SIM_Volz.cpp b/libraries/SITL/SIM_Volz.cpp index 455b5bdd379ae1..355d43fe6db3f8 100644 --- a/libraries/SITL/SIM_Volz.cpp +++ b/libraries/SITL/SIM_Volz.cpp @@ -38,6 +38,8 @@ extern const AP_HAL::HAL& hal; #include "SIM_Aircraft.h" #include +#include +#include using namespace SITL; @@ -95,11 +97,26 @@ void Volz::update_servos(const class Aircraft &aircraft) const float dt = delta_t_us / 1000000.0; for (auto &servo : servos) { +#if APM_BUILD_TYPE(APM_BUILD_AP_Periph) + // the simulated servo positions are not transfered in the + // sitl_fdm structure passed to a simulated AP_Periph. Turn + // the servo output (which appears in SRV_Channels) back into + // an angle + const uint8_t idx = servo.id - 1; + const auto ch = SRV_Channels::srv_channel(idx); + if (ch == nullptr) { + continue; + } + // magic 3-degree delta here to make effect clear in log + servo.position = ch->get_output_norm() + 0.03; + // servo.position = servo.desired_position; +#else const uint8_t idx = servo.id - 1; if (idx > ARRAY_SIZE(aircraft.servo_filter)) { continue; } servo.position = aircraft.servo_filter[idx].angle(); +#endif // update primary current - proportional to airspeed and aileron deflection: float airspeed = aircraft.get_airspeed_pitot(); diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index acb6021b89205f..faef6f41891444 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -288,6 +288,10 @@ class SRV_Channel { // used by DO_SET_SERVO commands void ignore_small_rcin_changes() { ign_small_rcin_changes = true; } + // get normalised output from -1 to 1, assuming 0 at mid point of + // servo_min/servo_max + float get_output_norm(void); + private: AP_Int16 servo_min; AP_Int16 servo_max; @@ -329,9 +333,6 @@ class SRV_Channel { // return PWM for a given limit value uint16_t get_limit_pwm(Limit limit) const; - // get normalised output from -1 to 1, assuming 0 at mid point of servo_min/servo_max - float get_output_norm(void); - // a bitmask type wide enough for NUM_SERVO_CHANNELS typedef uint32_t servo_mask_t;