From c2fbc833e5d4ad9b7eef34d537d384e6524743bd Mon Sep 17 00:00:00 2001 From: colejmero <57465269+colejmero@users.noreply.github.com> Date: Tue, 23 Feb 2021 01:37:49 -0500 Subject: [PATCH 01/10] Adds AS5600 library --- ArduPlane/ArduPlane.cpp | 20 +- ArduPlane/Plane.h | 5 + ArduPlane/wscript | 81 +++---- libraries/AP_AS5600_AOA/AS5600_AOA.cpp | 290 +++++++++++++++++++++++++ libraries/AP_AS5600_AOA/AS5600_AOA.h | 97 +++++++++ 5 files changed, 452 insertions(+), 41 deletions(-) create mode 100644 libraries/AP_AS5600_AOA/AS5600_AOA.cpp create mode 100644 libraries/AP_AS5600_AOA/AS5600_AOA.h diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index dd3f79ffb7303e..c7c90aa260fb5b 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -46,7 +46,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(navigate, 10, 150), SCHED_TASK(update_compass, 10, 200), SCHED_TASK(read_airspeed, 10, 100), - SCHED_TASK(update_alt, 10, 200), + SCHED_TASK(read_aoa, 10, 200), + SCHED_TASK(check_aoa, 10, 200),//This line added by Cole + SCHED_TASK(update_alt, 10, 200),//This line added by Cole SCHED_TASK(adjust_altitude_target, 10, 200), #if ADVANCED_FAILSAFE == ENABLED SCHED_TASK(afs_fs_check, 10, 100), @@ -193,6 +195,22 @@ void Plane::update_compass(void) } } +/* + * Check if the AoA sensor is up and running This function is code added by Cole + */ + +void Plane::check_aoa(void){ + aoa_sensor.checkConnect(); +} + +/* + * Read and log Angle of Attack This function is code added by Cole + */ + +void Plane::read_aoa(void){ + aoa_sensor.getRawAngle(); +} + /* do 10Hz logging */ diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 8ec76e21a98b04..5a5254d1e1dc74 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -76,6 +76,7 @@ #include #include +#include //Magnetic Encoder library, line added by Cole #include // Optical Flow library #include @@ -196,6 +197,8 @@ class Plane : public AP_Vehicle { AP_RPM rpm_sensor; + AS_5600 aoa_sensor; //This line added by Cole + AP_TECS TECS_controller{ahrs, aparm, landing}; AP_L1_Control L1_controller{ahrs, &TECS_controller}; @@ -996,6 +999,8 @@ class Plane : public AP_Vehicle { void read_airspeed(void); void rpm_update(void); void accel_cal_update(void); + void check_aoa(void); //This line added by Cole + void read_aoa(void); //This line added by Cole // system.cpp void init_ardupilot() override; diff --git a/ArduPlane/wscript b/ArduPlane/wscript index dde7456ca2535f..7d078b8eb0063f 100644 --- a/ArduPlane/wscript +++ b/ArduPlane/wscript @@ -1,40 +1,41 @@ -#!/usr/bin/env python -# encoding: utf-8 - -def build(bld): - vehicle = bld.path.name - bld.ap_stlib( - name=vehicle + '_libs', - ap_vehicle=vehicle, - ap_libraries=bld.ap_common_vehicle_libraries() + [ - 'APM_Control', - 'AP_AdvancedFailsafe', - 'AP_Avoidance', - 'AP_Arming', - 'AP_Camera', - 'AP_L1_Control', - 'AP_Navigation', - 'AP_RCMapper', - 'AP_SpdHgtControl', - 'AP_TECS', - 'AP_InertialNav', - 'AC_WPNav', - 'AC_AttitudeControl', - 'AP_Motors', - 'AP_Landing', - 'AP_Beacon', - 'PID', - 'AP_Soaring', - 'AP_LTM_Telem', - 'AP_Devo_Telem', - 'AP_OSD', - 'AC_AutoTune', - 'AP_KDECAN', - ], - ) - - bld.ap_program( - program_name='arduplane', - program_groups=['bin', 'plane'], - use=vehicle + '_libs', - ) +#!/usr/bin/env python +# encoding: utf-8 + +def build(bld): + vehicle = bld.path.name + bld.ap_stlib( + name=vehicle + '_libs', + ap_vehicle=vehicle, + ap_libraries=bld.ap_common_vehicle_libraries() + [ + 'APM_Control', + 'AP_AdvancedFailsafe', + 'AP_Avoidance', + 'AP_Arming', + 'AP_Camera', + 'AP_L1_Control', + 'AP_Navigation', + 'AP_RCMapper', + 'AP_SpdHgtControl', + 'AP_TECS', + 'AP_InertialNav', + 'AC_WPNav', + 'AC_AttitudeControl', + 'AP_Motors', + 'AP_Landing', + 'AP_Beacon', + 'PID', + 'AP_Soaring', + 'AP_LTM_Telem', + 'AP_Devo_Telem', + 'AP_OSD', + 'AC_AutoTune', + 'AP_AS5600_AOA', + 'AP_KDECAN', + ], + ) + + bld.ap_program( + program_name='arduplane', + program_groups=['bin', 'plane'], + use=vehicle + '_libs', + ) diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp new file mode 100644 index 00000000000000..d739301e2e7344 --- /dev/null +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp @@ -0,0 +1,290 @@ +/**************************************************** + * AMS 5600 Angle of Attack Class for Ardupilot platform + * Author: Cole Mero + * Date: 15 Dec 2014 + * File: AS5600_AOA.cpp + * Version 1.00 + * www.ams.com + * + * Description: This class has been designed to + * access the AS5600 magnetic encoder sensor to + * read the angle of attack and record it for experimental purposes + * + * It was adapted from the Arduino library available for the AS5600 sensor. + * +***************************************************/ + +#include +#include "AS5600_AOA.h" + +extern const AP_HAL::HAL &hal; + +/**************************************************** + * Method: AS_5600 + * In: none + * Out: none + * Description: constructor for AMS 5600 +***************************************************/ + AS_5600::AS_5600() +{ + + bus = 1; //Sets the bus number for the device, unclear what this number should be, trial and error to make it work + address = 0x36; //This is the I2C address for the device, it is set by the manufacturer + + bus_clock = 400000; + use_smbus = false; + timeout_ms = 4; + + //ChibiOS::I2CDeviceManager myDev; //Create an instance of an I2C device + + //busMaskExt = myDev.get_bus_mask_external(); //Used to see what I2C buses exist on the device + //busMaskInt = myDev.get_bus_mask_internal(); //Used to see what I2C buses exist on the device + + //dev = myDev.get_device(bus, address); //Get the specific device which is desired + + + + /*load register values*/ + /*c++ class forbids pre loading of variables */ + + + _zmco = 0x00; + _zpos_hi = 0x01; + _zpos_lo = 0x02; + _mpos_hi = 0x03; + _mpos_lo = 0x04; + _mang_hi = 0x05; + _mang_lo = 0x06; + _conf_hi = 0x07; + _conf_lo = 0x08; + _raw_ang_hi = 0x0c; + _raw_ang_lo = 0x0d; + _ang_hi = 0x0e; + _ang_lo = 0x0f; + _stat = 0x0b; + _agc = 0x1a; + _mag_hi = 0x1b; + _mag_lo = 0x1c; + _burn = 0xff; + +} + + +void AS_5600::init(){ + + dev = hal.i2c_mgr->get_device(bus, address); + + dev->get_semaphore()->take_blocking(); + dev->set_speed(AP_HAL::Device::SPEED_LOW); + dev->set_retries(2); + dev->get_semaphore()->give(); + +} + +/* mode = 0, output PWM, mode = 1 output analog (full range from 0% to 100% between GND and VDD*/ +void AS_5600::setOutPut(unsigned char mode){ + unsigned char config_status; + config_status = readOneByte(_conf_lo); + if(mode == 1){ + config_status = config_status & 0xcf; + }else{ + config_status = config_status & 0xef; + } + + /* Note significant variance from the Arduino AMS_5600 library in this line. + * + * -> writeOneByte(_conf_lo, lowByte(config_status)); + * vs writeOneByte(_conf_lo, config_status); I have removed the lowByte() function + * since it is part of the arduino.h library and I don't have access to it, however, + * since it accesses the lowest byte in the variable, and an unsigned char such as + * config_status only HAS one byte regardless, I think the end result should be identical? + */ + writeOneByte(_conf_lo, config_status); +} + +void AS_5600::checkConnect(){ + + + + AP::logger().Write("AoAC", "Status, TimeUS, busMaskExt, busMaskInt, checkVal", "iQIIi", int(bool(dev)), AP_HAL::micros64(), busMaskExt, busMaskInt, 42); + + } + + +/******************************************************* + * Method: getRawAngle + * In: none + * Out: value of raw angle register + * Description: gets raw value of magnet position. + * start, end, and max angle settings do not apply +*******************************************************/ +unsigned short AS_5600::getRawAngle(void) +{ + unsigned short angle = readTwoBytes(_raw_ang_hi, _raw_ang_lo); + + AP::logger().Write("AoAR", "Status, TimeUS, Angle", "iQH", int(bool(dev)), AP_HAL::micros64(), angle); + + return angle; +} +/******************************************************* + * Method: highByte + * In: Unsigned short + * Out: Highest or leftmost byte of the unsigned short + * Description: Takes in the unsigned short and returns + * the leftmost or highest bite +*******************************************************/ + +unsigned char AS_5600::highByte(unsigned short short_in){ + + unsigned char hiByte = ((short_in >> 8) & 0xff); + return hiByte; +} + +/******************************************************* + * Method: lowByte + * In: Unsigned short + * Out: Lowest or rightmost byte of the unsigned short + * Description: Takes in the unsigned short and returns + * the rightmost or lowest byte +*******************************************************/ + +unsigned char AS_5600::lowByte(unsigned short short_in){ + + unsigned char loByte = (short_in & 0xff); + return loByte; +} + + +/******************************************************* + * Method: setMaxAngle + * In: new maximum angle to set OR none + * Out: value of max angle register + * Description: sets a value in maximum angle register. + * If no value is provided, method will read position of + * magnet. Setting this register zeros out max position + * register. +*******************************************************/ +/*unsigned short AS_5600::setMaxAngle(unsigned short newMaxAngle) +{ + unsigned short retVal; + if(newMaxAngle == -1) + { + maxAngle = getRawAngle(); + } + else + maxAngle = newMaxAngle; + + writeOneByte(_mang_hi, highByte(maxAngle)); + usleep(2); + writeOneByte(_mang_lo, lowByte(maxAngle)); + usleep(2); + + retVal = readTwoBytes(_mang_hi, _mang_lo); + return retVal; +} + +*/ + +/******************************************************* + * Method: getMaxAngle + * In: none + * Out: value of max angle register + * Description: gets value of maximum angle register. +*******************************************************/ +unsigned short AS_5600::getMaxAngle() +{ + return readTwoBytes(_mang_hi, _mang_lo); +} + + +/******************************************************* + * Method: setStartPosition + * In: new start angle position + * Out: value of start position register + * Description: sets a value in start position register. + * If no value is provided, method will read position of + * magnet. +*******************************************************/ +/* +unsigned short AS_5600::setStartPosition(unsigned short startAngle) +{ + if(startAngle == -1) + { + rawStartAngle = getRawAngle(); + } + else + rawStartAngle = startAngle; + + writeOneByte(_zpos_hi, highByte(rawStartAngle)); + usleep(2); + writeOneByte(_zpos_lo, lowByte(rawStartAngle)); + usleep(2); + zPosition = readTwoBytes(_zpos_hi, _zpos_lo); + + return(zPosition); +} + +*/ + +int AS_5600::writeOneByte(uint8_t in_adr, uint8_t msg){ + + WITH_SEMAPHORE(sem); + + uint8_t send[2] = {in_adr, msg}; + + bool success = dev->transfer(send, sizeof(send), nullptr, 0); + + return success ? 0 : -1; + +} + + +/******************************************************* + * Method: readOneByte + * In: register to read + * Out: data read from i2c + * Description: reads one byte register from i2c +*******************************************************/ +int AS_5600::readOneByte(uint8_t in_adr) +{ + + WITH_SEMAPHORE(sem); + + uint8_t send[1] = {in_adr}; + uint8_t recv[1]; + + + bool success = dev->transfer(send, sizeof(send), recv, sizeof(recv)); + + return success ? recv[0] : -1; +} + + +/******************************************************* + * Method: readTwoBytes + * In: register to read + * Out: data read from i2c + * Description: reads two bytes register from i2c +*******************************************************/ +int AS_5600::readTwoBytes(uint8_t in_adr1, uint8_t in_adr2) +{ + WITH_SEMAPHORE(sem); + + int firstResult = readOneByte(in_adr1); + int secondResult = readOneByte(in_adr2); + + if (firstResult == -1 || secondResult == -1){ + + return -1; + } + + uint8_t firstByte = firstResult; + uint8_t secondByte = secondResult; + + uint16_t combined = (firstByte << 8) | secondByte; + + return combined; + +} + + diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.h b/libraries/AP_AS5600_AOA/AS5600_AOA.h new file mode 100644 index 00000000000000..ca439b055fb6e5 --- /dev/null +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.h @@ -0,0 +1,97 @@ +#include +#include +#include +#include +#include + +#include +#include + + + +class AS_5600 { + +public: + + AS_5600(void); + + void init(); + + uint32_t busMaskExt; + uint32_t busMaskInt; + + + void checkConnect(void); + + unsigned short setMaxAngle(unsigned short newMaxAngle = -1); + unsigned short getMaxAngle(); + + unsigned short setStartPosition(unsigned short startAngle = -1); + unsigned short getStartPosition(); + + unsigned short setEndPosition(unsigned short endAngle = -1); + unsigned short getEndPosition(); + + unsigned short getRawAngle(void); + unsigned short getScaledAngle(); + + int detectMagnet(); + int getMagnetStrength(); + int getAgc(); + unsigned short getMagnitude(); + + int getBurnCount(); + int burnAngle(); + int burnMaxAngleAndConfig(); + void setOutPut(unsigned char mode); + + HAL_Semaphore sem; + +private: + + AP_HAL::OwnPtr dev; + + unsigned char bus; + unsigned char address; + uint32_t bus_clock; + bool use_smbus; + uint32_t timeout_ms; + + int as5600_address; + + unsigned short rawStartAngle; + unsigned short zPosition; + unsigned short rawEndAngle; + unsigned short mPosition; + unsigned short maxAngle; + + /* Registers */ + + int _zmco; + int _zpos_hi; /*zpos[11:8] high nibble START POSITION */ + int _zpos_lo; /*zpos[7:0] */ + int _mpos_hi; /*mpos[11:8] high nibble STOP POSITION */ + int _mpos_lo; /*mpos[7:0] */ + int _mang_hi; /*mang[11:8] high nibble MAXIMUM ANGLE */ + int _mang_lo; /*mang[7:0] */ + int _conf_hi; + int _conf_lo; + int _raw_ang_hi; + int _raw_ang_lo; + int _ang_hi; + int _ang_lo; + int _stat; + int _agc; + int _mag_hi; + int _mag_lo; + int _burn; + + int readOneByte(uint8_t in_adr); + int readTwoBytes(uint8_t in_adr_hi, uint8_t in_adr_lo); + int writeOneByte(uint8_t adr_in, uint8_t dat_in); + + unsigned char highByte(unsigned short short_in); + unsigned char lowByte(unsigned short short_in); + + +}; From a5f0f813c28441dd204b1acdde38c599a60a46c7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 21 Jan 2021 16:15:18 +1100 Subject: [PATCH 02/10] SITL: add simulated AS5600 sensor --- libraries/SITL/SIM_AS5600.cpp | 9 ++++++ libraries/SITL/SIM_AS5600.h | 55 +++++++++++++++++++++++++++++++++++ libraries/SITL/SIM_I2C.cpp | 3 ++ 3 files changed, 67 insertions(+) create mode 100644 libraries/SITL/SIM_AS5600.cpp create mode 100644 libraries/SITL/SIM_AS5600.h diff --git a/libraries/SITL/SIM_AS5600.cpp b/libraries/SITL/SIM_AS5600.cpp new file mode 100644 index 00000000000000..dd6467766d16f6 --- /dev/null +++ b/libraries/SITL/SIM_AS5600.cpp @@ -0,0 +1,9 @@ +#include "SIM_AS5600.h" + +#include + +void SITL::AS5600::update(const class Aircraft &aircraft) +{ + + // gcs().send_text(MAV_SEVERITY_INFO, "SIM_AS5600: PWM0=%u PWM1=%u PWM2=%u ENABLE=%u", last_print_pwm0, last_print_pwm1, last_print_pwm2, last_print_enable); +} diff --git a/libraries/SITL/SIM_AS5600.h b/libraries/SITL/SIM_AS5600.h new file mode 100644 index 00000000000000..b53597bf30e93d --- /dev/null +++ b/libraries/SITL/SIM_AS5600.h @@ -0,0 +1,55 @@ +#include "SIM_I2CDevice.h" + +namespace SITL { + +// datasheet: https://ams.com/documents/20143/36005/AS5600_DS000365_5-00.pdf + +class AS5600DevReg : public I2CRegEnum { +public: + // low + static const uint8_t ZMCO = 0x00; + static const uint8_t ZPOS_HIGH = 0x01; + static const uint8_t ZPOS_LOW = 0x02; + static const uint8_t MPOS_HIGH = 0x03; + static const uint8_t MPOS_LOW = 0x04; + static const uint8_t MANG_HIGH = 0x05; + static const uint8_t MANG_LOW = 0x06; + static const uint8_t CONF_HIGH = 0x07; + static const uint8_t CONF_LOW = 0x08; + + // output registers + static const uint8_t RAW_ANGLE_HIGH = 0x0C; + static const uint8_t RAW_ANGLE_LOW = 0x0D; + static const uint8_t ANGLE_HIGH = 0x0E; + static const uint8_t ANGLE_LOW = 0x0F; + + // status registers + static const uint8_t STATUS = 0x0B; + static const uint8_t AGC = 0x1A; + static const uint8_t MAGNITUDE_HIGH = 0x1B; + static const uint8_t MAGNITUDE_LOW = 0x1C; + + // Burn commands + static const uint8_t BURN = 0xFF; +}; + +class AS5600 : public I2CDevice, protected I2CRegisters_8Bit +{ +public: + void init() override { + add_register("ZMCO", AS5600DevReg::ZMCO, O_RDONLY); + set_register(AS5600DevReg::ZMCO, (uint8_t)1U); + } + + void update(const class Aircraft &aircraft) override; + + int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override { + gcs().send_text(MAV_SEVERITY_INFO, "SIM_AS5600: rdwr called"); + return I2CRegisters_8Bit::rdwr(data); + } + +private: + +}; + +} // namespace SITL diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index 6a957be258a830..4e8b478c459f05 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/libraries/SITL/SIM_I2C.cpp @@ -28,6 +28,7 @@ #include "SIM_Airspeed_DLVR.h" #include "SIM_Temperature_TSYS01.h" #include "SIM_ICM40609.h" +#include "SIM_AS5600.h" #include @@ -54,6 +55,7 @@ static Rotoye rotoye; static Airspeed_DLVR airspeed_dlvr; static TSYS01 tsys01; static ICM40609 icm40609; +static AS5600 as5600; // AoA sensor struct i2c_device_at_address { uint8_t bus; @@ -71,6 +73,7 @@ struct i2c_device_at_address { { 1, 0x77, tsys01 }, { 1, 0x0B, rotoye }, { 2, 0x28, airspeed_dlvr }, + { 1, 0x36, as5600 }, }; void I2C::init() From da95f43b6c817eb02512cb8ba4b78ab922f20500 Mon Sep 17 00:00:00 2001 From: colejmero <57465269+colejmero@users.noreply.github.com> Date: Wed, 24 Feb 2021 21:09:48 -0500 Subject: [PATCH 03/10] Minor changes --- ArduPlane/mav_0_1.parm | 1386 ++++++++++++++++++++++++ libraries/AP_AS5600_AOA/AS5600_AOA.cpp | 23 +- libraries/AP_AS5600_AOA/AS5600_AOA.h | 2 + 3 files changed, 1399 insertions(+), 12 deletions(-) create mode 100644 ArduPlane/mav_0_1.parm diff --git a/ArduPlane/mav_0_1.parm b/ArduPlane/mav_0_1.parm new file mode 100644 index 00000000000000..6d1c6817799f4b --- /dev/null +++ b/ArduPlane/mav_0_1.parm @@ -0,0 +1,1386 @@ +ACRO_LOCKING 1 +ACRO_PITCH_RATE 180 +ACRO_ROLL_RATE 180 +ADSB_TYPE 0 +AFS_ENABLE 0 +AHRS_COMP_BETA 0.100000 +AHRS_CUSTOM_PIT 0.000000 +AHRS_CUSTOM_ROLL 0.000000 +AHRS_CUSTOM_YAW 0.000000 +AHRS_EKF_TYPE 3 +AHRS_GPS_GAIN 1.000000 +AHRS_GPS_MINSATS 6 +AHRS_GPS_USE 1 +AHRS_ORIENTATION 0 +AHRS_RP_P 0.200000 +AHRS_TRIM_X 0.000000 +AHRS_TRIM_Y 0.000000 +AHRS_TRIM_Z 0.000000 +AHRS_WIND_MAX 0 +AHRS_YAW_P 0.200000 +ALT_CTRL_ALG 2 +ALT_HOLD_FBWCM 0 +ALT_HOLD_RTL 10000 +ALT_OFFSET 0 +ARMING_ACCTHRESH 0.750000 +ARMING_CHECK 1 +ARMING_MIS_ITEMS 0 +ARMING_REQUIRE 1 +ARMING_RUDDER 1 +ARSPD2_TYPE 0 +ARSPD_AUTOCAL 0 +ARSPD_BUS 1 +ARSPD_FBW_MAX 30 +ARSPD_FBW_MIN 10 +ARSPD_OFFSET 0.000000 +ARSPD_OPTIONS 3 +ARSPD_PIN 1 +ARSPD_PRIMARY 0 +ARSPD_PSI_RANGE 1.000000 +ARSPD_RATIO 1.993600 +ARSPD_SKIP_CAL 0 +ARSPD_TUBE_ORDER 2 +ARSPD_TYPE 2 +ARSPD_USE 1 +ARSPD_WIND_MAX 0.000000 +ARSPD_WIND_WARN 0.000000 +AUTOTUNE_LEVEL 6 +AUTO_FBW_STEER 0 +AVD_ENABLE 0 +BARO1_DEVID 65540 +BARO1_GND_PRESS 94502.812500 +BARO1_WCF_ENABLE 0 +BARO2_DEVID 65796 +BARO2_GND_PRESS 94503.960938 +BARO2_WCF_ENABLE 0 +BARO3_DEVID 0 +BARO3_GND_PRESS 0.000000 +BARO3_WCF_ENABLE 0 +BARO_ALT_OFFSET 0.000000 +BARO_EXT_BUS -1 +BARO_FLTR_RNG 0 +BARO_GND_TEMP 0.000000 +BARO_PRIMARY 0 +BARO_PROBE_EXT 0 +BATT2_MONITOR 0 +BATT3_MONITOR 0 +BATT4_MONITOR 0 +BATT5_MONITOR 0 +BATT6_MONITOR 0 +BATT7_MONITOR 0 +BATT8_MONITOR 0 +BATT9_MONITOR 0 +BATT_AMP_OFFSET 0.000000 +BATT_AMP_PERVLT 17.000000 +BATT_ARM_MAH 0 +BATT_ARM_VOLT 0.000000 +BATT_BUS 0 +BATT_CAPACITY 3300 +BATT_CRT_MAH 0.000000 +BATT_CRT_VOLT 0.000000 +BATT_CURR_PIN 12 +BATT_FS_CRT_ACT 0 +BATT_FS_LOW_ACT 0 +BATT_FS_VOLTSRC 0 +BATT_LOW_MAH 0.000000 +BATT_LOW_TIMER 10 +BATT_LOW_VOLT 0.000000 +BATT_MONITOR 4 +BATT_OPTIONS 0 +BATT_SERIAL_NUM -1 +BATT_VOLT_MULT 10.100000 +BATT_VOLT_PIN 13 +BATT_WATT_MAX 0 +BRD_ALT_CONFIG 0 +BRD_BOOT_DELAY 0 +BRD_OPTIONS 0 +BRD_PWM_COUNT 8 +BRD_RTC_TYPES 1 +BRD_RTC_TZ_MIN 0 +BRD_SAFETYOPTION 3 +BRD_SERIAL_NUM 0 +BRD_VBUS_MIN 4.300000 +BRD_VSERVO_MIN 0.000000 +BTN_ENABLE 0 +CAM_AUTO_ONLY 0 +CAM_DURATION 10 +CAM_FEEDBACK_PIN -1 +CAM_FEEDBACK_POL 1 +CAM_MAX_ROLL 0 +CAM_MIN_INTERVAL 0 +CAM_RC_TYPE 0 +CAM_RELAY_ON 1 +CAM_SERVO_OFF 1100 +CAM_SERVO_ON 1300 +CAM_TRIGG_DIST 0.000000 +CAM_TRIGG_TYPE 0 +CAM_TYPE 0 +CAN_D1_PROTOCOL 1 +CAN_D2_PROTOCOL 1 +CAN_LOGLEVEL 0 +CAN_P1_DRIVER 0 +CAN_P2_DRIVER 0 +CAN_SLCAN_CPORT 0 +CAN_SLCAN_SDELAY 1 +CAN_SLCAN_SERNUM -1 +CAN_SLCAN_TIMOUT 0 +CHUTE_CHAN 0 +CHUTE_ENABLED 0 +COMPASS_AUTODEC 1 +COMPASS_AUTO_ROT 2 +COMPASS_CAL_FIT 16.000000 +COMPASS_CUS_PIT 0.000000 +COMPASS_CUS_ROLL 0.000000 +COMPASS_CUS_YAW 0.000000 +COMPASS_DEC 0.000000 +COMPASS_DEV_ID 97539 +COMPASS_DEV_ID2 131874 +COMPASS_DEV_ID3 263178 +COMPASS_DEV_ID4 97283 +COMPASS_DEV_ID5 97795 +COMPASS_DEV_ID6 98051 +COMPASS_DEV_ID7 983044 +COMPASS_DEV_ID8 0 +COMPASS_DIA2_X 1.000000 +COMPASS_DIA2_Y 1.000000 +COMPASS_DIA2_Z 1.000000 +COMPASS_DIA3_X 1.000000 +COMPASS_DIA3_Y 1.000000 +COMPASS_DIA3_Z 1.000000 +COMPASS_DIA_X 1.000000 +COMPASS_DIA_Y 1.000000 +COMPASS_DIA_Z 1.000000 +COMPASS_ENABLE 1 +COMPASS_EXTERN2 0 +COMPASS_EXTERN3 0 +COMPASS_EXTERNAL 1 +COMPASS_FLTR_RNG 0 +COMPASS_LEARN 0 +COMPASS_MOT2_X 0.000000 +COMPASS_MOT2_Y 0.000000 +COMPASS_MOT2_Z 0.000000 +COMPASS_MOT3_X 0.000000 +COMPASS_MOT3_Y 0.000000 +COMPASS_MOT3_Z 0.000000 +COMPASS_MOTCT 0 +COMPASS_MOT_X 0.000000 +COMPASS_MOT_Y 0.000000 +COMPASS_MOT_Z 0.000000 +COMPASS_ODI2_X 0.000000 +COMPASS_ODI2_Y 0.000000 +COMPASS_ODI2_Z 0.000000 +COMPASS_ODI3_X 0.000000 +COMPASS_ODI3_Y 0.000000 +COMPASS_ODI3_Z 0.000000 +COMPASS_ODI_X 0.000000 +COMPASS_ODI_Y 0.000000 +COMPASS_ODI_Z 0.000000 +COMPASS_OFFS_MAX 1800 +COMPASS_OFS2_X 5.000000 +COMPASS_OFS2_Y 13.000000 +COMPASS_OFS2_Z -18.000000 +COMPASS_OFS3_X 5.000000 +COMPASS_OFS3_Y 13.000000 +COMPASS_OFS3_Z -18.000000 +COMPASS_OFS_X 5.000000 +COMPASS_OFS_Y 13.000000 +COMPASS_OFS_Z -18.000000 +COMPASS_OPTIONS 0 +COMPASS_ORIENT 0 +COMPASS_ORIENT2 0 +COMPASS_ORIENT3 0 +COMPASS_PMOT_EN 0 +COMPASS_PRIO1_ID 97539 +COMPASS_PRIO2_ID 131874 +COMPASS_PRIO3_ID 263178 +COMPASS_SCALE 1.000000 +COMPASS_SCALE2 1.000000 +COMPASS_SCALE3 1.000000 +COMPASS_TYPEMASK 0 +COMPASS_USE 1 +COMPASS_USE2 1 +COMPASS_USE3 1 +CRASH_ACC_THRESH 0 +CRASH_DETECT 0 +DSPOILER_AILMTCH 100 +DSPOILER_CROW_W1 0 +DSPOILER_CROW_W2 0 +DSPOILER_OPTS 3 +DSPOILR_RUD_RATE 100 +EAHRS_TYPE 0 +EFI_TYPE 0 +EK2_ABIAS_P_NSE 0.005000 +EK2_ACC_P_NSE 0.600000 +EK2_ALT_M_NSE 3.000000 +EK2_ALT_SOURCE 0 +EK2_BCN_DELAY 50 +EK2_BCN_I_GTE 500 +EK2_BCN_M_NSE 1.000000 +EK2_CHECK_SCALE 150 +EK2_EAS_I_GATE 400 +EK2_EAS_M_NSE 1.400000 +EK2_ENABLE 1 +EK2_FLOW_DELAY 10 +EK2_FLOW_I_GATE 500 +EK2_FLOW_M_NSE 0.150000 +EK2_FLOW_USE 2 +EK2_GBIAS_P_NSE 0.000100 +EK2_GLITCH_RAD 25 +EK2_GPS_CHECK 31 +EK2_GPS_TYPE 0 +EK2_GSCL_P_NSE 0.000500 +EK2_GSF_DELAY 1000 +EK2_GSF_RST_MAX 2 +EK2_GSF_RUN_MASK 3 +EK2_GSF_USE_MASK 3 +EK2_GYRO_P_NSE 0.030000 +EK2_HGT_DELAY 60 +EK2_HGT_I_GATE 500 +EK2_HRT_FILT 2.000000 +EK2_IMU_MASK 3 +EK2_MAGB_P_NSE 0.000100 +EK2_MAGE_P_NSE 0.001000 +EK2_MAG_CAL 0 +EK2_MAG_EF_LIM 50 +EK2_MAG_I_GATE 300 +EK2_MAG_MASK 0 +EK2_MAG_M_NSE 0.050000 +EK2_MAX_FLOW 2.500000 +EK2_NOAID_M_NSE 10.000000 +EK2_OGN_HGT_MASK 0 +EK2_POSNE_M_NSE 1.000000 +EK2_POS_I_GATE 500 +EK2_RNG_I_GATE 500 +EK2_RNG_M_NSE 0.500000 +EK2_RNG_USE_HGT -1 +EK2_RNG_USE_SPD 2.000000 +EK2_TAU_OUTPUT 25 +EK2_TERR_GRAD 0.100000 +EK2_VELD_M_NSE 0.700000 +EK2_VELNE_M_NSE 0.500000 +EK2_VEL_I_GATE 500 +EK2_WIND_PSCALE 0.500000 +EK2_WIND_P_NSE 0.100000 +EK2_YAW_I_GATE 300 +EK2_YAW_M_NSE 0.500000 +EK3_ABIAS_P_NSE 0.003000 +EK3_ACC_BIAS_LIM 1.000000 +EK3_ACC_P_NSE 0.350000 +EK3_AFFINITY 0 +EK3_ALT_M_NSE 3.000000 +EK3_BCN_DELAY 50 +EK3_BCN_I_GTE 500 +EK3_BCN_M_NSE 1.000000 +EK3_CHECK_SCALE 150 +EK3_DRAG_BCOEF_X 0.000000 +EK3_DRAG_BCOEF_Y 0.000000 +EK3_DRAG_MCOEF 0.000000 +EK3_DRAG_M_NSE 0.500000 +EK3_EAS_I_GATE 400 +EK3_EAS_M_NSE 1.400000 +EK3_ENABLE 1 +EK3_ERR_THRESH 0.200000 +EK3_FLOW_DELAY 10 +EK3_FLOW_I_GATE 500 +EK3_FLOW_M_NSE 0.150000 +EK3_FLOW_USE 2 +EK3_GBIAS_P_NSE 0.001000 +EK3_GLITCH_RAD 25 +EK3_GPS_CHECK 31 +EK3_GSF_DELAY 1000 +EK3_GSF_RST_MAX 2 +EK3_GSF_RUN_MASK 3 +EK3_GSF_USE_MASK 3 +EK3_GYRO_P_NSE 0.015000 +EK3_HGT_DELAY 60 +EK3_HGT_I_GATE 500 +EK3_HRT_FILT 2.000000 +EK3_IMU_MASK 3 +EK3_MAGB_P_NSE 0.000100 +EK3_MAGE_P_NSE 0.001000 +EK3_MAG_CAL 0 +EK3_MAG_EF_LIM 50 +EK3_MAG_I_GATE 300 +EK3_MAG_MASK 0 +EK3_MAG_M_NSE 0.050000 +EK3_MAX_FLOW 2.500000 +EK3_NOAID_M_NSE 10.000000 +EK3_OGN_HGT_MASK 0 +EK3_POSNE_M_NSE 0.500000 +EK3_POS_I_GATE 500 +EK3_RNG_I_GATE 500 +EK3_RNG_M_NSE 0.500000 +EK3_RNG_USE_HGT -1 +EK3_RNG_USE_SPD 2.000000 +EK3_SRC1_POSXY 3 +EK3_SRC1_POSZ 1 +EK3_SRC1_VELXY 3 +EK3_SRC1_VELZ 3 +EK3_SRC1_YAW 1 +EK3_SRC2_POSXY 0 +EK3_SRC2_POSZ 1 +EK3_SRC2_VELXY 0 +EK3_SRC2_VELZ 0 +EK3_SRC2_YAW 0 +EK3_SRC3_POSXY 0 +EK3_SRC3_POSZ 1 +EK3_SRC3_VELXY 0 +EK3_SRC3_VELZ 0 +EK3_SRC3_YAW 0 +EK3_SRC_OPTIONS 1 +EK3_TAU_OUTPUT 25 +EK3_TERR_GRAD 0.100000 +EK3_VELD_M_NSE 0.700000 +EK3_VELNE_M_NSE 0.500000 +EK3_VEL_I_GATE 500 +EK3_VIS_VERR_MAX 0.900000 +EK3_VIS_VERR_MIN 0.100000 +EK3_WENC_VERR 0.100000 +EK3_WIND_PSCALE 0.500000 +EK3_WIND_P_NSE 0.100000 +EK3_YAW_I_GATE 300 +EK3_YAW_M_NSE 0.500000 +FBWA_TDRAG_CHAN 0 +FBWB_CLIMB_RATE 2 +FBWB_ELEV_REV 0 +FENCE_ACTION 0 +FENCE_AUTOENABLE 0 +FENCE_CHANNEL 0 +FENCE_MAXALT 0 +FENCE_MINALT 0 +FENCE_RETALT 0 +FENCE_RET_RALLY 0 +FENCE_TOTAL 0 +FFT_ENABLE 0 +FLAP_1_PERCNT 0 +FLAP_1_SPEED 0 +FLAP_2_PERCNT 0 +FLAP_2_SPEED 0 +FLAP_SLEWRATE 75 +FLIGHT_OPTIONS 0 +FLOW_ADDR 0 +FLOW_FXSCALER 0 +FLOW_FYSCALER 0 +FLOW_ORIENT_YAW 0 +FLOW_POS_X 0.000000 +FLOW_POS_Y 0.000000 +FLOW_POS_Z 0.000000 +FLOW_TYPE 0 +FLTMODE1 10 +FLTMODE2 11 +FLTMODE3 12 +FLTMODE4 5 +FLTMODE5 2 +FLTMODE6 0 +FLTMODE_CH 8 +FORMAT_VERSION 13 +FRSKY_DNLINK1_ID 20 +FRSKY_DNLINK2_ID 7 +FRSKY_UPLINK_ID 13 +FS_EKF_THRESH 0.800000 +FS_GCS_ENABL 0 +FS_LONG_ACTN 0 +FS_LONG_TIMEOUT 5.000000 +FS_SHORT_ACTN 0 +FS_SHORT_TIMEOUT 1.500000 +FWD_BAT_IDX 0 +FWD_BAT_VOLT_MAX 0.000000 +FWD_BAT_VOLT_MIN 0.000000 +GCS_PID_MASK 0 +GEN_TYPE 0 +GLIDE_SLOPE_MIN 15 +GLIDE_SLOPE_THR 5.000000 +GPS_AUTO_CONFIG 1 +GPS_AUTO_SWITCH 1 +GPS_BLEND_MASK 5 +GPS_BLEND_TC 10.000000 +GPS_COM_PORT 1 +GPS_COM_PORT2 1 +GPS_DELAY_MS 0 +GPS_DELAY_MS2 0 +GPS_DRV_OPTIONS 0 +GPS_GNSS_MODE 0 +GPS_GNSS_MODE2 0 +GPS_INJECT_TO 127 +GPS_MB1_TYPE 0 +GPS_MB2_TYPE 0 +GPS_MIN_DGPS 100 +GPS_MIN_ELEV -100 +GPS_NAVFILTER 8 +GPS_POS1_X 0.000000 +GPS_POS1_Y 0.000000 +GPS_POS1_Z 0.000000 +GPS_POS2_X 0.000000 +GPS_POS2_Y 0.000000 +GPS_POS2_Z 0.000000 +GPS_PRIMARY 0 +GPS_RATE_MS 200 +GPS_RATE_MS2 200 +GPS_RAW_DATA 0 +GPS_SAVE_CFG 2 +GPS_SBAS_MODE 2 +GPS_SBP_LOGMASK -256 +GPS_TYPE 1 +GPS_TYPE2 0 +GRIP_ENABLE 0 +GROUND_STEER_ALT 0.000000 +GROUND_STEER_DPS 90 +GUIDED_D 0.000000 +GUIDED_FF 0.000000 +GUIDED_FLTD 5.000000 +GUIDED_FLTE 5.000000 +GUIDED_FLTT 5.000000 +GUIDED_I 0.000000 +GUIDED_IMAX 10.000000 +GUIDED_P 5000.000000 +GUIDED_SMAX 0.000000 +HIL_ERR_LIMIT 5.000000 +HIL_MODE 0 +HIL_SERVOS 0 +HOME_RESET_ALT 0 +ICE_ENABLE 0 +INITIAL_MODE 0 +INS_ACC1_CALTEMP -300.000000 +INS_ACC2OFFS_X 0.001000 +INS_ACC2OFFS_Y 0.001000 +INS_ACC2OFFS_Z 0.001000 +INS_ACC2SCAL_X 1.001000 +INS_ACC2SCAL_Y 1.001000 +INS_ACC2SCAL_Z 1.001000 +INS_ACC2_CALTEMP -300.000000 +INS_ACC2_ID 2753036 +INS_ACC3OFFS_X 0.000000 +INS_ACC3OFFS_Y 0.000000 +INS_ACC3OFFS_Z 0.000000 +INS_ACC3SCAL_X 0.000000 +INS_ACC3SCAL_Y 0.000000 +INS_ACC3SCAL_Z 0.000000 +INS_ACC3_CALTEMP -300.000000 +INS_ACC3_ID 0 +INS_ACCEL_FILTER 20 +INS_ACCOFFS_X 0.001000 +INS_ACCOFFS_Y 0.001000 +INS_ACCOFFS_Z 0.001000 +INS_ACCSCAL_X 1.001000 +INS_ACCSCAL_Y 1.001000 +INS_ACCSCAL_Z 1.001000 +INS_ACC_BODYFIX 2 +INS_ACC_ID 2753028 +INS_ENABLE_MASK 127 +INS_FAST_SAMPLE 1 +INS_GYR1_CALTEMP -300.000000 +INS_GYR2OFFS_X 0.000000 +INS_GYR2OFFS_Y 0.000000 +INS_GYR2OFFS_Z 0.000000 +INS_GYR2_CALTEMP -300.000000 +INS_GYR2_ID 2752780 +INS_GYR3OFFS_X 0.000000 +INS_GYR3OFFS_Y 0.000000 +INS_GYR3OFFS_Z 0.000000 +INS_GYR3_CALTEMP -300.000000 +INS_GYR3_ID 0 +INS_GYROFFS_X 0.000000 +INS_GYROFFS_Y 0.000000 +INS_GYROFFS_Z 0.000000 +INS_GYRO_FILTER 20 +INS_GYRO_RATE 0 +INS_GYR_CAL 0 +INS_GYR_ID 2752772 +INS_HNTCH_ENABLE 0 +INS_LOG_BAT_CNT 1024 +INS_LOG_BAT_LGCT 32 +INS_LOG_BAT_LGIN 20 +INS_LOG_BAT_MASK 0 +INS_LOG_BAT_OPT 0 +INS_NOTCH_ENABLE 0 +INS_POS1_X 0.000000 +INS_POS1_Y 0.000000 +INS_POS1_Z 0.000000 +INS_POS2_X 0.000000 +INS_POS2_Y 0.000000 +INS_POS2_Z 0.000000 +INS_POS3_X 0.000000 +INS_POS3_Y 0.000000 +INS_POS3_Z 0.000000 +INS_STILL_THRESH 0.100000 +INS_TCAL1_ENABLE 0 +INS_TCAL2_ENABLE 0 +INS_TCAL3_ENABLE 0 +INS_TCAL_OPTIONS 0 +INS_TRIM_OPTION 1 +INS_USE 1 +INS_USE2 1 +INS_USE3 1 +KFF_RDDRMIX 0.500000 +KFF_THR2PTCH 0.000000 +LAND_ABORT_DEG 0.000000 +LAND_ABORT_THR 0 +LAND_DISARMDELAY 20 +LAND_DS_ABORTALT 0.000000 +LAND_DS_AIL_SCL 1.000000 +LAND_DS_APP_EXT 50.000000 +LAND_DS_ARSP_MAX 15.000000 +LAND_DS_ARSP_MIN 10.000000 +LAND_DS_D 0.000000 +LAND_DS_ELEV_PWM 1500 +LAND_DS_I 0.000000 +LAND_DS_IMAX 0 +LAND_DS_L1 30.000000 +LAND_DS_L1_I 0.000000 +LAND_DS_L1_TCON 0.400000 +LAND_DS_P 0.000000 +LAND_DS_SLEW_SPD 0.500000 +LAND_DS_SLOPE_A 1.000000 +LAND_DS_SLOPE_B 1.000000 +LAND_DS_V_DWN 2.000000 +LAND_DS_V_FWD 1.000000 +LAND_DS_YAW_LIM 10.000000 +LAND_FLAP_PERCNT 0 +LAND_FLARE_ALT 3.000000 +LAND_FLARE_SEC 5.000000 +LAND_OPTIONS 0 +LAND_PF_ALT 10.000000 +LAND_PF_ARSPD 0.000000 +LAND_PF_SEC 6.000000 +LAND_PITCH_CD 100 +LAND_SLOPE_RCALC 2.000000 +LAND_THEN_NEUTRL 0 +LAND_THR_SLEW 0 +LAND_TYPE 0 +LEVEL_ROLL_LIMIT 5 +LGR_DEPLOY_ALT 0 +LGR_DEPLOY_PIN -1 +LGR_DEPLOY_POL 0 +LGR_OPTIONS 3 +LGR_RETRACT_ALT 0 +LGR_STARTUP 0 +LGR_WOW_PIN 8 +LGR_WOW_POL 1 +LIM_PITCH_MAX 2500 +LIM_PITCH_MIN -2000 +LIM_ROLL_CD 6500 +LOG_BACKEND_TYPE 1 +LOG_BITMASK 65535 +LOG_DISARMED 0 +LOG_FILE_BUFSIZE 200 +LOG_FILE_DSRMROT 0 +LOG_FILE_MB_FREE 500 +LOG_FILE_TIMEOUT 5 +LOG_MAV_BUFSIZE 8 +LOG_REPLAY 0 +MANUAL_RCMASK 0 +MIN_GNDSPD_CM 0 +MIS_OPTIONS 0 +MIS_RESTART 0 +MIS_TOTAL 0 +MIXING_GAIN 0.500000 +MIXING_OFFSET 0 +MNT_ANGMAX_PAN 4500 +MNT_ANGMAX_ROL 4500 +MNT_ANGMAX_TIL 4500 +MNT_ANGMIN_PAN -4500 +MNT_ANGMIN_ROL -4500 +MNT_ANGMIN_TIL -4500 +MNT_DEFLT_MODE 3 +MNT_JSTICK_SPD 0 +MNT_LEAD_PTCH 0.000000 +MNT_LEAD_RLL 0.000000 +MNT_NEUTRAL_X 0.000000 +MNT_NEUTRAL_Y 0.000000 +MNT_NEUTRAL_Z 0.000000 +MNT_RC_IN_PAN 0 +MNT_RC_IN_ROLL 0 +MNT_RC_IN_TILT 0 +MNT_RETRACT_X 0.000000 +MNT_RETRACT_Y 0.000000 +MNT_RETRACT_Z 0.000000 +MNT_STAB_PAN 0 +MNT_STAB_ROLL 0 +MNT_STAB_TILT 0 +MNT_TYPE 0 +MSP_OPTIONS 0 +MSP_OSD_NCELLS 0 +NAVL1_DAMPING 0.750000 +NAVL1_LIM_BANK 0.000000 +NAVL1_PERIOD 15.000000 +NAVL1_XTRACK_I 0.020000 +NAV_CONTROLLER 1 +NTF_BUZZ_ENABLE 1 +NTF_BUZZ_ON_LVL 1 +NTF_BUZZ_PIN 0 +NTF_BUZZ_VOLUME 100 +NTF_DISPLAY_TYPE 0 +NTF_LED_BRIGHT 3 +NTF_LED_LEN 1 +NTF_LED_OVERRIDE 0 +NTF_LED_TYPES 199 +NTF_OREO_THEME 0 +OSD_TYPE 0 +PTCH2SRV_D 0.200000 +PTCH2SRV_FF 0.000000 +PTCH2SRV_I 0.300000 +PTCH2SRV_IMAX 3000 +PTCH2SRV_P 2.500000 +PTCH2SRV_RLL 1.000000 +PTCH2SRV_RMAX_DN 0 +PTCH2SRV_RMAX_UP 0 +PTCH2SRV_SRMAX 150.000000 +PTCH2SRV_SRTAU 1.000000 +PTCH2SRV_TCONST 0.500000 +Q_ENABLE 0 +RALLY_INCL_HOME 0 +RALLY_LIMIT_KM 5.000000 +RALLY_TOTAL 0 +RC10_DZ 0 +RC10_MAX 1900 +RC10_MIN 1100 +RC10_OPTION 0 +RC10_REVERSED 0 +RC10_TRIM 1500 +RC11_DZ 0 +RC11_MAX 1900 +RC11_MIN 1100 +RC11_OPTION 0 +RC11_REVERSED 0 +RC11_TRIM 1500 +RC12_DZ 0 +RC12_MAX 1900 +RC12_MIN 1100 +RC12_OPTION 0 +RC12_REVERSED 0 +RC12_TRIM 1500 +RC13_DZ 0 +RC13_MAX 1900 +RC13_MIN 1100 +RC13_OPTION 0 +RC13_REVERSED 0 +RC13_TRIM 1500 +RC14_DZ 0 +RC14_MAX 1900 +RC14_MIN 1100 +RC14_OPTION 0 +RC14_REVERSED 0 +RC14_TRIM 1500 +RC15_DZ 0 +RC15_MAX 1900 +RC15_MIN 1100 +RC15_OPTION 0 +RC15_REVERSED 0 +RC15_TRIM 1500 +RC16_DZ 0 +RC16_MAX 1900 +RC16_MIN 1100 +RC16_OPTION 0 +RC16_REVERSED 0 +RC16_TRIM 1500 +RC1_DZ 30 +RC1_MAX 2000 +RC1_MIN 1000 +RC1_OPTION 0 +RC1_REVERSED 0 +RC1_TRIM 1500 +RC2_DZ 30 +RC2_MAX 2000 +RC2_MIN 1000 +RC2_OPTION 0 +RC2_REVERSED 0 +RC2_TRIM 1500 +RC3_DZ 30 +RC3_MAX 2000 +RC3_MIN 1000 +RC3_OPTION 0 +RC3_REVERSED 0 +RC3_TRIM 1000 +RC4_DZ 30 +RC4_MAX 2000 +RC4_MIN 1000 +RC4_OPTION 0 +RC4_REVERSED 0 +RC4_TRIM 1500 +RC5_DZ 0 +RC5_MAX 2000 +RC5_MIN 1000 +RC5_OPTION 0 +RC5_REVERSED 0 +RC5_TRIM 1500 +RC6_DZ 0 +RC6_MAX 2000 +RC6_MIN 1000 +RC6_OPTION 0 +RC6_REVERSED 0 +RC6_TRIM 1500 +RC7_DZ 0 +RC7_MAX 2000 +RC7_MIN 1000 +RC7_OPTION 0 +RC7_REVERSED 0 +RC7_TRIM 1500 +RC8_DZ 0 +RC8_MAX 2000 +RC8_MIN 1000 +RC8_OPTION 0 +RC8_REVERSED 0 +RC8_TRIM 1500 +RC9_DZ 0 +RC9_MAX 1900 +RC9_MIN 1100 +RC9_OPTION 0 +RC9_REVERSED 0 +RC9_TRIM 1500 +RCMAP_PITCH 2 +RCMAP_ROLL 1 +RCMAP_THROTTLE 3 +RCMAP_YAW 4 +RC_OPTIONS 32 +RC_OVERRIDE_TIME 3.000000 +RC_PROTOCOLS 1 +RELAY_DEFAULT 0 +RELAY_PIN 13 +RELAY_PIN2 -1 +RELAY_PIN3 -1 +RELAY_PIN4 -1 +RELAY_PIN5 -1 +RELAY_PIN6 -1 +RLL2SRV_D 0.200000 +RLL2SRV_FF 0.000000 +RLL2SRV_I 0.300000 +RLL2SRV_IMAX 3000 +RLL2SRV_P 2.500000 +RLL2SRV_RMAX 0 +RLL2SRV_SRMAX 150.000000 +RLL2SRV_SRTAU 1.000000 +RLL2SRV_TCONST 0.500000 +RNGFND1_TYPE 0 +RNGFND2_TYPE 0 +RNGFND3_TYPE 0 +RNGFND4_TYPE 0 +RNGFND5_TYPE 0 +RNGFND6_TYPE 0 +RNGFND7_TYPE 0 +RNGFND8_TYPE 0 +RNGFND9_TYPE 0 +RNGFNDA_TYPE 0 +RNGFND_LANDING 0 +RPM2_PIN -1 +RPM2_SCALING 1.000000 +RPM2_TYPE 0 +RPM_MAX 100000.000000 +RPM_MIN 10.000000 +RPM_MIN_QUAL 0.500000 +RPM_PIN 54 +RPM_SCALING 1.000000 +RPM_TYPE 0 +RSSI_TYPE 0 +RST_MISSION_CH 0 +RST_SWITCH_CH 0 +RTL_AUTOLAND 0 +RTL_CLIMB_MIN 0 +RTL_RADIUS 0 +RUDDER_ONLY 0 +RUDD_DT_GAIN 10 +SCALING_SPEED 15.000000 +SCHED_DEBUG 0 +SCHED_LOOP_RATE 50 +SCHED_OPTIONS 0 +SCR_ENABLE 0 +SERIAL0_BAUD 115 +SERIAL0_PROTOCOL 2 +SERIAL1_BAUD 57 +SERIAL1_OPTIONS 0 +SERIAL1_PROTOCOL 2 +SERIAL2_BAUD 57 +SERIAL2_OPTIONS 0 +SERIAL2_PROTOCOL 2 +SERIAL3_BAUD 38 +SERIAL3_OPTIONS 0 +SERIAL3_PROTOCOL 5 +SERIAL4_BAUD 38 +SERIAL4_OPTIONS 0 +SERIAL4_PROTOCOL 5 +SERIAL5_BAUD 57 +SERIAL5_OPTIONS 0 +SERIAL5_PROTOCOL -1 +SERIAL6_BAUD 57 +SERIAL6_OPTIONS 0 +SERIAL6_PROTOCOL -1 +SERIAL7_BAUD 57 +SERIAL7_OPTIONS 0 +SERIAL7_PROTOCOL -1 +SERIAL_PASS1 0 +SERIAL_PASS2 -1 +SERIAL_PASSTIMO 15 +SERVO10_FUNCTION 0 +SERVO10_MAX 1900 +SERVO10_MIN 1100 +SERVO10_REVERSED 0 +SERVO10_TRIM 1500 +SERVO11_FUNCTION 0 +SERVO11_MAX 1900 +SERVO11_MIN 1100 +SERVO11_REVERSED 0 +SERVO11_TRIM 1500 +SERVO12_FUNCTION 0 +SERVO12_MAX 1900 +SERVO12_MIN 1100 +SERVO12_REVERSED 0 +SERVO12_TRIM 1500 +SERVO13_FUNCTION 0 +SERVO13_MAX 1900 +SERVO13_MIN 1100 +SERVO13_REVERSED 0 +SERVO13_TRIM 1500 +SERVO14_FUNCTION 0 +SERVO14_MAX 1900 +SERVO14_MIN 1100 +SERVO14_REVERSED 0 +SERVO14_TRIM 1500 +SERVO15_FUNCTION 0 +SERVO15_MAX 1900 +SERVO15_MIN 1100 +SERVO15_REVERSED 0 +SERVO15_TRIM 1500 +SERVO16_FUNCTION 0 +SERVO16_MAX 1900 +SERVO16_MIN 1100 +SERVO16_REVERSED 0 +SERVO16_TRIM 1500 +SERVO1_FUNCTION 4 +SERVO1_MAX 1900 +SERVO1_MIN 1100 +SERVO1_REVERSED 0 +SERVO1_TRIM 1500 +SERVO2_FUNCTION 19 +SERVO2_MAX 1900 +SERVO2_MIN 1100 +SERVO2_REVERSED 0 +SERVO2_TRIM 1500 +SERVO3_FUNCTION 70 +SERVO3_MAX 2000 +SERVO3_MIN 1000 +SERVO3_REVERSED 0 +SERVO3_TRIM 1000 +SERVO4_FUNCTION 21 +SERVO4_MAX 1900 +SERVO4_MIN 1100 +SERVO4_REVERSED 0 +SERVO4_TRIM 1500 +SERVO5_FUNCTION 0 +SERVO5_MAX 1900 +SERVO5_MIN 1100 +SERVO5_REVERSED 0 +SERVO5_TRIM 1500 +SERVO6_FUNCTION 0 +SERVO6_MAX 1900 +SERVO6_MIN 1100 +SERVO6_REVERSED 0 +SERVO6_TRIM 1500 +SERVO7_FUNCTION 0 +SERVO7_MAX 1900 +SERVO7_MIN 1100 +SERVO7_REVERSED 0 +SERVO7_TRIM 1500 +SERVO8_FUNCTION 0 +SERVO8_MAX 1900 +SERVO8_MIN 1100 +SERVO8_REVERSED 0 +SERVO8_TRIM 1500 +SERVO9_FUNCTION 0 +SERVO9_MAX 1900 +SERVO9_MIN 1100 +SERVO9_REVERSED 0 +SERVO9_TRIM 1500 +SERVO_AUTO_TRIM 0 +SERVO_RATE 50 +SERVO_ROB_POSMAX 4095 +SERVO_ROB_POSMIN 0 +SERVO_SBUS_RATE 50 +SERVO_VOLZ_MASK 0 +SIM_ACC1_BIAS_X 0.000000 +SIM_ACC1_BIAS_Y 0.000000 +SIM_ACC1_BIAS_Z 0.000000 +SIM_ACC1_RND 0.000000 +SIM_ACC1_SCAL_X 0.000000 +SIM_ACC1_SCAL_Y 0.000000 +SIM_ACC1_SCAL_Z 0.000000 +SIM_ACC2_BIAS_X 0.000000 +SIM_ACC2_BIAS_Y 0.000000 +SIM_ACC2_BIAS_Z 0.000000 +SIM_ACC2_RND 0.000000 +SIM_ACC2_SCAL_X 0.000000 +SIM_ACC2_SCAL_Y 0.000000 +SIM_ACC2_SCAL_Z 0.000000 +SIM_ACC3_BIAS_X 0.000000 +SIM_ACC3_BIAS_Y 0.000000 +SIM_ACC3_BIAS_Z 0.000000 +SIM_ACC3_RND 0.000000 +SIM_ACC3_SCAL_X 0.000000 +SIM_ACC3_SCAL_Y 0.000000 +SIM_ACC3_SCAL_Z 0.000000 +SIM_ACCEL1_FAIL 0.000000 +SIM_ACCEL2_FAIL 0.000000 +SIM_ACCEL3_FAIL 0.000000 +SIM_ACC_FAIL_MSK 0 +SIM_ACC_TRIM_X 0.000000 +SIM_ACC_TRIM_Y 0.000000 +SIM_ACC_TRIM_Z 0.000000 +SIM_ADSB_ALT 1000.000000 +SIM_ADSB_COUNT -1 +SIM_ADSB_RADIUS 10000.000000 +SIM_ADSB_TX 0 +SIM_ARSPD2_FAIL 0.000000 +SIM_ARSPD2_FAILP 0.000000 +SIM_ARSPD2_OFS 2013.000000 +SIM_ARSPD2_PITOT 0.000000 +SIM_ARSPD2_RND 2.000000 +SIM_ARSPD_FAIL 0.000000 +SIM_ARSPD_FAILP 0.000000 +SIM_ARSPD_OFS 2013.000000 +SIM_ARSPD_PITOT 0.000000 +SIM_ARSPD_RND 2.000000 +SIM_ARSPD_SIGN 0 +SIM_BAR2_DELAY 0 +SIM_BAR2_DISABLE 0 +SIM_BAR2_DRIFT 0.000000 +SIM_BAR2_FREEZE 0 +SIM_BAR2_GLITCH 0.000000 +SIM_BAR2_RND 0.200000 +SIM_BAR2_WCF_BAK 0.000000 +SIM_BAR2_WCF_FWD 0.000000 +SIM_BAR2_WCF_LFT 0.000000 +SIM_BAR2_WCF_RGT 0.000000 +SIM_BAR3_DELAY 0 +SIM_BAR3_DISABLE 0 +SIM_BAR3_DRIFT 0.000000 +SIM_BAR3_FREEZE 0 +SIM_BAR3_GLITCH 0.000000 +SIM_BAR3_RND 0.200000 +SIM_BAR3_WCF_BAK 0.000000 +SIM_BAR3_WCF_FWD 0.000000 +SIM_BAR3_WCF_LFT 0.000000 +SIM_BAR3_WCF_RGT 0.000000 +SIM_BARO_COUNT 2 +SIM_BARO_DELAY 0 +SIM_BARO_DISABLE 0 +SIM_BARO_DRIFT 0.000000 +SIM_BARO_FREEZE 0 +SIM_BARO_GLITCH 0.000000 +SIM_BARO_RND 0.200000 +SIM_BARO_WCF_BAK 0.000000 +SIM_BARO_WCF_FWD 0.000000 +SIM_BARO_WCF_LFT 0.000000 +SIM_BARO_WCF_RGT 0.000000 +SIM_BATT_CAP_AH 0.000000 +SIM_BATT_VOLTAGE 12.600000 +SIM_BAUDLIMIT_EN 0 +SIM_BZ_ENABLE 0 +SIM_BZ_PIN 0 +SIM_DRIFT_SPEED 0.050000 +SIM_DRIFT_TIME 5.000000 +SIM_EFI_TYPE 0 +SIM_ENGINE_FAIL 0 +SIM_ENGINE_MUL 1.000000 +SIM_FLOAT_EXCEPT 1 +SIM_FLOW_DELAY 0 +SIM_FLOW_ENABLE 0 +SIM_FLOW_POS_X 0.000000 +SIM_FLOW_POS_Y 0.000000 +SIM_FLOW_POS_Z 0.000000 +SIM_FLOW_RATE 10 +SIM_FLOW_RND 0.050000 +SIM_GND_BEHAV -1 +SIM_GPS2_ACC 0.300000 +SIM_GPS2_ALT_OFS 0 +SIM_GPS2_BYTELOS 0.000000 +SIM_GPS2_DELAY 1 +SIM_GPS2_DISABLE 1 +SIM_GPS2_DRFTALT 0.000000 +SIM_GPS2_GLTCH_X 0.000000 +SIM_GPS2_GLTCH_Y 0.000000 +SIM_GPS2_GLTCH_Z 0.000000 +SIM_GPS2_HDG 0 +SIM_GPS2_HZ 5 +SIM_GPS2_LCKTIME 0 +SIM_GPS2_NOISE 0.000000 +SIM_GPS2_NUMSATS 10 +SIM_GPS2_POS_X 0.000000 +SIM_GPS2_POS_Y 0.000000 +SIM_GPS2_POS_Z 0.000000 +SIM_GPS2_TYPE 1 +SIM_GPS2_VERR_X 0.000000 +SIM_GPS2_VERR_Y 0.000000 +SIM_GPS2_VERR_Z 0.000000 +SIM_GPS_ACC 0.300000 +SIM_GPS_ALT_OFS 0 +SIM_GPS_BYTELOSS 0.000000 +SIM_GPS_DELAY 1 +SIM_GPS_DISABLE 0 +SIM_GPS_DRIFTALT 0.000000 +SIM_GPS_GLITCH_X 0.000000 +SIM_GPS_GLITCH_Y 0.000000 +SIM_GPS_GLITCH_Z 0.000000 +SIM_GPS_HDG 0 +SIM_GPS_HZ 5 +SIM_GPS_LOCKTIME 0 +SIM_GPS_NOISE 0.000000 +SIM_GPS_NUMSATS 10 +SIM_GPS_POS_X 0.000000 +SIM_GPS_POS_Y 0.000000 +SIM_GPS_POS_Z 0.000000 +SIM_GPS_TYPE 1 +SIM_GPS_VERR_X 0.000000 +SIM_GPS_VERR_Y 0.000000 +SIM_GPS_VERR_Z 0.000000 +SIM_GRPE_ENABLE 0 +SIM_GRPE_PIN -1 +SIM_GRPS_ENABLE 0 +SIM_GRPS_GRAB 2000 +SIM_GRPS_PIN -1 +SIM_GRPS_RELEASE 1000 +SIM_GRPS_REVERSE 0 +SIM_GYR1_RND 0.000000 +SIM_GYR1_SCALE_X 0.000000 +SIM_GYR1_SCALE_Y 0.000000 +SIM_GYR1_SCALE_Z 0.000000 +SIM_GYR2_RND 0.000000 +SIM_GYR2_SCALE_X 0.000000 +SIM_GYR2_SCALE_Y 0.000000 +SIM_GYR2_SCALE_Z 0.000000 +SIM_GYR3_RND 0.000000 +SIM_GYR3_SCALE_X 0.000000 +SIM_GYR3_SCALE_Y 0.000000 +SIM_GYR3_SCALE_Z 0.000000 +SIM_GYR_FAIL_MSK 0 +SIM_IE24_ENABLE 0 +SIM_IE24_ERROR 0 +SIM_IE24_STATE -1 +SIM_IMUT1_ENABLE 0 +SIM_IMUT2_ENABLE 0 +SIM_IMUT3_ENABLE 0 +SIM_IMUT_END 45.000000 +SIM_IMUT_FIXED 0.000000 +SIM_IMUT_START 25.000000 +SIM_IMUT_TCONST 300.000000 +SIM_IMU_COUNT 2 +SIM_IMU_POS_X 0.000000 +SIM_IMU_POS_Y 0.000000 +SIM_IMU_POS_Z 0.000000 +SIM_INS_THR_MIN 0.100000 +SIM_LED_LAYOUT 0 +SIM_LOOP_DELAY 0 +SIM_MAG1_DEVID 97539 +SIM_MAG1_FAIL 0 +SIM_MAG1_SCALING 1.000000 +SIM_MAG2_DEVID 131874 +SIM_MAG2_DIA_X 0.000000 +SIM_MAG2_DIA_Y 0.000000 +SIM_MAG2_DIA_Z 0.000000 +SIM_MAG2_FAIL 0 +SIM_MAG2_ODI_X 0.000000 +SIM_MAG2_ODI_Y 0.000000 +SIM_MAG2_ODI_Z 0.000000 +SIM_MAG2_OFS_X 5.000000 +SIM_MAG2_OFS_Y 13.000000 +SIM_MAG2_OFS_Z -18.000000 +SIM_MAG2_ORIENT 0 +SIM_MAG2_SCALING 1.000000 +SIM_MAG3_DEVID 263178 +SIM_MAG3_DIA_X 0.000000 +SIM_MAG3_DIA_Y 0.000000 +SIM_MAG3_DIA_Z 0.000000 +SIM_MAG3_FAIL 0 +SIM_MAG3_ODI_X 0.000000 +SIM_MAG3_ODI_Y 0.000000 +SIM_MAG3_ODI_Z 0.000000 +SIM_MAG3_OFS_X 5.000000 +SIM_MAG3_OFS_Y 13.000000 +SIM_MAG3_OFS_Z -18.000000 +SIM_MAG3_ORIENT 0 +SIM_MAG3_SCALING 1.000000 +SIM_MAG4_DEVID 97283 +SIM_MAG5_DEVID 97795 +SIM_MAG6_DEVID 98051 +SIM_MAG7_DEVID 0 +SIM_MAG8_DEVID 0 +SIM_MAG_ALY_HGT 1.000000 +SIM_MAG_ALY_X 0.000000 +SIM_MAG_ALY_Y 0.000000 +SIM_MAG_ALY_Z 0.000000 +SIM_MAG_DELAY 0 +SIM_MAG_DIA_X 0.000000 +SIM_MAG_DIA_Y 0.000000 +SIM_MAG_DIA_Z 0.000000 +SIM_MAG_MOT_X 0.000000 +SIM_MAG_MOT_Y 0.000000 +SIM_MAG_MOT_Z 0.000000 +SIM_MAG_ODI_X 0.000000 +SIM_MAG_ODI_Y 0.000000 +SIM_MAG_ODI_Z 0.000000 +SIM_MAG_OFS_X 5.000000 +SIM_MAG_OFS_Y 13.000000 +SIM_MAG_OFS_Z -18.000000 +SIM_MAG_ORIENT 0 +SIM_MAG_RND 0.000000 +SIM_ODOM_ENABLE 0 +SIM_OPOS_ALT 584.000000 +SIM_OPOS_HDG 353.000000 +SIM_OPOS_LAT -35.363262 +SIM_OPOS_LNG 149.165237 +SIM_PARA_ENABLE 0 +SIM_PARA_PIN -1 +SIM_PIN_MASK 0 +SIM_PLD_ALT_LMT 15.000000 +SIM_PLD_DIST_LMT 10.000000 +SIM_PLD_ENABLE 0 +SIM_PLD_HEIGHT 0.000000 +SIM_PLD_LAT 0.000000 +SIM_PLD_LON 0.000000 +SIM_PLD_RATE 100 +SIM_PLD_TYPE 0 +SIM_PLD_YAW 0 +SIM_RATE_HZ 1200 +SIM_RC_CHANCOUNT 16 +SIM_RC_FAIL 0 +SIM_RICH_CTRL -1 +SIM_RICH_ENABLE 0 +SIM_SAFETY_STATE 0 +SIM_SERVO_SPEED 0.140000 +SIM_SHIP_DSIZE 10.000000 +SIM_SHIP_ENABLE 0 +SIM_SHIP_PSIZE 1000.000000 +SIM_SHIP_SPEED 3.000000 +SIM_SHIP_SYSID 17 +SIM_SHOVE_TIME 0 +SIM_SHOVE_X 0.000000 +SIM_SHOVE_Y 0.000000 +SIM_SHOVE_Z 0.000000 +SIM_SONAR_GLITCH 0.000000 +SIM_SONAR_POS_X 0.000000 +SIM_SONAR_POS_Y 0.000000 +SIM_SONAR_POS_Z 0.000000 +SIM_SONAR_RND 0.000000 +SIM_SONAR_SCALE 12.121200 +SIM_SPEEDUP 1.000000 +SIM_SPR_ENABLE 0 +SIM_SPR_PUMP -1 +SIM_SPR_SPIN -1 +SIM_TA_ENABLE 1 +SIM_TEMP_BFACTOR 0.000000 +SIM_TEMP_FLIGHT 35.000000 +SIM_TEMP_START 25.000000 +SIM_TEMP_TCONST 30.000000 +SIM_TERRAIN 1 +SIM_THML_SCENARI 0 +SIM_TIDE_DIR 0.000000 +SIM_TIDE_SPEED 0.000000 +SIM_TWIST_TIME 0 +SIM_TWIST_X 0.000000 +SIM_TWIST_Y 0.000000 +SIM_TWIST_Z 0.000000 +SIM_VIB_FREQ_X 0.000000 +SIM_VIB_FREQ_Y 0.000000 +SIM_VIB_FREQ_Z 0.000000 +SIM_VIB_MOT_MAX 0.000000 +SIM_VIB_MOT_MULT 1.000000 +SIM_VICON_FAIL 0 +SIM_VICON_GLIT_X 0.000000 +SIM_VICON_GLIT_Y 0.000000 +SIM_VICON_GLIT_Z 0.000000 +SIM_VICON_POS_X 0.000000 +SIM_VICON_POS_Y 0.000000 +SIM_VICON_POS_Z 0.000000 +SIM_VICON_TMASK 3 +SIM_VICON_VGLI_X 0.000000 +SIM_VICON_VGLI_Y 0.000000 +SIM_VICON_VGLI_Z 0.000000 +SIM_VICON_YAW 0 +SIM_VICON_YAWERR 0 +SIM_WAVE_AMP 0.500000 +SIM_WAVE_DIR 0.000000 +SIM_WAVE_ENABLE 0 +SIM_WAVE_LENGTH 10.000000 +SIM_WAVE_SPEED 0.500000 +SIM_WIND_DELAY 0 +SIM_WIND_DIR 180.000000 +SIM_WIND_DIR_Z 0.000000 +SIM_WIND_SPD 0.000000 +SIM_WIND_T 0 +SIM_WIND_TURB 0.000000 +SIM_WIND_T_ALT 60.000000 +SIM_WIND_T_COEF 0.010000 +SIM_WOW_PIN -1 +SOAR_ENABLE 0 +SR0_ADSB 4 +SR0_EXTRA1 4 +SR0_EXTRA2 4 +SR0_EXTRA3 4 +SR0_EXT_STAT 4 +SR0_PARAMS 10 +SR0_POSITION 4 +SR0_RAW_CTRL 4 +SR0_RAW_SENS 4 +SR0_RC_CHAN 4 +SR1_ADSB 5 +SR1_EXTRA1 1 +SR1_EXTRA2 1 +SR1_EXTRA3 1 +SR1_EXT_STAT 1 +SR1_PARAMS 10 +SR1_POSITION 1 +SR1_RAW_CTRL 1 +SR1_RAW_SENS 1 +SR1_RC_CHAN 1 +SR2_ADSB 5 +SR2_EXTRA1 1 +SR2_EXTRA2 1 +SR2_EXTRA3 1 +SR2_EXT_STAT 1 +SR2_PARAMS 10 +SR2_POSITION 1 +SR2_RAW_CTRL 1 +SR2_RAW_SENS 1 +SR2_RC_CHAN 1 +SR3_ADSB 5 +SR3_EXTRA1 1 +SR3_EXTRA2 1 +SR3_EXTRA3 1 +SR3_EXT_STAT 1 +SR3_PARAMS 10 +SR3_POSITION 1 +SR3_RAW_CTRL 1 +SR3_RAW_SENS 1 +SR3_RC_CHAN 1 +SR4_ADSB 5 +SR4_EXTRA1 1 +SR4_EXTRA2 1 +SR4_EXTRA3 1 +SR4_EXT_STAT 1 +SR4_PARAMS 10 +SR4_POSITION 1 +SR4_RAW_CTRL 1 +SR4_RAW_SENS 1 +SR4_RC_CHAN 1 +SR5_ADSB 5 +SR5_EXTRA1 1 +SR5_EXTRA2 1 +SR5_EXTRA3 1 +SR5_EXT_STAT 1 +SR5_PARAMS 10 +SR5_POSITION 1 +SR5_RAW_CTRL 1 +SR5_RAW_SENS 1 +SR5_RC_CHAN 1 +SR6_ADSB 5 +SR6_EXTRA1 1 +SR6_EXTRA2 1 +SR6_EXTRA3 1 +SR6_EXT_STAT 1 +SR6_PARAMS 10 +SR6_POSITION 1 +SR6_RAW_CTRL 1 +SR6_RAW_SENS 1 +SR6_RC_CHAN 1 +STAB_PITCH_DOWN 2.000000 +STALL_PREVENTION 1 +STAT_BOOTCNT 5 +STAT_FLTTIME 0 +STAT_RESET 0 +STAT_RUNTIME 0 +STEER2SRV_D 0.005000 +STEER2SRV_DRTFCT 10.000000 +STEER2SRV_DRTMIN 4500.000000 +STEER2SRV_DRTSPD 0.000000 +STEER2SRV_FF 0.000000 +STEER2SRV_I 0.200000 +STEER2SRV_IMAX 1500 +STEER2SRV_MINSPD 1.000000 +STEER2SRV_P 1.800000 +STEER2SRV_TCONST 0.750000 +STICK_MIXING 1 +SYSID_ENFORCE 0 +SYSID_MYGCS 255 +SYSID_THISMAV 1 +SYS_NUM_RESETS 5 +TECS_APPR_SMAX 0.000000 +TECS_CLMB_MAX 5.000000 +TECS_HGT_OMEGA 3.000000 +TECS_INTEG_GAIN 0.100000 +TECS_LAND_ARSPD -1.000000 +TECS_LAND_DAMP 0.500000 +TECS_LAND_IGAIN 0.000000 +TECS_LAND_PDAMP 0.000000 +TECS_LAND_PMAX 10 +TECS_LAND_SINK 0.250000 +TECS_LAND_SPDWGT -1.000000 +TECS_LAND_SRC 0.000000 +TECS_LAND_TCONST 2.000000 +TECS_LAND_TDAMP 0.000000 +TECS_LAND_THR -1.000000 +TECS_OPTIONS 0 +TECS_PITCH_MAX 15 +TECS_PITCH_MIN 0 +TECS_PTCH_DAMP 0.000000 +TECS_PTCH_FF_K 0.000000 +TECS_PTCH_FF_V0 12.000000 +TECS_RLL2THR 10.000000 +TECS_SINK_MAX 5.000000 +TECS_SINK_MIN 2.000000 +TECS_SPDWEIGHT 1.000000 +TECS_SPD_OMEGA 2.000000 +TECS_SYNAIRSPEED 0 +TECS_THR_DAMP 0.500000 +TECS_TIME_CONST 5.000000 +TECS_TKOFF_IGAIN 0.000000 +TECS_VERT_ACC 7.000000 +TELEM_DELAY 0 +TERRAIN_ENABLE 1 +TERRAIN_FOLLOW 0 +TERRAIN_LOOKAHD 2000 +TERRAIN_OPTIONS 0 +TERRAIN_SPACING 100 +THROTTLE_NUDGE 1 +THR_FAILSAFE 1 +THR_FS_VALUE 950 +THR_MAX 100 +THR_MIN 0 +THR_PASS_STAB 0 +THR_SLEWRATE 100 +THR_SUPP_MAN 0 +TKOFF_ACCEL_CNT 1 +TKOFF_ALT 50 +TKOFF_DIST 200 +TKOFF_FLAP_PCNT 0 +TKOFF_LVL_ALT 20 +TKOFF_LVL_PITCH 15 +TKOFF_PLIM_SEC 2.000000 +TKOFF_ROTATE_SPD 0.000000 +TKOFF_TDRAG_ELEV 0 +TKOFF_TDRAG_SPD1 0.000000 +TKOFF_THR_DELAY 2 +TKOFF_THR_MAX 0 +TKOFF_THR_MINACC 0.000000 +TKOFF_THR_MINSPD 0.000000 +TKOFF_THR_SLEW 0 +TKOFF_TIMEOUT 0 +TRIM_ARSPD_CM 2200 +TRIM_AUTO 0 +TRIM_PITCH_CD 0 +TRIM_THROTTLE 50 +TUNE_CHAN 0 +TUNE_CHAN_MAX 2000 +TUNE_CHAN_MIN 1000 +TUNE_ERR_THRESH 0.150000 +TUNE_MODE_REVERT 1 +TUNE_PARAM 0 +TUNE_RANGE 2.000000 +TUNE_SELECTOR 0 +USE_REV_THRUST 2 +VISO_TYPE 0 +VTX_ENABLE 0 +WP_LOITER_RAD 80 +WP_MAX_RADIUS 0 +WP_RADIUS 50 +YAW2SRV_DAMP 0.000000 +YAW2SRV_IMAX 1500 +YAW2SRV_INT 0.000000 +YAW2SRV_RLL 1.000000 +YAW2SRV_SLIP 0.000000 diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp index d739301e2e7344..453a3bb7358c4d 100644 --- a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp @@ -226,17 +226,17 @@ unsigned short AS_5600::setStartPosition(unsigned short startAngle) */ -int AS_5600::writeOneByte(uint8_t in_adr, uint8_t msg){ - - WITH_SEMAPHORE(sem); - - uint8_t send[2] = {in_adr, msg}; - - bool success = dev->transfer(send, sizeof(send), nullptr, 0); - - return success ? 0 : -1; - -} +//int AS_5600::writeOneByte(uint8_t in_adr, uint8_t msg){ +// +// WITH_SEMAPHORE(sem); +// +// send[2] = {in_adr, msg}; +// +// bool success = dev->transfer(send, sizeof(send), nullptr, 0); +// +// return success ? 0 : -1; +// +//} /******************************************************* @@ -253,7 +253,6 @@ int AS_5600::readOneByte(uint8_t in_adr) uint8_t send[1] = {in_adr}; uint8_t recv[1]; - bool success = dev->transfer(send, sizeof(send), recv, sizeof(recv)); return success ? recv[0] : -1; diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.h b/libraries/AP_AS5600_AOA/AS5600_AOA.h index ca439b055fb6e5..f6996e907b9eb0 100644 --- a/libraries/AP_AS5600_AOA/AS5600_AOA.h +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.h @@ -94,4 +94,6 @@ class AS_5600 { unsigned char lowByte(unsigned short short_in); + + }; From 20ed5a4f1378997a7dfa257cb5ff89246558d98b Mon Sep 17 00:00:00 2001 From: colejmero <57465269+colejmero@users.noreply.github.com> Date: Mon, 1 Mar 2021 01:39:25 -0500 Subject: [PATCH 04/10] Solves AS5600 communication issue on SITL --- ArduCopter/mav_0_1.parm | 1297 ++++++++++++++++++++++++ ArduPlane/mav_0_1.parm | 10 +- ArduPlane/system.cpp | 3 + libraries/AP_AS5600_AOA/AS5600_AOA.cpp | 9 +- libraries/SITL/SIM_AS5600.h | 9 + 5 files changed, 1318 insertions(+), 10 deletions(-) create mode 100644 ArduCopter/mav_0_1.parm diff --git a/ArduCopter/mav_0_1.parm b/ArduCopter/mav_0_1.parm new file mode 100644 index 00000000000000..9b26e7305c07af --- /dev/null +++ b/ArduCopter/mav_0_1.parm @@ -0,0 +1,1297 @@ +ACRO_BAL_PITCH 1.000000 +ACRO_BAL_ROLL 1.000000 +ACRO_OPTIONS 0 +ACRO_RP_EXPO 0.300000 +ACRO_RP_P 4.500000 +ACRO_THR_MID 0.000000 +ACRO_TRAINER 2 +ACRO_YAW_P 4.500000 +ACRO_Y_EXPO 0.000000 +ADSB_TYPE 0 +AHRS_COMP_BETA 0.100000 +AHRS_CUSTOM_PIT 0.000000 +AHRS_CUSTOM_ROLL 0.000000 +AHRS_CUSTOM_YAW 0.000000 +AHRS_EKF_TYPE 3 +AHRS_GPS_GAIN 1.000000 +AHRS_GPS_MINSATS 6 +AHRS_GPS_USE 1 +AHRS_ORIENTATION 0 +AHRS_RP_P 0.200000 +AHRS_TRIM_X 0.000000 +AHRS_TRIM_Y 0.000000 +AHRS_TRIM_Z 0.000000 +AHRS_WIND_MAX 0 +AHRS_YAW_P 0.200000 +ANGLE_MAX 4500 +ARMING_ACCTHRESH 0.750000 +ARMING_CHECK 1 +ARMING_MIS_ITEMS 0 +ARMING_RUDDER 2 +ATC_ACCEL_P_MAX 110000.000000 +ATC_ACCEL_R_MAX 110000.000000 +ATC_ACCEL_Y_MAX 27000.000000 +ATC_ANGLE_BOOST 1 +ATC_ANG_LIM_TC 1.000000 +ATC_ANG_PIT_P 4.500000 +ATC_ANG_RLL_P 4.500000 +ATC_ANG_YAW_P 4.500000 +ATC_INPUT_TC 0.150000 +ATC_RATE_FF_ENAB 1 +ATC_RATE_P_MAX 0.000000 +ATC_RATE_R_MAX 0.000000 +ATC_RATE_Y_MAX 0.000000 +ATC_RAT_PIT_D 0.003600 +ATC_RAT_PIT_FF 0.000000 +ATC_RAT_PIT_FLTD 20.000000 +ATC_RAT_PIT_FLTE 0.000000 +ATC_RAT_PIT_FLTT 20.000000 +ATC_RAT_PIT_I 0.135000 +ATC_RAT_PIT_IMAX 0.500000 +ATC_RAT_PIT_P 0.135000 +ATC_RAT_PIT_SMAX 0.000000 +ATC_RAT_RLL_D 0.003600 +ATC_RAT_RLL_FF 0.000000 +ATC_RAT_RLL_FLTD 20.000000 +ATC_RAT_RLL_FLTE 0.000000 +ATC_RAT_RLL_FLTT 20.000000 +ATC_RAT_RLL_I 0.135000 +ATC_RAT_RLL_IMAX 0.500000 +ATC_RAT_RLL_P 0.135000 +ATC_RAT_RLL_SMAX 0.000000 +ATC_RAT_YAW_D 0.000000 +ATC_RAT_YAW_FF 0.000000 +ATC_RAT_YAW_FLTD 0.000000 +ATC_RAT_YAW_FLTE 2.500000 +ATC_RAT_YAW_FLTT 20.000000 +ATC_RAT_YAW_I 0.018000 +ATC_RAT_YAW_IMAX 0.500000 +ATC_RAT_YAW_P 0.180000 +ATC_RAT_YAW_SMAX 0.000000 +ATC_SLEW_YAW 6000.000000 +ATC_THR_MIX_MAN 0.100000 +ATC_THR_MIX_MAX 0.500000 +ATC_THR_MIX_MIN 0.100000 +AUTOTUNE_AGGR 0.100000 +AUTOTUNE_AXES 7 +AUTOTUNE_MIN_D 0.001000 +AUTO_OPTIONS 0 +AVD_ENABLE 0 +AVOID_ACCEL_MAX 3.000000 +AVOID_ALT_MIN 2.000000 +AVOID_ANGLE_MAX 1000 +AVOID_BACKUP_SPD 0.500000 +AVOID_BEHAVE 0 +AVOID_DIST_MAX 5.000000 +AVOID_ENABLE 3 +AVOID_MARGIN 2.000000 +BARO1_DEVID 65540 +BARO1_GND_PRESS 94502.796875 +BARO1_WCF_ENABLE 0 +BARO2_DEVID 65796 +BARO2_GND_PRESS 94503.546875 +BARO2_WCF_ENABLE 0 +BARO3_DEVID 0 +BARO3_GND_PRESS 0.000000 +BARO3_WCF_ENABLE 0 +BARO_ALT_OFFSET 0.000000 +BARO_EXT_BUS -1 +BARO_FLTR_RNG 0 +BARO_GND_TEMP 0.000000 +BARO_PRIMARY 0 +BARO_PROBE_EXT 0 +BATT2_MONITOR 0 +BATT3_MONITOR 0 +BATT4_MONITOR 0 +BATT5_MONITOR 0 +BATT6_MONITOR 0 +BATT7_MONITOR 0 +BATT8_MONITOR 0 +BATT9_MONITOR 0 +BATT_AMP_OFFSET 0.000000 +BATT_AMP_PERVLT 17.000000 +BATT_ARM_MAH 0 +BATT_ARM_VOLT 0.000000 +BATT_BUS 0 +BATT_CAPACITY 0 +BATT_CRT_MAH 0.000000 +BATT_CRT_VOLT 0.000000 +BATT_CURR_PIN 12 +BATT_FS_CRT_ACT 0 +BATT_FS_LOW_ACT 0 +BATT_FS_VOLTSRC 0 +BATT_LOW_MAH 0.000000 +BATT_LOW_TIMER 10 +BATT_LOW_VOLT 10.500000 +BATT_MONITOR 4 +BATT_OPTIONS 0 +BATT_SERIAL_NUM -1 +BATT_VOLT_MULT 10.100000 +BATT_VOLT_PIN 13 +BCN_ALT 0.000000 +BCN_LATITUDE 0.000000 +BCN_LONGITUDE 0.000000 +BCN_ORIENT_YAW 0 +BCN_TYPE 0 +BRD_ALT_CONFIG 0 +BRD_BOOT_DELAY 0 +BRD_OPTIONS 0 +BRD_PWM_COUNT 8 +BRD_RTC_TYPES 1 +BRD_RTC_TZ_MIN 0 +BRD_SAFETYOPTION 3 +BRD_SERIAL_NUM 0 +BRD_VBUS_MIN 4.300000 +BRD_VSERVO_MIN 0.000000 +BTN_ENABLE 0 +CAM_AUTO_ONLY 0 +CAM_DURATION 10 +CAM_FEEDBACK_PIN -1 +CAM_FEEDBACK_POL 1 +CAM_MAX_ROLL 0 +CAM_MIN_INTERVAL 0 +CAM_RC_TYPE 0 +CAM_RELAY_ON 1 +CAM_SERVO_OFF 1100 +CAM_SERVO_ON 1300 +CAM_TRIGG_DIST 0.000000 +CAM_TRIGG_TYPE 0 +CAM_TYPE 0 +CAN_D1_PROTOCOL 1 +CAN_D2_PROTOCOL 1 +CAN_LOGLEVEL 0 +CAN_P1_DRIVER 0 +CAN_P2_DRIVER 0 +CAN_SLCAN_CPORT 0 +CAN_SLCAN_SDELAY 1 +CAN_SLCAN_SERNUM -1 +CAN_SLCAN_TIMOUT 0 +CHUTE_ENABLED 0 +CIRCLE_OPTIONS 1 +CIRCLE_RADIUS 1000.000000 +CIRCLE_RATE 20.000000 +COMPASS_AUTODEC 1 +COMPASS_AUTO_ROT 2 +COMPASS_CAL_FIT 16.000000 +COMPASS_CUS_PIT 0.000000 +COMPASS_CUS_ROLL 0.000000 +COMPASS_CUS_YAW 0.000000 +COMPASS_DEC 0.000000 +COMPASS_DEV_ID 97539 +COMPASS_DEV_ID2 131874 +COMPASS_DEV_ID3 263178 +COMPASS_DEV_ID4 97283 +COMPASS_DEV_ID5 97795 +COMPASS_DEV_ID6 98051 +COMPASS_DEV_ID7 983044 +COMPASS_DEV_ID8 0 +COMPASS_DIA2_X 1.000000 +COMPASS_DIA2_Y 1.000000 +COMPASS_DIA2_Z 1.000000 +COMPASS_DIA3_X 1.000000 +COMPASS_DIA3_Y 1.000000 +COMPASS_DIA3_Z 1.000000 +COMPASS_DIA_X 1.000000 +COMPASS_DIA_Y 1.000000 +COMPASS_DIA_Z 1.000000 +COMPASS_ENABLE 1 +COMPASS_EXTERN2 0 +COMPASS_EXTERN3 0 +COMPASS_EXTERNAL 1 +COMPASS_FLTR_RNG 0 +COMPASS_LEARN 0 +COMPASS_MOT2_X 0.000000 +COMPASS_MOT2_Y 0.000000 +COMPASS_MOT2_Z 0.000000 +COMPASS_MOT3_X 0.000000 +COMPASS_MOT3_Y 0.000000 +COMPASS_MOT3_Z 0.000000 +COMPASS_MOTCT 0 +COMPASS_MOT_X 0.000000 +COMPASS_MOT_Y 0.000000 +COMPASS_MOT_Z 0.000000 +COMPASS_ODI2_X 0.000000 +COMPASS_ODI2_Y 0.000000 +COMPASS_ODI2_Z 0.000000 +COMPASS_ODI3_X 0.000000 +COMPASS_ODI3_Y 0.000000 +COMPASS_ODI3_Z 0.000000 +COMPASS_ODI_X 0.000000 +COMPASS_ODI_Y 0.000000 +COMPASS_ODI_Z 0.000000 +COMPASS_OFFS_MAX 1800 +COMPASS_OFS2_X 5.000000 +COMPASS_OFS2_Y 13.000000 +COMPASS_OFS2_Z -18.000000 +COMPASS_OFS3_X 5.000000 +COMPASS_OFS3_Y 13.000000 +COMPASS_OFS3_Z -18.000000 +COMPASS_OFS_X 5.000000 +COMPASS_OFS_Y 13.000000 +COMPASS_OFS_Z -18.000000 +COMPASS_OPTIONS 0 +COMPASS_ORIENT 0 +COMPASS_ORIENT2 0 +COMPASS_ORIENT3 0 +COMPASS_PMOT_EN 0 +COMPASS_PRIO1_ID 97539 +COMPASS_PRIO2_ID 131874 +COMPASS_PRIO3_ID 263178 +COMPASS_SCALE 1.000000 +COMPASS_SCALE2 1.000000 +COMPASS_SCALE3 1.000000 +COMPASS_TYPEMASK 0 +COMPASS_USE 1 +COMPASS_USE2 1 +COMPASS_USE3 1 +DEV_OPTIONS 0 +DISARM_DELAY 10 +EAHRS_TYPE 0 +EK2_ENABLE 0 +EK3_ABIAS_P_NSE 0.003000 +EK3_ACC_BIAS_LIM 1.000000 +EK3_ACC_P_NSE 0.350000 +EK3_AFFINITY 0 +EK3_ALT_M_NSE 2.000000 +EK3_BCN_DELAY 50 +EK3_BCN_I_GTE 500 +EK3_BCN_M_NSE 1.000000 +EK3_CHECK_SCALE 100 +EK3_DRAG_BCOEF_X 0.000000 +EK3_DRAG_BCOEF_Y 0.000000 +EK3_DRAG_MCOEF 0.000000 +EK3_DRAG_M_NSE 0.500000 +EK3_EAS_I_GATE 400 +EK3_EAS_M_NSE 1.400000 +EK3_ENABLE 1 +EK3_ERR_THRESH 0.200000 +EK3_FLOW_DELAY 10 +EK3_FLOW_I_GATE 300 +EK3_FLOW_M_NSE 0.250000 +EK3_FLOW_USE 1 +EK3_GBIAS_P_NSE 0.001000 +EK3_GLITCH_RAD 25 +EK3_GPS_CHECK 31 +EK3_GSF_DELAY 1000 +EK3_GSF_RST_MAX 2 +EK3_GSF_RUN_MASK 3 +EK3_GSF_USE_MASK 3 +EK3_GYRO_P_NSE 0.015000 +EK3_HGT_DELAY 60 +EK3_HGT_I_GATE 500 +EK3_HRT_FILT 2.000000 +EK3_IMU_MASK 3 +EK3_MAGB_P_NSE 0.000100 +EK3_MAGE_P_NSE 0.001000 +EK3_MAG_CAL 3 +EK3_MAG_EF_LIM 50 +EK3_MAG_I_GATE 300 +EK3_MAG_MASK 0 +EK3_MAG_M_NSE 0.050000 +EK3_MAX_FLOW 2.500000 +EK3_NOAID_M_NSE 10.000000 +EK3_OGN_HGT_MASK 0 +EK3_POSNE_M_NSE 0.500000 +EK3_POS_I_GATE 500 +EK3_RNG_I_GATE 500 +EK3_RNG_M_NSE 0.500000 +EK3_RNG_USE_HGT -1 +EK3_RNG_USE_SPD 2.000000 +EK3_SRC1_POSXY 3 +EK3_SRC1_POSZ 1 +EK3_SRC1_VELXY 3 +EK3_SRC1_VELZ 3 +EK3_SRC1_YAW 1 +EK3_SRC2_POSXY 0 +EK3_SRC2_POSZ 1 +EK3_SRC2_VELXY 0 +EK3_SRC2_VELZ 0 +EK3_SRC2_YAW 0 +EK3_SRC3_POSXY 0 +EK3_SRC3_POSZ 1 +EK3_SRC3_VELXY 0 +EK3_SRC3_VELZ 0 +EK3_SRC3_YAW 0 +EK3_SRC_OPTIONS 1 +EK3_TAU_OUTPUT 25 +EK3_TERR_GRAD 0.100000 +EK3_VELD_M_NSE 0.500000 +EK3_VELNE_M_NSE 0.300000 +EK3_VEL_I_GATE 500 +EK3_VIS_VERR_MAX 0.900000 +EK3_VIS_VERR_MIN 0.100000 +EK3_WENC_VERR 0.100000 +EK3_WIND_PSCALE 0.500000 +EK3_WIND_P_NSE 0.200000 +EK3_YAW_I_GATE 300 +EK3_YAW_M_NSE 0.500000 +ESC_CALIBRATION 0 +FENCE_ACTION 1 +FENCE_ALT_MAX 100.000000 +FENCE_ENABLE 0 +FENCE_MARGIN 2.000000 +FENCE_RADIUS 150.000000 +FENCE_TOTAL 0 +FENCE_TYPE 7 +FFT_ENABLE 0 +FHLD_BRAKE_RATE 8 +FHLD_FILT_HZ 5.000000 +FHLD_FLOW_MAX 0.600000 +FHLD_QUAL_MIN 10 +FHLD_XY_FILT_HZ 5.000000 +FHLD_XY_I 0.300000 +FHLD_XY_IMAX 3000.000000 +FHLD_XY_P 0.200000 +FLOW_ADDR 0 +FLOW_FXSCALER 0 +FLOW_FYSCALER 0 +FLOW_ORIENT_YAW 0 +FLOW_POS_X 0.000000 +FLOW_POS_Y 0.000000 +FLOW_POS_Z 0.000000 +FLOW_TYPE 0 +FLTMODE1 7 +FLTMODE2 9 +FLTMODE3 6 +FLTMODE4 3 +FLTMODE5 5 +FLTMODE6 0 +FLTMODE_CH 5 +FOLL_ENABLE 0 +FORMAT_VERSION 120 +FRAME_CLASS 1 +FRAME_TYPE 0 +FRSKY_DNLINK1_ID 20 +FRSKY_DNLINK2_ID 7 +FRSKY_UPLINK_ID 13 +FS_CRASH_CHECK 1 +FS_EKF_ACTION 1 +FS_EKF_THRESH 0.800000 +FS_GCS_ENABLE 0 +FS_GCS_TIMEOUT 5.000000 +FS_OPTIONS 16 +FS_THR_ENABLE 1 +FS_THR_VALUE 975 +FS_VIBE_ENABLE 1 +GCS_PID_MASK 0 +GEN_TYPE 0 +GND_EFFECT_COMP 1 +GPS_AUTO_CONFIG 1 +GPS_AUTO_SWITCH 1 +GPS_BLEND_MASK 5 +GPS_BLEND_TC 10.000000 +GPS_COM_PORT 1 +GPS_COM_PORT2 1 +GPS_DELAY_MS 0 +GPS_DELAY_MS2 0 +GPS_DRV_OPTIONS 0 +GPS_GNSS_MODE 0 +GPS_GNSS_MODE2 0 +GPS_HDOP_GOOD 140 +GPS_INJECT_TO 127 +GPS_MB1_TYPE 0 +GPS_MB2_TYPE 0 +GPS_MIN_DGPS 100 +GPS_MIN_ELEV -100 +GPS_NAVFILTER 8 +GPS_POS1_X 0.000000 +GPS_POS1_Y 0.000000 +GPS_POS1_Z 0.000000 +GPS_POS2_X 0.000000 +GPS_POS2_Y 0.000000 +GPS_POS2_Z 0.000000 +GPS_PRIMARY 0 +GPS_RATE_MS 200 +GPS_RATE_MS2 200 +GPS_RAW_DATA 0 +GPS_SAVE_CFG 2 +GPS_SBAS_MODE 2 +GPS_SBP_LOGMASK -256 +GPS_TYPE 1 +GPS_TYPE2 0 +GRIP_ENABLE 0 +GUID_OPTIONS 0 +INITIAL_MODE 0 +INS_ACC1_CALTEMP -300.000000 +INS_ACC2OFFS_X 0.001000 +INS_ACC2OFFS_Y 0.001000 +INS_ACC2OFFS_Z 0.001000 +INS_ACC2SCAL_X 1.001000 +INS_ACC2SCAL_Y 1.001000 +INS_ACC2SCAL_Z 1.001000 +INS_ACC2_CALTEMP -300.000000 +INS_ACC2_ID 0 +INS_ACC3OFFS_X 0.000000 +INS_ACC3OFFS_Y 0.000000 +INS_ACC3OFFS_Z 0.000000 +INS_ACC3SCAL_X 1.000000 +INS_ACC3SCAL_Y 1.000000 +INS_ACC3SCAL_Z 1.000000 +INS_ACC3_CALTEMP -300.000000 +INS_ACC3_ID 0 +INS_ACCEL_FILTER 20 +INS_ACCOFFS_X 0.001000 +INS_ACCOFFS_Y 0.001000 +INS_ACCOFFS_Z 0.001000 +INS_ACCSCAL_X 1.001000 +INS_ACCSCAL_Y 1.001000 +INS_ACCSCAL_Z 1.001000 +INS_ACC_BODYFIX 2 +INS_ACC_ID 0 +INS_ENABLE_MASK 127 +INS_FAST_SAMPLE 1 +INS_GYR1_CALTEMP -300.000000 +INS_GYR2OFFS_X 0.000000 +INS_GYR2OFFS_Y 0.000000 +INS_GYR2OFFS_Z 0.000000 +INS_GYR2_CALTEMP -300.000000 +INS_GYR2_ID 0 +INS_GYR3OFFS_X 0.000000 +INS_GYR3OFFS_Y 0.000000 +INS_GYR3OFFS_Z 0.000000 +INS_GYR3_CALTEMP -300.000000 +INS_GYR3_ID 0 +INS_GYROFFS_X 0.000000 +INS_GYROFFS_Y 0.000000 +INS_GYROFFS_Z 0.000000 +INS_GYRO_FILTER 20 +INS_GYRO_RATE 0 +INS_GYR_CAL 1 +INS_GYR_ID 0 +INS_HNTCH_ENABLE 0 +INS_LOG_BAT_CNT 1024 +INS_LOG_BAT_LGCT 32 +INS_LOG_BAT_LGIN 20 +INS_LOG_BAT_MASK 0 +INS_LOG_BAT_OPT 0 +INS_NOTCH_ENABLE 0 +INS_POS1_X 0.000000 +INS_POS1_Y 0.000000 +INS_POS1_Z 0.000000 +INS_POS2_X 0.000000 +INS_POS2_Y 0.000000 +INS_POS2_Z 0.000000 +INS_POS3_X 0.000000 +INS_POS3_Y 0.000000 +INS_POS3_Z 0.000000 +INS_STILL_THRESH 2.500000 +INS_TCAL1_ENABLE 0 +INS_TCAL2_ENABLE 0 +INS_TCAL3_ENABLE 0 +INS_TCAL_OPTIONS 0 +INS_TRIM_OPTION 1 +INS_USE 1 +INS_USE2 1 +INS_USE3 1 +LAND_ALT_LOW 1000 +LAND_REPOSITION 1 +LAND_SPEED 50 +LAND_SPEED_HIGH 0 +LGR_DEPLOY_ALT 0 +LGR_DEPLOY_PIN -1 +LGR_DEPLOY_POL 0 +LGR_OPTIONS 3 +LGR_RETRACT_ALT 0 +LGR_STARTUP 0 +LGR_WOW_PIN 8 +LGR_WOW_POL 1 +LOG_BACKEND_TYPE 1 +LOG_BITMASK 176126 +LOG_DISARMED 0 +LOG_FILE_BUFSIZE 200 +LOG_FILE_DSRMROT 0 +LOG_FILE_MB_FREE 500 +LOG_FILE_TIMEOUT 5 +LOG_MAV_BUFSIZE 8 +LOG_REPLAY 0 +LOIT_ACC_MAX 500.000000 +LOIT_ANG_MAX 0.000000 +LOIT_BRK_ACCEL 250.000000 +LOIT_BRK_DELAY 1.000000 +LOIT_BRK_JERK 500.000000 +LOIT_SPEED 1250.000000 +MIS_OPTIONS 0 +MIS_RESTART 0 +MIS_TOTAL 0 +MNT_ANGMAX_PAN 4500 +MNT_ANGMAX_ROL 4500 +MNT_ANGMAX_TIL 4500 +MNT_ANGMIN_PAN -4500 +MNT_ANGMIN_ROL -4500 +MNT_ANGMIN_TIL -4500 +MNT_DEFLT_MODE 3 +MNT_JSTICK_SPD 0 +MNT_LEAD_PTCH 0.000000 +MNT_LEAD_RLL 0.000000 +MNT_NEUTRAL_X 0.000000 +MNT_NEUTRAL_Y 0.000000 +MNT_NEUTRAL_Z 0.000000 +MNT_RC_IN_PAN 0 +MNT_RC_IN_ROLL 0 +MNT_RC_IN_TILT 0 +MNT_RETRACT_X 0.000000 +MNT_RETRACT_Y 0.000000 +MNT_RETRACT_Z 0.000000 +MNT_STAB_PAN 0 +MNT_STAB_ROLL 0 +MNT_STAB_TILT 0 +MNT_TYPE 0 +MOT_BAT_CURR_MAX 0.000000 +MOT_BAT_CURR_TC 5.000000 +MOT_BAT_IDX 0 +MOT_BAT_VOLT_MAX 12.800000 +MOT_BAT_VOLT_MIN 9.600000 +MOT_BOOST_SCALE 0.000000 +MOT_HOVER_LEARN 2 +MOT_PWM_MAX 2000 +MOT_PWM_MIN 1000 +MOT_PWM_TYPE 0 +MOT_SAFE_DISARM 0 +MOT_SAFE_TIME 1.000000 +MOT_SLEW_DN_TIME 0.000000 +MOT_SLEW_UP_TIME 0.000000 +MOT_SPIN_ARM 0.100000 +MOT_SPIN_MAX 0.950000 +MOT_SPIN_MIN 0.150000 +MOT_SPOOL_TIME 0.500000 +MOT_THST_EXPO 0.650000 +MOT_THST_HOVER 0.390000 +MOT_YAW_HEADROOM 200 +MSP_OPTIONS 0 +MSP_OSD_NCELLS 0 +NTF_BUZZ_ENABLE 1 +NTF_BUZZ_ON_LVL 1 +NTF_BUZZ_PIN 0 +NTF_BUZZ_VOLUME 100 +NTF_DISPLAY_TYPE 0 +NTF_LED_BRIGHT 3 +NTF_LED_LEN 1 +NTF_LED_OVERRIDE 0 +NTF_LED_TYPES 199 +NTF_OREO_THEME 0 +OA_TYPE 0 +OSD_TYPE 0 +PHLD_BRAKE_ANGLE 3000 +PHLD_BRAKE_RATE 8 +PILOT_ACCEL_Z 250 +PILOT_SPEED_DN 0 +PILOT_SPEED_UP 250 +PILOT_THR_BHV 0 +PILOT_THR_FILT 0.000000 +PILOT_TKOFF_ALT 0.000000 +PLND_ENABLED 0 +PRX_IGN_ANG1 0 +PRX_IGN_ANG2 0 +PRX_IGN_ANG3 0 +PRX_IGN_ANG4 0 +PRX_IGN_ANG5 0 +PRX_IGN_ANG6 0 +PRX_IGN_WID1 0 +PRX_IGN_WID2 0 +PRX_IGN_WID3 0 +PRX_IGN_WID4 0 +PRX_IGN_WID5 0 +PRX_IGN_WID6 0 +PRX_ORIENT 0 +PRX_TYPE 0 +PRX_YAW_CORR 0 +PSC_ACCZ_D 0.000000 +PSC_ACCZ_FF 0.000000 +PSC_ACCZ_FLTD 0.000000 +PSC_ACCZ_FLTE 20.000000 +PSC_ACCZ_FLTT 0.000000 +PSC_ACCZ_I 1.000000 +PSC_ACCZ_IMAX 800.000000 +PSC_ACCZ_P 0.500000 +PSC_ACCZ_SMAX 0.000000 +PSC_ACC_XY_FILT 2.000000 +PSC_ANGLE_MAX 0.000000 +PSC_POSXY_P 1.000000 +PSC_POSZ_P 1.000000 +PSC_VELXY_D 0.500000 +PSC_VELXY_D_FILT 5.000000 +PSC_VELXY_FILT 5.000000 +PSC_VELXY_I 1.000000 +PSC_VELXY_IMAX 1000.000000 +PSC_VELXY_P 2.000000 +PSC_VELZ_P 5.000000 +RALLY_INCL_HOME 1 +RALLY_LIMIT_KM 0.300000 +RALLY_TOTAL 0 +RC10_DZ 0 +RC10_MAX 1900 +RC10_MIN 1100 +RC10_OPTION 0 +RC10_REVERSED 0 +RC10_TRIM 1500 +RC11_DZ 0 +RC11_MAX 1900 +RC11_MIN 1100 +RC11_OPTION 0 +RC11_REVERSED 0 +RC11_TRIM 1500 +RC12_DZ 0 +RC12_MAX 1900 +RC12_MIN 1100 +RC12_OPTION 0 +RC12_REVERSED 0 +RC12_TRIM 1500 +RC13_DZ 0 +RC13_MAX 1900 +RC13_MIN 1100 +RC13_OPTION 0 +RC13_REVERSED 0 +RC13_TRIM 1500 +RC14_DZ 0 +RC14_MAX 1900 +RC14_MIN 1100 +RC14_OPTION 0 +RC14_REVERSED 0 +RC14_TRIM 1500 +RC15_DZ 0 +RC15_MAX 1900 +RC15_MIN 1100 +RC15_OPTION 0 +RC15_REVERSED 0 +RC15_TRIM 1500 +RC16_DZ 0 +RC16_MAX 1900 +RC16_MIN 1100 +RC16_OPTION 0 +RC16_REVERSED 0 +RC16_TRIM 1500 +RC1_DZ 20 +RC1_MAX 2000 +RC1_MIN 1000 +RC1_OPTION 0 +RC1_REVERSED 0 +RC1_TRIM 1500 +RC2_DZ 20 +RC2_MAX 2000 +RC2_MIN 1000 +RC2_OPTION 0 +RC2_REVERSED 0 +RC2_TRIM 1500 +RC3_DZ 30 +RC3_MAX 2000 +RC3_MIN 1000 +RC3_OPTION 0 +RC3_REVERSED 0 +RC3_TRIM 1500 +RC4_DZ 20 +RC4_MAX 2000 +RC4_MIN 1000 +RC4_OPTION 0 +RC4_REVERSED 0 +RC4_TRIM 1500 +RC5_DZ 0 +RC5_MAX 2000 +RC5_MIN 1000 +RC5_OPTION 0 +RC5_REVERSED 0 +RC5_TRIM 1500 +RC6_DZ 0 +RC6_MAX 2000 +RC6_MIN 1000 +RC6_OPTION 0 +RC6_REVERSED 0 +RC6_TRIM 1500 +RC7_DZ 0 +RC7_MAX 2000 +RC7_MIN 1000 +RC7_OPTION 7 +RC7_REVERSED 0 +RC7_TRIM 1500 +RC8_DZ 0 +RC8_MAX 2000 +RC8_MIN 1000 +RC8_OPTION 0 +RC8_REVERSED 0 +RC8_TRIM 1500 +RC9_DZ 0 +RC9_MAX 1900 +RC9_MIN 1100 +RC9_OPTION 0 +RC9_REVERSED 0 +RC9_TRIM 1500 +RCMAP_PITCH 2 +RCMAP_ROLL 1 +RCMAP_THROTTLE 3 +RCMAP_YAW 4 +RC_OPTIONS 32 +RC_OVERRIDE_TIME 3.000000 +RC_PROTOCOLS 1 +RC_SPEED 490 +RELAY_DEFAULT 0 +RELAY_PIN 13 +RELAY_PIN2 -1 +RELAY_PIN3 -1 +RELAY_PIN4 -1 +RELAY_PIN5 -1 +RELAY_PIN6 -1 +RNGFND1_TYPE 0 +RNGFND2_TYPE 0 +RNGFND3_TYPE 0 +RNGFND4_TYPE 0 +RNGFND5_TYPE 0 +RNGFND6_TYPE 0 +RNGFND7_TYPE 0 +RNGFND8_TYPE 0 +RNGFND9_TYPE 0 +RNGFNDA_TYPE 0 +RNGFND_GAIN 0.800000 +RPM2_PIN -1 +RPM2_SCALING 1.000000 +RPM2_TYPE 0 +RPM_MAX 100000.000000 +RPM_MIN 10.000000 +RPM_MIN_QUAL 0.500000 +RPM_PIN 54 +RPM_SCALING 1.000000 +RPM_TYPE 0 +RSSI_TYPE 0 +RTL_ALT 1500 +RTL_ALT_FINAL 0 +RTL_ALT_TYPE 0 +RTL_CLIMB_MIN 0 +RTL_CONE_SLOPE 3.000000 +RTL_LOIT_TIME 5000 +RTL_OPTIONS 0 +RTL_SPEED 0 +SCHED_DEBUG 0 +SCHED_LOOP_RATE 400 +SCHED_OPTIONS 0 +SCR_ENABLE 0 +SERIAL0_BAUD 115 +SERIAL0_PROTOCOL 2 +SERIAL1_BAUD 57 +SERIAL1_OPTIONS 0 +SERIAL1_PROTOCOL 2 +SERIAL2_BAUD 57 +SERIAL2_OPTIONS 0 +SERIAL2_PROTOCOL 2 +SERIAL3_BAUD 38 +SERIAL3_OPTIONS 0 +SERIAL3_PROTOCOL 5 +SERIAL4_BAUD 38 +SERIAL4_OPTIONS 0 +SERIAL4_PROTOCOL 5 +SERIAL5_BAUD 57 +SERIAL5_OPTIONS 0 +SERIAL5_PROTOCOL -1 +SERIAL6_BAUD 57 +SERIAL6_OPTIONS 0 +SERIAL6_PROTOCOL -1 +SERIAL7_BAUD 57 +SERIAL7_OPTIONS 0 +SERIAL7_PROTOCOL -1 +SERIAL_PASS1 0 +SERIAL_PASS2 -1 +SERIAL_PASSTIMO 15 +SERVO10_FUNCTION 0 +SERVO10_MAX 1900 +SERVO10_MIN 1100 +SERVO10_REVERSED 0 +SERVO10_TRIM 1500 +SERVO11_FUNCTION 0 +SERVO11_MAX 1900 +SERVO11_MIN 1100 +SERVO11_REVERSED 0 +SERVO11_TRIM 1500 +SERVO12_FUNCTION 0 +SERVO12_MAX 1900 +SERVO12_MIN 1100 +SERVO12_REVERSED 0 +SERVO12_TRIM 1500 +SERVO13_FUNCTION 0 +SERVO13_MAX 1900 +SERVO13_MIN 1100 +SERVO13_REVERSED 0 +SERVO13_TRIM 1500 +SERVO14_FUNCTION 0 +SERVO14_MAX 1900 +SERVO14_MIN 1100 +SERVO14_REVERSED 0 +SERVO14_TRIM 1500 +SERVO15_FUNCTION 0 +SERVO15_MAX 1900 +SERVO15_MIN 1100 +SERVO15_REVERSED 0 +SERVO15_TRIM 1500 +SERVO16_FUNCTION 0 +SERVO16_MAX 1900 +SERVO16_MIN 1100 +SERVO16_REVERSED 0 +SERVO16_TRIM 1500 +SERVO1_FUNCTION 33 +SERVO1_MAX 1900 +SERVO1_MIN 1100 +SERVO1_REVERSED 0 +SERVO1_TRIM 1500 +SERVO2_FUNCTION 34 +SERVO2_MAX 1900 +SERVO2_MIN 1100 +SERVO2_REVERSED 0 +SERVO2_TRIM 1500 +SERVO3_FUNCTION 35 +SERVO3_MAX 1900 +SERVO3_MIN 1100 +SERVO3_REVERSED 0 +SERVO3_TRIM 1500 +SERVO4_FUNCTION 36 +SERVO4_MAX 1900 +SERVO4_MIN 1100 +SERVO4_REVERSED 0 +SERVO4_TRIM 1500 +SERVO5_FUNCTION 0 +SERVO5_MAX 1900 +SERVO5_MIN 1100 +SERVO5_REVERSED 0 +SERVO5_TRIM 1500 +SERVO6_FUNCTION 0 +SERVO6_MAX 1900 +SERVO6_MIN 1100 +SERVO6_REVERSED 0 +SERVO6_TRIM 1500 +SERVO7_FUNCTION 0 +SERVO7_MAX 1900 +SERVO7_MIN 1100 +SERVO7_REVERSED 0 +SERVO7_TRIM 1500 +SERVO8_FUNCTION 0 +SERVO8_MAX 1900 +SERVO8_MIN 1100 +SERVO8_REVERSED 0 +SERVO8_TRIM 1500 +SERVO9_FUNCTION 0 +SERVO9_MAX 1900 +SERVO9_MIN 1100 +SERVO9_REVERSED 0 +SERVO9_TRIM 1500 +SERVO_RATE 50 +SERVO_ROB_POSMAX 4095 +SERVO_ROB_POSMIN 0 +SERVO_SBUS_RATE 50 +SERVO_VOLZ_MASK 0 +SID_AXIS 0 +SIMPLE 0 +SIM_ACC1_BIAS_X 0.000000 +SIM_ACC1_BIAS_Y 0.000000 +SIM_ACC1_BIAS_Z 0.000000 +SIM_ACC1_RND 0.000000 +SIM_ACC1_SCAL_X 0.000000 +SIM_ACC1_SCAL_Y 0.000000 +SIM_ACC1_SCAL_Z 0.000000 +SIM_ACC2_BIAS_X 0.000000 +SIM_ACC2_BIAS_Y 0.000000 +SIM_ACC2_BIAS_Z 0.000000 +SIM_ACC2_RND 0.000000 +SIM_ACC2_SCAL_X 0.000000 +SIM_ACC2_SCAL_Y 0.000000 +SIM_ACC2_SCAL_Z 0.000000 +SIM_ACC3_BIAS_X 0.000000 +SIM_ACC3_BIAS_Y 0.000000 +SIM_ACC3_BIAS_Z 0.000000 +SIM_ACC3_RND 0.000000 +SIM_ACC3_SCAL_X 0.000000 +SIM_ACC3_SCAL_Y 0.000000 +SIM_ACC3_SCAL_Z 0.000000 +SIM_ACCEL1_FAIL 0.000000 +SIM_ACCEL2_FAIL 0.000000 +SIM_ACCEL3_FAIL 0.000000 +SIM_ACC_FAIL_MSK 0 +SIM_ACC_TRIM_X 0.000000 +SIM_ACC_TRIM_Y 0.000000 +SIM_ACC_TRIM_Z 0.000000 +SIM_ADSB_ALT 1000.000000 +SIM_ADSB_COUNT -1 +SIM_ADSB_RADIUS 10000.000000 +SIM_ADSB_TX 0 +SIM_ARSPD2_FAIL 0.000000 +SIM_ARSPD2_FAILP 0.000000 +SIM_ARSPD2_OFS 2013.000000 +SIM_ARSPD2_PITOT 0.000000 +SIM_ARSPD2_RND 2.000000 +SIM_ARSPD_FAIL 0.000000 +SIM_ARSPD_FAILP 0.000000 +SIM_ARSPD_OFS 2013.000000 +SIM_ARSPD_PITOT 0.000000 +SIM_ARSPD_RND 2.000000 +SIM_ARSPD_SIGN 0 +SIM_BAR2_DELAY 0 +SIM_BAR2_DISABLE 0 +SIM_BAR2_DRIFT 0.000000 +SIM_BAR2_FREEZE 0 +SIM_BAR2_GLITCH 0.000000 +SIM_BAR2_RND 0.200000 +SIM_BAR2_WCF_BAK 0.000000 +SIM_BAR2_WCF_FWD 0.000000 +SIM_BAR2_WCF_LFT 0.000000 +SIM_BAR2_WCF_RGT 0.000000 +SIM_BAR3_DELAY 0 +SIM_BAR3_DISABLE 0 +SIM_BAR3_DRIFT 0.000000 +SIM_BAR3_FREEZE 0 +SIM_BAR3_GLITCH 0.000000 +SIM_BAR3_RND 0.200000 +SIM_BAR3_WCF_BAK 0.000000 +SIM_BAR3_WCF_FWD 0.000000 +SIM_BAR3_WCF_LFT 0.000000 +SIM_BAR3_WCF_RGT 0.000000 +SIM_BARO_COUNT 2 +SIM_BARO_DELAY 0 +SIM_BARO_DISABLE 0 +SIM_BARO_DRIFT 0.000000 +SIM_BARO_FREEZE 0 +SIM_BARO_GLITCH 0.000000 +SIM_BARO_RND 0.000000 +SIM_BARO_WCF_BAK 0.000000 +SIM_BARO_WCF_FWD 0.000000 +SIM_BARO_WCF_LFT 0.000000 +SIM_BARO_WCF_RGT 0.000000 +SIM_BATT_CAP_AH 0.000000 +SIM_BATT_VOLTAGE 12.600000 +SIM_BAUDLIMIT_EN 0 +SIM_BZ_ENABLE 0 +SIM_BZ_PIN 0 +SIM_DRIFT_SPEED 0.050000 +SIM_DRIFT_TIME 5.000000 +SIM_EFI_TYPE 0 +SIM_ENGINE_FAIL 0 +SIM_ENGINE_MUL 1.000000 +SIM_FLOAT_EXCEPT 1 +SIM_FLOW_DELAY 0 +SIM_FLOW_ENABLE 0 +SIM_FLOW_POS_X 0.000000 +SIM_FLOW_POS_Y 0.000000 +SIM_FLOW_POS_Z 0.000000 +SIM_FLOW_RATE 10 +SIM_FLOW_RND 0.050000 +SIM_GND_BEHAV -1 +SIM_GPS2_ACC 0.300000 +SIM_GPS2_ALT_OFS 0 +SIM_GPS2_BYTELOS 0.000000 +SIM_GPS2_DELAY 1 +SIM_GPS2_DISABLE 1 +SIM_GPS2_DRFTALT 0.000000 +SIM_GPS2_GLTCH_X 0.000000 +SIM_GPS2_GLTCH_Y 0.000000 +SIM_GPS2_GLTCH_Z 0.000000 +SIM_GPS2_HDG 0 +SIM_GPS2_HZ 5 +SIM_GPS2_LCKTIME 0 +SIM_GPS2_NOISE 0.000000 +SIM_GPS2_NUMSATS 10 +SIM_GPS2_POS_X 0.000000 +SIM_GPS2_POS_Y 0.000000 +SIM_GPS2_POS_Z 0.000000 +SIM_GPS2_TYPE 1 +SIM_GPS2_VERR_X 0.000000 +SIM_GPS2_VERR_Y 0.000000 +SIM_GPS2_VERR_Z 0.000000 +SIM_GPS_ACC 0.300000 +SIM_GPS_ALT_OFS 0 +SIM_GPS_BYTELOSS 0.000000 +SIM_GPS_DELAY 1 +SIM_GPS_DISABLE 0 +SIM_GPS_DRIFTALT 0.000000 +SIM_GPS_GLITCH_X 0.000000 +SIM_GPS_GLITCH_Y 0.000000 +SIM_GPS_GLITCH_Z 0.000000 +SIM_GPS_HDG 0 +SIM_GPS_HZ 5 +SIM_GPS_LOCKTIME 0 +SIM_GPS_NOISE 0.000000 +SIM_GPS_NUMSATS 10 +SIM_GPS_POS_X 0.000000 +SIM_GPS_POS_Y 0.000000 +SIM_GPS_POS_Z 0.000000 +SIM_GPS_TYPE 1 +SIM_GPS_VERR_X 0.000000 +SIM_GPS_VERR_Y 0.000000 +SIM_GPS_VERR_Z 0.000000 +SIM_GRPE_ENABLE 0 +SIM_GRPE_PIN -1 +SIM_GRPS_ENABLE 0 +SIM_GRPS_GRAB 2000 +SIM_GRPS_PIN -1 +SIM_GRPS_RELEASE 1000 +SIM_GRPS_REVERSE 0 +SIM_GYR1_RND 0.000000 +SIM_GYR1_SCALE_X 0.000000 +SIM_GYR1_SCALE_Y 0.000000 +SIM_GYR1_SCALE_Z 0.000000 +SIM_GYR2_RND 0.000000 +SIM_GYR2_SCALE_X 0.000000 +SIM_GYR2_SCALE_Y 0.000000 +SIM_GYR2_SCALE_Z 0.000000 +SIM_GYR3_RND 0.000000 +SIM_GYR3_SCALE_X 0.000000 +SIM_GYR3_SCALE_Y 0.000000 +SIM_GYR3_SCALE_Z 0.000000 +SIM_GYR_FAIL_MSK 0 +SIM_IE24_ENABLE 0 +SIM_IE24_ERROR 0 +SIM_IE24_STATE -1 +SIM_IMUT1_ENABLE 0 +SIM_IMUT2_ENABLE 0 +SIM_IMUT3_ENABLE 0 +SIM_IMUT_END 45.000000 +SIM_IMUT_FIXED 0.000000 +SIM_IMUT_START 25.000000 +SIM_IMUT_TCONST 300.000000 +SIM_IMU_COUNT 2 +SIM_IMU_POS_X 0.000000 +SIM_IMU_POS_Y 0.000000 +SIM_IMU_POS_Z 0.000000 +SIM_INS_THR_MIN 0.100000 +SIM_LED_LAYOUT 0 +SIM_LOOP_DELAY 0 +SIM_MAG1_DEVID 97539 +SIM_MAG1_FAIL 0 +SIM_MAG1_SCALING 1.000000 +SIM_MAG2_DEVID 131874 +SIM_MAG2_DIA_X 0.000000 +SIM_MAG2_DIA_Y 0.000000 +SIM_MAG2_DIA_Z 0.000000 +SIM_MAG2_FAIL 0 +SIM_MAG2_ODI_X 0.000000 +SIM_MAG2_ODI_Y 0.000000 +SIM_MAG2_ODI_Z 0.000000 +SIM_MAG2_OFS_X 5.000000 +SIM_MAG2_OFS_Y 13.000000 +SIM_MAG2_OFS_Z -18.000000 +SIM_MAG2_ORIENT 0 +SIM_MAG2_SCALING 1.000000 +SIM_MAG3_DEVID 263178 +SIM_MAG3_DIA_X 0.000000 +SIM_MAG3_DIA_Y 0.000000 +SIM_MAG3_DIA_Z 0.000000 +SIM_MAG3_FAIL 0 +SIM_MAG3_ODI_X 0.000000 +SIM_MAG3_ODI_Y 0.000000 +SIM_MAG3_ODI_Z 0.000000 +SIM_MAG3_OFS_X 5.000000 +SIM_MAG3_OFS_Y 13.000000 +SIM_MAG3_OFS_Z -18.000000 +SIM_MAG3_ORIENT 0 +SIM_MAG3_SCALING 1.000000 +SIM_MAG4_DEVID 97283 +SIM_MAG5_DEVID 97795 +SIM_MAG6_DEVID 98051 +SIM_MAG7_DEVID 0 +SIM_MAG8_DEVID 0 +SIM_MAG_ALY_HGT 1.000000 +SIM_MAG_ALY_X 0.000000 +SIM_MAG_ALY_Y 0.000000 +SIM_MAG_ALY_Z 0.000000 +SIM_MAG_DELAY 0 +SIM_MAG_DIA_X 0.000000 +SIM_MAG_DIA_Y 0.000000 +SIM_MAG_DIA_Z 0.000000 +SIM_MAG_MOT_X 0.000000 +SIM_MAG_MOT_Y 0.000000 +SIM_MAG_MOT_Z 0.000000 +SIM_MAG_ODI_X 0.000000 +SIM_MAG_ODI_Y 0.000000 +SIM_MAG_ODI_Z 0.000000 +SIM_MAG_OFS_X 5.000000 +SIM_MAG_OFS_Y 13.000000 +SIM_MAG_OFS_Z -18.000000 +SIM_MAG_ORIENT 0 +SIM_MAG_RND 0.000000 +SIM_ODOM_ENABLE 0 +SIM_OPOS_ALT 584.000000 +SIM_OPOS_HDG 353.000000 +SIM_OPOS_LAT -35.363262 +SIM_OPOS_LNG 149.165237 +SIM_PARA_ENABLE 0 +SIM_PARA_PIN -1 +SIM_PIN_MASK 0 +SIM_PLD_ALT_LMT 15.000000 +SIM_PLD_DIST_LMT 10.000000 +SIM_PLD_ENABLE 0 +SIM_PLD_HEIGHT 0.000000 +SIM_PLD_LAT 0.000000 +SIM_PLD_LON 0.000000 +SIM_PLD_RATE 100 +SIM_PLD_TYPE 0 +SIM_PLD_YAW 0 +SIM_RATE_HZ 1200 +SIM_RC_CHANCOUNT 16 +SIM_RC_FAIL 0 +SIM_RICH_CTRL -1 +SIM_RICH_ENABLE 0 +SIM_SAFETY_STATE 0 +SIM_SERVO_SPEED 0.140000 +SIM_SHIP_DSIZE 10.000000 +SIM_SHIP_ENABLE 0 +SIM_SHIP_PSIZE 1000.000000 +SIM_SHIP_SPEED 3.000000 +SIM_SHIP_SYSID 17 +SIM_SHOVE_TIME 0 +SIM_SHOVE_X 0.000000 +SIM_SHOVE_Y 0.000000 +SIM_SHOVE_Z 0.000000 +SIM_SONAR_GLITCH 0.000000 +SIM_SONAR_POS_X 0.000000 +SIM_SONAR_POS_Y 0.000000 +SIM_SONAR_POS_Z 0.000000 +SIM_SONAR_RND 0.000000 +SIM_SONAR_SCALE 12.121200 +SIM_SPEEDUP 1.000000 +SIM_SPR_ENABLE 0 +SIM_SPR_PUMP -1 +SIM_SPR_SPIN -1 +SIM_TA_ENABLE 1 +SIM_TEMP_BFACTOR 0.000000 +SIM_TEMP_FLIGHT 35.000000 +SIM_TEMP_START 25.000000 +SIM_TEMP_TCONST 30.000000 +SIM_TERRAIN 1 +SIM_THML_SCENARI 0 +SIM_TIDE_DIR 0.000000 +SIM_TIDE_SPEED 0.000000 +SIM_TWIST_TIME 0 +SIM_TWIST_X 0.000000 +SIM_TWIST_Y 0.000000 +SIM_TWIST_Z 0.000000 +SIM_VIB_FREQ_X 0.000000 +SIM_VIB_FREQ_Y 0.000000 +SIM_VIB_FREQ_Z 0.000000 +SIM_VIB_MOT_MAX 0.000000 +SIM_VIB_MOT_MULT 1.000000 +SIM_VICON_FAIL 0 +SIM_VICON_GLIT_X 0.000000 +SIM_VICON_GLIT_Y 0.000000 +SIM_VICON_GLIT_Z 0.000000 +SIM_VICON_POS_X 0.000000 +SIM_VICON_POS_Y 0.000000 +SIM_VICON_POS_Z 0.000000 +SIM_VICON_TMASK 3 +SIM_VICON_VGLI_X 0.000000 +SIM_VICON_VGLI_Y 0.000000 +SIM_VICON_VGLI_Z 0.000000 +SIM_VICON_YAW 0 +SIM_VICON_YAWERR 0 +SIM_WAVE_AMP 0.500000 +SIM_WAVE_DIR 0.000000 +SIM_WAVE_ENABLE 0 +SIM_WAVE_LENGTH 10.000000 +SIM_WAVE_SPEED 0.500000 +SIM_WIND_DELAY 0 +SIM_WIND_DIR 180.000000 +SIM_WIND_DIR_Z 0.000000 +SIM_WIND_SPD 0.000000 +SIM_WIND_T 0 +SIM_WIND_TURB 0.000000 +SIM_WIND_T_ALT 60.000000 +SIM_WIND_T_COEF 0.010000 +SIM_WOW_PIN -1 +SPRAY_ENABLE 0 +SR0_ADSB 4 +SR0_EXTRA1 4 +SR0_EXTRA2 4 +SR0_EXTRA3 4 +SR0_EXT_STAT 4 +SR0_PARAMS 0 +SR0_POSITION 4 +SR0_RAW_CTRL 4 +SR0_RAW_SENS 4 +SR0_RC_CHAN 4 +SR1_ADSB 0 +SR1_EXTRA1 0 +SR1_EXTRA2 0 +SR1_EXTRA3 0 +SR1_EXT_STAT 0 +SR1_PARAMS 0 +SR1_POSITION 0 +SR1_RAW_CTRL 0 +SR1_RAW_SENS 0 +SR1_RC_CHAN 0 +SR2_ADSB 0 +SR2_EXTRA1 0 +SR2_EXTRA2 0 +SR2_EXTRA3 0 +SR2_EXT_STAT 0 +SR2_PARAMS 0 +SR2_POSITION 0 +SR2_RAW_CTRL 0 +SR2_RAW_SENS 0 +SR2_RC_CHAN 0 +SR3_ADSB 0 +SR3_EXTRA1 0 +SR3_EXTRA2 0 +SR3_EXTRA3 0 +SR3_EXT_STAT 0 +SR3_PARAMS 0 +SR3_POSITION 0 +SR3_RAW_CTRL 0 +SR3_RAW_SENS 0 +SR3_RC_CHAN 0 +SR4_ADSB 0 +SR4_EXTRA1 0 +SR4_EXTRA2 0 +SR4_EXTRA3 0 +SR4_EXT_STAT 0 +SR4_PARAMS 0 +SR4_POSITION 0 +SR4_RAW_CTRL 0 +SR4_RAW_SENS 0 +SR4_RC_CHAN 0 +SR5_ADSB 0 +SR5_EXTRA1 0 +SR5_EXTRA2 0 +SR5_EXTRA3 0 +SR5_EXT_STAT 0 +SR5_PARAMS 0 +SR5_POSITION 0 +SR5_RAW_CTRL 0 +SR5_RAW_SENS 0 +SR5_RC_CHAN 0 +SR6_ADSB 0 +SR6_EXTRA1 0 +SR6_EXTRA2 0 +SR6_EXTRA3 0 +SR6_EXT_STAT 0 +SR6_PARAMS 0 +SR6_POSITION 0 +SR6_RAW_CTRL 0 +SR6_RAW_SENS 0 +SR6_RC_CHAN 0 +SRTL_ACCURACY 2.000000 +SRTL_OPTIONS 0 +SRTL_POINTS 300 +STAT_BOOTCNT 1 +STAT_FLTTIME 0 +STAT_RESET 0 +STAT_RUNTIME 0 +SUPER_SIMPLE 0 +SYSID_ENFORCE 0 +SYSID_MYGCS 255 +SYSID_THISMAV 1 +TCAL_ENABLED 0 +TELEM_DELAY 0 +TERRAIN_ENABLE 1 +TERRAIN_OPTIONS 0 +TERRAIN_SPACING 100 +THROW_MOT_START 0 +THROW_NEXTMODE 18 +THROW_TYPE 0 +THR_DZ 100 +TUNE 0 +TUNE_MAX 0.000000 +TUNE_MIN 0.000000 +VISO_TYPE 0 +VTX_ENABLE 0 +WINCH_TYPE 0 +WPNAV_ACCEL 100.000000 +WPNAV_ACCEL_Z 100.000000 +WPNAV_RADIUS 200.000000 +WPNAV_RFND_USE 1 +WPNAV_SPEED 500.000000 +WPNAV_SPEED_DN 150.000000 +WPNAV_SPEED_UP 250.000000 +WP_NAVALT_MIN 0.000000 +WP_YAW_BEHAVIOR 2 +ZIGZ_AUTO_ENABLE 0 diff --git a/ArduPlane/mav_0_1.parm b/ArduPlane/mav_0_1.parm index 6d1c6817799f4b..fca32f07e601fd 100644 --- a/ArduPlane/mav_0_1.parm +++ b/ArduPlane/mav_0_1.parm @@ -32,7 +32,7 @@ ARSPD_AUTOCAL 0 ARSPD_BUS 1 ARSPD_FBW_MAX 30 ARSPD_FBW_MIN 10 -ARSPD_OFFSET 0.000000 +ARSPD_OFFSET 2013.474854 ARSPD_OPTIONS 3 ARSPD_PIN 1 ARSPD_PRIMARY 0 @@ -1281,10 +1281,10 @@ SR6_RAW_SENS 1 SR6_RC_CHAN 1 STAB_PITCH_DOWN 2.000000 STALL_PREVENTION 1 -STAT_BOOTCNT 5 +STAT_BOOTCNT 24 STAT_FLTTIME 0 -STAT_RESET 0 -STAT_RUNTIME 0 +STAT_RESET 162617959 +STAT_RUNTIME 45365 STEER2SRV_D 0.005000 STEER2SRV_DRTFCT 10.000000 STEER2SRV_DRTMIN 4500.000000 @@ -1299,7 +1299,7 @@ STICK_MIXING 1 SYSID_ENFORCE 0 SYSID_MYGCS 255 SYSID_THISMAV 1 -SYS_NUM_RESETS 5 +SYS_NUM_RESETS 29 TECS_APPR_SMAX 0.000000 TECS_CLMB_MAX 5.000000 TECS_HGT_OMEGA 3.000000 diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index daedaa10de12f9..5933a7ec852a5a 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -67,6 +67,9 @@ void Plane::init_ardupilot() rpm_sensor.init(); + //initialise AS5600_AOA sensor + aoa_sensor.init(); + // setup telem slots with serial ports gcs().setup_uarts(); diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp index 453a3bb7358c4d..da20899cedc8a8 100644 --- a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp @@ -74,10 +74,10 @@ void AS_5600::init(){ dev = hal.i2c_mgr->get_device(bus, address); - dev->get_semaphore()->take_blocking(); + WITH_SEMAPHORE(dev->get_semaphore()); dev->set_speed(AP_HAL::Device::SPEED_LOW); dev->set_retries(2); - dev->get_semaphore()->give(); + } @@ -124,7 +124,7 @@ unsigned short AS_5600::getRawAngle(void) AP::logger().Write("AoAR", "Status, TimeUS, Angle", "iQH", int(bool(dev)), AP_HAL::micros64(), angle); - return angle; + return angle; } /******************************************************* * Method: highByte @@ -248,7 +248,7 @@ unsigned short AS_5600::setStartPosition(unsigned short startAngle) int AS_5600::readOneByte(uint8_t in_adr) { - WITH_SEMAPHORE(sem); + WITH_SEMAPHORE(dev->get_semaphore()); uint8_t send[1] = {in_adr}; uint8_t recv[1]; @@ -267,7 +267,6 @@ int AS_5600::readOneByte(uint8_t in_adr) *******************************************************/ int AS_5600::readTwoBytes(uint8_t in_adr1, uint8_t in_adr2) { - WITH_SEMAPHORE(sem); int firstResult = readOneByte(in_adr1); int secondResult = readOneByte(in_adr2); diff --git a/libraries/SITL/SIM_AS5600.h b/libraries/SITL/SIM_AS5600.h index b53597bf30e93d..616d6b9a60e53c 100644 --- a/libraries/SITL/SIM_AS5600.h +++ b/libraries/SITL/SIM_AS5600.h @@ -39,6 +39,15 @@ class AS5600 : public I2CDevice, protected I2CRegisters_8Bit void init() override { add_register("ZMCO", AS5600DevReg::ZMCO, O_RDONLY); set_register(AS5600DevReg::ZMCO, (uint8_t)1U); + + add_register("ANGH", AS5600DevReg::RAW_ANGLE_HIGH, O_RDONLY); + set_register(AS5600DevReg::RAW_ANGLE_HIGH, (uint8_t)1U); + + add_register("ANGL", AS5600DevReg::RAW_ANGLE_LOW, O_RDONLY); + set_register(AS5600DevReg::RAW_ANGLE_LOW, (uint8_t)1U); + + + } void update(const class Aircraft &aircraft) override; From f223c832bf9301c9bd7a95144f499029149b7a50 Mon Sep 17 00:00:00 2001 From: colejmero <57465269+colejmero@users.noreply.github.com> Date: Tue, 2 Mar 2021 22:51:53 -0500 Subject: [PATCH 05/10] Removes unecessary code --- ArduPlane/ArduPlane.cpp | 9 ---- ArduPlane/Plane.h | 3 +- libraries/AP_AS5600_AOA/AS5600_AOA.cpp | 60 ++++++++------------------ libraries/AP_AS5600_AOA/AS5600_AOA.h | 10 +---- 4 files changed, 22 insertions(+), 60 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index c7c90aa260fb5b..a2ae61618e3ee0 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -47,7 +47,6 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(update_compass, 10, 200), SCHED_TASK(read_airspeed, 10, 100), SCHED_TASK(read_aoa, 10, 200), - SCHED_TASK(check_aoa, 10, 200),//This line added by Cole SCHED_TASK(update_alt, 10, 200),//This line added by Cole SCHED_TASK(adjust_altitude_target, 10, 200), #if ADVANCED_FAILSAFE == ENABLED @@ -195,14 +194,6 @@ void Plane::update_compass(void) } } -/* - * Check if the AoA sensor is up and running This function is code added by Cole - */ - -void Plane::check_aoa(void){ - aoa_sensor.checkConnect(); -} - /* * Read and log Angle of Attack This function is code added by Cole */ diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 5a5254d1e1dc74..b1778cf3d0a054 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -197,7 +197,7 @@ class Plane : public AP_Vehicle { AP_RPM rpm_sensor; - AS_5600 aoa_sensor; //This line added by Cole + AS5600_AOA aoa_sensor; //This line added by Cole AP_TECS TECS_controller{ahrs, aparm, landing}; AP_L1_Control L1_controller{ahrs, &TECS_controller}; @@ -999,7 +999,6 @@ class Plane : public AP_Vehicle { void read_airspeed(void); void rpm_update(void); void accel_cal_update(void); - void check_aoa(void); //This line added by Cole void read_aoa(void); //This line added by Cole // system.cpp diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp index da20899cedc8a8..8900a94b54df50 100644 --- a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp @@ -20,33 +20,18 @@ extern const AP_HAL::HAL &hal; /**************************************************** - * Method: AS_5600 + * Method: AS5600_AOA * In: none * Out: none * Description: constructor for AMS 5600 ***************************************************/ - AS_5600::AS_5600() + AS5600_AOA::AS5600_AOA() { bus = 1; //Sets the bus number for the device, unclear what this number should be, trial and error to make it work address = 0x36; //This is the I2C address for the device, it is set by the manufacturer - bus_clock = 400000; - use_smbus = false; - timeout_ms = 4; - - //ChibiOS::I2CDeviceManager myDev; //Create an instance of an I2C device - - //busMaskExt = myDev.get_bus_mask_external(); //Used to see what I2C buses exist on the device - //busMaskInt = myDev.get_bus_mask_internal(); //Used to see what I2C buses exist on the device - - //dev = myDev.get_device(bus, address); //Get the specific device which is desired - - - /*load register values*/ - /*c++ class forbids pre loading of variables */ - _zmco = 0x00; _zpos_hi = 0x01; @@ -70,7 +55,7 @@ extern const AP_HAL::HAL &hal; } -void AS_5600::init(){ +void AS5600_AOA::init(){ dev = hal.i2c_mgr->get_device(bus, address); @@ -78,11 +63,10 @@ void AS_5600::init(){ dev->set_speed(AP_HAL::Device::SPEED_LOW); dev->set_retries(2); - } /* mode = 0, output PWM, mode = 1 output analog (full range from 0% to 100% between GND and VDD*/ -void AS_5600::setOutPut(unsigned char mode){ +void AS5600_AOA::setOutPut(unsigned char mode){ unsigned char config_status; config_status = readOneByte(_conf_lo); if(mode == 1){ @@ -102,14 +86,6 @@ void AS_5600::setOutPut(unsigned char mode){ writeOneByte(_conf_lo, config_status); } -void AS_5600::checkConnect(){ - - - - AP::logger().Write("AoAC", "Status, TimeUS, busMaskExt, busMaskInt, checkVal", "iQIIi", int(bool(dev)), AP_HAL::micros64(), busMaskExt, busMaskInt, 42); - - } - /******************************************************* * Method: getRawAngle @@ -118,13 +94,15 @@ void AS_5600::checkConnect(){ * Description: gets raw value of magnet position. * start, end, and max angle settings do not apply *******************************************************/ -unsigned short AS_5600::getRawAngle(void) +unsigned short AS5600_AOA::getRawAngle(void) { - unsigned short angle = readTwoBytes(_raw_ang_hi, _raw_ang_lo); + unsigned short angleRaw = readTwoBytes(_raw_ang_hi, _raw_ang_lo); + + unsigned short angleDegrees = angleRaw*0.087; - AP::logger().Write("AoAR", "Status, TimeUS, Angle", "iQH", int(bool(dev)), AP_HAL::micros64(), angle); + AP::logger().Write("AoAR", "Status, TimeUS, Angle", "iQH", int(bool(dev)), AP_HAL::micros64(), angleDegrees); - return angle; + return angleDegrees; } /******************************************************* * Method: highByte @@ -134,7 +112,7 @@ unsigned short AS_5600::getRawAngle(void) * the leftmost or highest bite *******************************************************/ -unsigned char AS_5600::highByte(unsigned short short_in){ +unsigned char AS5600_AOA::highByte(unsigned short short_in){ unsigned char hiByte = ((short_in >> 8) & 0xff); return hiByte; @@ -148,7 +126,7 @@ unsigned char AS_5600::highByte(unsigned short short_in){ * the rightmost or lowest byte *******************************************************/ -unsigned char AS_5600::lowByte(unsigned short short_in){ +unsigned char AS5600_AOA::lowByte(unsigned short short_in){ unsigned char loByte = (short_in & 0xff); return loByte; @@ -164,7 +142,7 @@ unsigned char AS_5600::lowByte(unsigned short short_in){ * magnet. Setting this register zeros out max position * register. *******************************************************/ -/*unsigned short AS_5600::setMaxAngle(unsigned short newMaxAngle) +/*unsigned short AS5600_AOA::setMaxAngle(unsigned short newMaxAngle) { unsigned short retVal; if(newMaxAngle == -1) @@ -191,7 +169,7 @@ unsigned char AS_5600::lowByte(unsigned short short_in){ * Out: value of max angle register * Description: gets value of maximum angle register. *******************************************************/ -unsigned short AS_5600::getMaxAngle() +unsigned short AS5600_AOA::getMaxAngle() { return readTwoBytes(_mang_hi, _mang_lo); } @@ -206,7 +184,7 @@ unsigned short AS_5600::getMaxAngle() * magnet. *******************************************************/ /* -unsigned short AS_5600::setStartPosition(unsigned short startAngle) +unsigned short AS5600_AOA::setStartPosition(unsigned short startAngle) { if(startAngle == -1) { @@ -226,9 +204,9 @@ unsigned short AS_5600::setStartPosition(unsigned short startAngle) */ -//int AS_5600::writeOneByte(uint8_t in_adr, uint8_t msg){ +//int AS5600_AOA::writeOneByte(uint8_t in_adr, uint8_t msg){ // -// WITH_SEMAPHORE(sem); +// WITH_SEMAPHORE(dev->get_semaphore()); // // send[2] = {in_adr, msg}; // @@ -245,7 +223,7 @@ unsigned short AS_5600::setStartPosition(unsigned short startAngle) * Out: data read from i2c * Description: reads one byte register from i2c *******************************************************/ -int AS_5600::readOneByte(uint8_t in_adr) +int AS5600_AOA::readOneByte(uint8_t in_adr) { WITH_SEMAPHORE(dev->get_semaphore()); @@ -265,7 +243,7 @@ int AS_5600::readOneByte(uint8_t in_adr) * Out: data read from i2c * Description: reads two bytes register from i2c *******************************************************/ -int AS_5600::readTwoBytes(uint8_t in_adr1, uint8_t in_adr2) +int AS5600_AOA::readTwoBytes(uint8_t in_adr1, uint8_t in_adr2) { int firstResult = readOneByte(in_adr1); diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.h b/libraries/AP_AS5600_AOA/AS5600_AOA.h index f6996e907b9eb0..f3c2d70622c80e 100644 --- a/libraries/AP_AS5600_AOA/AS5600_AOA.h +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.h @@ -9,20 +9,17 @@ -class AS_5600 { +class AS5600_AOA { public: - AS_5600(void); + AS5600_AOA(void); void init(); uint32_t busMaskExt; uint32_t busMaskInt; - - void checkConnect(void); - unsigned short setMaxAngle(unsigned short newMaxAngle = -1); unsigned short getMaxAngle(); @@ -93,7 +90,4 @@ class AS_5600 { unsigned char highByte(unsigned short short_in); unsigned char lowByte(unsigned short short_in); - - - }; From f24e11125b17ca24e8ace930b1e2968b70f8e711 Mon Sep 17 00:00:00 2001 From: colejmero <57465269+colejmero@users.noreply.github.com> Date: Fri, 5 Mar 2021 01:21:41 -0500 Subject: [PATCH 06/10] Commit for peter to review --- ArduPlane/ArduPlane.cpp | 13 +- ArduPlane/Plane.h | 6 +- ArduPlane/mav_0_1.parm | 1386 ------------------------ ArduPlane/wscript | 82 +- libraries/AP_AS5600_AOA/AS5600_AOA.cpp | 247 ++--- libraries/AP_AS5600_AOA/AS5600_AOA.h | 109 +- 6 files changed, 189 insertions(+), 1654 deletions(-) delete mode 100644 ArduPlane/mav_0_1.parm diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index a2ae61618e3ee0..0a87e98529bb76 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -46,8 +46,8 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(navigate, 10, 150), SCHED_TASK(update_compass, 10, 200), SCHED_TASK(read_airspeed, 10, 100), - SCHED_TASK(read_aoa, 10, 200), - SCHED_TASK(update_alt, 10, 200),//This line added by Cole + //SCHED_TASK(read_aoa, 10, 200), + SCHED_TASK(update_alt, 10, 200), SCHED_TASK(adjust_altitude_target, 10, 200), #if ADVANCED_FAILSAFE == ENABLED SCHED_TASK(afs_fs_check, 10, 100), @@ -62,6 +62,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(read_rangefinder, 50, 100), SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100), SCHED_TASK_CLASS(Compass, &plane.compass, cal_update, 50, 50), + SCHED_TASK_CLASS(AS5600_AOA, &plane.aoa_sensor, getRawAngle, 10, 200), SCHED_TASK(accel_cal_update, 10, 50), #if OPTFLOW == ENABLED SCHED_TASK_CLASS(OpticalFlow, &plane.optflow, update, 50, 50), @@ -195,12 +196,12 @@ void Plane::update_compass(void) } /* - * Read and log Angle of Attack This function is code added by Cole + * Read and log Angle of Attack */ -void Plane::read_aoa(void){ - aoa_sensor.getRawAngle(); -} +//void Plane::read_aoa(void){ +// aoa_sensor.getRawAngle(); +//} /* do 10Hz logging diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b1778cf3d0a054..086e7e60556d03 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -76,7 +76,7 @@ #include #include -#include //Magnetic Encoder library, line added by Cole +#include #include // Optical Flow library #include @@ -197,7 +197,7 @@ class Plane : public AP_Vehicle { AP_RPM rpm_sensor; - AS5600_AOA aoa_sensor; //This line added by Cole + AS5600_AOA aoa_sensor; AP_TECS TECS_controller{ahrs, aparm, landing}; AP_L1_Control L1_controller{ahrs, &TECS_controller}; @@ -999,7 +999,7 @@ class Plane : public AP_Vehicle { void read_airspeed(void); void rpm_update(void); void accel_cal_update(void); - void read_aoa(void); //This line added by Cole + void read_aoa(void); // system.cpp void init_ardupilot() override; diff --git a/ArduPlane/mav_0_1.parm b/ArduPlane/mav_0_1.parm deleted file mode 100644 index fca32f07e601fd..00000000000000 --- a/ArduPlane/mav_0_1.parm +++ /dev/null @@ -1,1386 +0,0 @@ -ACRO_LOCKING 1 -ACRO_PITCH_RATE 180 -ACRO_ROLL_RATE 180 -ADSB_TYPE 0 -AFS_ENABLE 0 -AHRS_COMP_BETA 0.100000 -AHRS_CUSTOM_PIT 0.000000 -AHRS_CUSTOM_ROLL 0.000000 -AHRS_CUSTOM_YAW 0.000000 -AHRS_EKF_TYPE 3 -AHRS_GPS_GAIN 1.000000 -AHRS_GPS_MINSATS 6 -AHRS_GPS_USE 1 -AHRS_ORIENTATION 0 -AHRS_RP_P 0.200000 -AHRS_TRIM_X 0.000000 -AHRS_TRIM_Y 0.000000 -AHRS_TRIM_Z 0.000000 -AHRS_WIND_MAX 0 -AHRS_YAW_P 0.200000 -ALT_CTRL_ALG 2 -ALT_HOLD_FBWCM 0 -ALT_HOLD_RTL 10000 -ALT_OFFSET 0 -ARMING_ACCTHRESH 0.750000 -ARMING_CHECK 1 -ARMING_MIS_ITEMS 0 -ARMING_REQUIRE 1 -ARMING_RUDDER 1 -ARSPD2_TYPE 0 -ARSPD_AUTOCAL 0 -ARSPD_BUS 1 -ARSPD_FBW_MAX 30 -ARSPD_FBW_MIN 10 -ARSPD_OFFSET 2013.474854 -ARSPD_OPTIONS 3 -ARSPD_PIN 1 -ARSPD_PRIMARY 0 -ARSPD_PSI_RANGE 1.000000 -ARSPD_RATIO 1.993600 -ARSPD_SKIP_CAL 0 -ARSPD_TUBE_ORDER 2 -ARSPD_TYPE 2 -ARSPD_USE 1 -ARSPD_WIND_MAX 0.000000 -ARSPD_WIND_WARN 0.000000 -AUTOTUNE_LEVEL 6 -AUTO_FBW_STEER 0 -AVD_ENABLE 0 -BARO1_DEVID 65540 -BARO1_GND_PRESS 94502.812500 -BARO1_WCF_ENABLE 0 -BARO2_DEVID 65796 -BARO2_GND_PRESS 94503.960938 -BARO2_WCF_ENABLE 0 -BARO3_DEVID 0 -BARO3_GND_PRESS 0.000000 -BARO3_WCF_ENABLE 0 -BARO_ALT_OFFSET 0.000000 -BARO_EXT_BUS -1 -BARO_FLTR_RNG 0 -BARO_GND_TEMP 0.000000 -BARO_PRIMARY 0 -BARO_PROBE_EXT 0 -BATT2_MONITOR 0 -BATT3_MONITOR 0 -BATT4_MONITOR 0 -BATT5_MONITOR 0 -BATT6_MONITOR 0 -BATT7_MONITOR 0 -BATT8_MONITOR 0 -BATT9_MONITOR 0 -BATT_AMP_OFFSET 0.000000 -BATT_AMP_PERVLT 17.000000 -BATT_ARM_MAH 0 -BATT_ARM_VOLT 0.000000 -BATT_BUS 0 -BATT_CAPACITY 3300 -BATT_CRT_MAH 0.000000 -BATT_CRT_VOLT 0.000000 -BATT_CURR_PIN 12 -BATT_FS_CRT_ACT 0 -BATT_FS_LOW_ACT 0 -BATT_FS_VOLTSRC 0 -BATT_LOW_MAH 0.000000 -BATT_LOW_TIMER 10 -BATT_LOW_VOLT 0.000000 -BATT_MONITOR 4 -BATT_OPTIONS 0 -BATT_SERIAL_NUM -1 -BATT_VOLT_MULT 10.100000 -BATT_VOLT_PIN 13 -BATT_WATT_MAX 0 -BRD_ALT_CONFIG 0 -BRD_BOOT_DELAY 0 -BRD_OPTIONS 0 -BRD_PWM_COUNT 8 -BRD_RTC_TYPES 1 -BRD_RTC_TZ_MIN 0 -BRD_SAFETYOPTION 3 -BRD_SERIAL_NUM 0 -BRD_VBUS_MIN 4.300000 -BRD_VSERVO_MIN 0.000000 -BTN_ENABLE 0 -CAM_AUTO_ONLY 0 -CAM_DURATION 10 -CAM_FEEDBACK_PIN -1 -CAM_FEEDBACK_POL 1 -CAM_MAX_ROLL 0 -CAM_MIN_INTERVAL 0 -CAM_RC_TYPE 0 -CAM_RELAY_ON 1 -CAM_SERVO_OFF 1100 -CAM_SERVO_ON 1300 -CAM_TRIGG_DIST 0.000000 -CAM_TRIGG_TYPE 0 -CAM_TYPE 0 -CAN_D1_PROTOCOL 1 -CAN_D2_PROTOCOL 1 -CAN_LOGLEVEL 0 -CAN_P1_DRIVER 0 -CAN_P2_DRIVER 0 -CAN_SLCAN_CPORT 0 -CAN_SLCAN_SDELAY 1 -CAN_SLCAN_SERNUM -1 -CAN_SLCAN_TIMOUT 0 -CHUTE_CHAN 0 -CHUTE_ENABLED 0 -COMPASS_AUTODEC 1 -COMPASS_AUTO_ROT 2 -COMPASS_CAL_FIT 16.000000 -COMPASS_CUS_PIT 0.000000 -COMPASS_CUS_ROLL 0.000000 -COMPASS_CUS_YAW 0.000000 -COMPASS_DEC 0.000000 -COMPASS_DEV_ID 97539 -COMPASS_DEV_ID2 131874 -COMPASS_DEV_ID3 263178 -COMPASS_DEV_ID4 97283 -COMPASS_DEV_ID5 97795 -COMPASS_DEV_ID6 98051 -COMPASS_DEV_ID7 983044 -COMPASS_DEV_ID8 0 -COMPASS_DIA2_X 1.000000 -COMPASS_DIA2_Y 1.000000 -COMPASS_DIA2_Z 1.000000 -COMPASS_DIA3_X 1.000000 -COMPASS_DIA3_Y 1.000000 -COMPASS_DIA3_Z 1.000000 -COMPASS_DIA_X 1.000000 -COMPASS_DIA_Y 1.000000 -COMPASS_DIA_Z 1.000000 -COMPASS_ENABLE 1 -COMPASS_EXTERN2 0 -COMPASS_EXTERN3 0 -COMPASS_EXTERNAL 1 -COMPASS_FLTR_RNG 0 -COMPASS_LEARN 0 -COMPASS_MOT2_X 0.000000 -COMPASS_MOT2_Y 0.000000 -COMPASS_MOT2_Z 0.000000 -COMPASS_MOT3_X 0.000000 -COMPASS_MOT3_Y 0.000000 -COMPASS_MOT3_Z 0.000000 -COMPASS_MOTCT 0 -COMPASS_MOT_X 0.000000 -COMPASS_MOT_Y 0.000000 -COMPASS_MOT_Z 0.000000 -COMPASS_ODI2_X 0.000000 -COMPASS_ODI2_Y 0.000000 -COMPASS_ODI2_Z 0.000000 -COMPASS_ODI3_X 0.000000 -COMPASS_ODI3_Y 0.000000 -COMPASS_ODI3_Z 0.000000 -COMPASS_ODI_X 0.000000 -COMPASS_ODI_Y 0.000000 -COMPASS_ODI_Z 0.000000 -COMPASS_OFFS_MAX 1800 -COMPASS_OFS2_X 5.000000 -COMPASS_OFS2_Y 13.000000 -COMPASS_OFS2_Z -18.000000 -COMPASS_OFS3_X 5.000000 -COMPASS_OFS3_Y 13.000000 -COMPASS_OFS3_Z -18.000000 -COMPASS_OFS_X 5.000000 -COMPASS_OFS_Y 13.000000 -COMPASS_OFS_Z -18.000000 -COMPASS_OPTIONS 0 -COMPASS_ORIENT 0 -COMPASS_ORIENT2 0 -COMPASS_ORIENT3 0 -COMPASS_PMOT_EN 0 -COMPASS_PRIO1_ID 97539 -COMPASS_PRIO2_ID 131874 -COMPASS_PRIO3_ID 263178 -COMPASS_SCALE 1.000000 -COMPASS_SCALE2 1.000000 -COMPASS_SCALE3 1.000000 -COMPASS_TYPEMASK 0 -COMPASS_USE 1 -COMPASS_USE2 1 -COMPASS_USE3 1 -CRASH_ACC_THRESH 0 -CRASH_DETECT 0 -DSPOILER_AILMTCH 100 -DSPOILER_CROW_W1 0 -DSPOILER_CROW_W2 0 -DSPOILER_OPTS 3 -DSPOILR_RUD_RATE 100 -EAHRS_TYPE 0 -EFI_TYPE 0 -EK2_ABIAS_P_NSE 0.005000 -EK2_ACC_P_NSE 0.600000 -EK2_ALT_M_NSE 3.000000 -EK2_ALT_SOURCE 0 -EK2_BCN_DELAY 50 -EK2_BCN_I_GTE 500 -EK2_BCN_M_NSE 1.000000 -EK2_CHECK_SCALE 150 -EK2_EAS_I_GATE 400 -EK2_EAS_M_NSE 1.400000 -EK2_ENABLE 1 -EK2_FLOW_DELAY 10 -EK2_FLOW_I_GATE 500 -EK2_FLOW_M_NSE 0.150000 -EK2_FLOW_USE 2 -EK2_GBIAS_P_NSE 0.000100 -EK2_GLITCH_RAD 25 -EK2_GPS_CHECK 31 -EK2_GPS_TYPE 0 -EK2_GSCL_P_NSE 0.000500 -EK2_GSF_DELAY 1000 -EK2_GSF_RST_MAX 2 -EK2_GSF_RUN_MASK 3 -EK2_GSF_USE_MASK 3 -EK2_GYRO_P_NSE 0.030000 -EK2_HGT_DELAY 60 -EK2_HGT_I_GATE 500 -EK2_HRT_FILT 2.000000 -EK2_IMU_MASK 3 -EK2_MAGB_P_NSE 0.000100 -EK2_MAGE_P_NSE 0.001000 -EK2_MAG_CAL 0 -EK2_MAG_EF_LIM 50 -EK2_MAG_I_GATE 300 -EK2_MAG_MASK 0 -EK2_MAG_M_NSE 0.050000 -EK2_MAX_FLOW 2.500000 -EK2_NOAID_M_NSE 10.000000 -EK2_OGN_HGT_MASK 0 -EK2_POSNE_M_NSE 1.000000 -EK2_POS_I_GATE 500 -EK2_RNG_I_GATE 500 -EK2_RNG_M_NSE 0.500000 -EK2_RNG_USE_HGT -1 -EK2_RNG_USE_SPD 2.000000 -EK2_TAU_OUTPUT 25 -EK2_TERR_GRAD 0.100000 -EK2_VELD_M_NSE 0.700000 -EK2_VELNE_M_NSE 0.500000 -EK2_VEL_I_GATE 500 -EK2_WIND_PSCALE 0.500000 -EK2_WIND_P_NSE 0.100000 -EK2_YAW_I_GATE 300 -EK2_YAW_M_NSE 0.500000 -EK3_ABIAS_P_NSE 0.003000 -EK3_ACC_BIAS_LIM 1.000000 -EK3_ACC_P_NSE 0.350000 -EK3_AFFINITY 0 -EK3_ALT_M_NSE 3.000000 -EK3_BCN_DELAY 50 -EK3_BCN_I_GTE 500 -EK3_BCN_M_NSE 1.000000 -EK3_CHECK_SCALE 150 -EK3_DRAG_BCOEF_X 0.000000 -EK3_DRAG_BCOEF_Y 0.000000 -EK3_DRAG_MCOEF 0.000000 -EK3_DRAG_M_NSE 0.500000 -EK3_EAS_I_GATE 400 -EK3_EAS_M_NSE 1.400000 -EK3_ENABLE 1 -EK3_ERR_THRESH 0.200000 -EK3_FLOW_DELAY 10 -EK3_FLOW_I_GATE 500 -EK3_FLOW_M_NSE 0.150000 -EK3_FLOW_USE 2 -EK3_GBIAS_P_NSE 0.001000 -EK3_GLITCH_RAD 25 -EK3_GPS_CHECK 31 -EK3_GSF_DELAY 1000 -EK3_GSF_RST_MAX 2 -EK3_GSF_RUN_MASK 3 -EK3_GSF_USE_MASK 3 -EK3_GYRO_P_NSE 0.015000 -EK3_HGT_DELAY 60 -EK3_HGT_I_GATE 500 -EK3_HRT_FILT 2.000000 -EK3_IMU_MASK 3 -EK3_MAGB_P_NSE 0.000100 -EK3_MAGE_P_NSE 0.001000 -EK3_MAG_CAL 0 -EK3_MAG_EF_LIM 50 -EK3_MAG_I_GATE 300 -EK3_MAG_MASK 0 -EK3_MAG_M_NSE 0.050000 -EK3_MAX_FLOW 2.500000 -EK3_NOAID_M_NSE 10.000000 -EK3_OGN_HGT_MASK 0 -EK3_POSNE_M_NSE 0.500000 -EK3_POS_I_GATE 500 -EK3_RNG_I_GATE 500 -EK3_RNG_M_NSE 0.500000 -EK3_RNG_USE_HGT -1 -EK3_RNG_USE_SPD 2.000000 -EK3_SRC1_POSXY 3 -EK3_SRC1_POSZ 1 -EK3_SRC1_VELXY 3 -EK3_SRC1_VELZ 3 -EK3_SRC1_YAW 1 -EK3_SRC2_POSXY 0 -EK3_SRC2_POSZ 1 -EK3_SRC2_VELXY 0 -EK3_SRC2_VELZ 0 -EK3_SRC2_YAW 0 -EK3_SRC3_POSXY 0 -EK3_SRC3_POSZ 1 -EK3_SRC3_VELXY 0 -EK3_SRC3_VELZ 0 -EK3_SRC3_YAW 0 -EK3_SRC_OPTIONS 1 -EK3_TAU_OUTPUT 25 -EK3_TERR_GRAD 0.100000 -EK3_VELD_M_NSE 0.700000 -EK3_VELNE_M_NSE 0.500000 -EK3_VEL_I_GATE 500 -EK3_VIS_VERR_MAX 0.900000 -EK3_VIS_VERR_MIN 0.100000 -EK3_WENC_VERR 0.100000 -EK3_WIND_PSCALE 0.500000 -EK3_WIND_P_NSE 0.100000 -EK3_YAW_I_GATE 300 -EK3_YAW_M_NSE 0.500000 -FBWA_TDRAG_CHAN 0 -FBWB_CLIMB_RATE 2 -FBWB_ELEV_REV 0 -FENCE_ACTION 0 -FENCE_AUTOENABLE 0 -FENCE_CHANNEL 0 -FENCE_MAXALT 0 -FENCE_MINALT 0 -FENCE_RETALT 0 -FENCE_RET_RALLY 0 -FENCE_TOTAL 0 -FFT_ENABLE 0 -FLAP_1_PERCNT 0 -FLAP_1_SPEED 0 -FLAP_2_PERCNT 0 -FLAP_2_SPEED 0 -FLAP_SLEWRATE 75 -FLIGHT_OPTIONS 0 -FLOW_ADDR 0 -FLOW_FXSCALER 0 -FLOW_FYSCALER 0 -FLOW_ORIENT_YAW 0 -FLOW_POS_X 0.000000 -FLOW_POS_Y 0.000000 -FLOW_POS_Z 0.000000 -FLOW_TYPE 0 -FLTMODE1 10 -FLTMODE2 11 -FLTMODE3 12 -FLTMODE4 5 -FLTMODE5 2 -FLTMODE6 0 -FLTMODE_CH 8 -FORMAT_VERSION 13 -FRSKY_DNLINK1_ID 20 -FRSKY_DNLINK2_ID 7 -FRSKY_UPLINK_ID 13 -FS_EKF_THRESH 0.800000 -FS_GCS_ENABL 0 -FS_LONG_ACTN 0 -FS_LONG_TIMEOUT 5.000000 -FS_SHORT_ACTN 0 -FS_SHORT_TIMEOUT 1.500000 -FWD_BAT_IDX 0 -FWD_BAT_VOLT_MAX 0.000000 -FWD_BAT_VOLT_MIN 0.000000 -GCS_PID_MASK 0 -GEN_TYPE 0 -GLIDE_SLOPE_MIN 15 -GLIDE_SLOPE_THR 5.000000 -GPS_AUTO_CONFIG 1 -GPS_AUTO_SWITCH 1 -GPS_BLEND_MASK 5 -GPS_BLEND_TC 10.000000 -GPS_COM_PORT 1 -GPS_COM_PORT2 1 -GPS_DELAY_MS 0 -GPS_DELAY_MS2 0 -GPS_DRV_OPTIONS 0 -GPS_GNSS_MODE 0 -GPS_GNSS_MODE2 0 -GPS_INJECT_TO 127 -GPS_MB1_TYPE 0 -GPS_MB2_TYPE 0 -GPS_MIN_DGPS 100 -GPS_MIN_ELEV -100 -GPS_NAVFILTER 8 -GPS_POS1_X 0.000000 -GPS_POS1_Y 0.000000 -GPS_POS1_Z 0.000000 -GPS_POS2_X 0.000000 -GPS_POS2_Y 0.000000 -GPS_POS2_Z 0.000000 -GPS_PRIMARY 0 -GPS_RATE_MS 200 -GPS_RATE_MS2 200 -GPS_RAW_DATA 0 -GPS_SAVE_CFG 2 -GPS_SBAS_MODE 2 -GPS_SBP_LOGMASK -256 -GPS_TYPE 1 -GPS_TYPE2 0 -GRIP_ENABLE 0 -GROUND_STEER_ALT 0.000000 -GROUND_STEER_DPS 90 -GUIDED_D 0.000000 -GUIDED_FF 0.000000 -GUIDED_FLTD 5.000000 -GUIDED_FLTE 5.000000 -GUIDED_FLTT 5.000000 -GUIDED_I 0.000000 -GUIDED_IMAX 10.000000 -GUIDED_P 5000.000000 -GUIDED_SMAX 0.000000 -HIL_ERR_LIMIT 5.000000 -HIL_MODE 0 -HIL_SERVOS 0 -HOME_RESET_ALT 0 -ICE_ENABLE 0 -INITIAL_MODE 0 -INS_ACC1_CALTEMP -300.000000 -INS_ACC2OFFS_X 0.001000 -INS_ACC2OFFS_Y 0.001000 -INS_ACC2OFFS_Z 0.001000 -INS_ACC2SCAL_X 1.001000 -INS_ACC2SCAL_Y 1.001000 -INS_ACC2SCAL_Z 1.001000 -INS_ACC2_CALTEMP -300.000000 -INS_ACC2_ID 2753036 -INS_ACC3OFFS_X 0.000000 -INS_ACC3OFFS_Y 0.000000 -INS_ACC3OFFS_Z 0.000000 -INS_ACC3SCAL_X 0.000000 -INS_ACC3SCAL_Y 0.000000 -INS_ACC3SCAL_Z 0.000000 -INS_ACC3_CALTEMP -300.000000 -INS_ACC3_ID 0 -INS_ACCEL_FILTER 20 -INS_ACCOFFS_X 0.001000 -INS_ACCOFFS_Y 0.001000 -INS_ACCOFFS_Z 0.001000 -INS_ACCSCAL_X 1.001000 -INS_ACCSCAL_Y 1.001000 -INS_ACCSCAL_Z 1.001000 -INS_ACC_BODYFIX 2 -INS_ACC_ID 2753028 -INS_ENABLE_MASK 127 -INS_FAST_SAMPLE 1 -INS_GYR1_CALTEMP -300.000000 -INS_GYR2OFFS_X 0.000000 -INS_GYR2OFFS_Y 0.000000 -INS_GYR2OFFS_Z 0.000000 -INS_GYR2_CALTEMP -300.000000 -INS_GYR2_ID 2752780 -INS_GYR3OFFS_X 0.000000 -INS_GYR3OFFS_Y 0.000000 -INS_GYR3OFFS_Z 0.000000 -INS_GYR3_CALTEMP -300.000000 -INS_GYR3_ID 0 -INS_GYROFFS_X 0.000000 -INS_GYROFFS_Y 0.000000 -INS_GYROFFS_Z 0.000000 -INS_GYRO_FILTER 20 -INS_GYRO_RATE 0 -INS_GYR_CAL 0 -INS_GYR_ID 2752772 -INS_HNTCH_ENABLE 0 -INS_LOG_BAT_CNT 1024 -INS_LOG_BAT_LGCT 32 -INS_LOG_BAT_LGIN 20 -INS_LOG_BAT_MASK 0 -INS_LOG_BAT_OPT 0 -INS_NOTCH_ENABLE 0 -INS_POS1_X 0.000000 -INS_POS1_Y 0.000000 -INS_POS1_Z 0.000000 -INS_POS2_X 0.000000 -INS_POS2_Y 0.000000 -INS_POS2_Z 0.000000 -INS_POS3_X 0.000000 -INS_POS3_Y 0.000000 -INS_POS3_Z 0.000000 -INS_STILL_THRESH 0.100000 -INS_TCAL1_ENABLE 0 -INS_TCAL2_ENABLE 0 -INS_TCAL3_ENABLE 0 -INS_TCAL_OPTIONS 0 -INS_TRIM_OPTION 1 -INS_USE 1 -INS_USE2 1 -INS_USE3 1 -KFF_RDDRMIX 0.500000 -KFF_THR2PTCH 0.000000 -LAND_ABORT_DEG 0.000000 -LAND_ABORT_THR 0 -LAND_DISARMDELAY 20 -LAND_DS_ABORTALT 0.000000 -LAND_DS_AIL_SCL 1.000000 -LAND_DS_APP_EXT 50.000000 -LAND_DS_ARSP_MAX 15.000000 -LAND_DS_ARSP_MIN 10.000000 -LAND_DS_D 0.000000 -LAND_DS_ELEV_PWM 1500 -LAND_DS_I 0.000000 -LAND_DS_IMAX 0 -LAND_DS_L1 30.000000 -LAND_DS_L1_I 0.000000 -LAND_DS_L1_TCON 0.400000 -LAND_DS_P 0.000000 -LAND_DS_SLEW_SPD 0.500000 -LAND_DS_SLOPE_A 1.000000 -LAND_DS_SLOPE_B 1.000000 -LAND_DS_V_DWN 2.000000 -LAND_DS_V_FWD 1.000000 -LAND_DS_YAW_LIM 10.000000 -LAND_FLAP_PERCNT 0 -LAND_FLARE_ALT 3.000000 -LAND_FLARE_SEC 5.000000 -LAND_OPTIONS 0 -LAND_PF_ALT 10.000000 -LAND_PF_ARSPD 0.000000 -LAND_PF_SEC 6.000000 -LAND_PITCH_CD 100 -LAND_SLOPE_RCALC 2.000000 -LAND_THEN_NEUTRL 0 -LAND_THR_SLEW 0 -LAND_TYPE 0 -LEVEL_ROLL_LIMIT 5 -LGR_DEPLOY_ALT 0 -LGR_DEPLOY_PIN -1 -LGR_DEPLOY_POL 0 -LGR_OPTIONS 3 -LGR_RETRACT_ALT 0 -LGR_STARTUP 0 -LGR_WOW_PIN 8 -LGR_WOW_POL 1 -LIM_PITCH_MAX 2500 -LIM_PITCH_MIN -2000 -LIM_ROLL_CD 6500 -LOG_BACKEND_TYPE 1 -LOG_BITMASK 65535 -LOG_DISARMED 0 -LOG_FILE_BUFSIZE 200 -LOG_FILE_DSRMROT 0 -LOG_FILE_MB_FREE 500 -LOG_FILE_TIMEOUT 5 -LOG_MAV_BUFSIZE 8 -LOG_REPLAY 0 -MANUAL_RCMASK 0 -MIN_GNDSPD_CM 0 -MIS_OPTIONS 0 -MIS_RESTART 0 -MIS_TOTAL 0 -MIXING_GAIN 0.500000 -MIXING_OFFSET 0 -MNT_ANGMAX_PAN 4500 -MNT_ANGMAX_ROL 4500 -MNT_ANGMAX_TIL 4500 -MNT_ANGMIN_PAN -4500 -MNT_ANGMIN_ROL -4500 -MNT_ANGMIN_TIL -4500 -MNT_DEFLT_MODE 3 -MNT_JSTICK_SPD 0 -MNT_LEAD_PTCH 0.000000 -MNT_LEAD_RLL 0.000000 -MNT_NEUTRAL_X 0.000000 -MNT_NEUTRAL_Y 0.000000 -MNT_NEUTRAL_Z 0.000000 -MNT_RC_IN_PAN 0 -MNT_RC_IN_ROLL 0 -MNT_RC_IN_TILT 0 -MNT_RETRACT_X 0.000000 -MNT_RETRACT_Y 0.000000 -MNT_RETRACT_Z 0.000000 -MNT_STAB_PAN 0 -MNT_STAB_ROLL 0 -MNT_STAB_TILT 0 -MNT_TYPE 0 -MSP_OPTIONS 0 -MSP_OSD_NCELLS 0 -NAVL1_DAMPING 0.750000 -NAVL1_LIM_BANK 0.000000 -NAVL1_PERIOD 15.000000 -NAVL1_XTRACK_I 0.020000 -NAV_CONTROLLER 1 -NTF_BUZZ_ENABLE 1 -NTF_BUZZ_ON_LVL 1 -NTF_BUZZ_PIN 0 -NTF_BUZZ_VOLUME 100 -NTF_DISPLAY_TYPE 0 -NTF_LED_BRIGHT 3 -NTF_LED_LEN 1 -NTF_LED_OVERRIDE 0 -NTF_LED_TYPES 199 -NTF_OREO_THEME 0 -OSD_TYPE 0 -PTCH2SRV_D 0.200000 -PTCH2SRV_FF 0.000000 -PTCH2SRV_I 0.300000 -PTCH2SRV_IMAX 3000 -PTCH2SRV_P 2.500000 -PTCH2SRV_RLL 1.000000 -PTCH2SRV_RMAX_DN 0 -PTCH2SRV_RMAX_UP 0 -PTCH2SRV_SRMAX 150.000000 -PTCH2SRV_SRTAU 1.000000 -PTCH2SRV_TCONST 0.500000 -Q_ENABLE 0 -RALLY_INCL_HOME 0 -RALLY_LIMIT_KM 5.000000 -RALLY_TOTAL 0 -RC10_DZ 0 -RC10_MAX 1900 -RC10_MIN 1100 -RC10_OPTION 0 -RC10_REVERSED 0 -RC10_TRIM 1500 -RC11_DZ 0 -RC11_MAX 1900 -RC11_MIN 1100 -RC11_OPTION 0 -RC11_REVERSED 0 -RC11_TRIM 1500 -RC12_DZ 0 -RC12_MAX 1900 -RC12_MIN 1100 -RC12_OPTION 0 -RC12_REVERSED 0 -RC12_TRIM 1500 -RC13_DZ 0 -RC13_MAX 1900 -RC13_MIN 1100 -RC13_OPTION 0 -RC13_REVERSED 0 -RC13_TRIM 1500 -RC14_DZ 0 -RC14_MAX 1900 -RC14_MIN 1100 -RC14_OPTION 0 -RC14_REVERSED 0 -RC14_TRIM 1500 -RC15_DZ 0 -RC15_MAX 1900 -RC15_MIN 1100 -RC15_OPTION 0 -RC15_REVERSED 0 -RC15_TRIM 1500 -RC16_DZ 0 -RC16_MAX 1900 -RC16_MIN 1100 -RC16_OPTION 0 -RC16_REVERSED 0 -RC16_TRIM 1500 -RC1_DZ 30 -RC1_MAX 2000 -RC1_MIN 1000 -RC1_OPTION 0 -RC1_REVERSED 0 -RC1_TRIM 1500 -RC2_DZ 30 -RC2_MAX 2000 -RC2_MIN 1000 -RC2_OPTION 0 -RC2_REVERSED 0 -RC2_TRIM 1500 -RC3_DZ 30 -RC3_MAX 2000 -RC3_MIN 1000 -RC3_OPTION 0 -RC3_REVERSED 0 -RC3_TRIM 1000 -RC4_DZ 30 -RC4_MAX 2000 -RC4_MIN 1000 -RC4_OPTION 0 -RC4_REVERSED 0 -RC4_TRIM 1500 -RC5_DZ 0 -RC5_MAX 2000 -RC5_MIN 1000 -RC5_OPTION 0 -RC5_REVERSED 0 -RC5_TRIM 1500 -RC6_DZ 0 -RC6_MAX 2000 -RC6_MIN 1000 -RC6_OPTION 0 -RC6_REVERSED 0 -RC6_TRIM 1500 -RC7_DZ 0 -RC7_MAX 2000 -RC7_MIN 1000 -RC7_OPTION 0 -RC7_REVERSED 0 -RC7_TRIM 1500 -RC8_DZ 0 -RC8_MAX 2000 -RC8_MIN 1000 -RC8_OPTION 0 -RC8_REVERSED 0 -RC8_TRIM 1500 -RC9_DZ 0 -RC9_MAX 1900 -RC9_MIN 1100 -RC9_OPTION 0 -RC9_REVERSED 0 -RC9_TRIM 1500 -RCMAP_PITCH 2 -RCMAP_ROLL 1 -RCMAP_THROTTLE 3 -RCMAP_YAW 4 -RC_OPTIONS 32 -RC_OVERRIDE_TIME 3.000000 -RC_PROTOCOLS 1 -RELAY_DEFAULT 0 -RELAY_PIN 13 -RELAY_PIN2 -1 -RELAY_PIN3 -1 -RELAY_PIN4 -1 -RELAY_PIN5 -1 -RELAY_PIN6 -1 -RLL2SRV_D 0.200000 -RLL2SRV_FF 0.000000 -RLL2SRV_I 0.300000 -RLL2SRV_IMAX 3000 -RLL2SRV_P 2.500000 -RLL2SRV_RMAX 0 -RLL2SRV_SRMAX 150.000000 -RLL2SRV_SRTAU 1.000000 -RLL2SRV_TCONST 0.500000 -RNGFND1_TYPE 0 -RNGFND2_TYPE 0 -RNGFND3_TYPE 0 -RNGFND4_TYPE 0 -RNGFND5_TYPE 0 -RNGFND6_TYPE 0 -RNGFND7_TYPE 0 -RNGFND8_TYPE 0 -RNGFND9_TYPE 0 -RNGFNDA_TYPE 0 -RNGFND_LANDING 0 -RPM2_PIN -1 -RPM2_SCALING 1.000000 -RPM2_TYPE 0 -RPM_MAX 100000.000000 -RPM_MIN 10.000000 -RPM_MIN_QUAL 0.500000 -RPM_PIN 54 -RPM_SCALING 1.000000 -RPM_TYPE 0 -RSSI_TYPE 0 -RST_MISSION_CH 0 -RST_SWITCH_CH 0 -RTL_AUTOLAND 0 -RTL_CLIMB_MIN 0 -RTL_RADIUS 0 -RUDDER_ONLY 0 -RUDD_DT_GAIN 10 -SCALING_SPEED 15.000000 -SCHED_DEBUG 0 -SCHED_LOOP_RATE 50 -SCHED_OPTIONS 0 -SCR_ENABLE 0 -SERIAL0_BAUD 115 -SERIAL0_PROTOCOL 2 -SERIAL1_BAUD 57 -SERIAL1_OPTIONS 0 -SERIAL1_PROTOCOL 2 -SERIAL2_BAUD 57 -SERIAL2_OPTIONS 0 -SERIAL2_PROTOCOL 2 -SERIAL3_BAUD 38 -SERIAL3_OPTIONS 0 -SERIAL3_PROTOCOL 5 -SERIAL4_BAUD 38 -SERIAL4_OPTIONS 0 -SERIAL4_PROTOCOL 5 -SERIAL5_BAUD 57 -SERIAL5_OPTIONS 0 -SERIAL5_PROTOCOL -1 -SERIAL6_BAUD 57 -SERIAL6_OPTIONS 0 -SERIAL6_PROTOCOL -1 -SERIAL7_BAUD 57 -SERIAL7_OPTIONS 0 -SERIAL7_PROTOCOL -1 -SERIAL_PASS1 0 -SERIAL_PASS2 -1 -SERIAL_PASSTIMO 15 -SERVO10_FUNCTION 0 -SERVO10_MAX 1900 -SERVO10_MIN 1100 -SERVO10_REVERSED 0 -SERVO10_TRIM 1500 -SERVO11_FUNCTION 0 -SERVO11_MAX 1900 -SERVO11_MIN 1100 -SERVO11_REVERSED 0 -SERVO11_TRIM 1500 -SERVO12_FUNCTION 0 -SERVO12_MAX 1900 -SERVO12_MIN 1100 -SERVO12_REVERSED 0 -SERVO12_TRIM 1500 -SERVO13_FUNCTION 0 -SERVO13_MAX 1900 -SERVO13_MIN 1100 -SERVO13_REVERSED 0 -SERVO13_TRIM 1500 -SERVO14_FUNCTION 0 -SERVO14_MAX 1900 -SERVO14_MIN 1100 -SERVO14_REVERSED 0 -SERVO14_TRIM 1500 -SERVO15_FUNCTION 0 -SERVO15_MAX 1900 -SERVO15_MIN 1100 -SERVO15_REVERSED 0 -SERVO15_TRIM 1500 -SERVO16_FUNCTION 0 -SERVO16_MAX 1900 -SERVO16_MIN 1100 -SERVO16_REVERSED 0 -SERVO16_TRIM 1500 -SERVO1_FUNCTION 4 -SERVO1_MAX 1900 -SERVO1_MIN 1100 -SERVO1_REVERSED 0 -SERVO1_TRIM 1500 -SERVO2_FUNCTION 19 -SERVO2_MAX 1900 -SERVO2_MIN 1100 -SERVO2_REVERSED 0 -SERVO2_TRIM 1500 -SERVO3_FUNCTION 70 -SERVO3_MAX 2000 -SERVO3_MIN 1000 -SERVO3_REVERSED 0 -SERVO3_TRIM 1000 -SERVO4_FUNCTION 21 -SERVO4_MAX 1900 -SERVO4_MIN 1100 -SERVO4_REVERSED 0 -SERVO4_TRIM 1500 -SERVO5_FUNCTION 0 -SERVO5_MAX 1900 -SERVO5_MIN 1100 -SERVO5_REVERSED 0 -SERVO5_TRIM 1500 -SERVO6_FUNCTION 0 -SERVO6_MAX 1900 -SERVO6_MIN 1100 -SERVO6_REVERSED 0 -SERVO6_TRIM 1500 -SERVO7_FUNCTION 0 -SERVO7_MAX 1900 -SERVO7_MIN 1100 -SERVO7_REVERSED 0 -SERVO7_TRIM 1500 -SERVO8_FUNCTION 0 -SERVO8_MAX 1900 -SERVO8_MIN 1100 -SERVO8_REVERSED 0 -SERVO8_TRIM 1500 -SERVO9_FUNCTION 0 -SERVO9_MAX 1900 -SERVO9_MIN 1100 -SERVO9_REVERSED 0 -SERVO9_TRIM 1500 -SERVO_AUTO_TRIM 0 -SERVO_RATE 50 -SERVO_ROB_POSMAX 4095 -SERVO_ROB_POSMIN 0 -SERVO_SBUS_RATE 50 -SERVO_VOLZ_MASK 0 -SIM_ACC1_BIAS_X 0.000000 -SIM_ACC1_BIAS_Y 0.000000 -SIM_ACC1_BIAS_Z 0.000000 -SIM_ACC1_RND 0.000000 -SIM_ACC1_SCAL_X 0.000000 -SIM_ACC1_SCAL_Y 0.000000 -SIM_ACC1_SCAL_Z 0.000000 -SIM_ACC2_BIAS_X 0.000000 -SIM_ACC2_BIAS_Y 0.000000 -SIM_ACC2_BIAS_Z 0.000000 -SIM_ACC2_RND 0.000000 -SIM_ACC2_SCAL_X 0.000000 -SIM_ACC2_SCAL_Y 0.000000 -SIM_ACC2_SCAL_Z 0.000000 -SIM_ACC3_BIAS_X 0.000000 -SIM_ACC3_BIAS_Y 0.000000 -SIM_ACC3_BIAS_Z 0.000000 -SIM_ACC3_RND 0.000000 -SIM_ACC3_SCAL_X 0.000000 -SIM_ACC3_SCAL_Y 0.000000 -SIM_ACC3_SCAL_Z 0.000000 -SIM_ACCEL1_FAIL 0.000000 -SIM_ACCEL2_FAIL 0.000000 -SIM_ACCEL3_FAIL 0.000000 -SIM_ACC_FAIL_MSK 0 -SIM_ACC_TRIM_X 0.000000 -SIM_ACC_TRIM_Y 0.000000 -SIM_ACC_TRIM_Z 0.000000 -SIM_ADSB_ALT 1000.000000 -SIM_ADSB_COUNT -1 -SIM_ADSB_RADIUS 10000.000000 -SIM_ADSB_TX 0 -SIM_ARSPD2_FAIL 0.000000 -SIM_ARSPD2_FAILP 0.000000 -SIM_ARSPD2_OFS 2013.000000 -SIM_ARSPD2_PITOT 0.000000 -SIM_ARSPD2_RND 2.000000 -SIM_ARSPD_FAIL 0.000000 -SIM_ARSPD_FAILP 0.000000 -SIM_ARSPD_OFS 2013.000000 -SIM_ARSPD_PITOT 0.000000 -SIM_ARSPD_RND 2.000000 -SIM_ARSPD_SIGN 0 -SIM_BAR2_DELAY 0 -SIM_BAR2_DISABLE 0 -SIM_BAR2_DRIFT 0.000000 -SIM_BAR2_FREEZE 0 -SIM_BAR2_GLITCH 0.000000 -SIM_BAR2_RND 0.200000 -SIM_BAR2_WCF_BAK 0.000000 -SIM_BAR2_WCF_FWD 0.000000 -SIM_BAR2_WCF_LFT 0.000000 -SIM_BAR2_WCF_RGT 0.000000 -SIM_BAR3_DELAY 0 -SIM_BAR3_DISABLE 0 -SIM_BAR3_DRIFT 0.000000 -SIM_BAR3_FREEZE 0 -SIM_BAR3_GLITCH 0.000000 -SIM_BAR3_RND 0.200000 -SIM_BAR3_WCF_BAK 0.000000 -SIM_BAR3_WCF_FWD 0.000000 -SIM_BAR3_WCF_LFT 0.000000 -SIM_BAR3_WCF_RGT 0.000000 -SIM_BARO_COUNT 2 -SIM_BARO_DELAY 0 -SIM_BARO_DISABLE 0 -SIM_BARO_DRIFT 0.000000 -SIM_BARO_FREEZE 0 -SIM_BARO_GLITCH 0.000000 -SIM_BARO_RND 0.200000 -SIM_BARO_WCF_BAK 0.000000 -SIM_BARO_WCF_FWD 0.000000 -SIM_BARO_WCF_LFT 0.000000 -SIM_BARO_WCF_RGT 0.000000 -SIM_BATT_CAP_AH 0.000000 -SIM_BATT_VOLTAGE 12.600000 -SIM_BAUDLIMIT_EN 0 -SIM_BZ_ENABLE 0 -SIM_BZ_PIN 0 -SIM_DRIFT_SPEED 0.050000 -SIM_DRIFT_TIME 5.000000 -SIM_EFI_TYPE 0 -SIM_ENGINE_FAIL 0 -SIM_ENGINE_MUL 1.000000 -SIM_FLOAT_EXCEPT 1 -SIM_FLOW_DELAY 0 -SIM_FLOW_ENABLE 0 -SIM_FLOW_POS_X 0.000000 -SIM_FLOW_POS_Y 0.000000 -SIM_FLOW_POS_Z 0.000000 -SIM_FLOW_RATE 10 -SIM_FLOW_RND 0.050000 -SIM_GND_BEHAV -1 -SIM_GPS2_ACC 0.300000 -SIM_GPS2_ALT_OFS 0 -SIM_GPS2_BYTELOS 0.000000 -SIM_GPS2_DELAY 1 -SIM_GPS2_DISABLE 1 -SIM_GPS2_DRFTALT 0.000000 -SIM_GPS2_GLTCH_X 0.000000 -SIM_GPS2_GLTCH_Y 0.000000 -SIM_GPS2_GLTCH_Z 0.000000 -SIM_GPS2_HDG 0 -SIM_GPS2_HZ 5 -SIM_GPS2_LCKTIME 0 -SIM_GPS2_NOISE 0.000000 -SIM_GPS2_NUMSATS 10 -SIM_GPS2_POS_X 0.000000 -SIM_GPS2_POS_Y 0.000000 -SIM_GPS2_POS_Z 0.000000 -SIM_GPS2_TYPE 1 -SIM_GPS2_VERR_X 0.000000 -SIM_GPS2_VERR_Y 0.000000 -SIM_GPS2_VERR_Z 0.000000 -SIM_GPS_ACC 0.300000 -SIM_GPS_ALT_OFS 0 -SIM_GPS_BYTELOSS 0.000000 -SIM_GPS_DELAY 1 -SIM_GPS_DISABLE 0 -SIM_GPS_DRIFTALT 0.000000 -SIM_GPS_GLITCH_X 0.000000 -SIM_GPS_GLITCH_Y 0.000000 -SIM_GPS_GLITCH_Z 0.000000 -SIM_GPS_HDG 0 -SIM_GPS_HZ 5 -SIM_GPS_LOCKTIME 0 -SIM_GPS_NOISE 0.000000 -SIM_GPS_NUMSATS 10 -SIM_GPS_POS_X 0.000000 -SIM_GPS_POS_Y 0.000000 -SIM_GPS_POS_Z 0.000000 -SIM_GPS_TYPE 1 -SIM_GPS_VERR_X 0.000000 -SIM_GPS_VERR_Y 0.000000 -SIM_GPS_VERR_Z 0.000000 -SIM_GRPE_ENABLE 0 -SIM_GRPE_PIN -1 -SIM_GRPS_ENABLE 0 -SIM_GRPS_GRAB 2000 -SIM_GRPS_PIN -1 -SIM_GRPS_RELEASE 1000 -SIM_GRPS_REVERSE 0 -SIM_GYR1_RND 0.000000 -SIM_GYR1_SCALE_X 0.000000 -SIM_GYR1_SCALE_Y 0.000000 -SIM_GYR1_SCALE_Z 0.000000 -SIM_GYR2_RND 0.000000 -SIM_GYR2_SCALE_X 0.000000 -SIM_GYR2_SCALE_Y 0.000000 -SIM_GYR2_SCALE_Z 0.000000 -SIM_GYR3_RND 0.000000 -SIM_GYR3_SCALE_X 0.000000 -SIM_GYR3_SCALE_Y 0.000000 -SIM_GYR3_SCALE_Z 0.000000 -SIM_GYR_FAIL_MSK 0 -SIM_IE24_ENABLE 0 -SIM_IE24_ERROR 0 -SIM_IE24_STATE -1 -SIM_IMUT1_ENABLE 0 -SIM_IMUT2_ENABLE 0 -SIM_IMUT3_ENABLE 0 -SIM_IMUT_END 45.000000 -SIM_IMUT_FIXED 0.000000 -SIM_IMUT_START 25.000000 -SIM_IMUT_TCONST 300.000000 -SIM_IMU_COUNT 2 -SIM_IMU_POS_X 0.000000 -SIM_IMU_POS_Y 0.000000 -SIM_IMU_POS_Z 0.000000 -SIM_INS_THR_MIN 0.100000 -SIM_LED_LAYOUT 0 -SIM_LOOP_DELAY 0 -SIM_MAG1_DEVID 97539 -SIM_MAG1_FAIL 0 -SIM_MAG1_SCALING 1.000000 -SIM_MAG2_DEVID 131874 -SIM_MAG2_DIA_X 0.000000 -SIM_MAG2_DIA_Y 0.000000 -SIM_MAG2_DIA_Z 0.000000 -SIM_MAG2_FAIL 0 -SIM_MAG2_ODI_X 0.000000 -SIM_MAG2_ODI_Y 0.000000 -SIM_MAG2_ODI_Z 0.000000 -SIM_MAG2_OFS_X 5.000000 -SIM_MAG2_OFS_Y 13.000000 -SIM_MAG2_OFS_Z -18.000000 -SIM_MAG2_ORIENT 0 -SIM_MAG2_SCALING 1.000000 -SIM_MAG3_DEVID 263178 -SIM_MAG3_DIA_X 0.000000 -SIM_MAG3_DIA_Y 0.000000 -SIM_MAG3_DIA_Z 0.000000 -SIM_MAG3_FAIL 0 -SIM_MAG3_ODI_X 0.000000 -SIM_MAG3_ODI_Y 0.000000 -SIM_MAG3_ODI_Z 0.000000 -SIM_MAG3_OFS_X 5.000000 -SIM_MAG3_OFS_Y 13.000000 -SIM_MAG3_OFS_Z -18.000000 -SIM_MAG3_ORIENT 0 -SIM_MAG3_SCALING 1.000000 -SIM_MAG4_DEVID 97283 -SIM_MAG5_DEVID 97795 -SIM_MAG6_DEVID 98051 -SIM_MAG7_DEVID 0 -SIM_MAG8_DEVID 0 -SIM_MAG_ALY_HGT 1.000000 -SIM_MAG_ALY_X 0.000000 -SIM_MAG_ALY_Y 0.000000 -SIM_MAG_ALY_Z 0.000000 -SIM_MAG_DELAY 0 -SIM_MAG_DIA_X 0.000000 -SIM_MAG_DIA_Y 0.000000 -SIM_MAG_DIA_Z 0.000000 -SIM_MAG_MOT_X 0.000000 -SIM_MAG_MOT_Y 0.000000 -SIM_MAG_MOT_Z 0.000000 -SIM_MAG_ODI_X 0.000000 -SIM_MAG_ODI_Y 0.000000 -SIM_MAG_ODI_Z 0.000000 -SIM_MAG_OFS_X 5.000000 -SIM_MAG_OFS_Y 13.000000 -SIM_MAG_OFS_Z -18.000000 -SIM_MAG_ORIENT 0 -SIM_MAG_RND 0.000000 -SIM_ODOM_ENABLE 0 -SIM_OPOS_ALT 584.000000 -SIM_OPOS_HDG 353.000000 -SIM_OPOS_LAT -35.363262 -SIM_OPOS_LNG 149.165237 -SIM_PARA_ENABLE 0 -SIM_PARA_PIN -1 -SIM_PIN_MASK 0 -SIM_PLD_ALT_LMT 15.000000 -SIM_PLD_DIST_LMT 10.000000 -SIM_PLD_ENABLE 0 -SIM_PLD_HEIGHT 0.000000 -SIM_PLD_LAT 0.000000 -SIM_PLD_LON 0.000000 -SIM_PLD_RATE 100 -SIM_PLD_TYPE 0 -SIM_PLD_YAW 0 -SIM_RATE_HZ 1200 -SIM_RC_CHANCOUNT 16 -SIM_RC_FAIL 0 -SIM_RICH_CTRL -1 -SIM_RICH_ENABLE 0 -SIM_SAFETY_STATE 0 -SIM_SERVO_SPEED 0.140000 -SIM_SHIP_DSIZE 10.000000 -SIM_SHIP_ENABLE 0 -SIM_SHIP_PSIZE 1000.000000 -SIM_SHIP_SPEED 3.000000 -SIM_SHIP_SYSID 17 -SIM_SHOVE_TIME 0 -SIM_SHOVE_X 0.000000 -SIM_SHOVE_Y 0.000000 -SIM_SHOVE_Z 0.000000 -SIM_SONAR_GLITCH 0.000000 -SIM_SONAR_POS_X 0.000000 -SIM_SONAR_POS_Y 0.000000 -SIM_SONAR_POS_Z 0.000000 -SIM_SONAR_RND 0.000000 -SIM_SONAR_SCALE 12.121200 -SIM_SPEEDUP 1.000000 -SIM_SPR_ENABLE 0 -SIM_SPR_PUMP -1 -SIM_SPR_SPIN -1 -SIM_TA_ENABLE 1 -SIM_TEMP_BFACTOR 0.000000 -SIM_TEMP_FLIGHT 35.000000 -SIM_TEMP_START 25.000000 -SIM_TEMP_TCONST 30.000000 -SIM_TERRAIN 1 -SIM_THML_SCENARI 0 -SIM_TIDE_DIR 0.000000 -SIM_TIDE_SPEED 0.000000 -SIM_TWIST_TIME 0 -SIM_TWIST_X 0.000000 -SIM_TWIST_Y 0.000000 -SIM_TWIST_Z 0.000000 -SIM_VIB_FREQ_X 0.000000 -SIM_VIB_FREQ_Y 0.000000 -SIM_VIB_FREQ_Z 0.000000 -SIM_VIB_MOT_MAX 0.000000 -SIM_VIB_MOT_MULT 1.000000 -SIM_VICON_FAIL 0 -SIM_VICON_GLIT_X 0.000000 -SIM_VICON_GLIT_Y 0.000000 -SIM_VICON_GLIT_Z 0.000000 -SIM_VICON_POS_X 0.000000 -SIM_VICON_POS_Y 0.000000 -SIM_VICON_POS_Z 0.000000 -SIM_VICON_TMASK 3 -SIM_VICON_VGLI_X 0.000000 -SIM_VICON_VGLI_Y 0.000000 -SIM_VICON_VGLI_Z 0.000000 -SIM_VICON_YAW 0 -SIM_VICON_YAWERR 0 -SIM_WAVE_AMP 0.500000 -SIM_WAVE_DIR 0.000000 -SIM_WAVE_ENABLE 0 -SIM_WAVE_LENGTH 10.000000 -SIM_WAVE_SPEED 0.500000 -SIM_WIND_DELAY 0 -SIM_WIND_DIR 180.000000 -SIM_WIND_DIR_Z 0.000000 -SIM_WIND_SPD 0.000000 -SIM_WIND_T 0 -SIM_WIND_TURB 0.000000 -SIM_WIND_T_ALT 60.000000 -SIM_WIND_T_COEF 0.010000 -SIM_WOW_PIN -1 -SOAR_ENABLE 0 -SR0_ADSB 4 -SR0_EXTRA1 4 -SR0_EXTRA2 4 -SR0_EXTRA3 4 -SR0_EXT_STAT 4 -SR0_PARAMS 10 -SR0_POSITION 4 -SR0_RAW_CTRL 4 -SR0_RAW_SENS 4 -SR0_RC_CHAN 4 -SR1_ADSB 5 -SR1_EXTRA1 1 -SR1_EXTRA2 1 -SR1_EXTRA3 1 -SR1_EXT_STAT 1 -SR1_PARAMS 10 -SR1_POSITION 1 -SR1_RAW_CTRL 1 -SR1_RAW_SENS 1 -SR1_RC_CHAN 1 -SR2_ADSB 5 -SR2_EXTRA1 1 -SR2_EXTRA2 1 -SR2_EXTRA3 1 -SR2_EXT_STAT 1 -SR2_PARAMS 10 -SR2_POSITION 1 -SR2_RAW_CTRL 1 -SR2_RAW_SENS 1 -SR2_RC_CHAN 1 -SR3_ADSB 5 -SR3_EXTRA1 1 -SR3_EXTRA2 1 -SR3_EXTRA3 1 -SR3_EXT_STAT 1 -SR3_PARAMS 10 -SR3_POSITION 1 -SR3_RAW_CTRL 1 -SR3_RAW_SENS 1 -SR3_RC_CHAN 1 -SR4_ADSB 5 -SR4_EXTRA1 1 -SR4_EXTRA2 1 -SR4_EXTRA3 1 -SR4_EXT_STAT 1 -SR4_PARAMS 10 -SR4_POSITION 1 -SR4_RAW_CTRL 1 -SR4_RAW_SENS 1 -SR4_RC_CHAN 1 -SR5_ADSB 5 -SR5_EXTRA1 1 -SR5_EXTRA2 1 -SR5_EXTRA3 1 -SR5_EXT_STAT 1 -SR5_PARAMS 10 -SR5_POSITION 1 -SR5_RAW_CTRL 1 -SR5_RAW_SENS 1 -SR5_RC_CHAN 1 -SR6_ADSB 5 -SR6_EXTRA1 1 -SR6_EXTRA2 1 -SR6_EXTRA3 1 -SR6_EXT_STAT 1 -SR6_PARAMS 10 -SR6_POSITION 1 -SR6_RAW_CTRL 1 -SR6_RAW_SENS 1 -SR6_RC_CHAN 1 -STAB_PITCH_DOWN 2.000000 -STALL_PREVENTION 1 -STAT_BOOTCNT 24 -STAT_FLTTIME 0 -STAT_RESET 162617959 -STAT_RUNTIME 45365 -STEER2SRV_D 0.005000 -STEER2SRV_DRTFCT 10.000000 -STEER2SRV_DRTMIN 4500.000000 -STEER2SRV_DRTSPD 0.000000 -STEER2SRV_FF 0.000000 -STEER2SRV_I 0.200000 -STEER2SRV_IMAX 1500 -STEER2SRV_MINSPD 1.000000 -STEER2SRV_P 1.800000 -STEER2SRV_TCONST 0.750000 -STICK_MIXING 1 -SYSID_ENFORCE 0 -SYSID_MYGCS 255 -SYSID_THISMAV 1 -SYS_NUM_RESETS 29 -TECS_APPR_SMAX 0.000000 -TECS_CLMB_MAX 5.000000 -TECS_HGT_OMEGA 3.000000 -TECS_INTEG_GAIN 0.100000 -TECS_LAND_ARSPD -1.000000 -TECS_LAND_DAMP 0.500000 -TECS_LAND_IGAIN 0.000000 -TECS_LAND_PDAMP 0.000000 -TECS_LAND_PMAX 10 -TECS_LAND_SINK 0.250000 -TECS_LAND_SPDWGT -1.000000 -TECS_LAND_SRC 0.000000 -TECS_LAND_TCONST 2.000000 -TECS_LAND_TDAMP 0.000000 -TECS_LAND_THR -1.000000 -TECS_OPTIONS 0 -TECS_PITCH_MAX 15 -TECS_PITCH_MIN 0 -TECS_PTCH_DAMP 0.000000 -TECS_PTCH_FF_K 0.000000 -TECS_PTCH_FF_V0 12.000000 -TECS_RLL2THR 10.000000 -TECS_SINK_MAX 5.000000 -TECS_SINK_MIN 2.000000 -TECS_SPDWEIGHT 1.000000 -TECS_SPD_OMEGA 2.000000 -TECS_SYNAIRSPEED 0 -TECS_THR_DAMP 0.500000 -TECS_TIME_CONST 5.000000 -TECS_TKOFF_IGAIN 0.000000 -TECS_VERT_ACC 7.000000 -TELEM_DELAY 0 -TERRAIN_ENABLE 1 -TERRAIN_FOLLOW 0 -TERRAIN_LOOKAHD 2000 -TERRAIN_OPTIONS 0 -TERRAIN_SPACING 100 -THROTTLE_NUDGE 1 -THR_FAILSAFE 1 -THR_FS_VALUE 950 -THR_MAX 100 -THR_MIN 0 -THR_PASS_STAB 0 -THR_SLEWRATE 100 -THR_SUPP_MAN 0 -TKOFF_ACCEL_CNT 1 -TKOFF_ALT 50 -TKOFF_DIST 200 -TKOFF_FLAP_PCNT 0 -TKOFF_LVL_ALT 20 -TKOFF_LVL_PITCH 15 -TKOFF_PLIM_SEC 2.000000 -TKOFF_ROTATE_SPD 0.000000 -TKOFF_TDRAG_ELEV 0 -TKOFF_TDRAG_SPD1 0.000000 -TKOFF_THR_DELAY 2 -TKOFF_THR_MAX 0 -TKOFF_THR_MINACC 0.000000 -TKOFF_THR_MINSPD 0.000000 -TKOFF_THR_SLEW 0 -TKOFF_TIMEOUT 0 -TRIM_ARSPD_CM 2200 -TRIM_AUTO 0 -TRIM_PITCH_CD 0 -TRIM_THROTTLE 50 -TUNE_CHAN 0 -TUNE_CHAN_MAX 2000 -TUNE_CHAN_MIN 1000 -TUNE_ERR_THRESH 0.150000 -TUNE_MODE_REVERT 1 -TUNE_PARAM 0 -TUNE_RANGE 2.000000 -TUNE_SELECTOR 0 -USE_REV_THRUST 2 -VISO_TYPE 0 -VTX_ENABLE 0 -WP_LOITER_RAD 80 -WP_MAX_RADIUS 0 -WP_RADIUS 50 -YAW2SRV_DAMP 0.000000 -YAW2SRV_IMAX 1500 -YAW2SRV_INT 0.000000 -YAW2SRV_RLL 1.000000 -YAW2SRV_SLIP 0.000000 diff --git a/ArduPlane/wscript b/ArduPlane/wscript index 7d078b8eb0063f..cc3307602be509 100644 --- a/ArduPlane/wscript +++ b/ArduPlane/wscript @@ -1,41 +1,41 @@ -#!/usr/bin/env python -# encoding: utf-8 - -def build(bld): - vehicle = bld.path.name - bld.ap_stlib( - name=vehicle + '_libs', - ap_vehicle=vehicle, - ap_libraries=bld.ap_common_vehicle_libraries() + [ - 'APM_Control', - 'AP_AdvancedFailsafe', - 'AP_Avoidance', - 'AP_Arming', - 'AP_Camera', - 'AP_L1_Control', - 'AP_Navigation', - 'AP_RCMapper', - 'AP_SpdHgtControl', - 'AP_TECS', - 'AP_InertialNav', - 'AC_WPNav', - 'AC_AttitudeControl', - 'AP_Motors', - 'AP_Landing', - 'AP_Beacon', - 'PID', - 'AP_Soaring', - 'AP_LTM_Telem', - 'AP_Devo_Telem', - 'AP_OSD', - 'AC_AutoTune', - 'AP_AS5600_AOA', - 'AP_KDECAN', - ], - ) - - bld.ap_program( - program_name='arduplane', - program_groups=['bin', 'plane'], - use=vehicle + '_libs', - ) +#!/usr/bin/env python +# encoding: utf-8 + +def build(bld): + vehicle = bld.path.name + bld.ap_stlib( + name=vehicle + '_libs', + ap_vehicle=vehicle, + ap_libraries=bld.ap_common_vehicle_libraries() + [ + 'APM_Control', + 'AP_AdvancedFailsafe', + 'AP_Avoidance', + 'AP_Arming', + 'AP_Camera', + 'AP_L1_Control', + 'AP_Navigation', + 'AP_RCMapper', + 'AP_SpdHgtControl', + 'AP_TECS', + 'AP_InertialNav', + 'AC_WPNav', + 'AC_AttitudeControl', + 'AP_Motors', + 'AP_Landing', + 'AP_Beacon', + 'PID', + 'AP_Soaring', + 'AP_LTM_Telem', + 'AP_Devo_Telem', + 'AP_OSD', + 'AC_AutoTune', + 'AP_AS5600_AOA', + 'AP_KDECAN', + ], + ) + + bld.ap_program( + program_name='arduplane', + program_groups=['bin', 'plane'], + use=vehicle + '_libs', + ) diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp index 8900a94b54df50..49c933a8b9621b 100644 --- a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp @@ -1,10 +1,9 @@ /**************************************************** - * AMS 5600 Angle of Attack Class for Ardupilot platform + * AS5600 Angle of Attack Class for Ardupilot platform * Author: Cole Mero - * Date: 15 Dec 2014 + * Date: 15 Feb 2021 * File: AS5600_AOA.cpp - * Version 1.00 - * www.ams.com + * Based on: www.ams.com * * Description: This class has been designed to * access the AS5600 magnetic encoder sensor to @@ -16,75 +15,54 @@ #include #include "AS5600_AOA.h" +#include extern const AP_HAL::HAL &hal; -/**************************************************** +/************************************************** * Method: AS5600_AOA * In: none * Out: none - * Description: constructor for AMS 5600 + * Description: constructor for AS5600_AOA ***************************************************/ AS5600_AOA::AS5600_AOA() { - bus = 1; //Sets the bus number for the device, unclear what this number should be, trial and error to make it work address = 0x36; //This is the I2C address for the device, it is set by the manufacturer - - /*load register values*/ - - _zmco = 0x00; - _zpos_hi = 0x01; - _zpos_lo = 0x02; - _mpos_hi = 0x03; - _mpos_lo = 0x04; - _mang_hi = 0x05; - _mang_lo = 0x06; - _conf_hi = 0x07; - _conf_lo = 0x08; - _raw_ang_hi = 0x0c; - _raw_ang_lo = 0x0d; - _ang_hi = 0x0e; - _ang_lo = 0x0f; - _stat = 0x0b; - _agc = 0x1a; - _mag_hi = 0x1b; - _mag_lo = 0x1c; - _burn = 0xff; - } -void AS5600_AOA::init(){ - +bool AS5600_AOA::init() +{ dev = hal.i2c_mgr->get_device(bus, address); + if (!dev){ + return false; + } + WITH_SEMAPHORE(dev->get_semaphore()); dev->set_speed(AP_HAL::Device::SPEED_LOW); dev->set_retries(2); + return true; } /* mode = 0, output PWM, mode = 1 output analog (full range from 0% to 100% between GND and VDD*/ -void AS5600_AOA::setOutPut(unsigned char mode){ - unsigned char config_status; - config_status = readOneByte(_conf_lo); - if(mode == 1){ - config_status = config_status & 0xcf; - }else{ - config_status = config_status & 0xef; - } - - /* Note significant variance from the Arduino AMS_5600 library in this line. - * - * -> writeOneByte(_conf_lo, lowByte(config_status)); - * vs writeOneByte(_conf_lo, config_status); I have removed the lowByte() function - * since it is part of the arduino.h library and I don't have access to it, however, - * since it accesses the lowest byte in the variable, and an unsigned char such as - * config_status only HAS one byte regardless, I think the end result should be identical? - */ - writeOneByte(_conf_lo, config_status); -} +//void AS5600_AOA::setOutPut(uint8_t mode){ +// uint8_t config_status; +// if (!dev->read_registers(REG_CONF_LO, &config_status, 1)){ +// return; +// } +// if (mode == 1){ +// config_status = config_status & 0xcf; +// }else{ +// config_status = config_status & 0xef; +// } +// +// if (!dev->write_register(REG_CONF_LO, config_status)){ +// return; +// } +//} /******************************************************* @@ -94,42 +72,21 @@ void AS5600_AOA::setOutPut(unsigned char mode){ * Description: gets raw value of magnet position. * start, end, and max angle settings do not apply *******************************************************/ -unsigned short AS5600_AOA::getRawAngle(void) +uint16_t AS5600_AOA::getRawAngle(void) { - unsigned short angleRaw = readTwoBytes(_raw_ang_hi, _raw_ang_lo); - - unsigned short angleDegrees = angleRaw*0.087; - - AP::logger().Write("AoAR", "Status, TimeUS, Angle", "iQH", int(bool(dev)), AP_HAL::micros64(), angleDegrees); - - return angleDegrees; -} -/******************************************************* - * Method: highByte - * In: Unsigned short - * Out: Highest or leftmost byte of the unsigned short - * Description: Takes in the unsigned short and returns - * the leftmost or highest bite -*******************************************************/ - -unsigned char AS5600_AOA::highByte(unsigned short short_in){ + WITH_SEMAPHORE(dev->get_semaphore()); - unsigned char hiByte = ((short_in >> 8) & 0xff); - return hiByte; -} + //uint16_t angleRaw = readTwoBytes(REG_RAW_ANG_HI, REG_RAW_ANG_LO); + if (!dev->read_registers(REG_RAW_ANG_HI, &highByte, 1) || !dev->read_registers(REG_RAW_ANG_LO, &lowByte, 1)){ + return -1; + } -/******************************************************* - * Method: lowByte - * In: Unsigned short - * Out: Lowest or rightmost byte of the unsigned short - * Description: Takes in the unsigned short and returns - * the rightmost or lowest byte -*******************************************************/ + //Gets the two relevant register values and multiplies by conversion factor to get degrees + uint16_t angleRaw = ((highByte << 8) | lowByte)*0.087; -unsigned char AS5600_AOA::lowByte(unsigned short short_in){ + AP::logger().Write("AOAR", "TimeUS, Angle", "QH", AP_HAL::micros64(), angleRaw); - unsigned char loByte = (short_in & 0xff); - return loByte; + return angleRaw; } @@ -139,29 +96,33 @@ unsigned char AS5600_AOA::lowByte(unsigned short short_in){ * Out: value of max angle register * Description: sets a value in maximum angle register. * If no value is provided, method will read position of - * magnet. Setting this register zeros out max position + * magnet. Setting this register zeros out max position * register. *******************************************************/ -/*unsigned short AS5600_AOA::setMaxAngle(unsigned short newMaxAngle) +/*uint16_t AS5600_AOA::setMaxAngle(uint16_t newMaxAngle) { - unsigned short retVal; - if(newMaxAngle == -1) - { + if(newMaxAngle == -1){ maxAngle = getRawAngle(); - } - else + } else { maxAngle = newMaxAngle; + } - writeOneByte(_mang_hi, highByte(maxAngle)); - usleep(2); - writeOneByte(_mang_lo, lowByte(maxAngle)); - usleep(2); + WITH_SEMAPHORE(dev->get_semaphore()); - retVal = readTwoBytes(_mang_hi, _mang_lo); - return retVal; -} + if (!dev->write_register(REG_MANG_HI, HIGHBYTE(rawStartAngle))){ + return -1; + } + if (!dev->write_register(REG_MANG_LO, LOWBYTE(rawStartAngle))){ + return -1; + } + if (!dev->read_registers(REG_MANG_HI, &highByte, 1) || !dev->read_registers(REG_MANG_LO, &lowByte, 1)){ + return -1; + } + + maxAngle = ((highByte << 8) | lowByte); + return maxAngle; +}*/ -*/ /******************************************************* * Method: getMaxAngle @@ -169,9 +130,17 @@ unsigned char AS5600_AOA::lowByte(unsigned short short_in){ * Out: value of max angle register * Description: gets value of maximum angle register. *******************************************************/ -unsigned short AS5600_AOA::getMaxAngle() +uint16_t AS5600_AOA::getMaxAngle() { - return readTwoBytes(_mang_hi, _mang_lo); + WITH_SEMAPHORE(dev->get_semaphore()); + + if (!dev->read_registers(REG_MANG_HI, &highByte, 1) || !dev->read_registers(REG_MANG_LO, &lowByte, 1)){ + return -1; + } + + maxAngle = ((highByte << 8) | lowByte); + + return maxAngle; } @@ -183,84 +152,28 @@ unsigned short AS5600_AOA::getMaxAngle() * If no value is provided, method will read position of * magnet. *******************************************************/ -/* -unsigned short AS5600_AOA::setStartPosition(unsigned short startAngle) +/*uint16_t AS5600_AOA::setStartPosition(uint16_t startAngle = -1) { if(startAngle == -1) { rawStartAngle = getRawAngle(); - } - else + } else { rawStartAngle = startAngle; - - writeOneByte(_zpos_hi, highByte(rawStartAngle)); - usleep(2); - writeOneByte(_zpos_lo, lowByte(rawStartAngle)); - usleep(2); - zPosition = readTwoBytes(_zpos_hi, _zpos_lo); - - return(zPosition); -} - -*/ - -//int AS5600_AOA::writeOneByte(uint8_t in_adr, uint8_t msg){ -// -// WITH_SEMAPHORE(dev->get_semaphore()); -// -// send[2] = {in_adr, msg}; -// -// bool success = dev->transfer(send, sizeof(send), nullptr, 0); -// -// return success ? 0 : -1; -// -//} - - -/******************************************************* - * Method: readOneByte - * In: register to read - * Out: data read from i2c - * Description: reads one byte register from i2c -*******************************************************/ -int AS5600_AOA::readOneByte(uint8_t in_adr) -{ + } WITH_SEMAPHORE(dev->get_semaphore()); - uint8_t send[1] = {in_adr}; - uint8_t recv[1]; - - bool success = dev->transfer(send, sizeof(send), recv, sizeof(recv)); - - return success ? recv[0] : -1; -} - - -/******************************************************* - * Method: readTwoBytes - * In: register to read - * Out: data read from i2c - * Description: reads two bytes register from i2c -*******************************************************/ -int AS5600_AOA::readTwoBytes(uint8_t in_adr1, uint8_t in_adr2) -{ - - int firstResult = readOneByte(in_adr1); - int secondResult = readOneByte(in_adr2); - - if (firstResult == -1 || secondResult == -1){ - + if (!dev->write_register(REG_ZPOS_HI, HIGHBYTE(rawStartAngle))){ + return -1; + } + if (!dev->write_register(REG_ZPOS_LO, LOWBYTE(rawStartAngle))){ + return -1; + } + if (!dev->read_registers(REG_ZPOS_HI, &highByte, 1) || !dev->read_registers(REG_ZPOS_LO, &lowByte, 1)){ return -1; } - uint8_t firstByte = firstResult; - uint8_t secondByte = secondResult; - - uint16_t combined = (firstByte << 8) | secondByte; - - return combined; - -} - + uint16_t zPosition = ((highByte << 8) | lowByte); + return(zPosition); +}*/ diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.h b/libraries/AP_AS5600_AOA/AS5600_AOA.h index f3c2d70622c80e..a29445e2400b02 100644 --- a/libraries/AP_AS5600_AOA/AS5600_AOA.h +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.h @@ -15,32 +15,26 @@ class AS5600_AOA { AS5600_AOA(void); - void init(); + bool init(); - uint32_t busMaskExt; - uint32_t busMaskInt; - - unsigned short setMaxAngle(unsigned short newMaxAngle = -1); - unsigned short getMaxAngle(); - - unsigned short setStartPosition(unsigned short startAngle = -1); - unsigned short getStartPosition(); - - unsigned short setEndPosition(unsigned short endAngle = -1); - unsigned short getEndPosition(); - - unsigned short getRawAngle(void); - unsigned short getScaledAngle(); + uint16_t setMaxAngle(uint16_t newMaxAngle = -1); + uint16_t getMaxAngle(); + uint16_t setStartPosition(uint16_t startAngle = -1); + uint16_t getStartPosition(); + uint16_t setEndPosition(uint16_t endAngle = -1); + uint16_t getEndPosition(); + uint16_t getRawAngle(void); + uint16_t getScaledAngle(); int detectMagnet(); int getMagnetStrength(); int getAgc(); - unsigned short getMagnitude(); + uint16_t getMagnitude(); int getBurnCount(); int burnAngle(); int burnMaxAngleAndConfig(); - void setOutPut(unsigned char mode); + void setOutPut(uint8_t mode); HAL_Semaphore sem; @@ -48,46 +42,59 @@ class AS5600_AOA { AP_HAL::OwnPtr dev; - unsigned char bus; - unsigned char address; - uint32_t bus_clock; - bool use_smbus; - uint32_t timeout_ms; + uint8_t bus; + uint8_t address; - int as5600_address; - - unsigned short rawStartAngle; - unsigned short zPosition; - unsigned short rawEndAngle; - unsigned short mPosition; - unsigned short maxAngle; + uint16_t rawStartAngle; + uint16_t zPosition; + uint16_t rawEndAngle; + uint16_t mPosition; + uint16_t maxAngle; /* Registers */ - - int _zmco; - int _zpos_hi; /*zpos[11:8] high nibble START POSITION */ - int _zpos_lo; /*zpos[7:0] */ - int _mpos_hi; /*mpos[11:8] high nibble STOP POSITION */ - int _mpos_lo; /*mpos[7:0] */ - int _mang_hi; /*mang[11:8] high nibble MAXIMUM ANGLE */ - int _mang_lo; /*mang[7:0] */ - int _conf_hi; - int _conf_lo; - int _raw_ang_hi; - int _raw_ang_lo; - int _ang_hi; - int _ang_lo; - int _stat; - int _agc; - int _mag_hi; - int _mag_lo; - int _burn; +// +// int _zmco; +// int _zpos_hi; /*zpos[11:8] high nibble START POSITION */ +// int _zpos_lo; /*zpos[7:0] */ +// int _mpos_hi; /*mpos[11:8] high nibble STOP POSITION */ +// int _mpos_lo; /*mpos[7:0] */ +// int _mang_hi; /*mang[11:8] high nibble MAXIMUM ANGLE */ +// int _mang_lo; /*mang[7:0] */ +// int _conf_hi; +// int _conf_lo; +// int _raw_ang_hi; +// int _raw_ang_lo; +// int _ang_hi; +// int _ang_lo; +// int _stat; +// int _agc; +// int _mag_hi; +// int _mag_lo; +// int _burn; + + static const uint8_t REG_ZMCO = 0x00; + static const uint8_t REG_ZPOS_HI = 0x01; + static const uint8_t REG_ZPOS_LO = 0x02; + static const uint8_t REG_MPOS_HI = 0x03; + static const uint8_t REG_MPOS_LO = 0x04; + static const uint8_t REG_MANG_HI = 0x05; + static const uint8_t REG_MANG_LO = 0x06; + static const uint8_t REG_CONF_HI = 0x07; + static const uint8_t REG_CONF_LO = 0x08; + static const uint8_t REG_RAW_ANG_HI = 0x0c; + static const uint8_t REG_RAW_ANG_LO = 0x0d; + static const uint8_t REG_ANG_HI = 0x0e; + static const uint8_t REG_ANG_LO = 0x0f; + static const uint8_t REG_STAT = 0x0b; + static const uint8_t REG_AGC = 0x1a; + static const uint8_t REG_MAG_HI = 0x1b; + static const uint8_t REG_MAG_LO = 0x1c; + static const uint8_t REG_BURN = 0xff; int readOneByte(uint8_t in_adr); int readTwoBytes(uint8_t in_adr_hi, uint8_t in_adr_lo); int writeOneByte(uint8_t adr_in, uint8_t dat_in); - unsigned char highByte(unsigned short short_in); - unsigned char lowByte(unsigned short short_in); - + uint8_t lowByte; + uint8_t highByte; }; From c26e328edf41326e3438b8809afbefde2bb47b79 Mon Sep 17 00:00:00 2001 From: colejmero <57465269+colejmero@users.noreply.github.com> Date: Fri, 5 Mar 2021 02:09:53 -0500 Subject: [PATCH 07/10] Adds recommended changes from previous PR review --- ArduPlane/ArduPlane.cpp | 3 +-- libraries/AP_AS5600_AOA/AS5600_AOA.cpp | 6 ++---- libraries/AP_AS5600_AOA/AS5600_AOA.h | 3 ++- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 0a87e98529bb76..f20615208507fb 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -46,7 +46,6 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(navigate, 10, 150), SCHED_TASK(update_compass, 10, 200), SCHED_TASK(read_airspeed, 10, 100), - //SCHED_TASK(read_aoa, 10, 200), SCHED_TASK(update_alt, 10, 200), SCHED_TASK(adjust_altitude_target, 10, 200), #if ADVANCED_FAILSAFE == ENABLED @@ -62,7 +61,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(read_rangefinder, 50, 100), SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100), SCHED_TASK_CLASS(Compass, &plane.compass, cal_update, 50, 50), - SCHED_TASK_CLASS(AS5600_AOA, &plane.aoa_sensor, getRawAngle, 10, 200), + SCHED_TASK_CLASS(AS5600_AOA, &plane.aoa_sensor, update, 10, 200), SCHED_TASK(accel_cal_update, 10, 50), #if OPTFLOW == ENABLED SCHED_TASK_CLASS(OpticalFlow, &plane.optflow, update, 50, 50), diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp index 49c933a8b9621b..c6b67520e7358b 100644 --- a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp @@ -72,21 +72,19 @@ bool AS5600_AOA::init() * Description: gets raw value of magnet position. * start, end, and max angle settings do not apply *******************************************************/ -uint16_t AS5600_AOA::getRawAngle(void) +void AS5600_AOA::update(void) { WITH_SEMAPHORE(dev->get_semaphore()); //uint16_t angleRaw = readTwoBytes(REG_RAW_ANG_HI, REG_RAW_ANG_LO); if (!dev->read_registers(REG_RAW_ANG_HI, &highByte, 1) || !dev->read_registers(REG_RAW_ANG_LO, &lowByte, 1)){ - return -1; + return; } //Gets the two relevant register values and multiplies by conversion factor to get degrees uint16_t angleRaw = ((highByte << 8) | lowByte)*0.087; AP::logger().Write("AOAR", "TimeUS, Angle", "QH", AP_HAL::micros64(), angleRaw); - - return angleRaw; } diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.h b/libraries/AP_AS5600_AOA/AS5600_AOA.h index a29445e2400b02..195d17de79268f 100644 --- a/libraries/AP_AS5600_AOA/AS5600_AOA.h +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.h @@ -17,13 +17,14 @@ class AS5600_AOA { bool init(); + void update(void); + uint16_t setMaxAngle(uint16_t newMaxAngle = -1); uint16_t getMaxAngle(); uint16_t setStartPosition(uint16_t startAngle = -1); uint16_t getStartPosition(); uint16_t setEndPosition(uint16_t endAngle = -1); uint16_t getEndPosition(); - uint16_t getRawAngle(void); uint16_t getScaledAngle(); int detectMagnet(); From 82225f82171b9c09474dd93497c2241fcadda0d8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 11 Mar 2021 11:57:56 +1100 Subject: [PATCH 08/10] AP_AS5600_AOA: use a thread to read AOA sensor --- libraries/AP_AS5600_AOA/AS5600_AOA.cpp | 30 ++++++++++++++++++++------ libraries/AP_AS5600_AOA/AS5600_AOA.h | 7 ++++-- 2 files changed, 29 insertions(+), 8 deletions(-) diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp index c6b67520e7358b..cd11099bf0abb2 100644 --- a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp @@ -44,6 +44,10 @@ bool AS5600_AOA::init() dev->set_speed(AP_HAL::Device::SPEED_LOW); dev->set_retries(2); + dev->register_periodic_callback( + 50000, + FUNCTOR_BIND_MEMBER(&AS5600_AOA::_timer, void)); + return true; } @@ -74,12 +78,7 @@ bool AS5600_AOA::init() *******************************************************/ void AS5600_AOA::update(void) { - WITH_SEMAPHORE(dev->get_semaphore()); - - //uint16_t angleRaw = readTwoBytes(REG_RAW_ANG_HI, REG_RAW_ANG_LO); - if (!dev->read_registers(REG_RAW_ANG_HI, &highByte, 1) || !dev->read_registers(REG_RAW_ANG_LO, &lowByte, 1)){ - return; - } + WITH_SEMAPHORE(sem); //Gets the two relevant register values and multiplies by conversion factor to get degrees uint16_t angleRaw = ((highByte << 8) | lowByte)*0.087; @@ -88,6 +87,25 @@ void AS5600_AOA::update(void) } +void AS5600_AOA::_timer(void) +{ + uint8_t low, high; + + { + WITH_SEMAPHORE(dev->get_semaphore()); + + if (!dev->read_registers(REG_RAW_ANG_HI, &high, 1) || + !dev->read_registers(REG_RAW_ANG_LO, &low, 1)){ + return; + } + } + + WITH_SEMAPHORE(sem); + lowByte = low; + highByte = high; +} + + /******************************************************* * Method: setMaxAngle * In: new maximum angle to set OR none diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.h b/libraries/AP_AS5600_AOA/AS5600_AOA.h index 195d17de79268f..c3ff4d1ec22800 100644 --- a/libraries/AP_AS5600_AOA/AS5600_AOA.h +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.h @@ -37,10 +37,10 @@ class AS5600_AOA { int burnMaxAngleAndConfig(); void setOutPut(uint8_t mode); - HAL_Semaphore sem; - private: + HAL_Semaphore sem; + AP_HAL::OwnPtr dev; uint8_t bus; @@ -98,4 +98,7 @@ class AS5600_AOA { uint8_t lowByte; uint8_t highByte; + + + void _timer(void); }; From 892bb6e0178d152ba75f74d103271bca9754d8fd Mon Sep 17 00:00:00 2001 From: colejmero <57465269+colejmero@users.noreply.github.com> Date: Sun, 14 Mar 2021 23:27:14 -0400 Subject: [PATCH 09/10] AS5600: Commit for Peter to review bug --- libraries/AP_AS5600_AOA/AS5600_AOA.cpp | 22 +++++++++++++++++----- libraries/AP_AS5600_AOA/AS5600_AOA.h | 23 +++-------------------- 2 files changed, 20 insertions(+), 25 deletions(-) diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp index cd11099bf0abb2..8513e3a3e4ac17 100644 --- a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp @@ -16,6 +16,8 @@ #include #include "AS5600_AOA.h" #include +#include + extern const AP_HAL::HAL &hal; @@ -27,11 +29,10 @@ extern const AP_HAL::HAL &hal; ***************************************************/ AS5600_AOA::AS5600_AOA() { - bus = 1; //Sets the bus number for the device, unclear what this number should be, trial and error to make it work + bus = 2; //Sets the bus number for the device, unclear what this number should be, trial and error to make it work address = 0x36; //This is the I2C address for the device, it is set by the manufacturer } - bool AS5600_AOA::init() { dev = hal.i2c_mgr->get_device(bus, address); @@ -70,7 +71,7 @@ bool AS5600_AOA::init() /******************************************************* - * Method: getRawAngle + * Method: update * In: none * Out: value of raw angle register * Description: gets raw value of magnet position. @@ -89,15 +90,26 @@ void AS5600_AOA::update(void) void AS5600_AOA::_timer(void) { + gcs().send_text(MAV_SEVERITY_INFO, "update called - regHiRead: %d regLoRead: %d", regHiRead, regLoRead); + uint8_t low, high; { WITH_SEMAPHORE(dev->get_semaphore()); - if (!dev->read_registers(REG_RAW_ANG_HI, &high, 1) || - !dev->read_registers(REG_RAW_ANG_LO, &low, 1)){ + if (!dev->read_registers(REG_RAW_ANG_HI, &high, 1)){ + return; + } + + regHiRead = true; + gcs().send_text(MAV_SEVERITY_INFO, "REG_RAW_ANG_HI: %d", high); + + if (!dev->read_registers(REG_RAW_ANG_LO, &low, 1)){ return; } + + gcs().send_text(MAV_SEVERITY_INFO, "REG_RAW_ANG_LO: %d", low); + regLoRead = true; } WITH_SEMAPHORE(sem); diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.h b/libraries/AP_AS5600_AOA/AS5600_AOA.h index c3ff4d1ec22800..331c503912775e 100644 --- a/libraries/AP_AS5600_AOA/AS5600_AOA.h +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.h @@ -8,7 +8,6 @@ #include - class AS5600_AOA { public: @@ -37,6 +36,9 @@ class AS5600_AOA { int burnMaxAngleAndConfig(); void setOutPut(uint8_t mode); + bool regHiRead = false; + bool regLoRead = false; + private: HAL_Semaphore sem; @@ -53,25 +55,6 @@ class AS5600_AOA { uint16_t maxAngle; /* Registers */ -// -// int _zmco; -// int _zpos_hi; /*zpos[11:8] high nibble START POSITION */ -// int _zpos_lo; /*zpos[7:0] */ -// int _mpos_hi; /*mpos[11:8] high nibble STOP POSITION */ -// int _mpos_lo; /*mpos[7:0] */ -// int _mang_hi; /*mang[11:8] high nibble MAXIMUM ANGLE */ -// int _mang_lo; /*mang[7:0] */ -// int _conf_hi; -// int _conf_lo; -// int _raw_ang_hi; -// int _raw_ang_lo; -// int _ang_hi; -// int _ang_lo; -// int _stat; -// int _agc; -// int _mag_hi; -// int _mag_lo; -// int _burn; static const uint8_t REG_ZMCO = 0x00; static const uint8_t REG_ZPOS_HI = 0x01; From 43c25dd101d2669ed7ea448fbccae62dfc7f2bbb Mon Sep 17 00:00:00 2001 From: colejmero <57465269+colejmero@users.noreply.github.com> Date: Sun, 14 Mar 2021 23:35:24 -0400 Subject: [PATCH 10/10] AS5600: Adds line forgoten from previous commit --- libraries/AP_AS5600_AOA/AS5600_AOA.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp index 8513e3a3e4ac17..ba89593f1200a2 100644 --- a/libraries/AP_AS5600_AOA/AS5600_AOA.cpp +++ b/libraries/AP_AS5600_AOA/AS5600_AOA.cpp @@ -85,6 +85,8 @@ void AS5600_AOA::update(void) uint16_t angleRaw = ((highByte << 8) | lowByte)*0.087; AP::logger().Write("AOAR", "TimeUS, Angle", "QH", AP_HAL::micros64(), angleRaw); + + gcs().send_text(MAV_SEVERITY_INFO, "Angle: %d", angleRaw); }