diff --git a/build.sh b/build.sh index 59d9221b6e..58db51aa42 100755 --- a/build.sh +++ b/build.sh @@ -1,3 +1,4 @@ +#!/usr/bin/env bash set -e if [[ $# == 0 ]]; then @@ -6,9 +7,15 @@ Usage syntax: ./build.sh Notes: * You can specify multiple targets. - * If no targets are specified, *all* of them will be built. + ./build.sh + * To get a list of all targets use \"help\". Hint: pipe the output through a pager. + ./build.sh help | less + * To build all targets use \"all\" + ./build.sh all * To clean a target prefix it with \"clean_\". - * To clean all targets just use \"clean\"." + ./build.sh clean_MATEKF405SE + * To clean all targets just use \"clean\". + ./build.sh clean" exit 1 fi diff --git a/docs/Battery.md b/docs/Battery.md index f7277b264a..6fc3a12570 100644 --- a/docs/Battery.md +++ b/docs/Battery.md @@ -195,7 +195,25 @@ Note that in this example even though your warning capacity (`battery_capacity_w ## Battery profiles -Up to 3 battery profiles are supported. You can select the battery profile from the GUI, OSD menu, [stick commands](Controls.md) and CLI command `battery_profile n`. Battery profiles store the following settings (see above for an explanation of each setting): `bat_cells`, `vbat_cell_detect_voltage`, `vbat_max_cell_voltage`, `vbat_warning_cell_voltage`, `vbat_min_cell_voltage`, `battery_capacity_unit`, `battery_capacity`, `battery_capacity_warning`, `battery_capacity_critical` +Up to 3 battery profiles are supported. You can select the battery profile from the GUI, OSD menu, [stick commands](Controls.md) and CLI command `battery_profile n`. Battery profiles store the following settings (see above for an explanation of each setting): +- `bat_cells` +- `vbat_cell_detect_voltage` +- `vbat_max_cell_voltage` +- `vbat_warning_cell_voltage` +- `vbat_min_cell_voltage` +- `battery_capacity_unit` +- `battery_capacity` +- `battery_capacity_warning` +- `battery_capacity_critical` +- `throttle_idle` +- `fw_min_throttle_down_pitch` +- `nav_fw_cruise_thr` +- `nav_fw_min_thr` +- `nav_fw_pitch2thr` +- `nav_fw_launch_thr` +- `nav_fw_launch_idle_thr` +- `failsafe_throttle` +- `nav_mc_hover_thr` To enable the automatic battery profile switching based on battery voltage enable the `BAT_PROF_AUTOSWITCH` feature. For a profile to be automatically selected the number of cells of the battery needs to be specified (>0). diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 91a1003693..3052b4f8ef 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -76,6 +76,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 40 | MOD | Divide `Operand A` by `Operand B` and returns the remainder | | 41 | LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. | | 42 | SET_PROFILE | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change | +| 43 | MIN | Finds the lowest value of `Operand A` and `Operand B` | +| 44 | MAX | Finds the highest value of `Operand A` and `Operand B` | ### Operands diff --git a/docs/Settings.md b/docs/Settings.md index 8073301585..09008d1d36 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -188,7 +188,7 @@ Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THR | Default | Min | Max | | --- | --- | --- | -| 1300 | 1000 | 2000 | +| 1150 | 1000 | 2000 | --- @@ -1602,6 +1602,36 @@ Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro --- +### gyro_zero_x + +Calculated gyro zero calibration of axis X + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### gyro_zero_y + +Calculated gyro zero calibration of axis Y + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### gyro_zero_z + +Calculated gyro zero calibration of axis Z + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + ### has_flaps Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot @@ -2102,6 +2132,26 @@ _// TODO_ --- +### init_gyro_cal + +If defined to 'OFF', it will ignore the gyroscope calibration done at each startup. Instead, the gyroscope last calibration from when you calibrated will be used. It also means you don't have to keep the UAV stationary during a startup. + +| Default | Min | Max | +| --- | --- | --- | +| ON | OFF | ON | + +--- + +### ins_gravity_cmss + +Calculated 1G of Acc axis Z to use in INS + +| Default | Min | Max | +| --- | --- | --- | +| 0.0 | 0 | 1000 | + +--- + ### iterm_windup Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) @@ -2818,7 +2868,7 @@ Craft name | Default | Min | Max | | --- | --- | --- | -| _empty_ | | | +| _empty_ | | MAX_NAME_LENGTH | --- @@ -3852,6 +3902,16 @@ Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: AT --- +### nav_wp_enforce_altitude + +Forces craft to achieve the set WP altitude as well as position before moving to next WP. Position is held and altitude adjusted as required before moving on. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### nav_wp_load_on_boot If set to ON, waypoints will be automatically loaded from EEPROM to the FC during startup. @@ -4582,6 +4642,16 @@ Auto swap display time interval between disarm stats pages (seconds). Reverts to --- +### osd_system_msg_display_time + +System message display cycle time for multiple messages (milliseconds). + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | 500 | 5000 | + +--- + ### osd_telemetry To enable OSD telemetry for antenna tracker. Possible values are `OFF`, `ON` and `TEST` @@ -4624,7 +4694,7 @@ IMPERIAL, METRIC, UK ### osd_video_system -Video system used. Possible values are `AUTO`, `PAL` and `NTSC` +Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, and `HD` | Default | Min | Max | | --- | --- | --- | @@ -5404,7 +5474,7 @@ Total flight distance [in meters]. The value is updated on every disarm when "st ### stats_total_energy -_// TODO_ +Total energy consumption [in mWh]. The value is updated on every disarm when "stats" are enabled. | Default | Min | Max | | --- | --- | --- | diff --git a/docs/development/Building in Docker.md b/docs/development/Building in Docker.md index 99a8784e6d..eafb9b5e63 100755 --- a/docs/development/Building in Docker.md +++ b/docs/development/Building in Docker.md @@ -20,6 +20,12 @@ Where `` must be replaced with the name of the target that you want to b ./build.sh MATEKF405SE ``` +Run the script with no arguments to get more details on its usage: + +``` +./build.sh +``` + ## Windows 10 Docker on Windows requires full paths for mounting volumes in `docker run` commands. For example: `c:\Users\pspyc\Documents\Projects\inav` becomes `//c/Users/pspyc/Documents/Projects/inav` . diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index decb759a75..9ad0afba9a 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -496,6 +496,8 @@ main_sources(COMMON_SRC io/displayport_msp.h io/displayport_oled.c io/displayport_oled.h + io/displayport_hdzero_osd.c + io/displayport_hdzero_osd.h io/displayport_srxl.c io/displayport_srxl.h io/displayport_hott.c diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 84663d8da0..80f7bf09e1 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -192,6 +192,7 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] = OSD_SETTING_ENTRY("MISSION FAILSAFE", SETTING_FAILSAFE_MISSION), OSD_SETTING_ENTRY("WP LOAD ON BOOT", SETTING_NAV_WP_LOAD_ON_BOOT), OSD_SETTING_ENTRY("WP REACHED RADIUS", SETTING_NAV_WP_RADIUS), + OSD_SETTING_ENTRY("WP ENFORCE ALTITUDE", SETTING_NAV_WP_ENFORCE_ALTITUDE), OSD_SETTING_ENTRY("WP SAFE DISTANCE", SETTING_NAV_WP_SAFE_DISTANCE), #ifdef USE_MULTI_MISSION OSD_SETTING_ENTRY("MULTI MISSION NUMBER", SETTING_NAV_WP_MULTI_MISSION_INDEX), diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 4450723a2b..4348a77fc3 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -66,6 +66,8 @@ #define CMSEC_TO_CENTIKPH(cms) (cms * 3.6) #define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845) +#define C_TO_KELVIN(temp) (temp + 273.15f) + // copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70 #define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar) \ ( __extension__ ({ \ diff --git a/src/main/drivers/io_port_expander.c b/src/main/drivers/io_port_expander.c index 5125fe2260..40717633aa 100644 --- a/src/main/drivers/io_port_expander.c +++ b/src/main/drivers/io_port_expander.c @@ -60,7 +60,7 @@ void ioPortExpanderSync(void) { if (ioPortExpanderState.active && ioPortExpanderState.shouldSync) { pcf8574Write(ioPortExpanderState.state); - ioPortExpanderState.shouldSync = false;; + ioPortExpanderState.shouldSync = false; } } diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c index 436bddc3cf..3b731eaa49 100644 --- a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c +++ b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c @@ -97,7 +97,7 @@ static void ms4525_calculate(pitotDev_t * pitot, float *pressure, float *tempera //float dP = ctx->ms4525_up * 10.0f * 0.1052120688f; const float dP_psi = -((ctx->ms4525_up - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min); float dP = dP_psi * PSI_to_Pa; - float T = (float)(200.0f * (int32_t)ctx->ms4525_ut) / 2047.0f - 50.0f + 273.15f; + float T = C_TO_KELVIN((float)(200.0f * (int32_t)ctx->ms4525_ut) / 2047.0f - 50.0f); if (pressure) { *pressure = dP; // Pa diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index aaed828375..255087f004 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -572,7 +572,7 @@ uint32_t softSerialTxBytesFree(const serialPort_t *instance) softSerial_t *s = (softSerial_t *)instance; - uint8_t bytesUsed = (s->port.txBufferHead - s->port.txBufferTail) & (s->port.txBufferSize - 1); + uint32_t bytesUsed = (s->port.txBufferHead - s->port.txBufferTail) & (s->port.txBufferSize - 1); return (s->port.txBufferSize - 1) - bytesUsed; } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 5d75c0342a..a2c6362fc4 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2248,6 +2248,11 @@ static void cliFlashInfo(char *cmdline) UNUSED(cmdline); const flashGeometry_t *layout = flashGetGeometry(); + + if (layout->totalSize == 0) { + cliPrintLine("Flash not available"); + return; + } cliPrintLinef("Flash sectors=%u, sectorSize=%u, pagesPerSector=%u, pageSize=%u, totalSize=%u", layout->sectors, layout->sectorSize, layout->pagesPerSector, layout->pageSize, layout->totalSize); @@ -2277,6 +2282,13 @@ static void cliFlashErase(char *cmdline) { UNUSED(cmdline); + const flashGeometry_t *layout = flashGetGeometry(); + + if (layout->totalSize == 0) { + cliPrintLine("Flash not available"); + return; + } + cliPrintLine("Erasing..."); flashfsEraseCompletely(); @@ -3344,7 +3356,11 @@ static void cliStatus(char *cmdline) hardwareSensorStatusNames[getHwRangefinderStatus()], hardwareSensorStatusNames[getHwOpticalFlowStatus()], hardwareSensorStatusNames[getHwGPSStatus()], +#ifdef USE_SECONDARY_IMU hardwareSensorStatusNames[getHwSecondaryImuStatus()] +#else + hardwareSensorStatusNames[0] +#endif ); #ifdef USE_ESC_SENSOR diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 6fb451344a..0e07552090 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -459,6 +459,22 @@ void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex) beeperConfirmationBeeps(profileIndex + 1); } +void setGyroCalibrationAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]) { + gyroConfigMutable()->gyro_zero_cal[X] = getGyroZero[X]; + gyroConfigMutable()->gyro_zero_cal[Y] = getGyroZero[Y]; + gyroConfigMutable()->gyro_zero_cal[Z] = getGyroZero[Z]; + // save the calibration + writeEEPROM(); + readEEPROM(); +} + +void setGravityCalibrationAndWriteEEPROM(float getGravity) { + gyroConfigMutable()->gravity_cmss_cal = getGravity; + // save the calibration + writeEEPROM(); + readEEPROM(); +} + void beeperOffSet(uint32_t mask) { beeperConfigMutable()->beeper_off_flags |= mask; diff --git a/src/main/fc/config.h b/src/main/fc/config.h index dda257670c..ed9deaabd2 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -19,6 +19,7 @@ #include #include +#include "common/axis.h" #include "common/time.h" #include "config/parameter_group.h" #include "drivers/adc.h" @@ -29,6 +30,7 @@ #define MAX_NAME_LENGTH 16 #define TASK_GYRO_LOOPTIME 250 // Task gyro always runs at 4kHz + typedef enum { FEATURE_THR_VBAT_COMP = 1 << 0, FEATURE_VBAT = 1 << 1, @@ -130,6 +132,9 @@ uint8_t getConfigBatteryProfile(void); bool setConfigBatteryProfile(uint8_t profileIndex); void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex); +void setGyroCalibrationAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]); +void setGravityCalibrationAndWriteEEPROM(float getGravity); + bool canSoftwareSerialBeUsed(void); void applyAndSaveBoardAlignmentDelta(int16_t roll, int16_t pitch); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 74e90fadc7..0a91630b1c 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -978,12 +978,12 @@ void taskUpdateRxMain(timeUs_t currentTimeUs) // returns seconds float getFlightTime() { - return (float)(flightTime / 1000) / 1000; + return US2S(flightTime); } float getArmTime() { - return (float)(armTime / 1000) / 1000; + return US2S(armTime); } void fcReboot(bool bootLoader) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 23fc8a2069..4bf83d3d05 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -109,6 +109,7 @@ #include "io/displayport_frsky_osd.h" #include "io/displayport_msp.h" #include "io/displayport_max7456.h" +#include "io/displayport_hdzero_osd.h" #include "io/displayport_srxl.h" #include "io/flashfs.h" #include "io/gps.h" @@ -553,6 +554,11 @@ void init(void) osdDisplayPort = frskyOSDDisplayPortInit(osdConfig()->video_system); } #endif +#ifdef USE_HDZERO_OSD + if (!osdDisplayPort) { + osdDisplayPort = hdzeroOsdDisplayPortInit(); + } +#endif #if defined(USE_MAX7456) // If there is a max7456 chip for the OSD and we have no // external OSD initialized, use it. diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f0194a72c5..36d91b0bbb 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -425,7 +425,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, getHwRangefinderStatus()); sbufWriteU8(dst, getHwPitotmeterStatus()); sbufWriteU8(dst, getHwOpticalFlowStatus()); +#ifdef USE_SECONDARY_IMU sbufWriteU8(dst, getHwSecondaryImuStatus()); +#else + sbufWriteU8(dst, 0); +#endif break; case MSP_ACTIVEBOXES: diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index f90b89315b..1b1ac0793e 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -188,48 +188,49 @@ void initActiveBoxIds(void) ADD_ACTIVE_BOX(BOXHEADINGHOLD); - if ((sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) && STATE(MULTIROTOR)) { - ADD_ACTIVE_BOX(BOXHEADFREE); - ADD_ACTIVE_BOX(BOXHEADADJ); - } - - if (STATE(ALTITUDE_CONTROL) && STATE(MULTIROTOR)) { - ADD_ACTIVE_BOX(BOXFPVANGLEMIX); - } - //Camstab mode is enabled always ADD_ACTIVE_BOX(BOXCAMSTAB); -#ifdef USE_GPS - if (STATE(ALTITUDE_CONTROL) && (sensors(SENSOR_BARO) || (feature(FEATURE_GPS) && (STATE(AIRPLANE) || positionEstimationConfig()->use_gps_no_baro)))) { - ADD_ACTIVE_BOX(BOXNAVALTHOLD); - if (STATE(MULTIROTOR)) { + if (STATE(MULTIROTOR)) { + if ((sensors(SENSOR_ACC) || sensors(SENSOR_MAG))) { + ADD_ACTIVE_BOX(BOXHEADFREE); + ADD_ACTIVE_BOX(BOXHEADADJ); + } + if (sensors(SENSOR_BARO) && sensors(SENSOR_RANGEFINDER) && sensors(SENSOR_OPFLOW)) { ADD_ACTIVE_BOX(BOXSURFACE); } + ADD_ACTIVE_BOX(BOXFPVANGLEMIX); } - const bool navReadyMultirotor = STATE(MULTIROTOR) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); - const bool navReadyOther = !STATE(MULTIROTOR) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); + bool navReadyAltControl = sensors(SENSOR_BARO); +#ifdef USE_GPS + navReadyAltControl = navReadyAltControl || (feature(FEATURE_GPS) && (STATE(AIRPLANE) || positionEstimationConfig()->use_gps_no_baro)); + const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning; - if (navFlowDeadReckoning || navReadyMultirotor || navReadyOther) { - if (!STATE(ROVER) && !STATE(BOAT)) { - ADD_ACTIVE_BOX(BOXNAVPOSHOLD); - } + bool navReadyPosControl = sensors(SENSOR_ACC) && feature(FEATURE_GPS); + if (STATE(MULTIROTOR)) { + navReadyPosControl = navReadyPosControl && getHwCompassStatus() != HW_SENSOR_NONE; + } + + if (STATE(ALTITUDE_CONTROL) && navReadyAltControl && (navReadyPosControl || navFlowDeadReckoning)) { + ADD_ACTIVE_BOX(BOXNAVPOSHOLD); if (STATE(AIRPLANE)) { ADD_ACTIVE_BOX(BOXLOITERDIRCHN); } } - if (navReadyMultirotor || navReadyOther) { - ADD_ACTIVE_BOX(BOXNAVRTH); - ADD_ACTIVE_BOX(BOXNAVWP); - ADD_ACTIVE_BOX(BOXHOMERESET); - ADD_ACTIVE_BOX(BOXGCSNAV); - ADD_ACTIVE_BOX(BOXPLANWPMISSION); + if (navReadyPosControl) { + if (!STATE(ALTITUDE_CONTROL) || (STATE(ALTITUDE_CONTROL) && navReadyAltControl)) { + ADD_ACTIVE_BOX(BOXNAVRTH); + ADD_ACTIVE_BOX(BOXNAVWP); + ADD_ACTIVE_BOX(BOXHOMERESET); + ADD_ACTIVE_BOX(BOXGCSNAV); + ADD_ACTIVE_BOX(BOXPLANWPMISSION); + } if (STATE(AIRPLANE)) { - ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD); ADD_ACTIVE_BOX(BOXNAVCRUISE); + ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD); ADD_ACTIVE_BOX(BOXSOARING); } } @@ -239,8 +240,10 @@ void initActiveBoxIds(void) ADD_ACTIVE_BOX(BOXBRAKING); } #endif - -#endif +#endif // GPS + if (STATE(ALTITUDE_CONTROL) && navReadyAltControl) { + ADD_ACTIVE_BOX(BOXNAVALTHOLD); + } if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) { ADD_ACTIVE_BOX(BOXMANUAL); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 3e54359f4d..262e7fa6e2 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -67,6 +67,7 @@ #include "io/smartport_master.h" #include "io/vtx.h" #include "io/osd_dji_hd.h" +#include "io/displayport_hdzero_osd.h" #include "io/servo_sbus.h" #include "msp/msp_serial.h" @@ -108,6 +109,11 @@ void taskHandleSerial(timeUs_t currentTimeUs) // DJI OSD uses a special flavour of MSP (subset of Betaflight 4.1.1 MSP) - process as part of serial task djiOsdSerialProcess(); #endif + +#ifdef USE_HDZERO_OSD + // Capture HDZero messages to determine if VTX is connected + hdzeroOsdSerialProcess(mspFcProcessCommand); +#endif } void taskUpdateBattery(timeUs_t currentTimeUs) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 2e2e65a610..eed67b85c1 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -75,7 +75,7 @@ tables: values: ["BATTERY", "CELL"] enum: osd_stats_min_voltage_unit_e - name: osd_video_system - values: ["AUTO", "PAL", "NTSC"] + values: ["AUTO", "PAL", "NTSC", "HD"] enum: videoSystem_e - name: osd_telemetry values: ["OFF", "ON","TEST"] @@ -309,6 +309,35 @@ groups: condition: USE_GYRO_KALMAN min: 1 max: 16000 + - name: init_gyro_cal + description: "If defined to 'OFF', it will ignore the gyroscope calibration done at each startup. Instead, the gyroscope last calibration from when you calibrated will be used. It also means you don't have to keep the UAV stationary during a startup." + default_value: ON + field: init_gyro_cal_enabled + type: bool + - name: gyro_zero_x + description: "Calculated gyro zero calibration of axis X" + default_value: 0 + field: gyro_zero_cal[X] + min: INT16_MIN + max: INT16_MAX + - name: gyro_zero_y + description: "Calculated gyro zero calibration of axis Y" + default_value: 0 + field: gyro_zero_cal[Y] + min: INT16_MIN + max: INT16_MAX + - name: gyro_zero_z + description: "Calculated gyro zero calibration of axis Z" + default_value: 0 + field: gyro_zero_cal[Z] + min: INT16_MIN + max: INT16_MAX + - name: ins_gravity_cmss + description: "Calculated 1G of Acc axis Z to use in INS" + default_value: 0.0 + field: gravity_cmss_cal + min: 0 + max: 1000 - name: PG_ADC_CHANNEL_CONFIG type: adcChannelConfig_t @@ -1648,7 +1677,7 @@ groups: table: airmodeHandlingType - name: airmode_throttle_threshold description: "Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used" - default_value: 1300 + default_value: 1150 field: airmodeThrottleThreshold min: 1000 max: 2000 @@ -2392,6 +2421,11 @@ groups: field: general.waypoint_radius min: 10 max: 10000 + - name: nav_wp_enforce_altitude + description: "Forces craft to achieve the set WP altitude as well as position before moving to next WP. Position is held and altitude adjusted as required before moving on." + default_value: OFF + field: general.flags.waypoint_enforce_altitude + type: bool - name: nav_wp_safe_distance description: "First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check." default_value: 10000 @@ -3049,7 +3083,7 @@ groups: type: uint8_t default_value: "OFF" - name: osd_video_system - description: "Video system used. Possible values are `AUTO`, `PAL` and `NTSC`" + description: "Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, and `HD`" default_value: "AUTO" table: osd_video_system field: video_system @@ -3470,6 +3504,13 @@ groups: max: 6 default_value: 3 + - name: osd_system_msg_display_time + description: System message display cycle time for multiple messages (milliseconds). + field: system_msg_display_time + default_value: 1000 + min: 500 + max: 5000 + - name: PG_OSD_COMMON_CONFIG type: osdCommonConfig_t headers: ["io/osd_common.h"] @@ -3510,6 +3551,7 @@ groups: - name: name description: "Craft name" default_value: "" + max: MAX_NAME_LENGTH - name: PG_MODE_ACTIVATION_OPERATOR_CONFIG type: modeActivationOperatorConfig_t @@ -3541,6 +3583,7 @@ groups: default_value: 0 max: INT32_MAX - name: stats_total_energy + description: "Total energy consumption [in mWh]. The value is updated on every disarm when \"stats\" are enabled." max: INT32_MAX condition: USE_ADC default_value: 0 diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 43d399a13f..af974a3734 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -540,6 +540,17 @@ void FAST_CODE mixTable() motorValueWhenStopped = getReversibleMotorsThrottleDeadband(); mixerThrottleCommand = constrain(rcCommand[THROTTLE], throttleRangeMin, throttleRangeMax); + +#ifdef USE_DSHOT + if(isMotorProtocolDigital() && feature(FEATURE_REVERSIBLE_MOTORS) && reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) { + /* + * We need to start the throttle output from stick input to start in the middle of the stick at the low and. + * Without this, it's starting at the high side. + */ + int throttleDistanceToMax = throttleRangeMax - rcCommand[THROTTLE]; + mixerThrottleCommand = throttleRangeMin + throttleDistanceToMax; + } +#endif } else { mixerThrottleCommand = rcCommand[THROTTLE]; throttleRangeMin = throttleIdleValue; diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index 98ca4b9ba6..d9688f1e82 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -198,7 +198,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa tuneCurrent[axis].updateCount++; tuneCurrent[axis].absDesiredRateAccum += (absDesiredRate - tuneCurrent[axis].absDesiredRateAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES); tuneCurrent[axis].absReachedRateAccum += (absReachedRate - tuneCurrent[axis].absReachedRateAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES); - tuneCurrent[axis].absPidOutputAccum += (absPidOutput - tuneCurrent[axis].absPidOutputAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);; + tuneCurrent[axis].absPidOutputAccum += (absPidOutput - tuneCurrent[axis].absPidOutputAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES); if ((tuneCurrent[axis].updateCount & 25) == 0 && tuneCurrent[axis].updateCount >= AUTOTUNE_FIXED_WING_MIN_SAMPLES) { if (pidAutotuneConfig()->fw_rate_adjustment != FIXED && !FLIGHT_MODE(ANGLE_MODE)) { // Rate discovery is not possible in ANGLE mode diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index 21a0f613af..c8323f9940 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -22,6 +22,9 @@ * along with this program. If not, see http://www.gnu.org/licenses/. */ +#include "platform.h" +#ifdef USE_SECONDARY_IMU + #include "stdint.h" #include "build/debug.h" @@ -256,4 +259,6 @@ hardwareSensorStatus_e getHwSecondaryImuStatus(void) #else return HW_SENSOR_NONE; #endif -} \ No newline at end of file +} + +#endif diff --git a/src/main/io/displayport_hdzero_osd.c b/src/main/io/displayport_hdzero_osd.c new file mode 100644 index 0000000000..ee2462e5fc --- /dev/null +++ b/src/main/io/displayport_hdzero_osd.c @@ -0,0 +1,427 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include +#include + +#include "platform.h" + +FILE_COMPILE_FOR_SPEED + +//#define HDZERO_STATS + +#if defined(USE_OSD) && defined(USE_HDZERO_OSD) + +#include "common/utils.h" +#include "common/printf.h" +#include "common/time.h" +#include "common/bitarray.h" + +#include "drivers/display.h" +#include "drivers/display_font_metadata.h" +#include "drivers/osd_symbols.h" + +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" + +#include "displayport_hdzero_osd.h" + +#define FONT_VERSION 3 + +#define MSP_CLEAR_SCREEN 2 +#define MSP_WRITE_STRING 3 +#define MSP_DRAW_SCREEN 4 +#define MSP_SET_OPTIONS 5 +#define DRAW_FREQ_DENOM 4 // 60Hz +#define TX_BUFFER_SIZE 1024 +#define VTX_TIMEOUT 1000 // 1 second timer + +static mspProcessCommandFnPtr mspProcessCommand; +static mspPort_t hdZeroMspPort; +static displayPort_t hdZeroOsdDisplayPort; +static bool vtxSeen, vtxActive, vtxReset; +static timeMs_t vtxHeartbeat; + +// HD screen size +#define ROWS 18 +#define COLS 50 +#define SCREENSIZE (ROWS*COLS) +static uint8_t screen[SCREENSIZE]; +static BITARRAY_DECLARE(fontPage, SCREENSIZE); // font page for each character on the screen +static BITARRAY_DECLARE(dirty, SCREENSIZE); // change status for each character on the screen +static bool screenCleared; + +extern uint8_t cliMode; + +#ifdef HDZERO_STATS +static uint32_t dataSent; +static uint8_t resetCount; +#endif + +static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int len) +{ + UNUSED(displayPort); + + int sent = 0; + + if (!cliMode && vtxActive) { + sent = mspSerialPushPort(cmd, subcmd, len, &hdZeroMspPort, MSP_V1); + } + +#ifdef HDZERO_STATS + dataSent += sent; +#endif + + return sent; +} + +static void checkVtxPresent(void) +{ + if (vtxActive && (millis()-vtxHeartbeat) > VTX_TIMEOUT) { + vtxActive = false; + } +} + +static int setHdMode(displayPort_t *displayPort) +{ + checkVtxPresent(); + uint8_t subcmd[] = { MSP_SET_OPTIONS, 0, 1 }; // font selection, mode (SD/HD) + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); +} + +static void hdZeroInit(void) +{ + memset(screen, SYM_BLANK, sizeof(screen)); + BITARRAY_CLR_ALL(fontPage); + BITARRAY_CLR_ALL(dirty); +} + +static int clearScreen(displayPort_t *displayPort) +{ + uint8_t subcmd[] = { MSP_CLEAR_SCREEN }; + + hdZeroInit(); + setHdMode(displayPort); + screenCleared = true; + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); +} + +static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t *c, textAttributes_t *attr) +{ + UNUSED(displayPort); + + uint16_t pos = (row * COLS) + col; + if (pos >= SCREENSIZE) { + return false; + } + + *c = screen[pos]; + if (bitArrayGet(fontPage, pos)) { + *c |= 0x100; + } + + if (attr) { + *attr = TEXT_ATTRIBUTES_NONE; + } + + return true; +} + +static int setChar(const uint16_t pos, const uint16_t c) +{ + if (pos < SCREENSIZE) { + uint8_t ch = c & 0xFF; + bool page = (c >> 8); + if (screen[pos] != ch || bitArrayGet(fontPage, pos) != page) { + screen[pos] = ch; + (page) ? bitArraySet(fontPage, pos) : bitArrayClr(fontPage, pos); + bitArraySet(dirty, pos); + } + } + return 0; +} + +static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr) +{ + UNUSED(displayPort); + UNUSED(attr); + + return setChar((row * COLS) + col, c); +} + +static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string, textAttributes_t attr) +{ + UNUSED(displayPort); + UNUSED(attr); + + uint16_t pos = (row * COLS) + col; + while (*string) { + setChar(pos++, *string++); + } + return 0; +} + +#ifdef HDZERO_STATS +static void printStats(displayPort_t *displayPort, uint32_t updates) +{ + static timeMs_t lastTime; + static uint32_t maxDataSent, maxBufferUsed, maxUpdates; + timeMs_t currentTime = millis(); + char lineBuffer[100]; + + if (updates > maxUpdates) { + maxUpdates = updates; // updates sent per displayWrite + } + + uint32_t bufferUsed = TX_BUFFER_SIZE - serialTxBytesFree(hdZeroMspPort.port); + if (bufferUsed > maxBufferUsed) { + maxBufferUsed = bufferUsed; // serial buffer used after displayWrite + } + + uint32_t diff = (currentTime - lastTime); + if (diff > 1000) { // Data sampled in 1 second + if (dataSent > maxDataSent) { + maxDataSent = dataSent; // bps (max 11520 allowed) + } + + dataSent = 0; + lastTime = currentTime; + } + + + tfp_sprintf(lineBuffer, "R:%2d %4ld %5ld(%5ld) U:%2ld(%2ld) B:%3ld(%4ld,%4ld)", resetCount, (millis()-vtxHeartbeat), + dataSent, maxDataSent, updates, maxUpdates, bufferUsed, maxBufferUsed, hdZeroMspPort.port->txBufferSize); + writeString(displayPort, 0, 17, lineBuffer, 0); +} +#endif + +/** + * Write only changed characters to the VTX + */ +static int drawScreen(displayPort_t *displayPort) // 250Hz +{ + static uint8_t counter = 0; + + if ((counter++ % DRAW_FREQ_DENOM) == 0) { + uint8_t subcmd[COLS + 4]; + uint8_t updateCount = 0; + subcmd[0] = MSP_WRITE_STRING; + + int next = BITARRAY_FIND_FIRST_SET(dirty, 0); + while (next >= 0) { + // Look for sequential dirty characters on the same line for the same font page + int pos = next; + uint8_t row = pos / COLS; + uint8_t col = pos % COLS; + int endOfLine = row * COLS + COLS; + bool page = bitArrayGet(fontPage, pos); + + uint8_t len = 4; + do { + bitArrayClr(dirty, pos); + subcmd[len++] = screen[pos++]; + + if (bitArrayGet(dirty, pos)) { + next = pos; + } + } while (next == pos && next < endOfLine && bitArrayGet(fontPage, next) == page); + + subcmd[1] = row; + subcmd[2] = col; + subcmd[3] = page; + output(displayPort, MSP_DISPLAYPORT, subcmd, len); + updateCount++; + next = BITARRAY_FIND_FIRST_SET(dirty, pos); + } + + if (updateCount > 0 || screenCleared) { + if (screenCleared) { + screenCleared = false; + } + subcmd[0] = MSP_DRAW_SCREEN; + output(displayPort, MSP_DISPLAYPORT, subcmd, 1); + } + +#ifdef HDZERO_STATS + printStats(displayPort, updateCount); +#endif + checkVtxPresent(); + + if (vtxReset) { +#ifdef HDZERO_STATS + resetCount++; +#endif + clearScreen(displayPort); + vtxReset = false; + } + } + + return 0; +} + +static void resync(displayPort_t *displayPort) +{ + displayPort->rows = ROWS; + displayPort->cols = COLS; + setHdMode(displayPort); +} + +static int screenSize(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return SCREENSIZE; +} + +static uint32_t txBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return mspSerialTxBytesFree(); +} + +static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort) +{ + UNUSED(displayPort); + metadata->charCount = 512; + metadata->version = FONT_VERSION; + return true; +} + +static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return TEXT_ATTRIBUTES_NONE; +} + +static bool isTransferInProgress(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return false; +} + +static bool isReady(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return vtxActive; +} + +static int grab(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int heartbeat(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int release(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static const displayPortVTable_t hdzeroOsdVTable = { + .grab = grab, + .release = release, + .clearScreen = clearScreen, + .drawScreen = drawScreen, + .screenSize = screenSize, + .writeString = writeString, + .writeChar = writeChar, + .readChar = readChar, + .isTransferInProgress = isTransferInProgress, + .heartbeat = heartbeat, + .resync = resync, + .txBytesFree = txBytesFree, + .supportedTextAttributes = supportedTextAttributes, + .getFontMetadata = getFontMetadata, + .isReady = isReady, +}; + +bool hdzeroOsdSerialInit(void) +{ + static volatile uint8_t txBuffer[TX_BUFFER_SIZE]; + memset(&hdZeroMspPort, 0, sizeof(mspPort_t)); + + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_HDZERO_OSD); + if (portConfig) { + serialPort_t *port = openSerialPort(portConfig->identifier, FUNCTION_HDZERO_OSD, NULL, NULL, + baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); + + if (port) { + // Use a bigger TX buffer size to accommodate the configuration menus + port->txBuffer = txBuffer; + port->txBufferSize = TX_BUFFER_SIZE; + port->txBufferTail = 0; + port->txBufferHead = 0; + + resetMspPort(&hdZeroMspPort, port); + + return true; + } + } + + return false; +} + +displayPort_t* hdzeroOsdDisplayPortInit(void) +{ + if (hdzeroOsdSerialInit()) { + hdZeroInit(); + displayInit(&hdZeroOsdDisplayPort, &hdzeroOsdVTable); + return &hdZeroOsdDisplayPort; + } + return NULL; +} + +/* + * Intercept MSP processor. + * VTX sends an MSP command every 125ms or so. + * VTX will have be marked as not ready if no commands received within VTX_TIMEOUT. + */ +static mspResult_e hdZeroProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) +{ + if (vtxSeen && !vtxActive) { + vtxReset = true; + } + + vtxSeen = vtxActive = true; + vtxHeartbeat = millis(); + + // Process MSP command + return mspProcessCommand(cmd, reply, mspPostProcessFn); +} + +void hdzeroOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn) +{ + if (hdZeroMspPort.port) { + mspProcessCommand = mspProcessCommandFn; + mspSerialProcessOnePort(&hdZeroMspPort, MSP_SKIP_NON_MSP_DATA, hdZeroProcessMspCommand); + } +} + +#endif // USE_HDZERO_OSD diff --git a/src/main/io/displayport_hdzero_osd.h b/src/main/io/displayport_hdzero_osd.h new file mode 100644 index 0000000000..54d9f8480b --- /dev/null +++ b/src/main/io/displayport_hdzero_osd.h @@ -0,0 +1,33 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include "drivers/osd.h" +#include "msp/msp_serial.h" + +typedef struct displayPort_s displayPort_t; + +displayPort_t *hdzeroOsdDisplayPortInit(void); +void hdzeroOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn); diff --git a/src/main/io/gps_naza.c b/src/main/io/gps_naza.c index 9abef4c878..1b1a35e258 100644 --- a/src/main/io/gps_naza.c +++ b/src/main/io/gps_naza.c @@ -189,17 +189,12 @@ static bool NAZA_parse_gps(void) uint16_t pdop = decodeShort(_buffernaza.nav.pdop, mask); // pdop - //uint16_t vdop = decodeShort(_buffernaza.nav.vdop, mask); // vdop - //uint16_t ndop = decodeShort(_buffernaza.nav.ndop, mask); - //uint16_t edop = decodeShort(_buffernaza.nav.edop, mask); - //gpsSol.hdop = sqrtf(powf(ndop,2)+powf(edop,2)); - //gpsSol.vdop = decodeShort(_buffernaza.nav.vdop, mask); // vdop gpsSol.hdop = gpsConstrainEPE(pdop); // PDOP gpsSol.eph = gpsConstrainEPE(h_acc / 10); // hAcc in cm gpsSol.epv = gpsConstrainEPE(v_acc / 10); // vAcc in cm gpsSol.numSat = _buffernaza.nav.satellites; - gpsSol.groundSpeed = fast_fsqrtf(powf(gpsSol.velNED[X], 2) + powf(gpsSol.velNED[Y], 2)); //cm/s + gpsSol.groundSpeed = fast_fsqrtf(sq(gpsSol.velNED[X]) + sq(gpsSol.velNED[Y])); //cm/s // calculate gps heading from VELNE gpsSol.groundCourse = (uint16_t)(fmodf(RADIANS_TO_DECIDEGREES(atan2_approx(gpsSol.velNED[Y], gpsSol.velNED[X])) + 3600.0f, 3600.0f)); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 724eceb16b..bd499bc43d 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -196,8 +196,8 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 5); -PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 6); +PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1); static int digitCount(int32_t value) { @@ -965,7 +965,7 @@ static void osdFormatMessage(char *buff, size_t size, const char *message, bool strncpy(buff + rem / 2, message, MIN((int)size - rem / 2, (int)messageLength)); } // Ensure buff is zero terminated - buff[size - 1] = '\0'; + buff[size] = '\0'; } /** @@ -2015,7 +2015,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_CRSF_RSSI_DBM: { int16_t rssi = rxLinkStatistics.uplinkRSSI; - buff[0] = (rxLinkStatistics.activeAnt == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna + buff[0] = (rxLinkStatistics.activeAntenna == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna if (rssi <= -100) { tfp_sprintf(buff + 1, "%4d%c", rssi, SYM_DBM); } else { @@ -3138,11 +3138,11 @@ void osdDrawNextElement(void) } while(!osdDrawSingleElement(elementIndex) && index != elementIndex); // Draw artificial horizon + tracking telemtry last - osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); - if (osdConfig()->telemetry>0){ - osdDisplayTelemetry(); + osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + if (osdConfig()->telemetry>0){ + osdDisplayTelemetry(); + } } -} PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .rssi_alarm = SETTING_OSD_RSSI_ALARM_DEFAULT, @@ -3208,7 +3208,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT, .pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT, .esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT, - + .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT, .units = SETTING_OSD_UNITS_DEFAULT, .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, @@ -3541,7 +3541,7 @@ void osdInit(displayPort_t *osdDisplayPortToUse) if (!osdDisplayPortToUse) return; - BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(31,31)); + BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(63,63)); osdDisplayPort = osdDisplayPortToUse; @@ -4102,7 +4102,7 @@ void osdUpdate(timeUs_t currentTimeUs) osdUpdateStats(); } - if ((counter & DRAW_FREQ_DENOM) == 0) { + if ((counter % DRAW_FREQ_DENOM) == 0) { // redraw values in buffer osdRefresh(currentTimeUs); } else { @@ -4165,6 +4165,18 @@ displayCanvas_t *osdGetDisplayPortCanvas(void) return NULL; } +timeMs_t systemMessageCycleTime(unsigned messageCount, const char **messages){ + uint8_t i = 0; + float factor = 1.0f; + while (i < messageCount) { + if ((float)strlen(messages[i]) / 15.0f > factor) { + factor = (float)strlen(messages[i]) / 15.0f; + } + i++; + } + return osdConfig()->system_msg_display_time * factor; +} + textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenteredText) { textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; @@ -4193,13 +4205,17 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf); messages[messageCount++] = messageBuf; } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { - // WP hold time countdown in seconds - timeMs_t currentTime = millis(); - int holdTimeRemaining = posControl.waypointList[posControl.activeWaypointIndex].p1 - (int)((currentTime - posControl.wpReachedTime)/1000); - if (holdTimeRemaining >=0) { + if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ADJUSTING_WP_ALT); + } else { + // WP hold time countdown in seconds + timeMs_t currentTime = millis(); + int holdTimeRemaining = posControl.waypointList[posControl.activeWaypointIndex].p1 - (int)(MS2S(currentTime - posControl.wpReachedTime)); + holdTimeRemaining = holdTimeRemaining >= 0 ? holdTimeRemaining : 0; tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining); + + messages[messageCount++] = messageBuf; } - messages[messageCount++] = messageBuf; } else { const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { @@ -4292,7 +4308,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } if (messageCount > 0) { - message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; + message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), messageCount)]; if (message == failsafeInfoMessage) { // failsafeInfoMessage is not useful for recovering // a lost model, but might help avoiding a crash. diff --git a/src/main/io/osd.h b/src/main/io/osd.h index a690ca7f8a..be32f84180 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -29,12 +29,21 @@ #endif #define OSD_LAYOUT_COUNT (OSD_ALTERNATE_LAYOUT_COUNT + 1) -#define OSD_VISIBLE_FLAG 0x0800 +// 00vb yyyy yyxx xxxx +// (visible)(blink)(yCoord)(xCoord) + +#define OSD_VISIBLE_FLAG 0x2000 #define OSD_VISIBLE(x) ((x) & OSD_VISIBLE_FLAG) -#define OSD_POS(x,y) ((x) | ((y) << 5)) -#define OSD_X(x) ((x) & 0x001F) -#define OSD_Y(x) (((x) >> 5) & 0x001F) -#define OSD_POS_MAX 0x3FF + +#define OSD_POS(x,y) (((x) & 0x3F) | (((y) & 0x3F) << 6)) +#define OSD_X(x) ((x) & 0x3F) +#define OSD_Y(x) (((x) >> 6) & 0x3F) +#define OSD_POS_MAX 0xFFF + +// For DJI compatibility +#define OSD_VISIBLE_FLAG_SD 0x0800 +#define OSD_POS_SD(x,y) (((x) & 0x1F) | (((y) & 0x1F) << 5)) + #define OSD_POS_MAX_CLI (OSD_POS_MAX | OSD_VISIBLE_FLAG) #define OSD_HOMING_LIM_H1 6 @@ -85,6 +94,7 @@ #define OSD_MSG_HEADING_HOME "EN ROUTE TO HOME" #define OSD_MSG_WP_FINISHED "WP END>HOLDING POSITION" #define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT" +#define OSD_MSG_ADJUSTING_WP_ALT "ADJUSTING WP ALTITUDE" #define OSD_MSG_MISSION_PLANNER "(WP MISSION PLANNER)" #define OSD_MSG_WP_RTH_CANCEL "CANCEL WP TO EXIT RTH" #define OSD_MSG_WP_MISSION_LOADED "* MISSION LOADED *" @@ -391,6 +401,7 @@ typedef struct osdConfig_s { uint8_t pan_servo_index; // Index of the pan servo used for home direction offset int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm uint8_t crsf_lq_format; + uint16_t system_msg_display_time; // system message display time for multiple messages (ms) uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows uint8_t telemetry; // use telemetry on displayed pixel line 0 uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers. diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 3ebb3200e1..f8f88904e5 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -357,33 +357,34 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst) const bool itemIsSupported = ((djiOSDItemIndexMap[i].depFeature == 0) || feature(djiOSDItemIndexMap[i].depFeature)); if (inavOSDIdx >= 0 && itemIsSupported) { - // Position & visibility encoded in 16 bits. Position encoding is the same between BF/DJI and INAV - // However visibility is different. INAV has 3 layouts, while BF only has visibility profiles - // Here we use only one OSD layout mapped to first OSD BF profile + // Position & visibility are encoded in 16 bits, and is the same between BF/DJI. + // However INAV supports co-ords of 0-63 and has 3 layouts, while BF has co-ords 0-31 and visibility profiles. + // Re-encode for co-ords of 0-31 and map the layout to all three BF profiles. uint16_t itemPos = osdLayoutsConfig()->item_pos[0][inavOSDIdx]; + uint16_t itemPosSD = OSD_POS_SD(OSD_X(itemPos), OSD_Y(itemPos)); // Workarounds for certain OSD element positions // INAV calculates these dynamically, while DJI expects the config to have defined coordinates switch(inavOSDIdx) { case OSD_CROSSHAIRS: - itemPos = (itemPos & (~OSD_POS_MAX)) | OSD_POS(13, 6); + itemPosSD = OSD_POS_SD(15, 8); break; case OSD_ARTIFICIAL_HORIZON: - itemPos = (itemPos & (~OSD_POS_MAX)) | OSD_POS(14, 2); + itemPosSD = OSD_POS_SD(9, 8); break; case OSD_HORIZON_SIDEBARS: - itemPos = (itemPos & (~OSD_POS_MAX)) | OSD_POS(14, 5); + itemPosSD = OSD_POS_SD(16, 7); break; } // Enforce visibility in 3 BF OSD profiles if (OSD_VISIBLE(itemPos)) { - itemPos |= 0x3000; + itemPosSD |= (0x3000 | OSD_VISIBLE_FLAG_SD); } - sbufWriteU16(dst, itemPos); + sbufWriteU16(dst, itemPosSD); } else { // Hide OSD elements unsupported by INAV @@ -1051,7 +1052,7 @@ static bool djiFormatMessages(char *buff) // Pick one of the available messages. Each message lasts // a second. if (messageCount > 0) { - strcpy(buff, messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]);; + strcpy(buff, messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]); haveMessage = true; } } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 20d389048c..3b2b5d68a6 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -55,7 +55,8 @@ typedef enum { FUNCTION_DJI_HD_OSD = (1 << 21), // 2097152 FUNCTION_SERVO_SERIAL = (1 << 22), // 4194304 FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608 - FUNCTION_IMU2 = (1 << 24), // 16777216 + FUNCTION_IMU2 = (1 << 24), // 16777216 + FUNCTION_HDZERO_OSD = (1 << 25), // 33554432 } serialPortFunction_e; typedef enum { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 0e54290a8a..a131ae832a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -60,11 +60,12 @@ #include "sensors/acceleration.h" #include "sensors/boardalignment.h" - +#define WP_ALTITUDE_MARGIN_CM 100 // WP enforce altitude tolerance, used when WP altitude setting enforced when WP reached // Multirotors: #define MR_RTH_CLIMB_OVERSHOOT_CM 100 // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt) #define MR_RTH_CLIMB_MARGIN_MIN_CM 100 // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend) #define MR_RTH_CLIMB_MARGIN_PERCENT 15 // on high RTH altitudes use even bigger margin - percent of the altitude set + // Planes: #define FW_RTH_CLIMB_OVERSHOOT_CM 100 #define FW_RTH_CLIMB_MARGIN_MIN_CM 100 @@ -97,7 +98,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 1); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 14); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 15); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -117,8 +118,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT, .safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_DEFAULT, .mission_planner_reset = SETTING_NAV_MISSION_PLANNER_RESET_DEFAULT, // Allow mode switch toggle to reset Mission Planner WPs - .waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT, // WP mission restart action + .waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT, // WP mission restart action .soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled + .waypoint_enforce_altitude = SETTING_NAV_WP_ENFORCE_ALTITUDE_DEFAULT, // Forces set wp altitude to be achieved }, // General navigation parameters @@ -243,6 +245,7 @@ void calculateInitialHoldPosition(fpVector3_t * pos); void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance); void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance); static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome); +bool isWaypointAltitudeReached(void); static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv); static navigationFSMEvent_t nextForNonGeoStates(void); static bool isWaypointMissionValid(void); @@ -817,7 +820,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, } }, @@ -835,7 +837,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_FINISHED, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, } }, @@ -1482,6 +1483,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]); posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; + posControl.wpAltitudeReached = false; return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS // We use p3 as the volatile jump counter (p2 is the static value) @@ -1583,9 +1585,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga { UNUSED(previousState); + if (navConfig()->general.flags.waypoint_enforce_altitude) { + posControl.wpAltitudeReached = isWaypointAltitudeReached(); + } + switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) { case NAV_WP_ACTION_WAYPOINT: - return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT + if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { + return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME; + } else { + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT + } case NAV_WP_ACTION_JUMP: case NAV_WP_ACTION_SET_HEAD: @@ -1614,6 +1624,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } + if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { + // Adjust altitude to waypoint setting + if (STATE(AIRPLANE)) { + int8_t altitudeChangeDirection = posControl.activeWaypoint.pos.z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; + updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); + } else { + setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z); + } + + posControl.wpAltitudeReached = isWaypointAltitudeReached(); + + if (posControl.wpAltitudeReached) { + posControl.wpReachedTime = millis(); + } else { + return NAV_FSM_EVENT_NONE; + } + } + timeMs_t currentTime = millis(); if (posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0 || @@ -1621,7 +1649,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi return NAV_FSM_EVENT_SUCCESS; } - return NAV_FSM_EVENT_NONE; // will re-process state in >10ms } @@ -2132,6 +2159,11 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp return isWaypointPositionReached(&waypoint->pos, isWaypointHome); } +bool isWaypointAltitudeReached(void) +{ + return ABS(navGetCurrentActualPositionAndVelocity()->pos.z - posControl.activeWaypoint.pos.z) < WP_ALTITUDE_MARGIN_CM; +} + static void updateHomePositionCompatibility(void) { geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos); @@ -2596,7 +2628,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti lastUpdateTimeUs = currentTimeUs; posControl.desiredState.pos.z = altitudeToUse; } - else { + else { // ROC_TO_ALT_NORMAL /* * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0 diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 05b077a3c0..201cd502cf 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -222,6 +222,7 @@ typedef struct navConfig_s { uint8_t soaring_motor_stop; // stop motor when Soaring mode enabled uint8_t mission_planner_reset; // Allow WP Mission Planner reset using mode toggle (resets WPs to 0) uint8_t waypoint_mission_restart; // Waypoint mission restart action + uint8_t waypoint_enforce_altitude; // Forces waypoint altitude to be achieved } flags; uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 2247d05835..89cccaf875 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -493,7 +493,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA const float setpointX = posControl.desiredState.vel.x; const float setpointY = posControl.desiredState.vel.y; - const float setpointXY = fast_fsqrtf(powf(setpointX, 2)+powf(setpointY, 2)); + const float setpointXY = fast_fsqrtf(sq(setpointX) + sq(setpointY)); // Calculate velocity error const float velErrorX = setpointX - measurementX; @@ -696,20 +696,20 @@ bool isMulticopterLandingDetected(void) } // Average climb rate should be low enough - bool verticalMovement = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) > 25.0f; + bool verticalMovement = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) > MC_LAND_CHECK_VEL_Z_MOVING; // check if we are moving horizontally - bool horizontalMovement = posControl.actualState.velXY > 100.0f; + bool horizontalMovement = posControl.actualState.velXY > MC_LAND_CHECK_VEL_XY_MOVING; // We have likely landed if throttle is 40 units below average descend throttle // We use rcCommandAdjustedThrottle to keep track of NAV corrected throttle (isLandingDetected is executed // from processRx() and rcCommand at that moment holds rc input, not adjusted values from NAV core) // Wait for 1 second so throttle has stabilized. bool isAtMinimalThrust = false; - if (currentTimeUs - landingDetectorStartedAt > 1000 * 1000) { + if (currentTimeUs - landingDetectorStartedAt > HZ2US(MC_LAND_THR_SUM_RATE)) { landingThrSamples += 1; landingThrSum += rcCommandAdjustedThrottle; - isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - 40); + isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - MC_LAND_DESCEND_THROTTLE); } bool possibleLandingDetected = isAtMinimalThrust && !verticalMovement && !horizontalMovement; @@ -719,7 +719,7 @@ bool isMulticopterLandingDetected(void) // TODO: Come up with a clever way to let sonar increase detection performance, not just add extra safety. // TODO: Out of range sonar may give reading that looks like we landed, find a way to check if sonar is healthy. // surfaceMin is our ground reference. If we are less than 5cm above the ground - we are likely landed - possibleLandingDetected = possibleLandingDetected && (posControl.actualState.agl.pos.z <= (posControl.actualState.surfaceMin + 5.0f)); + possibleLandingDetected = possibleLandingDetected && (posControl.actualState.agl.pos.z <= (posControl.actualState.surfaceMin + MC_LAND_SAFE_SURFACE)); } if (!possibleLandingDetected) { @@ -727,7 +727,7 @@ bool isMulticopterLandingDetected(void) return false; } else { - return ((currentTimeUs - landingTimer) > (navConfig()->mc.auto_disarm_delay * 1000)) ? true : false; + return ((currentTimeUs - landingTimer) > MS2US(navConfig()->mc.auto_disarm_delay)) ? true : false; } } diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 043c18d8a6..508f2a222a 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -49,6 +49,7 @@ #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/compass.h" +#include "sensors/gyro.h" #include "sensors/pitotmeter.h" #include "sensors/opflow.h" @@ -351,11 +352,19 @@ void updatePositionEstimator_PitotTopic(timeUs_t currentTimeUs) */ static void restartGravityCalibration(void) { + if (!gyroConfig()->init_gyro_cal_enabled) { + return; + } + zeroCalibrationStartS(&posEstimator.imu.gravityCalibration, CALIBRATING_GRAVITY_TIME_MS, positionEstimationConfig()->gravity_calibration_tolerance, false); } static bool gravityCalibrationComplete(void) -{ +{ + if (!gyroConfig()->init_gyro_cal_enabled) { + return true; + } + return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration); } @@ -391,9 +400,9 @@ static void updateIMUTopic(timeUs_t currentTimeUs) posEstimator.imu.lastUpdateTime = currentTimeUs; if (!isImuReady()) { - posEstimator.imu.accelNEU.x = 0; - posEstimator.imu.accelNEU.y = 0; - posEstimator.imu.accelNEU.z = 0; + posEstimator.imu.accelNEU.x = 0.0f; + posEstimator.imu.accelNEU.y = 0.0f; + posEstimator.imu.accelNEU.z = 0.0f; restartGravityCalibration(); } @@ -422,13 +431,19 @@ static void updateIMUTopic(timeUs_t currentTimeUs) posEstimator.imu.accelNEU.z = accelBF.z; /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */ - if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) { - zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z); + if (gyroConfig()->init_gyro_cal_enabled) { + if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) { + zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z); - if (gravityCalibrationComplete()) { - zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS); - LOG_D(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS)); + if (gravityCalibrationComplete()) { + zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS); + setGravityCalibrationAndWriteEEPROM(posEstimator.imu.calibratedGravityCMSS); + LOG_D(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS)); + } } + } else { + posEstimator.imu.gravityCalibration.params.state = ZERO_CALIBRATION_DONE; + posEstimator.imu.calibratedGravityCMSS = gyroConfig()->gravity_cmss_cal; } /* If calibration is incomplete - report zero acceleration */ @@ -436,9 +451,9 @@ static void updateIMUTopic(timeUs_t currentTimeUs) posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS; } else { - posEstimator.imu.accelNEU.x = 0; - posEstimator.imu.accelNEU.y = 0; - posEstimator.imu.accelNEU.z = 0; + posEstimator.imu.accelNEU.x = 0.0f; + posEstimator.imu.accelNEU.y = 0.0f; + posEstimator.imu.accelNEU.z = 0.0f; } /* Update blackbox values */ diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 67497f5ec2..32de6f7451 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -35,6 +35,12 @@ #define INAV_SURFACE_MAX_DISTANCE 40 +#define MC_LAND_CHECK_VEL_XY_MOVING 100.0f // cm/s +#define MC_LAND_CHECK_VEL_Z_MOVING 25.0f // cm/s +#define MC_LAND_THR_SUM_RATE 1 // hz +#define MC_LAND_DESCEND_THROTTLE 40 // uS +#define MC_LAND_SAFE_SURFACE 5.0f // cm + #define MAX_POSITION_UPDATE_INTERVAL_US HZ2US(MIN_POSITION_UPDATE_RATE_HZ) // convenience macro _Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!"); @@ -371,12 +377,13 @@ typedef struct { int8_t loadedMultiMissionStartWP; // selected multi mission start WP int8_t loadedMultiMissionWPCount; // number of WPs in selected multi mission #endif - navWaypointPosition_t activeWaypoint; // Local position and initial bearing, filled on waypoint activation + navWaypointPosition_t activeWaypoint; // Local position and initial bearing, filled on waypoint activation int8_t activeWaypointIndex; - float wpInitialAltitude; // Altitude at start of WP - float wpInitialDistance; // Distance when starting flight to WP - float wpDistance; // Distance to active WP - timeMs_t wpReachedTime; // Time the waypoint was reached + float wpInitialAltitude; // Altitude at start of WP + float wpInitialDistance; // Distance when starting flight to WP + float wpDistance; // Distance to active WP + timeMs_t wpReachedTime; // Time the waypoint was reached + bool wpAltitudeReached; // WP altitude achieved /* Internals & statistics */ int16_t rcAdjustment[4]; diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 9ad94591e8..0fc6d0d6fd 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -303,6 +303,14 @@ static int logicConditionCompute( temporaryValue = (operandB == 0) ? 500 : operandB; return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; break; + + case LOGIC_CONDITION_MIN: + return (operandA < operandB) ? operandA : operandB; + break; + + case LOGIC_CONDITION_MAX: + return (operandA > operandB) ? operandA : operandB; + break; case LOGIC_CONDITION_MAP_INPUT: return scaleRange(constrain(operandA, 0, operandB), 0, operandB, 0, 1000); @@ -423,6 +431,11 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE: // V / 10 return getBatteryAverageCellVoltage(); break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS: + return getBatteryCellCount(); + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100 return getAmperage(); break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 4e32bd0ac9..69f3d9c7d9 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -72,7 +72,9 @@ typedef enum { LOGIC_CONDITION_MODULUS = 40, LOGIC_CONDITION_LOITER_OVERRIDE = 41, LOGIC_CONDITION_SET_PROFILE = 42, - LOGIC_CONDITION_LAST = 43, + LOGIC_CONDITION_MIN = 43, + LOGIC_CONDITION_MAX = 44, + LOGIC_CONDITION_LAST = 45, } logicOperation_e; typedef enum logicOperandType_s { @@ -124,6 +126,7 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 34 LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS, // 35 LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 36 + LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS, // 37 } logicFlightOperands_e; diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 01b9a72abd..9aace4660a 100755 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -48,6 +48,7 @@ FILE_COMPILE_FOR_SPEED #define CRSF_DIGITAL_CHANNEL_MIN 172 #define CRSF_DIGITAL_CHANNEL_MAX 1811 #define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type) +#define CRSF_POWER_COUNT 9 STATIC_UNIT_TESTED bool crsfFrameDone = false; STATIC_UNIT_TESTED crsfFrame_t crsfFrame; @@ -59,8 +60,7 @@ static timeUs_t crsfFrameStartAt = 0; static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX]; static uint8_t telemetryBufLen = 0; -// The power levels represented by uplinkTXPower above in mW (250mW added to full TX in v4.00 firmware, 50mW added for ExpressLRS) -const uint16_t crsfPowerStates[] = {0, 10, 25, 100, 500, 1000, 2000, 250, 50}; +const uint16_t crsfTxPowerStatesmW[CRSF_POWER_COUNT] = {0, 10, 25, 100, 500, 1000, 2000, 250, 50}; /* * CRSF protocol @@ -122,7 +122,6 @@ typedef struct crsfPayloadLinkStatistics_s { uint8_t downlinkRSSI; uint8_t downlinkLQ; int8_t downlinkSNR; - uint8_t activeAnt; } __attribute__ ((__packed__)) crsfPayloadLinkStatistics_t; typedef struct crsfPayloadLinkStatistics_s crsfPayloadLinkStatistics_t; @@ -234,13 +233,14 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) crsfFrame.frame.frameLength = CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC; const crsfPayloadLinkStatistics_t* linkStats = (crsfPayloadLinkStatistics_t*)&crsfFrame.frame.payload; + const uint8_t crsftxpowerindex = (linkStats->uplinkTXPower < CRSF_POWER_COUNT) ? linkStats->uplinkTXPower : 0; rxLinkStatistics.uplinkRSSI = -1* (linkStats->activeAntenna ? linkStats->uplinkRSSIAnt2 : linkStats->uplinkRSSIAnt1); rxLinkStatistics.uplinkLQ = linkStats->uplinkLQ; rxLinkStatistics.uplinkSNR = linkStats->uplinkSNR; rxLinkStatistics.rfMode = linkStats->rfMode; - rxLinkStatistics.uplinkTXPower = crsfPowerStates[linkStats->uplinkTXPower]; - rxLinkStatistics.activeAnt = linkStats->activeAntenna; + rxLinkStatistics.uplinkTXPower = crsfTxPowerStatesmW[crsftxpowerindex]; + rxLinkStatistics.activeAntenna = linkStats->activeAntenna; if (rxLinkStatistics.uplinkLQ > 0) { int16_t uplinkStrength; // RSSI dBm converted to % diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 5b00281905..665f6d6e60 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -191,7 +191,7 @@ typedef struct rxLinkStatistics_s { int8_t uplinkSNR; // The SNR of the uplink in dB uint8_t rfMode; // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz] uint16_t uplinkTXPower; // power in mW - uint8_t activeAnt; + uint8_t activeAntenna; } rxLinkStatistics_t; extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount @@ -227,5 +227,5 @@ uint16_t rxGetRefreshRate(void); // Processed RC channel value. These values might include // filtering and some extra processing like value holding -// during failsafe. +// during failsafe. int16_t rxGetChannelValue(unsigned channelNumber); diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index 2825440b49..ed36e3e23c 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -227,7 +227,11 @@ bool isHardwareHealthy(void) const hardwareSensorStatus_e pitotStatus = getHwPitotmeterStatus(); const hardwareSensorStatus_e gpsStatus = getHwGPSStatus(); const hardwareSensorStatus_e opflowStatus = getHwOpticalFlowStatus(); +#ifdef USE_SECONDARY_IMU const hardwareSensorStatus_e imu2Status = getHwSecondaryImuStatus(); +#else + const hardwareSensorStatus_e imu2Status = HW_SENSOR_NONE; +#endif // Sensor is considered failing if it's either unavailable (selected but not detected) or unhealthy (returning invalid readings) if (gyroStatus == HW_SENSOR_UNAVAILABLE || gyroStatus == HW_SENSOR_UNHEALTHY) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 22a1ee5521..816139841f 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -78,7 +78,7 @@ FILE_COMPILE_FOR_SPEED FASTRAM gyro_t gyro; // gyro sensor object -#define MAX_GYRO_COUNT 1 +#define MAX_GYRO_COUNT 1 STATIC_UNIT_TESTED gyroDev_t gyroDev[MAX_GYRO_COUNT]; // Not in FASTRAM since it may hold DMA buffers STATIC_FASTRAM int16_t gyroTemperature[MAX_GYRO_COUNT]; @@ -97,7 +97,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 3); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, @@ -124,6 +124,9 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT, .kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT, #endif + .init_gyro_cal_enabled = SETTING_INIT_GYRO_CAL_DEFAULT, + .gyro_zero_cal = {SETTING_GYRO_ZERO_X_DEFAULT, SETTING_GYRO_ZERO_Y_DEFAULT, SETTING_GYRO_ZERO_Z_DEFAULT}, + .gravity_cmss_cal = SETTING_INS_GRAVITY_CMSS_DEFAULT, ); STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware) @@ -328,6 +331,12 @@ void gyroStartCalibration(void) return; } +#ifndef USE_IMU_FAKE // fixes Test Unit compilation error + if (!gyroConfig()->init_gyro_cal_enabled) { + return; + } +#endif + zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false); } @@ -336,6 +345,12 @@ bool gyroIsCalibrationComplete(void) if (!gyro.initialized) { return true; } + +#ifndef USE_IMU_FAKE // fixes Test Unit compilation error + if (!gyroConfig()->init_gyro_cal_enabled) { + return true; + } +#endif return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]); } @@ -358,10 +373,13 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe dev->gyroZero[Y] = v.v[Y]; dev->gyroZero[Z] = v.v[Z]; +#ifndef USE_IMU_FAKE // fixes Test Unit compilation error + setGyroCalibrationAndWriteEEPROM(dev->gyroZero); +#endif + LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics - } - else { + } else { dev->gyroZero[X] = 0; dev->gyroZero[Y] = 0; dev->gyroZero[Z] = 0; @@ -380,8 +398,21 @@ void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate) static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroCalibrationVector_t * gyroCal, float * gyroADCf) { + // range: +/- 8192; +/- 2000 deg/sec if (gyroDev->readFn(gyroDev)) { + +#ifndef USE_IMU_FAKE // fixes Test Unit compilation error + if (!gyroConfig()->init_gyro_cal_enabled) { + // marks that the gyro calibration has ended + gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE; + // pass the calibration values + gyroDev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X]; + gyroDev->gyroZero[Y] = gyroConfig()->gyro_zero_cal[Y]; + gyroDev->gyroZero[Z] = gyroConfig()->gyro_zero_cal[Z]; + } +#endif + if (zeroCalibrationIsCompleteV(gyroCal)) { int32_t gyroADCtmp[XYZ_AXIS_COUNT]; @@ -534,4 +565,4 @@ void gyroUpdateDynamicLpf(float cutoffFreq) { biquadFilterUpdate(&gyroLpf2State[axis].biquad, cutoffFreq, getLooptime(), BIQUAD_Q, FILTER_LPF); } } -} +} \ No newline at end of file diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index cbc4d72cda..bc9b6d0c28 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -77,6 +77,9 @@ typedef struct gyroConfig_s { uint16_t kalman_q; uint8_t kalmanEnabled; #endif + bool init_gyro_cal_enabled; + int16_t gyro_zero_cal[XYZ_AXIS_COUNT]; + float gravity_cmss_cal; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); @@ -90,4 +93,4 @@ bool gyroIsCalibrationComplete(void); bool gyroReadTemperature(void); int16_t gyroGetTemperature(void); int16_t gyroRateDps(int axis); -void gyroUpdateDynamicLpf(float cutoffFreq); +void gyroUpdateDynamicLpf(float cutoffFreq); \ No newline at end of file diff --git a/src/main/target/FOXEERF745AIO/CMakeLists.txt b/src/main/target/FOXEERF745AIO/CMakeLists.txt new file mode 100644 index 0000000000..a0c1257288 --- /dev/null +++ b/src/main/target/FOXEERF745AIO/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f745xg(FOXEERF745AIO) diff --git a/src/main/target/FOXEERF745AIO/target.c b/src/main/target/FOXEERF745AIO/target.c new file mode 100644 index 0000000000..28786c70ba --- /dev/null +++ b/src/main/target/FOXEERF745AIO/target.c @@ -0,0 +1,37 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include "drivers/io.h" +#include "drivers/bus.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" +#include "drivers/pwm_mapping.h" + +const timerHardware_t timerHardware[] = { + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP - D(2, 6, 0) + + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), // M1 - D(1, 4, 5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR, 0, 0), // M2 - D(1, 5, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // M3 - D(1, 2, 5) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 0, 0), // M4 - D(1, 7, 5) + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FOXEERF745AIO/target.h b/src/main/target/FOXEERF745AIO/target.h new file mode 100644 index 0000000000..dd1f307a3e --- /dev/null +++ b/src/main/target/FOXEERF745AIO/target.h @@ -0,0 +1,146 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "FXF5" +#define USBD_PRODUCT_STRING "FOXEERF745AIO" + +/*** Indicators ***/ +#define LED0 PC13 +#define BEEPER PD2 +#define BEEPER_INVERTED + +/*** IMU sensors ***/ +#define USE_EXTI + + +#define USE_MPU_DATA_READY_SIGNAL + +// MPU6000 +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW180_DEG +#define MPU6000_CS_PIN PA15 +#define MPU6000_SPI_BUS BUS_SPI3 +#define MPU6000_EXTI_PIN PD0 + + +/*** SPI/I2C bus ***/ +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE2 +#define SPI4_MISO_PIN PE5 +#define SPI4_MOSI_PIN PE6 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +/*** Onboard flash ***/ +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PE4 +#define M25P16_SPI_BUS BUS_SPI4 + +/*** OSD ***/ +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PA4 + +/*** Serial ports ***/ +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART7 +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 + + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_VBAT | FEATURE_CURRENT_METER) + +/*** BARO & MAG ***/ +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 + +/*** ADC ***/ +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC3 +#define ADC_CHANNEL_2_PIN PC5 +#define ADC_CHANNEL_3_PIN PC2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 + +/*** LED STRIP ***/ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +/*** Default settings ***/ +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define SERIALRX_UART SERIAL_PORT_USART1 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define CURRENT_METER_SCALE 100 + +/*** Timer/PWM output ***/ +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define MAX_PWM_OUTPUT_PORTS 4 +#define USE_DSHOT +#define USE_ESC_SENSOR + +/*** Used pins ***/ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff diff --git a/src/main/target/MAMBAF722_X8/CMakeLists.txt b/src/main/target/MAMBAF722_X8/CMakeLists.txt new file mode 100644 index 0000000000..dcec183c1c --- /dev/null +++ b/src/main/target/MAMBAF722_X8/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(MAMBAF722_X8) \ No newline at end of file diff --git a/src/main/target/MAMBAF722_X8/config.c b/src/main/target/MAMBAF722_X8/config.c new file mode 100644 index 0000000000..3165a2efb4 --- /dev/null +++ b/src/main/target/MAMBAF722_X8/config.c @@ -0,0 +1,61 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include + +#include + +#include "common/axis.h" + +#include "config/config_master.h" +#include "config/feature.h" + +#include "drivers/sensor.h" +#include "drivers/pwm_esc_detect.h" +#include "drivers/pwm_output.h" +#include "drivers/serial.h" + +#include "fc/rc_controls.h" + +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/pid.h" + +#include "rx/rx.h" + +#include "io/serial.h" + +#include "sensors/battery.h" +#include "sensors/sensors.h" + +#include "telemetry/telemetry.h" + +void targetConfiguration(void) +{ + + /* + * UART1 is SerialRX + */ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL; + + /* + * Enable MSP at 115200 at UART4 + */ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200; +} diff --git a/src/main/target/MAMBAF722_X8/target.c b/src/main/target/MAMBAF722_X8/target.c new file mode 100644 index 0000000000..f630fdfa26 --- /dev/null +++ b/src/main/target/MAMBAF722_X8/target.c @@ -0,0 +1,43 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + + +#include + +#include "platform.h" +#include "drivers/io.h" +#include "drivers/timer.h" +#include "drivers/pwm_mapping.h" +#include "drivers/bus.h" + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S3 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4 + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S6 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S7 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S8 + + DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – D(1, 6, 3) +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MAMBAF722_X8/target.h b/src/main/target/MAMBAF722_X8/target.h new file mode 100644 index 0000000000..c9d86ec22e --- /dev/null +++ b/src/main/target/MAMBAF722_X8/target.h @@ -0,0 +1,172 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define USE_TARGET_CONFIG + +#define TARGET_BOARD_IDENTIFIER "M7X8" +#define USBD_PRODUCT_STRING "MAMBAF722_X8" + +// ******** Board LEDs ********************** +#define LED0 PC15 +#define LED1 PC14 + +// ******* Beeper *********** +#define BEEPER PB2 +#define BEEPER_INVERTED + +// ******* GYRO and ACC ******** +#define USE_EXTI +#define GYRO_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + +#define MPU6000_CS_PIN SPI1_NSS_PIN +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW180_DEG + +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +#define DEFAULT_I2C_BUS BUS_I2C1 + + +// *************** Baro ************************** +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 + +//*********** Magnetometer / Compass ************* +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS + +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +// ******* SERIAL ******** +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +#define USE_UART6 + +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 7 + +// ******* SPI ******** +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +// ******* ADC ******** +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC3 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 + +#define VBAT_SCALE_DEFAULT 1100 + +// ******* OSD ******** +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + +//******* FLASH ******** +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN SPI3_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI3 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +//************ LEDSTRIP ***************** +#define USE_LED_STRIP +#define WS2811_PIN PB3 + +// ******* FEATURES ******** +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART1 +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY) + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 8 +#define TARGET_MOTOR_COUNT 8 + +// ESC-related features +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define PITOT_I2C_BUS DEFAULT_I2C_BUS + +#define BNO055_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/MATEKH743/target.c b/src/main/target/MATEKH743/target.c index 08a7dae478..723c084e40 100644 --- a/src/main/target/MATEKH743/target.c +++ b/src/main/target/MATEKH743/target.c @@ -47,7 +47,7 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S12 DMA_NONE - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 8), // LED_2812 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // RX6 PPM diff --git a/src/main/target/SPEEDYBEEF7V2/target.h b/src/main/target/SPEEDYBEEF7V2/target.h index 0e5f5d190a..dbd4263db0 100644 --- a/src/main/target/SPEEDYBEEF7V2/target.h +++ b/src/main/target/SPEEDYBEEF7V2/target.h @@ -137,6 +137,17 @@ #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 +// *************** LED ***************************** +#define USE_LED_STRIP +#define WS2811_PIN PB0 + +// ********** Optiical Flow adn Lidar ************** +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_OPFLOW +#define USE_OPFLOW_MSP + + #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) #define USE_DSHOT diff --git a/src/main/target/common.h b/src/main/target/common.h index cafacb493c..02218bd63d 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -129,6 +129,7 @@ #define USE_OSD #define USE_FRSKYOSD #define USE_DJI_HD_OSD +#define USE_HDZERO_OSD #define USE_SMARTPORT_MASTER #define NAV_NON_VOLATILE_WAYPOINT_CLI diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index f80ecf7db3..579f1f9445 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -137,7 +137,6 @@ TEST(SensorGyro, Update) extern "C" { static timeMs_t milliTime = 0; - timeMs_t millis(void) {return milliTime++;} uint32_t micros(void) {return 0;} void beeper(beeperMode_e) {}