From 0e08d9e4a24ddbf5dbbd1b56bc7e313c206753f0 Mon Sep 17 00:00:00 2001 From: patrickelectric Date: Tue, 26 Mar 2024 11:19:38 +0000 Subject: [PATCH] =?UTF-8?q?Deploying=20to=20gh-pages=20from=20@=20bluerobo?= =?UTF-8?q?tics/Blueos-Parameter-Repository@6e38d95fe1e92d811566d5f0407ff3?= =?UTF-8?q?fbb67e3133=20=F0=9F=9A=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- params_v1.json | 502 +++++++++---------- scripts/ardupilot/ArduRover/4.x/navlight.lua | 145 ++++++ scripts_v1.json | 3 + 3 files changed, 399 insertions(+), 251 deletions(-) create mode 100644 scripts/ardupilot/ArduRover/4.x/navlight.lua create mode 100644 scripts_v1.json diff --git a/params_v1.json b/params_v1.json index 4697750..12d1199 100644 --- a/params_v1.json +++ b/params_v1.json @@ -1,4 +1,225 @@ { + "params/ardupilot/ArduSub/4.1/heavy.params": { + "ATC_ANG_PIT_P": 6.0, + "ATC_ANG_RLL_P": 6.0, + "ATC_ANG_YAW_P": 6.0, + "FRAME_CONFIG": 2.0, + "SERVO7_FUNCTION": 39.0, + "SERVO8_FUNCTION": 40.0, + "SERVO8_REVERSED": 0.0, + "SERVO9_FUNCTION": 59.0, + "SERVO10_FUNCTION": 7.0, + "SERVO10_REVERSED": 1.0 + }, + "params/ardupilot/ArduSub/4.1/base.params": { + "BATT_CAPACITY": 0.0, + "BATT_AMP_OFFSET": 0.33, + "BATT_AMP_PERVLT": 37.8788, + "BATT_VOLT_MULT": 11.0, + "BATT_MONITOR": 4.0, + "AHRS_ORIENTATION": 16.0, + "BRD_RTC_TYPES": 3.0, + "MNT_RC_IN_TILT": 8.0, + "MNT_TYPE": 1.0, + "SERIAL0_PROTOCOL": 2.0 + }, + "params/ardupilot/ArduSub/4.1/standard.params": { + "ATC_ANG_RLL_P": 0.0, + "FRAME_CONFIG": 1.0 + }, + "params/ardupilot/ArduSub/4.1/pixhawk1/Heavy BlueROV2.params": { + "LEAK1_PIN": 55.0, + "SERVO7_FUNCTION": 39.0, + "SERVO8_FUNCTION": 40.0, + "SERVO8_REVERSED": 0.0, + "BATT_CAPACITY": 0.0, + "BATT_AMP_OFFSET": 0.33, + "BATT_AMP_PERVLT": 37.8788, + "BATT_VOLT_MULT": 11.0, + "BATT_MONITOR": 4.0, + "AHRS_ORIENTATION": 16.0, + "BRD_RTC_TYPES": 3.0, + "MNT_RC_IN_TILT": 8.0, + "MNT_TYPE": 1.0, + "SERIAL0_PROTOCOL": 2.0, + "ATC_ANG_PIT_P": 6.0, + "ATC_ANG_RLL_P": 6.0, + "ATC_ANG_YAW_P": 6.0, + "FRAME_CONFIG": 2.0, + "SERVO9_FUNCTION": 59.0, + "SERVO10_FUNCTION": 7.0, + "SERVO10_REVERSED": 1.0 + }, + "params/ardupilot/ArduSub/4.1/pixhawk1/Standard BlueROV2.params": { + "LEAK1_PIN": 55.0, + "SERVO7_FUNCTION": 59.0, + "SERVO8_FUNCTION": 7.0, + "SERVO8_REVERSED": 1.0, + "BATT_CAPACITY": 0.0, + "BATT_AMP_OFFSET": 0.33, + "BATT_AMP_PERVLT": 37.8788, + "BATT_VOLT_MULT": 11.0, + "BATT_MONITOR": 4.0, + "AHRS_ORIENTATION": 16.0, + "BRD_RTC_TYPES": 3.0, + "MNT_RC_IN_TILT": 8.0, + "MNT_TYPE": 1.0, + "SERIAL0_PROTOCOL": 2.0, + "ATC_ANG_RLL_P": 0.0, + "FRAME_CONFIG": 1.0 + }, + "params/ardupilot/ArduSub/4.1/sitl/heavy.params": { + "BARO_RND": 0.02, + "BATT_MONITOR": 4.0, + "BTN0_FUNCTION": 0.0, + "BTN10_SFUNCTION": 0.0, + "BTN10_FUNCTION": 22.0, + "BTN11_FUNCTION": 42.0, + "BTN11_SFUNCTION": 47.0, + "BTN12_FUNCTION": 43.0, + "BTN12_SFUNCTION": 46.0, + "BTN13_FUNCTION": 33.0, + "BTN13_SFUNCTION": 45.0, + "BTN14_FUNCTION": 32.0, + "BTN14_SFUNCTION": 44.0, + "BTN15_FUNCTION": 0.0, + "BTN15_SFUNCTION": 0.0, + "BTN1_FUNCTION": 6.0, + "BTN1_SFUNCTION": 0.0, + "BTN2_FUNCTION": 8.0, + "BTN2_SFUNCTION": 0.0, + "BTN3_FUNCTION": 7.0, + "BTN3_SFUNCTION": 0.0, + "BTN4_FUNCTION": 4.0, + "BTN4_SFUNCTION": 0.0, + "BTN5_FUNCTION": 1.0, + "BTN5_SFUNCTION": 0.0, + "BTN6_FUNCTION": 3.0, + "BTN6_SFUNCTION": 0.0, + "BTN7_FUNCTION": 21.0, + "BTN7_SFUNCTION": 0.0, + "BTN8_FUNCTION": 48.0, + "BTN8_SFUNCTION": 0.0, + "BTN9_FUNCTION": 23.0, + "BTN9_SFUNCTION": 0.0, + "FRAME_CONFIG": 2.0 + }, + "params/ardupilot/ArduSub/4.1/sitl/standard.params": { + "ARSPD_PIN": 1.0, + "ARSPD_BUS": 2.0, + "BATT_MONITOR": 4.0, + "BTN0_FUNCTION": 0.0, + "BTN10_SFUNCTION": 0.0, + "BTN10_FUNCTION": 22.0, + "BTN11_FUNCTION": 42.0, + "BTN11_SFUNCTION": 47.0, + "BTN12_FUNCTION": 43.0, + "BTN12_SFUNCTION": 46.0, + "BTN13_FUNCTION": 33.0, + "BTN13_SFUNCTION": 45.0, + "BTN14_FUNCTION": 32.0, + "BTN14_SFUNCTION": 44.0, + "BTN15_FUNCTION": 0.0, + "BTN15_SFUNCTION": 0.0, + "BTN1_FUNCTION": 6.0, + "BTN1_SFUNCTION": 0.0, + "BTN2_FUNCTION": 8.0, + "BTN2_SFUNCTION": 0.0, + "BTN3_FUNCTION": 7.0, + "BTN3_SFUNCTION": 0.0, + "BTN4_FUNCTION": 4.0, + "BTN4_SFUNCTION": 0.0, + "BTN5_FUNCTION": 1.0, + "BTN5_SFUNCTION": 0.0, + "BTN6_FUNCTION": 3.0, + "BTN6_SFUNCTION": 0.0, + "BTN7_FUNCTION": 21.0, + "BTN7_SFUNCTION": 0.0, + "BTN8_FUNCTION": 48.0, + "BTN8_SFUNCTION": 0.0, + "BTN9_FUNCTION": 23.0, + "BTN9_SFUNCTION": 0.0, + "RC8_OPTION": 213.0, + "MNT1_PITCH_MAX": 45.0, + "MNT1_PITCH_MIN": -45.0, + "MNT1_DEFLT_MODE": 3.0, + "MNT1_RC_RATE": 30.0, + "MNT1_TYPE": 7.0, + "PSC_ACCZ_P": 2.0, + "PSC_ACCZ_I": 4.0, + "PSC_ACCZ_FF": 0.75, + "PSC_VELZ_P": 8.0, + "PSC_POSZ_P": 3.0, + "PSC_POSXY_P": 2.5, + "PSC_VELXY_D": 0.8, + "PSC_VELXY_I": 0.5, + "PSC_VELXY_P": 5.0, + "RNGFND1_MAX_CM": 3000.0, + "RNGFND1_PIN": 0.0, + "RNGFND1_SCALING": 12.12, + "RNGFND1_TYPE": 1.0, + "SERVO8_FUNCTION": 7.0, + "SERVO8_MAX": 1900.0, + "SERVO8_MIN": 1100.0, + "BARO_EXT_BUS": 1.0, + "PILOT_ACCEL_Z": 200.0, + "PILOT_SPEED_UP": 200.0, + "PSC_JERK_Z": 8.0 + }, + "params/ardupilot/ArduSub/4.1/navigator/Heavy BlueROV2.params": { + "LEAK1_PIN": 27.0, + "BATT_CAPACITY": 0.0, + "BATT_AMP_OFFSET": 0.33, + "BATT_AMP_PERVLT": 37.8788, + "BATT_VOLT_MULT": 11.0, + "BATT_MONITOR": 4.0, + "AHRS_ORIENTATION": 16.0, + "BRD_RTC_TYPES": 3.0, + "MNT_RC_IN_TILT": 8.0, + "MNT_TYPE": 1.0, + "SERIAL0_PROTOCOL": 2.0, + "ATC_ANG_PIT_P": 6.0, + "ATC_ANG_RLL_P": 6.0, + "ATC_ANG_YAW_P": 6.0, + "FRAME_CONFIG": 2.0, + "SERVO7_FUNCTION": 39.0, + "SERVO8_FUNCTION": 40.0, + "SERVO8_REVERSED": 0.0, + "SERVO9_FUNCTION": 59.0, + "SERVO10_FUNCTION": 7.0, + "SERVO10_REVERSED": 1.0 + }, + "params/ardupilot/ArduSub/4.1/navigator/Standard BlueROV2.params": { + "LEAK1_PIN": 27.0, + "BATT_CAPACITY": 0.0, + "BATT_AMP_OFFSET": 0.33, + "BATT_AMP_PERVLT": 37.8788, + "BATT_VOLT_MULT": 11.0, + "BATT_MONITOR": 4.0, + "AHRS_ORIENTATION": 16.0, + "BRD_RTC_TYPES": 3.0, + "MNT_RC_IN_TILT": 8.0, + "MNT_TYPE": 1.0, + "SERIAL0_PROTOCOL": 2.0, + "ATC_ANG_RLL_P": 0.0, + "FRAME_CONFIG": 1.0 + }, + "params/ardupilot/ArduSub/hardware/power_sense_module.params": { + "BATT_CAPACITY": 0.0, + "BATT_AMP_OFFSET": 0.33, + "BATT_AMP_PERVLT": 37.8788, + "BATT_VOLT_MULT": 11.0, + "BATT_MONITOR": 4.0 + }, + "params/ardupilot/ArduSub/hardware/boards/navigator.params": { + "LEAK1_PIN": 27.0 + }, + "params/ardupilot/ArduSub/hardware/boards/pixhawk1.params": { + "LEAK1_PIN": 55.0, + "SERVO7_FUNCTION": 59.0, + "SERVO8_FUNCTION": 7.0, + "SERVO8_REVERSED": 1.0 + }, "params/ardupilot/ArduRover/4.4/Navigator/BlueBoat120.params": { "ACRO_TURN_RATE": 45.0, "AHRS_COMP_BETA": 0.1, @@ -844,6 +1065,36 @@ "WP_SPEED": 1.0, "WRC_ENABLE": 0.0 }, + "params/ardupilot/ArduRover/4.2/sitl/SITL.params": { + "ARSPD_PIN": 1.0, + "ARSPD_BUS": 2.0, + "ATC_SPEED_P": 0.1, + "ATC_STR_RAT_FF": 0.75, + "BATT_MONITOR": 4.0, + "CRUISE_SPEED": 5.0, + "CRUISE_THROTTLE": 30.0, + "MODE3": 11.0, + "MODE4": 10.0, + "MODE5": 2.0, + "RC1_MAX": 2000.0, + "RC1_MIN": 1000.0, + "RC3_MAX": 2000.0, + "RC3_MIN": 1000.0, + "RELAY_PIN": 1.0, + "RELAY_PIN2": 2.0, + "SERVO1_MIN": 1000.0, + "SERVO1_MAX": 2000.0, + "SERVO3_MAX": 2000.0, + "SERVO3_MIN": 1000.0, + "SIM_PIN_MASK": 127.0, + "WP_RADIUS": 3.0, + "WP_SPEED": 5.0, + "INS_LOG_BAT_MASK": 127.0, + "FRAME_CLASS": 2.0, + "SIM_WIND_SPD": 3.0, + "SIM_WIND_T": 1.0, + "SIM_WAVE_ENABLE": 1.0 + }, "params/ardupilot/ArduRover/4.2/Navigator/BlueBoat120.params": { "ACRO_TURN_RATE": 45.0, "AHRS_COMP_BETA": 0.1, @@ -1665,256 +1916,5 @@ "WP_SPEED": 1.0, "WP_SPEED_MIN": 0.0, "WRC_ENABLE": 0.0 - }, - "params/ardupilot/ArduRover/4.2/sitl/SITL.params": { - "ARSPD_PIN": 1.0, - "ARSPD_BUS": 2.0, - "ATC_SPEED_P": 0.1, - "ATC_STR_RAT_FF": 0.75, - "BATT_MONITOR": 4.0, - "CRUISE_SPEED": 5.0, - "CRUISE_THROTTLE": 30.0, - "MODE3": 11.0, - "MODE4": 10.0, - "MODE5": 2.0, - "RC1_MAX": 2000.0, - "RC1_MIN": 1000.0, - "RC3_MAX": 2000.0, - "RC3_MIN": 1000.0, - "RELAY_PIN": 1.0, - "RELAY_PIN2": 2.0, - "SERVO1_MIN": 1000.0, - "SERVO1_MAX": 2000.0, - "SERVO3_MAX": 2000.0, - "SERVO3_MIN": 1000.0, - "SIM_PIN_MASK": 127.0, - "WP_RADIUS": 3.0, - "WP_SPEED": 5.0, - "INS_LOG_BAT_MASK": 127.0, - "FRAME_CLASS": 2.0, - "SIM_WIND_SPD": 3.0, - "SIM_WIND_T": 1.0, - "SIM_WAVE_ENABLE": 1.0 - }, - "params/ardupilot/ArduSub/hardware/power_sense_module.params": { - "BATT_CAPACITY": 0.0, - "BATT_AMP_OFFSET": 0.33, - "BATT_AMP_PERVLT": 37.8788, - "BATT_VOLT_MULT": 11.0, - "BATT_MONITOR": 4.0 - }, - "params/ardupilot/ArduSub/hardware/boards/navigator.params": { - "LEAK1_PIN": 27.0 - }, - "params/ardupilot/ArduSub/hardware/boards/pixhawk1.params": { - "LEAK1_PIN": 55.0, - "SERVO7_FUNCTION": 59.0, - "SERVO8_FUNCTION": 7.0, - "SERVO8_REVERSED": 1.0 - }, - "params/ardupilot/ArduSub/4.1/heavy.params": { - "ATC_ANG_PIT_P": 6.0, - "ATC_ANG_RLL_P": 6.0, - "ATC_ANG_YAW_P": 6.0, - "FRAME_CONFIG": 2.0, - "SERVO7_FUNCTION": 39.0, - "SERVO8_FUNCTION": 40.0, - "SERVO8_REVERSED": 0.0, - "SERVO9_FUNCTION": 59.0, - "SERVO10_FUNCTION": 7.0, - "SERVO10_REVERSED": 1.0 - }, - "params/ardupilot/ArduSub/4.1/base.params": { - "BATT_CAPACITY": 0.0, - "BATT_AMP_OFFSET": 0.33, - "BATT_AMP_PERVLT": 37.8788, - "BATT_VOLT_MULT": 11.0, - "BATT_MONITOR": 4.0, - "AHRS_ORIENTATION": 16.0, - "BRD_RTC_TYPES": 3.0, - "MNT_RC_IN_TILT": 8.0, - "MNT_TYPE": 1.0, - "SERIAL0_PROTOCOL": 2.0 - }, - "params/ardupilot/ArduSub/4.1/standard.params": { - "ATC_ANG_RLL_P": 0.0, - "FRAME_CONFIG": 1.0 - }, - "params/ardupilot/ArduSub/4.1/pixhawk1/Heavy BlueROV2.params": { - "LEAK1_PIN": 55.0, - "SERVO7_FUNCTION": 39.0, - "SERVO8_FUNCTION": 40.0, - "SERVO8_REVERSED": 0.0, - "BATT_CAPACITY": 0.0, - "BATT_AMP_OFFSET": 0.33, - "BATT_AMP_PERVLT": 37.8788, - "BATT_VOLT_MULT": 11.0, - "BATT_MONITOR": 4.0, - "AHRS_ORIENTATION": 16.0, - "BRD_RTC_TYPES": 3.0, - "MNT_RC_IN_TILT": 8.0, - "MNT_TYPE": 1.0, - "SERIAL0_PROTOCOL": 2.0, - "ATC_ANG_PIT_P": 6.0, - "ATC_ANG_RLL_P": 6.0, - "ATC_ANG_YAW_P": 6.0, - "FRAME_CONFIG": 2.0, - "SERVO9_FUNCTION": 59.0, - "SERVO10_FUNCTION": 7.0, - "SERVO10_REVERSED": 1.0 - }, - "params/ardupilot/ArduSub/4.1/pixhawk1/Standard BlueROV2.params": { - "LEAK1_PIN": 55.0, - "SERVO7_FUNCTION": 59.0, - "SERVO8_FUNCTION": 7.0, - "SERVO8_REVERSED": 1.0, - "BATT_CAPACITY": 0.0, - "BATT_AMP_OFFSET": 0.33, - "BATT_AMP_PERVLT": 37.8788, - "BATT_VOLT_MULT": 11.0, - "BATT_MONITOR": 4.0, - "AHRS_ORIENTATION": 16.0, - "BRD_RTC_TYPES": 3.0, - "MNT_RC_IN_TILT": 8.0, - "MNT_TYPE": 1.0, - "SERIAL0_PROTOCOL": 2.0, - "ATC_ANG_RLL_P": 0.0, - "FRAME_CONFIG": 1.0 - }, - "params/ardupilot/ArduSub/4.1/sitl/heavy.params": { - "BARO_RND": 0.02, - "BATT_MONITOR": 4.0, - "BTN0_FUNCTION": 0.0, - "BTN10_SFUNCTION": 0.0, - "BTN10_FUNCTION": 22.0, - "BTN11_FUNCTION": 42.0, - "BTN11_SFUNCTION": 47.0, - "BTN12_FUNCTION": 43.0, - "BTN12_SFUNCTION": 46.0, - "BTN13_FUNCTION": 33.0, - "BTN13_SFUNCTION": 45.0, - "BTN14_FUNCTION": 32.0, - "BTN14_SFUNCTION": 44.0, - "BTN15_FUNCTION": 0.0, - "BTN15_SFUNCTION": 0.0, - "BTN1_FUNCTION": 6.0, - "BTN1_SFUNCTION": 0.0, - "BTN2_FUNCTION": 8.0, - "BTN2_SFUNCTION": 0.0, - "BTN3_FUNCTION": 7.0, - "BTN3_SFUNCTION": 0.0, - "BTN4_FUNCTION": 4.0, - "BTN4_SFUNCTION": 0.0, - "BTN5_FUNCTION": 1.0, - "BTN5_SFUNCTION": 0.0, - "BTN6_FUNCTION": 3.0, - "BTN6_SFUNCTION": 0.0, - "BTN7_FUNCTION": 21.0, - "BTN7_SFUNCTION": 0.0, - "BTN8_FUNCTION": 48.0, - "BTN8_SFUNCTION": 0.0, - "BTN9_FUNCTION": 23.0, - "BTN9_SFUNCTION": 0.0, - "FRAME_CONFIG": 2.0 - }, - "params/ardupilot/ArduSub/4.1/sitl/standard.params": { - "ARSPD_PIN": 1.0, - "ARSPD_BUS": 2.0, - "BATT_MONITOR": 4.0, - "BTN0_FUNCTION": 0.0, - "BTN10_SFUNCTION": 0.0, - "BTN10_FUNCTION": 22.0, - "BTN11_FUNCTION": 42.0, - "BTN11_SFUNCTION": 47.0, - "BTN12_FUNCTION": 43.0, - "BTN12_SFUNCTION": 46.0, - "BTN13_FUNCTION": 33.0, - "BTN13_SFUNCTION": 45.0, - "BTN14_FUNCTION": 32.0, - "BTN14_SFUNCTION": 44.0, - "BTN15_FUNCTION": 0.0, - "BTN15_SFUNCTION": 0.0, - "BTN1_FUNCTION": 6.0, - "BTN1_SFUNCTION": 0.0, - "BTN2_FUNCTION": 8.0, - "BTN2_SFUNCTION": 0.0, - "BTN3_FUNCTION": 7.0, - "BTN3_SFUNCTION": 0.0, - "BTN4_FUNCTION": 4.0, - "BTN4_SFUNCTION": 0.0, - "BTN5_FUNCTION": 1.0, - "BTN5_SFUNCTION": 0.0, - "BTN6_FUNCTION": 3.0, - "BTN6_SFUNCTION": 0.0, - "BTN7_FUNCTION": 21.0, - "BTN7_SFUNCTION": 0.0, - "BTN8_FUNCTION": 48.0, - "BTN8_SFUNCTION": 0.0, - "BTN9_FUNCTION": 23.0, - "BTN9_SFUNCTION": 0.0, - "RC8_OPTION": 213.0, - "MNT1_PITCH_MAX": 45.0, - "MNT1_PITCH_MIN": -45.0, - "MNT1_DEFLT_MODE": 3.0, - "MNT1_RC_RATE": 30.0, - "MNT1_TYPE": 7.0, - "PSC_ACCZ_P": 2.0, - "PSC_ACCZ_I": 4.0, - "PSC_ACCZ_FF": 0.75, - "PSC_VELZ_P": 8.0, - "PSC_POSZ_P": 3.0, - "PSC_POSXY_P": 2.5, - "PSC_VELXY_D": 0.8, - "PSC_VELXY_I": 0.5, - "PSC_VELXY_P": 5.0, - "RNGFND1_MAX_CM": 3000.0, - "RNGFND1_PIN": 0.0, - "RNGFND1_SCALING": 12.12, - "RNGFND1_TYPE": 1.0, - "SERVO8_FUNCTION": 7.0, - "SERVO8_MAX": 1900.0, - "SERVO8_MIN": 1100.0, - "BARO_EXT_BUS": 1.0, - "PILOT_ACCEL_Z": 200.0, - "PILOT_SPEED_UP": 200.0, - "PSC_JERK_Z": 8.0 - }, - "params/ardupilot/ArduSub/4.1/navigator/Heavy BlueROV2.params": { - "LEAK1_PIN": 27.0, - "BATT_CAPACITY": 0.0, - "BATT_AMP_OFFSET": 0.33, - "BATT_AMP_PERVLT": 37.8788, - "BATT_VOLT_MULT": 11.0, - "BATT_MONITOR": 4.0, - "AHRS_ORIENTATION": 16.0, - "BRD_RTC_TYPES": 3.0, - "MNT_RC_IN_TILT": 8.0, - "MNT_TYPE": 1.0, - "SERIAL0_PROTOCOL": 2.0, - "ATC_ANG_PIT_P": 6.0, - "ATC_ANG_RLL_P": 6.0, - "ATC_ANG_YAW_P": 6.0, - "FRAME_CONFIG": 2.0, - "SERVO7_FUNCTION": 39.0, - "SERVO8_FUNCTION": 40.0, - "SERVO8_REVERSED": 0.0, - "SERVO9_FUNCTION": 59.0, - "SERVO10_FUNCTION": 7.0, - "SERVO10_REVERSED": 1.0 - }, - "params/ardupilot/ArduSub/4.1/navigator/Standard BlueROV2.params": { - "LEAK1_PIN": 27.0, - "BATT_CAPACITY": 0.0, - "BATT_AMP_OFFSET": 0.33, - "BATT_AMP_PERVLT": 37.8788, - "BATT_VOLT_MULT": 11.0, - "BATT_MONITOR": 4.0, - "AHRS_ORIENTATION": 16.0, - "BRD_RTC_TYPES": 3.0, - "MNT_RC_IN_TILT": 8.0, - "MNT_TYPE": 1.0, - "SERIAL0_PROTOCOL": 2.0, - "ATC_ANG_RLL_P": 0.0, - "FRAME_CONFIG": 1.0 } } \ No newline at end of file diff --git a/scripts/ardupilot/ArduRover/4.x/navlight.lua b/scripts/ardupilot/ArduRover/4.x/navlight.lua new file mode 100644 index 0000000..73aa736 --- /dev/null +++ b/scripts/ardupilot/ArduRover/4.x/navlight.lua @@ -0,0 +1,145 @@ +-- This scripts is used to control Blue Robotics`s NavLight on the Blue Boat + +local RELAY_NUM = 0 + +local MODES = { + "MANU", + "ACRO", + "STEE", + "HOLD", + "LOIT", + "FOLL", + "SIMP", + "DOCK", + "AUTO", + "RTL", + "SMAR", + "GUID", + "DSRM" +} + +local MODE_MAPPING = { + [0] = "MANU", + [1] = "ACRO", + [3] = "STEE", + [4] = "HOLD", + [5] = "LOIT", + [6] = "FOLL", + [7] = "SIMP", + [8] = "DOCK", + [10] = "AUTO", + [11] = "RTL", + [12] = "SMAR", + [15] = "GUID", + [16] = "INIT" +} + +local PARAM_TABLE_KEY = 70 +local PARAM_TABLE_PREFIX = "NL_" +assert(param:add_table(PARAM_TABLE_KEY, "NL_", 52), "could not add param table") + +-- bind a parameter to a variable +function bind_param(name) + local p = Parameter() + assert(p:init(name), string.format("could not find %s parameter", name)) + 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)) + return bind_param(PARAM_TABLE_PREFIX .. name) +end + +local PARAMS_IDLE = {} -- Dictionary for interval parameters +local PARAMS_ON_TMH = {} -- Dictionary for high time parameters +local PARAMS_OFF_TM = {} -- Dictionary for low time parameters +local PARAMS_BLINKS = {} -- Dictionary for times parameters +local param_idx = 1 + +local DEFAULTS = { + ["MANU"] = {500, 20, 150, 1}, + ["ACRO"] = {500, 20, 150, 1}, + ["STEE"] = {500, 20, 150, 1}, + ["HOLD"] = {2000, 20, 150, 1}, -- Arm + ["LOIT"] = {0, 0, 0, 0}, -- Large blink value to simulate constant light + ["FOLL"] = {0, 0, 0, 0}, -- Large blink value to simulate constant light + ["SIMP"] = {500, 20, 150, 1}, + ["DOCK"] = {1000, 20, 150, 3}, + ["AUTO"] = {1000, 20, 150, 3}, + ["RTL"] = {1000, 20, 150, 3}, + ["SMAR"] = {1000, 20, 150, 3}, + ["GUID"] = {1000, 20, 150, 3}, + ["DSRM"] = {2000, 20, 150, 1} -- Disarm +} +for _, mode in ipairs(MODES) do + local defaults = DEFAULTS[mode] + PARAMS_IDLE[mode] = bind_add_param(mode .. "_IDLE", param_idx, defaults[1]) + PARAMS_ON_TMH[mode] = bind_add_param(mode .. "_ON_TM", param_idx + 1, defaults[2]) -- High time parameters + PARAMS_OFF_TM[mode] = bind_add_param(mode .. "_OFF_TM", param_idx + 2, defaults[3]) -- Low time parameters + PARAMS_BLINKS[mode] = bind_add_param(mode .. "_BLINKS", param_idx + 3, defaults[4]) + param_idx = param_idx + 4 +end + +local last_blink = 0 +local blink_counter = 0 +local is_light_on_tm = false + +function update() -- this is the loop which periodically runs + local now = millis():tofloat() + local mode_num = vehicle:get_mode() + local mode = MODE_MAPPING[mode_num] + + if mode == nil then + -- gcs:send_text(0, string.format("Unrecognized mode number: %s", mode_num)) + mode = "DSRM" -- or another default mode + end + + -- check if the vehicle is disarmed + if not arming:is_armed() then + mode = "DSRM" + end + + local interval = PARAMS_IDLE[mode]:get() + local high = PARAMS_ON_TMH[mode]:get() -- High time parameter + local low = PARAMS_OFF_TM[mode]:get() -- Low time parameter + local times = PARAMS_BLINKS[mode]:get() + + if interval == 0 then -- Special case, light always on + if not is_light_on_tm then + relay:on(RELAY_NUM) + is_light_on_tm = true + end + return update, 50 -- reschedules + end + + -- Check if it's time to start a new cycle + if blink_counter >= times and now - last_blink >= interval then + blink_counter = 0 + is_light_on_tm = false + end + + -- Check if it's time to toggle the light + if blink_counter < times and now - last_blink >= (is_light_on_tm and high or low) then + is_light_on_tm = not is_light_on_tm + if is_light_on_tm then + relay:on(RELAY_NUM) + else + relay:off(RELAY_NUM) + end + last_blink = now + if not is_light_on_tm then -- Increment the counter when the light turns off + blink_counter = blink_counter + 1 + end + end + + -- Turn off the light at the end of the cycle + if blink_counter >= times and is_light_on_tm then + relay:off(RELAY_NUM) + is_light_on_tm = false + end + + return update, 50 -- reschedules +end + +return update() -- run immediately before starting to reschedule diff --git a/scripts_v1.json b/scripts_v1.json new file mode 100644 index 0000000..1fab3f0 --- /dev/null +++ b/scripts_v1.json @@ -0,0 +1,3 @@ +[ + "scripts/ardupilot/ArduRover/4.x/navlight.lua" +] \ No newline at end of file