Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Path: blob/master/libraries/AP_BoardConfig/AP_BoardConfig.cpp
Views: 1798
/*1This program is free software: you can redistribute it and/or modify2it under the terms of the GNU General Public License as published by3the Free Software Foundation, either version 3 of the License, or4(at your option) any later version.56This program is distributed in the hope that it will be useful,7but WITHOUT ANY WARRANTY; without even the implied warranty of8MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the9GNU General Public License for more details.1011You should have received a copy of the GNU General Public License12along with this program. If not, see <http://www.gnu.org/licenses/>.13*/14/*15* AP_BoardConfig - board specific configuration16*/1718#include "AP_BoardConfig.h"1920#include <AP_Common/AP_Common.h>21#include <AP_HAL/AP_HAL.h>22#include <AP_Math/AP_Math.h>23#include <AP_RTC/AP_RTC.h>24#include <AP_Vehicle/AP_Vehicle_Type.h>25#include <GCS_MAVLink/GCS.h>26#include <AP_Filesystem/AP_Filesystem.h>27#include <GCS_MAVLink/GCS.h>28#include <GCS_MAVLink/GCS_MAVLink.h>2930#include <stdio.h>3132#ifndef BOARD_TYPE_DEFAULT33#define BOARD_TYPE_DEFAULT PX4_BOARD_AUTO34#endif3536#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS37#ifndef BOARD_SER1_RTSCTS_DEFAULT38# define BOARD_SER1_RTSCTS_DEFAULT 239#endif40#ifndef BOARD_TYPE_DEFAULT41# define BOARD_TYPE_DEFAULT PX4_BOARD_AUTO42#endif43#endif4445#ifndef BOARD_SAFETY_ENABLE_DEFAULT46#if defined(HAL_GPIO_PIN_SAFETY_IN)47// have safety startup enabled if we have a safety pin48# define BOARD_SAFETY_ENABLE_DEFAULT 149#elif defined(HAL_WITH_IO_MCU)50// if we have an IOMCU then enable by default51# define BOARD_SAFETY_ENABLE_DEFAULT HAL_WITH_IO_MCU52#else53# define BOARD_SAFETY_ENABLE_DEFAULT 054#endif55#endif5657#ifndef HAL_IMU_TEMP_DEFAULT58#define HAL_IMU_TEMP_DEFAULT -1 // disabled59#endif6061#ifndef HAL_IMU_TEMP_MARGIN_LOW_DEFAULT62#define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 0 // disabled63#endif6465#ifndef BOARD_SAFETY_OPTION_DEFAULT66# define BOARD_SAFETY_OPTION_DEFAULT (BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF|BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)67#endif68#ifndef BOARD_SAFETY_ENABLE69# define BOARD_SAFETY_ENABLE 170#endif7172#ifndef BOARD_CONFIG_BOARD_VOLTAGE_MIN73#define BOARD_CONFIG_BOARD_VOLTAGE_MIN 4.3f74#endif7576#ifndef HAL_BRD_OPTIONS_DEFAULT77#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) && !APM_BUILD_TYPE(APM_BUILD_Replay)78#ifdef HAL_DEBUG_BUILD79#define HAL_BRD_OPTIONS_DEFAULT BOARD_OPTION_WATCHDOG | BOARD_OPTION_DEBUG_ENABLE80#else81#define HAL_BRD_OPTIONS_DEFAULT BOARD_OPTION_WATCHDOG82#endif83#else84#define HAL_BRD_OPTIONS_DEFAULT 085#endif86#endif8788#ifndef HAL_DEFAULT_BOOT_DELAY89#define HAL_DEFAULT_BOOT_DELAY 090#endif9192extern const AP_HAL::HAL& hal;93AP_BoardConfig *AP_BoardConfig::_singleton;9495// constructor96AP_BoardConfig::AP_BoardConfig()97#if HAL_HAVE_IMU_HEATER98// initialise heater PI controller. Note we do this in the cpp file99// for ccache efficiency100: heater{{HAL_IMUHEAT_P_DEFAULT, HAL_IMUHEAT_I_DEFAULT, 70},}101#endif102{103_singleton = this;104AP_Param::setup_object_defaults(this, var_info);105};106107// table of user settable parameters108const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {109110// index 0 was used by PWM_COUNT111112#if AP_FEATURE_RTSCTS113#ifdef HAL_HAVE_RTSCTS_SERIAL1114// @Param: SER1_RTSCTS115// @DisplayName: Serial 1 flow control116// @Description: Enable flow control on serial 1 (telemetry 1). You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup. Note that the PX4v1 does not have hardware flow control pins on this port, so you should leave this disabled.117// @Values: 0:Disabled,1:Enabled,2:Auto,3:RS-485 Driver enable RTS pin118// @RebootRequired: True119// @User: Advanced120AP_GROUPINFO("SER1_RTSCTS", 1, AP_BoardConfig, state.ser_rtscts[1], BOARD_SER1_RTSCTS_DEFAULT),121#endif122123#ifdef HAL_HAVE_RTSCTS_SERIAL2124// @Param: SER2_RTSCTS125// @CopyFieldsFrom: BRD_SER1_RTSCTS126// @DisplayName: Serial 2 flow control127// @Description: Enable flow control on serial 2 (telemetry 2). You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup.128AP_GROUPINFO("SER2_RTSCTS", 2, AP_BoardConfig, state.ser_rtscts[2], 2),129#endif130131#ifdef HAL_HAVE_RTSCTS_SERIAL3132// @Param: SER3_RTSCTS133// @CopyFieldsFrom: BRD_SER1_RTSCTS134// @DisplayName: Serial 3 flow control135// @Description: Enable flow control on serial 3. You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup.136AP_GROUPINFO("SER3_RTSCTS", 26, AP_BoardConfig, state.ser_rtscts[3], 2),137#endif138139#ifdef HAL_HAVE_RTSCTS_SERIAL4140// @Param: SER4_RTSCTS141// @CopyFieldsFrom: BRD_SER1_RTSCTS142// @DisplayName: Serial 4 flow control143// @Description: Enable flow control on serial 4. You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup.144AP_GROUPINFO("SER4_RTSCTS", 27, AP_BoardConfig, state.ser_rtscts[4], 2),145#endif146147#ifdef HAL_HAVE_RTSCTS_SERIAL5148// @Param: SER5_RTSCTS149// @CopyFieldsFrom: BRD_SER1_RTSCTS150// @DisplayName: Serial 5 flow control151// @Description: Enable flow control on serial 5. You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup.152AP_GROUPINFO("SER5_RTSCTS", 25, AP_BoardConfig, state.ser_rtscts[5], 2),153#endif154#endif155156// @Param: SAFETY_DEFLT157// @DisplayName: Sets default state of the safety switch158// @Description: This controls the default state of the safety switch at startup. When set to 1 the safety switch will start in the safe state (flashing) at boot. When set to zero the safety switch will start in the unsafe state (solid) at startup. Note that if a safety switch is fitted the user can still control the safety state after startup using the switch. The safety state can also be controlled in software using a MAVLink message.159// @Values: 0:Disabled,1:Enabled160// @RebootRequired: True161// @User: Standard162AP_GROUPINFO("SAFETY_DEFLT", 3, AP_BoardConfig, state.safety_enable, BOARD_SAFETY_ENABLE_DEFAULT),163164#if AP_FEATURE_SBUS_OUT165// @Param: SBUS_OUT166// @DisplayName: SBUS output rate167// @Description: This sets the SBUS output frame rate in Hz168// @Values: 0:Disabled,1:50Hz,2:75Hz,3:100Hz,4:150Hz,5:200Hz,6:250Hz,7:300Hz169// @RebootRequired: True170// @User: Advanced171AP_GROUPINFO("SBUS_OUT", 4, AP_BoardConfig, state.sbus_out_rate, 0),172#endif173174// @Param: SERIAL_NUM175// @DisplayName: User-defined serial number176// @Description: User-defined serial number of this vehicle, it can be any arbitrary number you want and has no effect on the autopilot177// @Range: -8388608 8388607178// @User: Standard179AP_GROUPINFO("SERIAL_NUM", 5, AP_BoardConfig, vehicleSerialNumber, 0),180181// @Param: SAFETY_MASK182// @DisplayName: Outputs which ignore the safety switch state183// @Description: A bitmask which controls what outputs can move while the safety switch has not been pressed184// @Bitmask: 0:Output1,1:Output2,2:Output3,3:Output4,4:Output5,5:Output6,6:Output7,7:Output8,8:Output9,9:Output10,10:Output11,11:Output12,12:Output13,13:Output14185// @RebootRequired: True186// @User: Advanced187AP_GROUPINFO("SAFETY_MASK", 7, AP_BoardConfig, state.ignore_safety_channels, 0),188189#if HAL_HAVE_IMU_HEATER190// @Param: HEAT_TARG191// @DisplayName: Board heater temperature target192// @Description: Board heater target temperature for boards with controllable heating units. Set to -1 to disable the heater, please reboot after setting to -1.193// @Range: -1 80194// @Units: degC195// @User: Advanced196AP_GROUPINFO("HEAT_TARG", 8, AP_BoardConfig, heater.imu_target_temperature, HAL_IMU_TEMP_DEFAULT),197#endif198199#if AP_FEATURE_BOARD_DETECT200// @Param: TYPE201// @DisplayName: Board type202// @Description: This allows selection of a PX4 or VRBRAIN board type. If set to zero then the board type is auto-detected (PX4)203// @Values: 0:AUTO,1:PX4V1,2:Pixhawk,3:Cube/Pixhawk2,4:Pixracer,5:PixhawkMini,6:Pixhawk2Slim,13:Intel Aero FC,14:Pixhawk Pro,20:AUAV2.1,21:PCNC1,22:MINDPXV2,23:SP01,24:CUAVv5/FMUV5,30:VRX BRAIN51,32:VRX BRAIN52,33:VRX BRAIN52E,34:VRX UBRAIN51,35:VRX UBRAIN52,36:VRX CORE10,38:VRX BRAIN54,39:PX4 FMUV6,100:PX4 OLDDRIVERS204// @RebootRequired: True205// @User: Advanced206AP_GROUPINFO("TYPE", 9, AP_BoardConfig, state.board_type, BOARD_TYPE_DEFAULT),207#endif208209#if HAL_WITH_IO_MCU210// @Param: IO_ENABLE211// @DisplayName: Enable IO co-processor212// @Description: This allows for the IO co-processor on boards with an IOMCU to be disabled. Setting to 2 will enable the IOMCU but not attempt to update firmware on startup213// @Values: 0:Disabled,1:Enabled,2:EnableNoFWUpdate214// @RebootRequired: True215// @User: Advanced216AP_GROUPINFO("IO_ENABLE", 10, AP_BoardConfig, state.io_enable, 1),217#endif218219#if AP_RADIO_ENABLED220// @Group: RADIO221// @Path: ../AP_Radio/AP_Radio.cpp222AP_SUBGROUPINFO(_radio, "RADIO", 11, AP_BoardConfig, AP_Radio),223#endif224225// @Param: SAFETYOPTION226// @DisplayName: Options for safety button behavior227// @Description: This controls the activation of the safety button. It allows you to control if the safety button can be used for safety enable and/or disable, and whether the button is only active when disarmed228// @Bitmask: 0:ActiveForSafetyDisable,1:ActiveForSafetyEnable,2:ActiveWhenArmed,3:Force safety on when the aircraft disarms229// @User: Standard230AP_GROUPINFO("SAFETYOPTION", 13, AP_BoardConfig, state.safety_option, BOARD_SAFETY_OPTION_DEFAULT),231232#if AP_RTC_ENABLED233// @Group: RTC234// @Path: ../AP_RTC/AP_RTC.cpp235AP_SUBGROUPINFO(rtc, "RTC", 14, AP_BoardConfig, AP_RTC),236#endif237238#if HAL_HAVE_BOARD_VOLTAGE239// @Param: VBUS_MIN240// @DisplayName: Autopilot board voltage requirement241// @Description: Minimum voltage on the autopilot power rail to allow the aircraft to arm. 0 to disable the check.242// @Units: V243// @Range: 4.0 5.5244// @Increment: 0.1245// @User: Advanced246AP_GROUPINFO("VBUS_MIN", 15, AP_BoardConfig, _vbus_min, BOARD_CONFIG_BOARD_VOLTAGE_MIN),247248#endif249250#if HAL_HAVE_SERVO_VOLTAGE251// @Param: VSERVO_MIN252// @DisplayName: Servo voltage requirement253// @Description: Minimum voltage on the servo rail to allow the aircraft to arm. 0 to disable the check.254// @Units: V255// @Range: 3.3 12.0256// @Increment: 0.1257// @User: Advanced258AP_GROUPINFO("VSERVO_MIN", 16, AP_BoardConfig, _vservo_min, 0),259#endif260261#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS262// @Param: SD_SLOWDOWN263// @DisplayName: microSD slowdown264// @Description: This is a scaling factor to slow down microSD operation. It can be used on flight board and microSD card combinations where full speed is not reliable. For normal full speed operation a value of 0 should be used.265// @Range: 0 32266// @Increment: 1267// @User: Advanced268AP_GROUPINFO("SD_SLOWDOWN", 17, AP_BoardConfig, _sdcard_slowdown, 0),269#endif270271#ifdef HAL_GPIO_PWM_VOLT_PIN272// @Param: PWM_VOLT_SEL273// @DisplayName: Set PWM Out Voltage274// @Description: This sets the voltage max for PWM output pulses. 0 for 3.3V and 1 for 5V output. On boards with an IOMCU that support this parameter this option only affects the 8 main outputs, not the 6 auxiliary outputs. Using 5V output can help to reduce the impact of ESC noise interference corrupting signals to the ESCs.275// @Values: 0:3.3V,1:5V276// @User: Advanced277AP_GROUPINFO("PWM_VOLT_SEL", 18, AP_BoardConfig, _pwm_volt_sel, 0),278#endif279280// @Param: OPTIONS281// @DisplayName: Board options282// @Description: Board specific option flags283// @Bitmask: 0:Enable hardware watchdog, 1:Disable MAVftp, 2:Enable set of internal parameters, 3:Enable Debug Pins, 4:Unlock flash on reboot, 5:Write protect firmware flash on reboot, 6:Write protect bootloader flash on reboot, 7:Skip board validation, 8:Disable board arming gpio output change on arm/disarm, 9:Use safety pins as profiled284// @User: Advanced285AP_GROUPINFO("OPTIONS", 19, AP_BoardConfig, _options, HAL_BRD_OPTIONS_DEFAULT),286287// @Param: BOOT_DELAY288// @DisplayName: Boot delay289// @Description: This adds a delay in milliseconds to boot to ensure peripherals initialise fully290// @Range: 0 10000291// @Units: ms292// @User: Advanced293AP_GROUPINFO("BOOT_DELAY", 20, AP_BoardConfig, _boot_delay_ms, HAL_DEFAULT_BOOT_DELAY),294295#if HAL_HAVE_IMU_HEATER296// @Param: HEAT_P297// @DisplayName: Board Heater P gain298// @Description: Board Heater P gain299// @Range: 1 500300// @Increment: 1301// @User: Advanced302303// @Param: HEAT_I304// @DisplayName: Board Heater I gain305// @Description: Board Heater integrator gain306// @Range: 0 1307// @Increment: 0.1308// @User: Advanced309310// @Param: HEAT_IMAX311// @DisplayName: Board Heater IMAX312// @Description: Board Heater integrator maximum313// @Range: 0 100314// @Increment: 1315// @User: Advanced316AP_SUBGROUPINFO(heater.pi_controller, "HEAT_", 21, AP_BoardConfig, AC_PI),317#endif318319#ifdef HAL_PIN_ALT_CONFIG320// @Param: ALT_CONFIG321// @DisplayName: Alternative HW config322// @Description: Select an alternative hardware configuration. A value of zero selects the default configuration for this board. Other values are board specific. Please see the documentation for your board for details on any alternative configuration values that may be available.323// @Range: 0 10324// @Increment: 1325// @User: Advanced326// @RebootRequired: True327AP_GROUPINFO("ALT_CONFIG", 22, AP_BoardConfig, _alt_config, 0),328#endif // HAL_PIN_ALT_CONFIG329330#if HAL_HAVE_IMU_HEATER331// @Param: HEAT_LOWMGN332// @DisplayName: Board heater temp lower margin333// @Description: Arming check will fail if temp is lower than this margin below BRD_HEAT_TARG. 0 disables the low temperature check334// @Range: 0 20335// @Units: degC336// @User: Advanced337AP_GROUPINFO("HEAT_LOWMGN", 23, AP_BoardConfig, heater.imu_arming_temperature_margin_low, HAL_IMU_TEMP_MARGIN_LOW_DEFAULT),338#endif339340#if AP_SDCARD_STORAGE_ENABLED341// @Param: SD_MISSION342// @DisplayName: SDCard Mission size343// @Description: This sets the amount of storage in kilobytes reserved on the microsd card in mission.stg for waypoint storage. Each waypoint uses 15 bytes.344// @Range: 0 64345// @RebootRequired: True346// @User: Advanced347AP_GROUPINFO("SD_MISSION", 24, AP_BoardConfig, sdcard_storage.mission_kb, 0),348349// @Param: SD_FENCE350// @DisplayName: SDCard Fence size351// @Description: This sets the amount of storage in kilobytes reserved on the microsd card in fence.stg for fence storage.352// @Range: 0 64353// @RebootRequired: True354// @User: Advanced355AP_GROUPINFO("SD_FENCE", 29, AP_BoardConfig, sdcard_storage.fence_kb, 0),356#endif357358// index 25 used by SER5_RTSCTS359// index 26 used by SER3_RTSCTS360// index 27 used by SER4_RTSCTS361362363#if HAL_WITH_IO_MCU_DSHOT364// @Param: IO_DSHOT365// @DisplayName: Load DShot FW on IO366// @Description: This loads the DShot firmware on the IO co-processor367// @Values: 0:StandardFW,1:DshotFW368// @RebootRequired: True369// @User: Advanced370AP_GROUPINFO("IO_DSHOT", 28, AP_BoardConfig, state.io_dshot, 0),371#endif372AP_GROUPEND373};374375void AP_BoardConfig::init()376{377// PARAMETER_CONVERSION - Added: APR-2022378vehicleSerialNumber.convert_parameter_width(AP_PARAM_INT16);379380board_setup();381382#if AP_RTC_ENABLED383AP::rtc().set_utc_usec(hal.util->get_hw_rtc(), AP_RTC::SOURCE_HW);384#endif385386if (_boot_delay_ms > 0) {387uint16_t delay_ms = uint16_t(_boot_delay_ms.get());388if (hal.util->was_watchdog_armed() && delay_ms > 200) {389// don't delay a long time on watchdog reset, the pilot390// may be able to save the vehicle391delay_ms = 200;392}393hal.scheduler->delay(delay_ms);394}395396#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && defined(USE_POSIX)397uint8_t slowdown = constrain_int16(_sdcard_slowdown.get(), 0, 32);398const uint8_t max_slowdown = 8;399do {400if (AP::FS().retry_mount()) {401break;402}403slowdown++;404hal.scheduler->delay(5);405} while (slowdown < max_slowdown);406if (slowdown < max_slowdown) {407_sdcard_slowdown.set(slowdown);408} else {409printf("SDCard failed to start\n");410}411#endif412}413414// set default value for BRD_SAFETY_MASK415void AP_BoardConfig::set_default_safety_ignore_mask(uint32_t mask)416{417state.ignore_safety_channels.set_default(mask);418}419420void AP_BoardConfig::init_safety()421{422board_init_safety();423board_init_debug();424}425426/*427notify user of a fatal startup error related to available sensors.428*/429bool AP_BoardConfig::_in_error_loop;430431void AP_BoardConfig::throw_error(const char *err_type, const char *fmt, va_list arg)432{433_in_error_loop = true;434/*435to give the user the opportunity to connect to USB we keep436repeating the error. The mavlink delay callback is initialised437before this, so the user can change parameters (and in438particular BRD_TYPE if needed)439*/440uint32_t last_print_ms = 0;441while (true) {442uint32_t now = AP_HAL::millis();443if (now - last_print_ms >= 5000) {444last_print_ms = now;445char printfmt[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+2];446hal.util->snprintf(printfmt, sizeof(printfmt), "%s: %s\n", err_type, fmt);447{448va_list arg_copy;449va_copy(arg_copy, arg);450vprintf(printfmt, arg_copy);451va_end(arg_copy);452}453#if HAL_GCS_ENABLED454hal.util->snprintf(printfmt, sizeof(printfmt), "%s: %s", err_type, fmt);455{456va_list arg_copy;457va_copy(arg_copy, arg);458gcs().send_textv(MAV_SEVERITY_CRITICAL, printfmt, arg_copy);459va_end(arg_copy);460}461#endif462}463#if HAL_GCS_ENABLED464gcs().update_receive();465gcs().update_send();466#endif467EXPECT_DELAY_MS(10);468hal.scheduler->delay(5);469}470}471472void AP_BoardConfig::allocation_error(const char *fmt, ...)473{474va_list arg_list;475va_start(arg_list, fmt);476char newfmt[64] {};477snprintf(newfmt, sizeof(newfmt), "Unable to allocate %s", fmt);478throw_error("Allocation Error", newfmt, arg_list);479va_end(arg_list);480}481482void AP_BoardConfig::config_error(const char *fmt, ...)483{484va_list arg_list;485va_start(arg_list, fmt);486throw_error("Config Error", fmt, arg_list);487va_end(arg_list);488}489490/*491handle logic for safety state button press. This should be called at49210Hz when the button is pressed. The button can either be directly493on a pin or on a UAVCAN device494This function returns true if the safety state should be toggled495*/496bool AP_BoardConfig::safety_button_handle_pressed(uint8_t press_count)497{498if (press_count != 10) {499return false;500}501// get button options502uint16_t safety_options = get_safety_button_options();503if (!(safety_options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) &&504hal.util->get_soft_armed()) {505return false;506}507AP_HAL::Util::safety_state safety_state = hal.util->safety_switch_state();508if (safety_state == AP_HAL::Util::SAFETY_DISARMED &&509!(safety_options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF)) {510return false;511}512if (safety_state == AP_HAL::Util::SAFETY_ARMED &&513!(safety_options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)) {514return false;515}516return true;517}518519namespace AP {520AP_BoardConfig *boardConfig(void) {521return AP_BoardConfig::get_singleton();522}523};524525526