Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

Merge branch 'master' into dzikuvx-bno055-secondary-imu

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2021-04-01 19:09:04 +02:00
commit 2b13c855ef
64 changed files with 1495 additions and 254 deletions
Download patch file Download diff file Expand all files Collapse all files

1
AUTHORS
View file

@ -7,6 +7,7 @@ Alberto García Hierro
Alex Gorbatchev
Alex Zaitsev
Alexander Fedorov
Alexander van Saase
Alexey Stankevich
Andre Bernet
Andreas Tacke

2
CMakeLists.txt
View file

@ -29,7 +29,7 @@ else()
include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake")
endif()
project(INAV VERSION 2.7.0)
project(INAV VERSION 3.0.0)
enable_language(ASM)

6
docs/Programming Framework.md
View file

@ -73,6 +73,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 37 | MAP_OUTPUT | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled |
| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` |
| 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
| 40 | MOD | Divide `Operand A` by `Operand B` and returns the remainder |
### Operands
@ -95,8 +96,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 1 | HOME_DISTANCE | in `meters` |
| 2 | TRIP_DISTANCE | in `meters` |
| 3 | RSSI | |
| 4 | VBAT | in `Volts * 10`, eg. `12.1V` is `121` |
| 5 | CELL_VOLTAGE | in `Volts * 10`, eg. `12.1V` is `121` |
| 4 | VBAT | in `Volts * 100`, eg. `12.1V` is `1210` |
| 5 | CELL_VOLTAGE | in `Volts * 100`, eg. `12.1V` is `1210` |
| 6 | CURRENT | in `Amps * 100`, eg. `9A` is `900` |
| 7 | MAH_DRAWN | in `mAh` |
| 8 | GPS_SATS | |
@ -125,6 +126,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
| 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol |
| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol |
| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
#### ACTIVE_WAYPOINT_ACTION

21
docs/Settings.md
View file

@ -82,7 +82,7 @@
| eleres_signature | | |
| eleres_telemetry_en | | |
| eleres_telemetry_power | | |
| esc_sensor_listen_only | | |
| esc_sensor_listen_only | OFF | Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case |
| failsafe_delay | 5 | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). |
| failsafe_fw_pitch_angle | 100 | Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb |
| failsafe_fw_roll_angle | -200 | Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll |
@ -101,6 +101,7 @@
| failsafe_throttle_low_delay | 100 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout |
| fixed_wing_auto_arm | OFF | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. |
| flaperon_throw_offset | 200 | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. |
| flip_over_after_crash_power_factor | 65 | flip over after crash power factor |
| fpv_mix_degrees | | |
| frsky_coordinates_format | 0 | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA |
| frsky_default_latitude | 0.000 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. |
@ -114,6 +115,9 @@
| fw_autotune_threshold | 50 | Threshold [%] of max rate to consider overshoot/undershoot detection |
| fw_autotune_undershoot_time | 200 | Time [ms] to detect sustained undershoot |
| fw_d_level | 75 | Fixed-wing attitude stabilisation HORIZON transition point |
| fw_d_pitch | 0 | Fixed wing rate stabilisation D-gain for PITCH |
| fw_d_roll | 0 | Fixed wing rate stabilisation D-gain for ROLL |
| fw_d_yaw | 0 | Fixed wing rate stabilisation D-gain for YAW |
| fw_ff_pitch | 50 | Fixed-wing rate stabilisation FF-gain for PITCH |
| fw_ff_roll | 50 | Fixed-wing rate stabilisation FF-gain for ROLL |
| fw_ff_yaw | 60 | Fixed-wing rate stabilisation FF-gain for YAW |
@ -145,7 +149,7 @@
| gyro_dyn_lpf_curve_expo | 5 | Expo value for the throttle-to-frequency mapping for Dynamic LPF |
| gyro_dyn_lpf_max_hz | 500 | Maximum frequency of the gyro Dynamic LPF |
| gyro_dyn_lpf_min_hz | 200 | Minimum frequency of the gyro Dynamic LPF |
| gyro_hardware_lpf | 42HZ | Hardware lowpass filter for gyro. Allowed values depend on the driver - For example MPU6050 allows 10HZ,20HZ,42HZ,98HZ,188HZ,256Hz (8khz mode). If you have to set gyro lpf below 42Hz generally means the frame is vibrating too much, and that should be fixed first. |
| gyro_hardware_lpf | 256HZ | Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first. |
| gyro_lpf_hz | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. |
| gyro_lpf_type | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. |
| gyro_notch_cutoff | | |
@ -262,7 +266,7 @@
| motor_accel_time | 0 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] |
| motor_decel_time | 0 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] |
| motor_direction_inverted | OFF | Use if you need to inverse yaw motor direction. |
| motor_poles | | |
| motor_poles | 14 | The number of motor poles. Required to compute motor RPM |
| motor_pwm_protocol | ONESHOT125 | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600, DSHOT1200, BRUSHED |
| motor_pwm_rate | 400 | Output frequency (in Hz) for motor pins. Default is 400Hz for motor with motor_pwm_protocol set to STANDARD. For *SHOT (e.g. ONESHOT125) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For BRUSHED values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set max_throttle to 2000. |
| msp_override_channels | 0 | Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode. |
@ -317,7 +321,7 @@
| nav_landing_speed | 200 | Vertical descent velocity during the RTH landing phase. [cm/s] |
| nav_manual_climb_rate | 200 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] |
| nav_manual_speed | 500 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] |
| nav_max_terrain_follow_alt | | |
| nav_max_terrain_follow_alt | 100 | Max allowed above the ground altitude for terrain following mode |
| nav_mc_auto_disarm_delay | 2000 | Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s) |
| nav_mc_bank_angle | 30 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude |
| nav_mc_braking_bank_angle | 40 | max angle that MR is allowed to bank in BOOST mode |
@ -345,6 +349,7 @@
| nav_mc_vel_z_d | 10 | D gain of velocity PID controller |
| nav_mc_vel_z_i | 50 | I gain of velocity PID controller |
| nav_mc_vel_z_p | 100 | P gain of velocity PID controller |
| nav_mc_wp_slowdown | ON | When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes. |
| nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. |
| nav_overrides_motor_stop | ALL_NAV | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
| nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
@ -426,10 +431,10 @@
| pid_type | | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` |
| pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
| pidsum_limit_yaw | 400 | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
| pinio_box1 | | |
| pinio_box2 | | |
| pinio_box3 | | |
| pinio_box4 | | |
| pinio_box1 | target specific | Mode assignment for PINIO#1 |
| pinio_box2 | target specific | Mode assignment for PINIO#1 |
| pinio_box3 | target specific | Mode assignment for PINIO#1 |
| pinio_box4 | target specific | Mode assignment for PINIO#1 |
| pitch_rate | 20 | Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. |
| pitot_hardware | NONE | Selection of pitot hardware. |
| pitot_lpf_milli_hz | | |

4
src/main/CMakeLists.txt
View file

@ -48,6 +48,8 @@ main_sources(COMMON_SRC
common/typeconversion.h
common/uvarint.c
common/uvarint.h
common/circular_queue.c
common/circular_queue.h
config/config_eeprom.c
config/config_eeprom.h
@ -156,6 +158,8 @@ main_sources(COMMON_SRC
drivers/compass/compass_mpu9250.h
drivers/compass/compass_qmc5883l.c
drivers/compass/compass_qmc5883l.h
drivers/compass/compass_rm3100.c
drivers/compass/compass_rm3100.h
drivers/compass/compass_msp.c
drivers/compass/compass_msp.h

1
src/main/build/debug.h
View file

@ -80,6 +80,7 @@ typedef enum {
DEBUG_SPM_VARIO, // Smartport master variometer
DEBUG_PCF8574,
DEBUG_DYNAMIC_GYRO_LPF,
DEBUG_FW_D,
DEBUG_IMU2,
DEBUG_COUNT
} debugType_e;

2
src/main/cms/cms_menu_imu.c
View file

@ -55,7 +55,7 @@
#define RPY_PIDFF_MAX 200
#define OTHER_PIDDF_MAX 255
#define PIDFF_ENTRY(label, ptr, max) OSD_UINT8_ENTRY(label, (&(const OSD_UINT16_t){ ptr, PIDFF_MIN, max, PIDFF_STEP }))
#define PIDFF_ENTRY(label, ptr, max) OSD_UINT16_ENTRY(label, (&(const OSD_UINT16_t){ ptr, PIDFF_MIN, max, PIDFF_STEP }))
#define RPY_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, RPY_PIDFF_MAX)
#define OTHER_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, OTHER_PIDDF_MAX)

1
src/main/cms/cms_menu_misc.c
View file

@ -47,6 +47,7 @@ static const OSD_Entry menuMiscEntries[]=
OSD_SETTING_ENTRY("OSD VOLT DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS),
OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT),
#endif
OSD_SETTING_ENTRY("FS PROCEDURE", SETTING_FAILSAFE_PROCEDURE),
OSD_BACK_AND_END_ENTRY,
};

66
src/main/common/circular_queue.c Normal file
View file

@ -0,0 +1,66 @@
/*
* 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 "circular_queue.h"
void circularBufferInit(circularBuffer_t *circular_buffer, uint8_t *buffer, size_t buffer_size,
size_t buffer_element_size) {
circular_buffer->buffer = buffer;
circular_buffer->bufferSize = buffer_size;
circular_buffer->elementSize = buffer_element_size;
circular_buffer->head = 0;
circular_buffer->tail = 0;
circular_buffer->size = 0;
}
void circularBufferPushElement(circularBuffer_t *circularBuffer, uint8_t *element) {
if (circularBufferIsFull(circularBuffer))
return;
memcpy(circularBuffer->buffer + circularBuffer->tail, element, circularBuffer->elementSize);
circularBuffer->tail += circularBuffer->elementSize;
circularBuffer->tail %= circularBuffer->bufferSize;
circularBuffer->size += 1;
}
void circularBufferPopHead(circularBuffer_t *circularBuffer, uint8_t *element) {
memcpy(element, circularBuffer->buffer + circularBuffer->head, circularBuffer->elementSize);
circularBuffer->head += circularBuffer->elementSize;
circularBuffer->head %= circularBuffer->bufferSize;
circularBuffer->size -= 1;
}
int circularBufferIsFull(circularBuffer_t *circularBuffer) {
return circularBufferCountElements(circularBuffer) * circularBuffer->elementSize == circularBuffer->bufferSize;
}
int circularBufferIsEmpty(circularBuffer_t *circularBuffer) {
return circularBuffer==NULL || circularBufferCountElements(circularBuffer) == 0;
}
size_t circularBufferCountElements(circularBuffer_t *circularBuffer) {
return circularBuffer->size;
}

47
src/main/common/circular_queue.h Normal file
View file

@ -0,0 +1,47 @@
/*
* 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/.
*/
#ifndef INAV_CIRCULAR_QUEUE_H
#define INAV_CIRCULAR_QUEUE_H
#include "stdint.h"
#include "string.h"
typedef struct circularBuffer_s{
size_t head;
size_t tail;
size_t bufferSize;
uint8_t * buffer;
size_t elementSize;
size_t size;
}circularBuffer_t;
void circularBufferInit(circularBuffer_t * circularBuffer, uint8_t * buffer, size_t bufferSize, size_t bufferElementSize);
void circularBufferPushElement(circularBuffer_t * circularBuffer, uint8_t * element);
void circularBufferPopHead(circularBuffer_t * circularBuffer, uint8_t * element);
int circularBufferIsFull(circularBuffer_t * circularBuffer);
int circularBufferIsEmpty(circularBuffer_t *circularBuffer);
size_t circularBufferCountElements(circularBuffer_t * circularBuffer);
#endif //INAV_CIRCULAR_QUEUE_H

2
src/main/common/maths.c
View file

@ -142,7 +142,7 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
return value;
}
int constrain(int amt, int low, int high)
int32_t constrain(int32_t amt, int32_t low, int32_t high)
{
if (amt < low)
return low;

4
src/main/common/maths.h
View file

@ -88,6 +88,8 @@
#define _ABS_I(x, var) _ABS_II(x, var)
#define ABS(x) _ABS_I(x, _CHOOSE_VAR(_abs, __COUNTER__))
#define power3(x) ((x)*(x)*(x))
// Floating point Euler angles.
typedef struct fp_angles {
float roll;
@ -131,7 +133,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
int gcd(int num, int denom);
int32_t applyDeadband(int32_t value, int32_t deadband);
int constrain(int amt, int low, int high);
int32_t constrain(int32_t amt, int32_t low, int32_t high);
float constrainf(float amt, float low, float high);
void devClear(stdev_t *dev);

1
src/main/drivers/bus.h
View file

@ -118,6 +118,7 @@ typedef enum {
DEVHW_QMC5883,
DEVHW_MAG3110,
DEVHW_LIS3MDL,
DEVHW_RM3100,
/* Temp sensor chips */
DEVHW_LM75_0,

179
src/main/drivers/compass/compass_rm3100.c Normal file
View file

@ -0,0 +1,179 @@
/*
* 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 <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "platform.h"
#ifdef USE_MAG_RM3100
#include "build/build_config.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/time.h"
#include "drivers/bus_i2c.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
#include "drivers/sensor.h"
#include "drivers/compass/compass.h"
#include "drivers/compass/compass_rm3100.h"
#define RM3100_REG_POLL 0x00
#define RM3100_REG_CMM 0x01
#define RM3100_REG_CCX1 0x04
#define RM3100_REG_CCX0 0x05
#define RM3100_REG_CCY1 0x06
#define RM3100_REG_CCY0 0x07
#define RM3100_REG_CCZ1 0x08
#define RM3100_REG_CCZ0 0x09
#define RM3100_REG_TMRC 0x0B
#define RM3100_REG_MX 0x24
#define RM3100_REG_MY 0x27
#define RM3100_REG_MZ 0x2A
#define RM3100_REG_BIST 0x33
#define RM3100_REG_STATUS 0x34
#define RM3100_REG_HSHAKE 0x35
#define RM3100_REG_REVID 0x36
#define RM3100_REVID 0x22
#define CCX_DEFAULT_MSB 0x00
#define CCX_DEFAULT_LSB 0xC8
#define CCY_DEFAULT_MSB CCX_DEFAULT_MSB
#define CCY_DEFAULT_LSB CCX_DEFAULT_LSB
#define CCZ_DEFAULT_MSB CCX_DEFAULT_MSB
#define CCZ_DEFAULT_LSB CCX_DEFAULT_LSB
#define CMM_DEFAULT 0x71 // Continuous mode
#define TMRC_DEFAULT 0x94
static bool deviceInit(magDev_t * mag)
{
busWrite(mag->busDev, RM3100_REG_TMRC, TMRC_DEFAULT);
busWrite(mag->busDev, RM3100_REG_CMM, CMM_DEFAULT);
busWrite(mag->busDev, RM3100_REG_CCX1, CCX_DEFAULT_MSB);
busWrite(mag->busDev, RM3100_REG_CCX0, CCX_DEFAULT_LSB);
busWrite(mag->busDev, RM3100_REG_CCY1, CCY_DEFAULT_MSB);
busWrite(mag->busDev, RM3100_REG_CCY0, CCY_DEFAULT_LSB);
busWrite(mag->busDev, RM3100_REG_CCZ1, CCZ_DEFAULT_MSB);
busWrite(mag->busDev, RM3100_REG_CCZ0, CCZ_DEFAULT_LSB);
return true;
}
static bool deviceRead(magDev_t * mag)
{
uint8_t status;
#pragma pack(push, 1)
struct {
uint8_t x[3];
uint8_t y[3];
uint8_t z[3];
} rm_report;
#pragma pack(pop)
mag->magADCRaw[X] = 0;
mag->magADCRaw[Y] = 0;
mag->magADCRaw[Z] = 0;
/* Check if new measurement is ready */
bool ack = busRead(mag->busDev, RM3100_REG_STATUS, &status);
if (!ack || (status & 0x80) == 0) {
return false;
}
ack = busReadBuf(mag->busDev, RM3100_REG_MX, (uint8_t *)&rm_report, sizeof(rm_report));
if (!ack) {
return false;
}
int32_t xraw;
int32_t yraw;
int32_t zraw;
/* Rearrange mag data */
xraw = ((rm_report.x[0] << 24) | (rm_report.x[1] << 16) | (rm_report.x[2]) << 8);
yraw = ((rm_report.y[0] << 24) | (rm_report.y[1] << 16) | (rm_report.y[2]) << 8);
zraw = ((rm_report.z[0] << 24) | (rm_report.z[1] << 16) | (rm_report.z[2]) << 8);
/* Truncate to 16-bit integers and pass along */
mag->magADCRaw[X] = (int16_t)(xraw >> 16);
mag->magADCRaw[Y] = (int16_t)(yraw >> 16);
mag->magADCRaw[Z] = (int16_t)(zraw >> 16);
return true;
}
#define DETECTION_MAX_RETRY_COUNT 5
static bool deviceDetect(magDev_t * mag)
{
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
uint8_t revid = 0;
bool ack = busRead(mag->busDev, RM3100_REG_REVID, &revid);
if (ack && revid == RM3100_REVID) {
return true;
}
}
return false;
}
bool rm3100MagDetect(magDev_t * mag)
{
busSetSpeed(mag->busDev, BUS_SPEED_STANDARD);
mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_RM3100, mag->magSensorToUse, OWNER_COMPASS);
if (mag->busDev == NULL) {
return false;
}
if (!deviceDetect(mag)) {
busDeviceDeInit(mag->busDev);
return false;
}
mag->init = deviceInit;
mag->read = deviceRead;
return true;
}
#endif

27
src/main/drivers/compass/compass_rm3100.h Normal file
View file

@ -0,0 +1,27 @@
/*
* 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
bool rm3100MagDetect(magDev_t *mag);

2
src/main/drivers/lights_io.c
View file

@ -10,7 +10,7 @@
static IO_t lightsIO = DEFIO_IO(NONE);
bool lightsHardwareInit()
bool lightsHardwareInit(void)
{
lightsIO = IOGetByTag(IO_TAG(LIGHTS_PIN));

2
src/main/drivers/lights_io.h
View file

@ -4,7 +4,7 @@
#ifdef USE_LIGHTS
bool lightsHardwareInit();
bool lightsHardwareInit(void);
void lightsHardwareSetStatus(bool status);
#endif /* USE_LIGHTS */

70
src/main/drivers/pwm_output.c
View file

@ -28,6 +28,7 @@ FILE_COMPILE_FOR_SPEED
#include "common/log.h"
#include "common/maths.h"
#include "common/circular_queue.h"
#include "drivers/io.h"
#include "drivers/timer.h"
@ -60,6 +61,10 @@ FILE_COMPILE_FOR_SPEED
#define DSHOT_MOTOR_BITLENGTH 20
#define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */
#define DSHOT_COMMAND_INTERVAL_US 1000
#define DSHOT_COMMAND_QUEUE_LENGTH 8
#define DHSOT_COMMAND_QUEUE_SIZE DSHOT_COMMAND_QUEUE_LENGTH * sizeof(dshotCommands_e)
#endif
typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors
@ -110,6 +115,12 @@ static uint8_t allocatedOutputPortCount = 0;
static bool pwmMotorsEnabled = true;
#ifdef USE_DSHOT
static circularBuffer_t commandsCircularBuffer;
static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE];
static currentExecutingCommand_t currentExecutingCommand;
#endif
static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value)
{
p->tch = tch;
@ -340,8 +351,60 @@ void pwmRequestMotorTelemetry(int motorIndex)
}
}
void pwmCompleteMotorUpdate(void)
{
#ifdef USE_DSHOT
void sendDShotCommand(dshotCommands_e cmd) {
circularBufferPushElement(&commandsCircularBuffer, (uint8_t *) &cmd);
}
void initDShotCommands(void) {
circularBufferInit(&commandsCircularBuffer, commandsBuff,DHSOT_COMMAND_QUEUE_SIZE, sizeof(dshotCommands_e));
currentExecutingCommand.remainingRepeats = 0;
}
static int getDShotCommandRepeats(dshotCommands_e cmd) {
int repeats = 1;
switch (cmd) {
case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
repeats = 6;
break;
default:
break;
}
return repeats;
}
static void executeDShotCommands(void){
if(currentExecutingCommand.remainingRepeats == 0) {
const int isTherePendingCommands = !circularBufferIsEmpty(&commandsCircularBuffer);
if (isTherePendingCommands) {
//Load the command
dshotCommands_e cmd;
circularBufferPopHead(&commandsCircularBuffer, (uint8_t *) &cmd);
currentExecutingCommand.cmd = cmd;
currentExecutingCommand.remainingRepeats = getDShotCommandRepeats(cmd);
}
else {
return;
}
}
for (uint8_t i = 0; i < getMotorCount(); i++) {
motors[i].requestTelemetry = true;
motors[i].value = currentExecutingCommand.cmd;
}
currentExecutingCommand.remainingRepeats--;
}
#endif
void pwmCompleteMotorUpdate(void) {
// This only makes sense for digital motor protocols
if (!isMotorProtocolDigital()) {
return;
@ -359,6 +422,9 @@ void pwmCompleteMotorUpdate(void)
#ifdef USE_DSHOT
if (isMotorProtocolDshot()) {
executeDShotCommands();
// Generate DMA buffers
for (int index = 0; index < motorCount; index++) {
if (motors[index].pwmPort && motors[index].pwmPort->configured) {

14
src/main/drivers/pwm_output.h
View file

@ -20,6 +20,16 @@
#include "drivers/io_types.h"
#include "drivers/time.h"
typedef enum {
DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
} dshotCommands_e;
typedef struct {
dshotCommands_e cmd;
int remainingRepeats;
} currentExecutingCommand_t;
void pwmRequestMotorTelemetry(int motorIndex);
ioTag_t pwmGetMotorPinTag(int motorIndex);
@ -28,6 +38,7 @@ void pwmWriteMotor(uint8_t index, uint16_t value);
void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
void pwmCompleteMotorUpdate(void);
bool isMotorProtocolDigital(void);
bool isMotorProtocolDshot(void);
void pwmWriteServo(uint8_t index, uint16_t value);
@ -42,3 +53,6 @@ void pwmServoPreconfigure(void);
bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput);
void pwmWriteBeeper(bool onoffBeep);
void beeperPwmInit(ioTag_t tag, uint16_t frequency);
void sendDShotCommand(dshotCommands_e directionSpin);
void initDShotCommands(void);

2
src/main/drivers/sdcard/sdmmc_sdio_f4xx.c
View file

@ -1081,7 +1081,7 @@ SD_Error_t SD_GetStatus(void)
}
}
else {
ErrorState = SD_CARD_ERROR;
ErrorState = SD_ERROR;
}
return ErrorState;

2
src/main/fc/controlrate_profile.c
View file

@ -64,7 +64,7 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance)
},
.misc = {
.fpvCamAngleDegrees = 0
.fpvCamAngleDegrees = 0,
}
);
}

27
src/main/fc/fc_core.c
View file

@ -400,10 +400,17 @@ void disarm(disarmReason_t disarmReason)
blackboxFinish();
}
#endif
#ifdef USE_DSHOT
if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_NORMAL);
DISABLE_FLIGHT_MODE(FLIP_OVER_AFTER_CRASH);
}
#endif
statsOnDisarm();
logicConditionReset();
#ifdef USE_PROGRAMMING_FRAMEWORK
programmingPidReset();
#endif
beeper(BEEPER_DISARMING); // emit disarm tone
}
}
@ -459,6 +466,21 @@ void releaseSharedTelemetryPorts(void) {
void tryArm(void)
{
updateArmingStatus();
#ifdef USE_DSHOT
if (
STATE(MULTIROTOR) &&
IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) &&
emergencyArmingCanOverrideArmingDisabled() &&
isMotorProtocolDshot() &&
!FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)
) {
sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
ENABLE_ARMING_FLAG(ARMED);
enableFlightMode(FLIP_OVER_AFTER_CRASH);
return;
}
#endif
#ifdef USE_PROGRAMMING_FRAMEWORK
if (
!isArmingDisabled() ||
@ -494,7 +516,10 @@ void tryArm(void)
//It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction
ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
logicConditionReset();
#ifdef USE_PROGRAMMING_FRAMEWORK
programmingPidReset();
#endif
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));

4
src/main/fc/fc_init.c
View file

@ -674,6 +674,10 @@ void init(void)
rcdeviceInit();
#endif // USE_RCDEVICE
#ifdef USE_DSHOT
initDShotCommands();
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;

8
src/main/fc/fc_msp_box.c
View file

@ -33,6 +33,8 @@
#include "io/osd.h"
#include "drivers/pwm_output.h"
#include "sensors/diagnostics.h"
#include "sensors/sensors.h"
@ -87,6 +89,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXLOITERDIRCHN, "LOITER CHANGE", 49 },
{ BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
{ BOXPREARM, "PREARM", 51 },
{ BOXFLIPOVERAFTERCRASH, "TURTLE", 52 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -306,6 +309,11 @@ void initActiveBoxIds(void)
#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
activeBoxIds[activeBoxIdCount++] = BOXMSPRCOVERRIDE;
#endif
#ifdef USE_DSHOT
if(STATE(MULTIROTOR) && isMotorProtocolDshot())
activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH;
#endif
}
#define IS_ENABLED(mask) (mask == 0 ? 0 : 1)

102
src/main/fc/rc_adjustments.c
View file

@ -101,7 +101,43 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D_FF,
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_FF,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_FF,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_FF,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
@ -113,7 +149,11 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_YAW_D_FF,
.adjustmentFunction = ADJUSTMENT_YAW_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_YAW_FF,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
@ -128,30 +168,6 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.adjustmentFunction = ADJUSTMENT_ROLL_RATE,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_D_FF,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_D_FF,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_RC_YAW_EXPO,
.mode = ADJUSTMENT_MODE_STEP,
@ -447,18 +463,32 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
applyAdjustmentPID(ADJUSTMENT_ROLL_I, &pidBankMutable()->pid[PID_ROLL].I, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_PITCH_ROLL_D_FF:
case ADJUSTMENT_PITCH_D_FF:
applyAdjustmentPID(ADJUSTMENT_PITCH_D_FF, getD_FFRefByBank(pidBankMutable(), PID_PITCH), delta);
if (adjustmentFunction == ADJUSTMENT_PITCH_D_FF) {
case ADJUSTMENT_PITCH_ROLL_D:
case ADJUSTMENT_PITCH_D:
applyAdjustmentPID(ADJUSTMENT_PITCH_D, &pidBankMutable()->pid[PID_PITCH].D, delta);
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
schedulePidGainsUpdate();
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_D
FALLTHROUGH;
case ADJUSTMENT_ROLL_D_FF:
applyAdjustmentPID(ADJUSTMENT_ROLL_D_FF, getD_FFRefByBank(pidBankMutable(), PID_ROLL), delta);
case ADJUSTMENT_ROLL_D:
applyAdjustmentPID(ADJUSTMENT_ROLL_D, &pidBankMutable()->pid[PID_ROLL].D, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_PITCH_ROLL_FF:
case ADJUSTMENT_PITCH_FF:
applyAdjustmentPID(ADJUSTMENT_PITCH_FF, &pidBankMutable()->pid[PID_PITCH].FF, delta);
if (adjustmentFunction == ADJUSTMENT_PITCH_FF) {
schedulePidGainsUpdate();
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_FF
FALLTHROUGH;
case ADJUSTMENT_ROLL_FF:
applyAdjustmentPID(ADJUSTMENT_ROLL_FF, &pidBankMutable()->pid[PID_ROLL].FF, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_P:
@ -469,8 +499,12 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
applyAdjustmentPID(ADJUSTMENT_YAW_I, &pidBankMutable()->pid[PID_YAW].I, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_D_FF:
applyAdjustmentPID(ADJUSTMENT_YAW_D_FF, getD_FFRefByBank(pidBankMutable(), PID_YAW), delta);
case ADJUSTMENT_YAW_D:
applyAdjustmentPID(ADJUSTMENT_YAW_D, &pidBankMutable()->pid[PID_YAW].D, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_FF:
applyAdjustmentPID(ADJUSTMENT_YAW_FF, &pidBankMutable()->pid[PID_YAW].FF, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_NAV_FW_CRUISE_THR:

94
src/main/fc/rc_adjustments.h
View file

@ -33,51 +33,55 @@ typedef enum {
ADJUSTMENT_YAW_RATE = 5,
ADJUSTMENT_PITCH_ROLL_P = 6,
ADJUSTMENT_PITCH_ROLL_I = 7,
ADJUSTMENT_PITCH_ROLL_D_FF = 8,
ADJUSTMENT_YAW_P = 9,
ADJUSTMENT_YAW_I = 10,
ADJUSTMENT_YAW_D_FF = 11,
ADJUSTMENT_RATE_PROFILE = 12, // Unused, placeholder for compatibility
ADJUSTMENT_PITCH_RATE = 13,
ADJUSTMENT_ROLL_RATE = 14,
ADJUSTMENT_PITCH_P = 15,
ADJUSTMENT_PITCH_I = 16,
ADJUSTMENT_PITCH_D_FF = 17,
ADJUSTMENT_ROLL_P = 18,
ADJUSTMENT_ROLL_I = 19,
ADJUSTMENT_ROLL_D_FF = 20,
ADJUSTMENT_RC_YAW_EXPO = 21,
ADJUSTMENT_MANUAL_RC_EXPO = 22,
ADJUSTMENT_MANUAL_RC_YAW_EXPO = 23,
ADJUSTMENT_MANUAL_PITCH_ROLL_RATE = 24,
ADJUSTMENT_MANUAL_ROLL_RATE = 25,
ADJUSTMENT_MANUAL_PITCH_RATE = 26,
ADJUSTMENT_MANUAL_YAW_RATE = 27,
ADJUSTMENT_NAV_FW_CRUISE_THR = 28,
ADJUSTMENT_NAV_FW_PITCH2THR = 29,
ADJUSTMENT_ROLL_BOARD_ALIGNMENT = 30,
ADJUSTMENT_PITCH_BOARD_ALIGNMENT = 31,
ADJUSTMENT_LEVEL_P = 32,
ADJUSTMENT_LEVEL_I = 33,
ADJUSTMENT_LEVEL_D = 34,
ADJUSTMENT_POS_XY_P = 35,
ADJUSTMENT_POS_XY_I = 36,
ADJUSTMENT_POS_XY_D = 37,
ADJUSTMENT_POS_Z_P = 38,
ADJUSTMENT_POS_Z_I = 39,
ADJUSTMENT_POS_Z_D = 40,
ADJUSTMENT_HEADING_P = 41,
ADJUSTMENT_VEL_XY_P = 42,
ADJUSTMENT_VEL_XY_I = 43,
ADJUSTMENT_VEL_XY_D = 44,
ADJUSTMENT_VEL_Z_P = 45,
ADJUSTMENT_VEL_Z_I = 46,
ADJUSTMENT_VEL_Z_D = 47,
ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 48,
ADJUSTMENT_VTX_POWER_LEVEL = 49,
ADJUSTMENT_TPA = 50,
ADJUSTMENT_TPA_BREAKPOINT = 51,
ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS = 52,
ADJUSTMENT_PITCH_ROLL_D = 8,
ADJUSTMENT_PITCH_ROLL_FF = 9,
ADJUSTMENT_PITCH_P = 10,
ADJUSTMENT_PITCH_I = 11,
ADJUSTMENT_PITCH_D = 12,
ADJUSTMENT_PITCH_FF = 13,
ADJUSTMENT_ROLL_P = 14,
ADJUSTMENT_ROLL_I = 15,
ADJUSTMENT_ROLL_D = 16,
ADJUSTMENT_ROLL_FF = 17,
ADJUSTMENT_YAW_P = 18,
ADJUSTMENT_YAW_I = 19,
ADJUSTMENT_YAW_D = 20,
ADJUSTMENT_YAW_FF = 21,
ADJUSTMENT_RATE_PROFILE = 22, // Unused, placeholder for compatibility
ADJUSTMENT_PITCH_RATE = 23,
ADJUSTMENT_ROLL_RATE = 24,
ADJUSTMENT_RC_YAW_EXPO = 25,
ADJUSTMENT_MANUAL_RC_EXPO = 26,
ADJUSTMENT_MANUAL_RC_YAW_EXPO = 27,
ADJUSTMENT_MANUAL_PITCH_ROLL_RATE = 28,
ADJUSTMENT_MANUAL_ROLL_RATE = 29,
ADJUSTMENT_MANUAL_PITCH_RATE = 30,
ADJUSTMENT_MANUAL_YAW_RATE = 31,
ADJUSTMENT_NAV_FW_CRUISE_THR = 32,
ADJUSTMENT_NAV_FW_PITCH2THR = 33,
ADJUSTMENT_ROLL_BOARD_ALIGNMENT = 34,
ADJUSTMENT_PITCH_BOARD_ALIGNMENT = 35,
ADJUSTMENT_LEVEL_P = 36,
ADJUSTMENT_LEVEL_I = 37,
ADJUSTMENT_LEVEL_D = 38,
ADJUSTMENT_POS_XY_P = 39,
ADJUSTMENT_POS_XY_I = 40,
ADJUSTMENT_POS_XY_D = 41,
ADJUSTMENT_POS_Z_P = 42,
ADJUSTMENT_POS_Z_I = 43,
ADJUSTMENT_POS_Z_D = 44,
ADJUSTMENT_HEADING_P = 45,
ADJUSTMENT_VEL_XY_P = 46,
ADJUSTMENT_VEL_XY_I = 47,
ADJUSTMENT_VEL_XY_D = 48,
ADJUSTMENT_VEL_Z_P = 49,
ADJUSTMENT_VEL_Z_I = 50,
ADJUSTMENT_VEL_Z_D = 51,
ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 52,
ADJUSTMENT_VTX_POWER_LEVEL = 53,
ADJUSTMENT_TPA = 54,
ADJUSTMENT_TPA_BREAKPOINT = 55,
ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS = 56,
ADJUSTMENT_FUNCTION_COUNT // must be last
} adjustmentFunction_e;

1
src/main/fc/rc_modes.h
View file

@ -69,6 +69,7 @@ typedef enum {
BOXLOITERDIRCHN = 40,
BOXMSPRCOVERRIDE = 41,
BOXPREARM = 42,
BOXFLIPOVERAFTERCRASH = 43,
CHECKBOX_ITEM_COUNT
} boxId_e;

1
src/main/fc/runtime_config.h
View file

@ -97,6 +97,7 @@ typedef enum {
NAV_CRUISE_MODE = (1 << 12),
FLAPERON = (1 << 13),
TURN_ASSISTANT = (1 << 14),
FLIP_OVER_AFTER_CRASH = (1 << 15),
} flightModeFlags_e;
extern uint32_t flightModeFlags;

55
src/main/fc/settings.yaml
View file

@ -10,10 +10,10 @@ tables:
values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE"]
enum: rangefinderType_e
- name: mag_hardware
values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "FAKE"]
values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", FAKE"]
enum: magSensor_e
- name: opflow_hardware
values: ["NONE", "PMW3901", "CXOF", "MSP", "FAKE"]
values: ["NONE", "CXOF", "MSP", "FAKE"]
enum: opticalFlowSensor_e
- name: baro_hardware
values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "MSP", "FAKE"]
@ -84,8 +84,7 @@ tables:
"FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
"ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
"IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574",
"DYN_GYRO_LPF", "IMU2"]
"IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2"]
- name: async_mode
values: ["NONE", "GYRO", "ALL"]
- name: aux_operator
@ -183,8 +182,8 @@ groups:
type: uint8_t
table: alignment
- name: gyro_hardware_lpf
description: "Hardware lowpass filter for gyro. Allowed values depend on the driver - For example MPU6050 allows 10HZ,20HZ,42HZ,98HZ,188HZ,256Hz (8khz mode). If you have to set gyro lpf below 42Hz generally means the frame is vibrating too much, and that should be fixed first."
default_value: "42HZ"
description: "Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first."
default_value: "256HZ"
field: gyro_lpf
table: gyro_lpf
- name: gyro_lpf_hz
@ -781,8 +780,17 @@ groups:
max: 30
- name: motor_poles
field: motorPoleCount
description: "The number of motor poles. Required to compute motor RPM"
default_value: "14"
min: 4
max: 255
- name: flip_over_after_crash_power_factor
field: flipOverAfterPowerFactor
default_value: "65"
description: "flip over after crash power factor"
condition: "USE_DSHOT"
min: 0
max: 100
- name: PG_FAILSAFE_CONFIG
type: failsafeConfig_t
@ -1527,6 +1535,12 @@ groups:
field: bank_fw.pid[PID_PITCH].I
min: 0
max: 200
- name: fw_d_pitch
description: "Fixed wing rate stabilisation D-gain for PITCH"
default_value: "0"
field: bank_fw.pid[PID_PITCH].D
min: 0
max: 200
- name: fw_ff_pitch
description: "Fixed-wing rate stabilisation FF-gain for PITCH"
default_value: "50"
@ -1545,6 +1559,12 @@ groups:
field: bank_fw.pid[PID_ROLL].I
min: 0
max: 200
- name: fw_d_roll
description: "Fixed wing rate stabilisation D-gain for ROLL"
default_value: "0"
field: bank_fw.pid[PID_ROLL].D
min: 0
max: 200
- name: fw_ff_roll
description: "Fixed-wing rate stabilisation FF-gain for ROLL"
default_value: "50"
@ -1563,6 +1583,12 @@ groups:
field: bank_fw.pid[PID_YAW].I
min: 0
max: 200
- name: fw_d_yaw
description: "Fixed wing rate stabilisation D-gain for YAW"
default_value: "0"
field: bank_fw.pid[PID_YAW].D
min: 0
max: 200
- name: fw_ff_yaw
description: "Fixed-wing rate stabilisation FF-gain for YAW"
default_value: "60"
@ -2246,6 +2272,8 @@ groups:
max: 65000
- name: nav_max_terrain_follow_alt
field: general.max_terrain_follow_altitude
default_value: "100"
description: "Max allowed above the ground altitude for terrain following mode"
max: 1000
- name: nav_rth_altitude
description: "Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters)"
@ -2351,6 +2379,11 @@ groups:
condition: USE_NAV
min: 0
max: 255
- name: nav_mc_wp_slowdown
description: "When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes."
default_value: "ON"
field: mc.slowDownForTurning
type: bool
- name: nav_fw_cruise_thr
description: "Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded )"
default_value: "1400"
@ -3257,21 +3290,29 @@ groups:
members:
- name: pinio_box1
field: permanentId[0]
description: "Mode assignment for PINIO#1"
default_value: "target specific"
min: 0
max: 255
type: uint8_t
- name: pinio_box2
field: permanentId[1]
default_value: "target specific"
description: "Mode assignment for PINIO#1"
min: 0
max: 255
type: uint8_t
- name: pinio_box3
field: permanentId[2]
default_value: "target specific"
description: "Mode assignment for PINIO#1"
min: 0
max: 255
type: uint8_t
- name: pinio_box4
field: permanentId[3]
default_value: "target specific"
description: "Mode assignment for PINIO#1"
min: 0
max: 255
type: uint8_t
@ -3299,6 +3340,8 @@ groups:
condition: USE_ESC_SENSOR
members:
- name: esc_sensor_listen_only
default_value: "OFF"
description: "Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case"
field: listenOnly
type: bool

95
src/main/flight/mixer.c
View file

@ -42,6 +42,7 @@ FILE_COMPILE_FOR_SPEED
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "fc/controlrate_profile.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
@ -99,7 +100,7 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
#define DEFAULT_MAX_THROTTLE 1850
PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 6);
PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 7);
PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
.motorPwmProtocol = DEFAULT_PWM_PROTOCOL,
@ -110,11 +111,15 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
.motorDecelTimeMs = 0,
.throttleIdle = 15.0f,
.throttleScale = 1.0f,
.motorPoleCount = 14 // Most brushless motors that we use are 14 poles
.motorPoleCount = 14, // Most brushless motors that we use are 14 poles
.flipOverAfterPowerFactor = 65
);
PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
#define CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND 20.0f
#define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f
typedef void (*motorRateLimitingApplyFnPtr)(const float dT);
static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn;
@ -319,6 +324,85 @@ static uint16_t handleOutputScaling(
}
return value;
}
static void applyFlipOverAfterCrashModeToMotors(void) {
if (ARMING_FLAG(ARMED)) {
const float flipPowerFactor = ((float)motorConfig()->flipOverAfterPowerFactor)/100.0f;
const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f);
const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f);
const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f);
//deflection stick position
const float stickDeflectionPitchExpo =
flipPowerFactor * stickDeflectionPitchAbs + power3(stickDeflectionPitchAbs) * (1 - flipPowerFactor);
const float stickDeflectionRollExpo =
flipPowerFactor * stickDeflectionRollAbs + power3(stickDeflectionRollAbs) * (1 - flipPowerFactor);
const float stickDeflectionYawExpo =
flipPowerFactor * stickDeflectionYawAbs + power3(stickDeflectionYawAbs) * (1 - flipPowerFactor);
float signPitch = rcCommand[PITCH] < 0 ? 1 : -1;
float signRoll = rcCommand[ROLL] < 0 ? 1 : -1;
float signYaw = (float)((rcCommand[YAW] < 0 ? 1 : -1) * (mixerConfig()->motorDirectionInverted ? 1 : -1));
float stickDeflectionLength = sqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs));
float stickDeflectionExpoLength = sqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo));
if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) {
// If yaw is the dominant, disable pitch and roll
stickDeflectionLength = stickDeflectionYawAbs;
stickDeflectionExpoLength = stickDeflectionYawExpo;
signRoll = 0;
signPitch = 0;
} else {
// If pitch/roll dominant, disable yaw
signYaw = 0;
}
const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) /
(sqrtf(2.0f) * stickDeflectionLength) : 0;
const float cosThreshold = sqrtf(3.0f) / 2.0f; // cos(PI/6.0f)
if (cosPhi < cosThreshold) {
// Enforce either roll or pitch exclusively, if not on diagonal
if (stickDeflectionRollAbs > stickDeflectionPitchAbs) {
signPitch = 0;
} else {
signRoll = 0;
}
}
// Apply a reasonable amount of stick deadband
const float crashFlipStickMinExpo =
flipPowerFactor * CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN + power3(CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN) * (1 - flipPowerFactor);
const float flipStickRange = 1.0f - crashFlipStickMinExpo;
const float flipPower = MAX(0.0f, stickDeflectionExpoLength - crashFlipStickMinExpo) / flipStickRange;
for (int i = 0; i < motorCount; ++i) {
float motorOutputNormalised =
signPitch * currentMixer[i].pitch +
signRoll * currentMixer[i].roll +
signYaw * currentMixer[i].yaw;
if (motorOutputNormalised < 0) {
motorOutputNormalised = 0;
}
motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised);
float motorOutput = (float)motorConfig()->mincommand + motorOutputNormalised * (float)motorConfig()->maxthrottle;
// Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
motorOutput = (motorOutput < (float)motorConfig()->mincommand + CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND) ? DSHOT_DISARM_COMMAND : (
motorOutput - CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND);
motor[i] = motorOutput;
}
} else {
// Disarmed mode
stopMotors();
}
}
#endif
void FAST_CODE writeMotors(void)
@ -443,6 +527,13 @@ static int getReversibleMotorsThrottleDeadband(void)
void FAST_CODE mixTable(const float dT)
{
#ifdef USE_DSHOT
if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
applyFlipOverAfterCrashModeToMotors();
return;
}
#endif
int16_t input[3]; // RPY, range [-500:+500]
// Allow direct stick input to motors in passthrough mode on airplanes
if (STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(MANUAL_MODE)) {

1
src/main/flight/mixer.h
View file

@ -92,6 +92,7 @@ typedef struct motorConfig_s {
float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent
float throttleScale; // Scaling factor for throttle.
uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
uint8_t flipOverAfterPowerFactor; // Power factor from 0 to 100% of flip over after crash
} motorConfig_t;
PG_DECLARE(motorConfig_t, motorConfig);

183
src/main/flight/pid.c
View file

@ -484,7 +484,7 @@ void updatePIDCoefficients()
// Airplanes - scale all PIDs according to TPA
pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor;
pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * tpaFactor;
pidState[axis].kD = 0.0f;
pidState[axis].kD = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * tpaFactor;
pidState[axis].kFF = pidBank()->pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor;
pidState[axis].kCD = 0.0f;
pidState[axis].kT = 0.0f;
@ -498,7 +498,7 @@ void updatePIDCoefficients()
pidState[axis].kFF = 0.0f;
// Tracking anti-windup requires P/I/D to be all defined which is only true for MC
if ((pidBank()->pid[axis].P != 0) && (pidBank()->pid[axis].I != 0)) {
if ((pidBank()->pid[axis].P != 0) && (pidBank()->pid[axis].I != 0) && (usedPidControllerType == PID_TYPE_PID)) {
pidState[axis].kT = 2.0f / ((pidState[axis].kP / pidState[axis].kI) + (pidState[axis].kD / pidState[axis].kP));
} else {
pidState[axis].kT = 0;
@ -625,71 +625,6 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
}
static void applyItermLimiting(pidState_t *pidState) {
if (pidState->itermLimitActive) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else
{
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
}
}
static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
UNUSED(pidState);
UNUSED(axis);
UNUSED(dT);
}
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
{
const float rateError = pidState->rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
const float newFFTerm = pidState->rateTarget * pidState->kFF;
/*
* Integral should be updated only if axis Iterm is not frozen
*/
if (!pidState->itermFreezeActive) {
pidState->errorGyroIf += rateError * pidState->kI * dT;
}
applyItermLimiting(pidState);
if (pidProfile()->fixedWingItermThrowLimit != 0) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
}
#ifdef USE_AUTOTUNE_FIXED_WING
if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) {
autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, newPTerm + newFFTerm);
}
#endif
axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit);
#ifdef USE_BLACKBOX
axisPID_P[axis] = newPTerm;
axisPID_I[axis] = pidState->errorGyroIf;
axisPID_D[axis] = newFFTerm;
axisPID_Setpoint[axis] = pidState->rateTarget;
#endif
}
static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, float itermErrorRate)
{
if (itermRelax) {
if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY) {
const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD);
return itermErrorRate * itermRelaxFactor;
}
}
return itermErrorRate;
}
#ifdef USE_D_BOOST
static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
@ -716,10 +651,100 @@ static float applyDBoost(pidState_t *pidState, float dT) {
}
#endif
static float dTermProcess(pidState_t *pidState, float dT) {
// Calculate new D-term
float newDTerm = 0;
if (pidState->kD == 0) {
// optimisation for when D is zero, often used by YAW axis
newDTerm = 0;
} else {
float delta = pidState->previousRateGyro - pidState->gyroRate;
delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta);
// Calculate derivative
newDTerm = delta * (pidState->kD / dT) * applyDBoost(pidState, dT);
}
return(newDTerm);
}
static void applyItermLimiting(pidState_t *pidState) {
if (pidState->itermLimitActive) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else
{
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
}
}
static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
UNUSED(pidState);
UNUSED(axis);
UNUSED(dT);
}
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
{
const float rateError = pidState->rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
const float newDTerm = dTermProcess(pidState, dT);
const float newFFTerm = pidState->rateTarget * pidState->kFF;
DEBUG_SET(DEBUG_FW_D, axis, newDTerm);
/*
* Integral should be updated only if axis Iterm is not frozen
*/
if (!pidState->itermFreezeActive) {
pidState->errorGyroIf += rateError * pidState->kI * dT;
}
applyItermLimiting(pidState);
if (pidProfile()->fixedWingItermThrowLimit != 0) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
}
#ifdef USE_AUTOTUNE_FIXED_WING
if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) {
autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, newPTerm + newFFTerm);
}
#endif
axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit);
#ifdef USE_BLACKBOX
axisPID_P[axis] = newPTerm;
axisPID_I[axis] = pidState->errorGyroIf;
axisPID_D[axis] = newDTerm;
axisPID_Setpoint[axis] = pidState->rateTarget;
#endif
pidState->previousRateGyro = pidState->gyroRate;
}
static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, float itermErrorRate)
{
if (itermRelax) {
if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY) {
const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD);
return itermErrorRate * itermRelaxFactor;
}
}
return itermErrorRate;
}
static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
{
const float rateError = pidState->rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
const float newDTerm = dTermProcess(pidState, dT);
const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget;
const float rateTargetDeltaFiltered = biquadFilterApply(&pidState->rateTargetFilter, rateTargetDelta);
@ -736,21 +761,6 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
}
DEBUG_SET(DEBUG_CD, axis, newCDTerm);
// Calculate new D-term
float newDTerm;
if (pidState->kD == 0) {
// optimisation for when D8 is zero, often used by YAW axis
newDTerm = 0;
} else {
float delta = pidState->previousRateGyro - pidState->gyroRate;
delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta);
// Calculate derivative
newDTerm = delta * (pidState->kD / dT) * applyDBoost(pidState, dT);
}
// TODO: Get feedback from mixer on available correction range for each axis
const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm;
const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit);
@ -1180,12 +1190,3 @@ const pidBank_t * pidBank(void) {
pidBank_t * pidBankMutable(void) {
return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
}
uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex)
{
if (pidIndexGetType(pidIndex) == PID_TYPE_PIFF) {
return &pidBank->pid[pidIndex].FF;
} else {
return &pidBank->pid[pidIndex].D;
}
}

3
src/main/flight/pid.h
View file

@ -76,7 +76,7 @@ typedef enum {
typedef enum {
PID_TYPE_NONE = 0, // Not used in the current platform/mixer/configuration
PID_TYPE_PID, // Uses P, I and D terms
PID_TYPE_PIFF, // Uses P, I and FF, ignoring D
PID_TYPE_PIFF, // Uses P, I, D and FF
PID_TYPE_AUTO, // Autodetect
} pidType_e;
@ -202,4 +202,3 @@ void autotuneUpdateState(void);
void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput);
pidType_e pidIndexGetType(pidIndex_e pidIndex);
uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex);

6
src/main/flight/pid_autotune.c
View file

@ -230,15 +230,15 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
switch (axis) {
case FD_ROLL:
blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D_FF, tuneCurrent[axis].gainFF);
blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_FF, tuneCurrent[axis].gainFF);
break;
case FD_PITCH:
blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D_FF, tuneCurrent[axis].gainFF);
blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_FF, tuneCurrent[axis].gainFF);
break;
case FD_YAW:
blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D_FF, tuneCurrent[axis].gainFF);
blackboxLogAutotuneEvent(ADJUSTMENT_YAW_FF, tuneCurrent[axis].gainFF);
break;
}
}

2
src/main/io/lights.c
View file

@ -72,7 +72,7 @@ void lightsUpdate(timeUs_t currentTimeUs)
lightsSetStatus(IS_RC_MODE_ACTIVE(BOXLIGHTS), currentTimeUs);
}
void lightsInit()
void lightsInit(void)
{
lightsHardwareInit();
}

2
src/main/io/lights.h
View file

@ -33,6 +33,6 @@ typedef struct lightsConfig_s {
PG_DECLARE(lightsConfig_t, lightsConfig);
void lightsUpdate(timeUs_t currentTimeUs);
void lightsInit();
void lightsInit(void);
#endif /* USE_LIGHTS */

79
src/main/io/osd.c
View file

@ -1182,7 +1182,49 @@ static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_
displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr);
}
static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD)
static void osdDisplayFlightPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD, adjustmentFunction_e adjFuncFF)
{
textAttributes_t elemAttr;
char buff[4];
const pid8_t *pid = &pidBank()->pid[pidIndex];
pidType_e pidType = pidIndexGetType(pidIndex);
displayWrite(osdDisplayPort, elemPosX, elemPosY, str);
if (pidType == PID_TYPE_NONE) {
// PID is not used in this configuration. Draw dashes.
// XXX: Keep this in sync with the %3d format and spacing used below
displayWrite(osdDisplayPort, elemPosX + 6, elemPosY, "- - - -");
return;
}
elemAttr = TEXT_ATTRIBUTES_NONE;
tfp_sprintf(buff, "%3d", pid->P);
if ((isAdjustmentFunctionSelected(adjFuncP)) || (((adjFuncP == ADJUSTMENT_ROLL_P) || (adjFuncP == ADJUSTMENT_PITCH_P)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P))))
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
elemAttr = TEXT_ATTRIBUTES_NONE;
tfp_sprintf(buff, "%3d", pid->I);
if ((isAdjustmentFunctionSelected(adjFuncI)) || (((adjFuncI == ADJUSTMENT_ROLL_I) || (adjFuncI == ADJUSTMENT_PITCH_I)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I))))
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr);
elemAttr = TEXT_ATTRIBUTES_NONE;
tfp_sprintf(buff, "%3d", pid->D);
if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D))))
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr);
elemAttr = TEXT_ATTRIBUTES_NONE;
tfp_sprintf(buff, "%3d", pid->FF);
if ((isAdjustmentFunctionSelected(adjFuncFF)) || (((adjFuncFF == ADJUSTMENT_ROLL_FF) || (adjFuncFF == ADJUSTMENT_PITCH_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_FF))))
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX + 16, elemPosY, buff, elemAttr);
}
static void osdDisplayNavPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD)
{
textAttributes_t elemAttr;
char buff[4];
@ -1213,7 +1255,7 @@ static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *
elemAttr = TEXT_ATTRIBUTES_NONE;
tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIFF ? pid->FF : pid->D);
if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D_FF) || (adjFuncD == ADJUSTMENT_PITCH_D_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D_FF))))
if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D))))
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr);
}
@ -1626,6 +1668,8 @@ static bool osdDrawSingleElement(uint8_t item)
p = "ANGL";
else if (FLIGHT_MODE(HORIZON_MODE))
p = "HOR ";
else if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH))
p = "TURT";
displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
return true;
@ -1892,35 +1936,35 @@ static bool osdDrawSingleElement(uint8_t item)
#endif
case OSD_ROLL_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D_FF);
osdDisplayFlightPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D, ADJUSTMENT_ROLL_FF);
return true;
case OSD_PITCH_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D_FF);
osdDisplayFlightPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D, ADJUSTMENT_PITCH_FF);
return true;
case OSD_YAW_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D_FF);
osdDisplayFlightPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D, ADJUSTMENT_YAW_FF);
return true;
case OSD_LEVEL_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "LEV", PID_LEVEL, ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D);
osdDisplayNavPIDValues(elemPosX, elemPosY, "LEV", PID_LEVEL, ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D);
return true;
case OSD_POS_XY_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "PXY", PID_POS_XY, ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D);
osdDisplayNavPIDValues(elemPosX, elemPosY, "PXY", PID_POS_XY, ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D);
return true;
case OSD_POS_Z_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "PZ", PID_POS_Z, ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D);
osdDisplayNavPIDValues(elemPosX, elemPosY, "PZ", PID_POS_Z, ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D);
return true;
case OSD_VEL_XY_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "VXY", PID_VEL_XY, ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D);
osdDisplayNavPIDValues(elemPosX, elemPosY, "VXY", PID_VEL_XY, ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D);
return true;
case OSD_VEL_Z_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D);
osdDisplayNavPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D);
return true;
case OSD_HEADING_P:
@ -2224,12 +2268,21 @@ static bool osdDrawSingleElement(uint8_t item)
}
break;
}
case OSD_DEBUG:
{
// Longest representable string is -2147483648, hence 11 characters
/*
* Longest representable string is -2147483648 does not fit in the screen.
* Only 7 digits for negative and 8 digits for positive values allowed
*/
for (uint8_t bufferIndex = 0; bufferIndex < DEBUG32_VALUE_COUNT; ++elemPosY, bufferIndex += 2) {
tfp_sprintf(buff, "[%u]=%11ld [%u]=%11ld", bufferIndex, debug[bufferIndex], bufferIndex+1, debug[bufferIndex+1]);
tfp_sprintf(
buff,
"[%u]=%8ld [%u]=%8ld",
bufferIndex,
constrain(debug[bufferIndex], -9999999, 99999999),
bufferIndex+1,
constrain(debug[bufferIndex+1], -9999999, 99999999)
);
displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
}
break;

2
src/main/io/osd_dji_hd.c
View file

@ -668,7 +668,7 @@ static void osdDJIFormatThrottlePosition(char *buff, bool autoThr )
thr = rcCommand[THROTTLE];
}
tfp_sprintf(buff, "%3d%s", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN), "%THR");
tfp_sprintf(buff, "%3ld%s", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN), "%THR");
}
/**

3
src/main/navigation/navigation.c
View file

@ -91,7 +91,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, 0);
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 9);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 10);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -144,6 +144,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.braking_bank_angle = 40, // Max braking angle
.posDecelerationTime = 120, // posDecelerationTime * 100
.posResponseExpo = 10, // posResponseExpo * 100
.slowDownForTurning = true,
},
// Fixed wing

3
src/main/navigation/navigation.h
View file

@ -219,6 +219,7 @@ typedef struct navConfig_s {
uint8_t braking_bank_angle; // Max angle [deg] that MR is allowed duing braking boost phase
uint8_t posDecelerationTime; // Brake time parameter
uint8_t posResponseExpo; // Position controller expo (taret vel expo for MC)
bool slowDownForTurning; // Slow down during WP missions when changing heading on next waypoint
} mc;
struct {
@ -250,7 +251,7 @@ typedef struct navConfig_s {
uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
bool allow_manual_thr_increase;
bool useFwNavYawControl;
bool useFwNavYawControl;
uint8_t yawControlDeadband;
} fw;
} navConfig_t;

5
src/main/navigation/navigation_fw_launch.c
View file

@ -48,6 +48,8 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#include "io/gps.h"
#define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate
#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms
#define UNUSED(x) ((void)(x))
@ -316,10 +318,11 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU
const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel;
const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0);
const bool isForwardLaunched = isGPSHeadingValid() && (gpsSol.groundSpeed > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0);
applyThrottleIdleLogic(false);
if (isBungeeLaunched || isSwingLaunched) {
if (isBungeeLaunched || isSwingLaunched || isForwardLaunched) {
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) {
return FW_LAUNCH_EVENT_SUCCESS; // the launch is detected now, go to FW_LAUNCH_STATE_DETECTED
}

16
src/main/navigation/navigation_multicopter.c
View file

@ -406,7 +406,7 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR
static float getVelocityHeadingAttenuationFactor(void)
{
// In WP mode scale velocity if heading is different from bearing
if (navGetCurrentStateFlags() & NAV_AUTO_WP) {
if (navConfig()->mc.slowDownForTurning && (navGetCurrentStateFlags() & NAV_AUTO_WP)) {
const int32_t headingError = constrain(wrap_18000(posControl.desiredState.yaw - posControl.actualState.yaw), -9000, 9000);
const float velScaling = cos_approx(CENTIDEGREES_TO_RADIANS(headingError));
@ -438,7 +438,19 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
// Scale velocity to respect max_speed
float newVelTotal = sqrtf(sq(newVelX) + sq(newVelY));
if (newVelTotal > maxSpeed) {
/*
* We override computed speed with max speed in following cases:
* 1 - computed velocity is > maxSpeed
* 2 - in WP mission when: slowDownForTurning is OFF, we do not fly towards na last waypoint and computed speed is < maxSpeed
*/
if (
(navGetCurrentStateFlags() & NAV_AUTO_WP &&
!isApproachingLastWaypoint() &&
newVelTotal < maxSpeed &&
!navConfig()->mc.slowDownForTurning
) || newVelTotal > maxSpeed
) {
newVelX = maxSpeed * (newVelX / newVelTotal);
newVelY = maxSpeed * (newVelY / newVelTotal);
newVelTotal = maxSpeed;

21
src/main/programming/logic_condition.c
View file

@ -45,6 +45,7 @@
#include "flight/pid.h"
#include "drivers/io_port_expander.h"
#include "io/osd_common.h"
#include "sensors/diagnostics.h"
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
@ -324,6 +325,14 @@ static int logicConditionCompute(
return temporaryValue;
break;
case LOGIC_CONDITION_MODULUS:
if (operandB != 0) {
return constrain(operandA % operandB, INT16_MIN, INT16_MAX);
} else {
return operandA;
}
break;
default:
return false;
break;
@ -384,7 +393,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 10
case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 100
return getBatteryVoltage();
break;
@ -400,7 +409,15 @@ static int logicConditionGetFlightOperandValue(int operand) {
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS:
return gpsSol.numSat;
if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
return 0;
} else {
return gpsSol.numSat;
}
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID: // 0/1
return STATE(GPS_FIX) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED: // cm/s

4
src/main/programming/logic_condition.h
View file

@ -69,7 +69,8 @@ typedef enum {
LOGIC_CONDITION_MAP_OUTPUT = 37,
LOGIC_CONDITION_RC_CHANNEL_OVERRIDE = 38,
LOGIC_CONDITION_SET_HEADING_TARGET = 39,
LOGIC_CONDITION_LAST = 40,
LOGIC_CONDITION_MODULUS = 40,
LOGIC_CONDITION_LAST = 41,
} logicOperation_e;
typedef enum logicOperandType_s {
@ -118,6 +119,7 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 31
LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 32
LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 33
LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 34
} logicFlightOperands_e;

9
src/main/rx/ghst_protocol.h
View file

@ -65,6 +65,8 @@ typedef enum {
GHST_DL_LINK_STAT = 0x21,
GHST_DL_VTX_STAT = 0x22,
GHST_DL_PACK_STAT = 0x23, // Battery (Pack) Status
GHST_DL_GPS_PRIMARY = 0x25, // Primary GPS data (position)
GHST_DL_GPS_SECONDARY = 0x26 // Secondary GPS data (auxiliary)
} ghstDl_e;
#define GHST_RC_CTR_VAL_12BIT 0x7C0 // servo center for 12 bit values (0x3e0 << 1)
@ -72,9 +74,12 @@ typedef enum {
#define GHST_FRAME_SIZE 14 // including addr, type, len, crc, and payload
#define GHST_PAYLOAD_SIZE_MAX 14
#define GHST_PAYLOAD_SIZE_MAX 14
#define GHST_FRAME_SIZE_MAX 24
#define GHST_FRAME_SIZE_MAX 24
#define GPS_FLAGS_FIX 0x01
#define GPS_FLAGS_FIX_HOME 0x02
typedef struct ghstFrameDef_s {
uint8_t addr;

17
src/main/sensors/compass.c
View file

@ -40,6 +40,7 @@
#include "drivers/compass/compass_qmc5883l.h"
#include "drivers/compass/compass_mpu9250.h"
#include "drivers/compass/compass_lis3mdl.h"
#include "drivers/compass/compass_rm3100.h"
#include "drivers/compass/compass_msp.h"
#include "drivers/io.h"
#include "drivers/light_led.h"
@ -264,6 +265,22 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
}
FALLTHROUGH;
case MAG_RM3100:
#ifdef USE_MAG_RM3100
if (rm3100MagDetect(dev)) {
#ifdef MAG_RM3100_ALIGN
dev->magAlign.onBoard = MAG_RM3100_ALIGN;
#endif
magHardware = MAG_RM3100;
break;
}
#endif
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (magHardwareToUse != MAG_AUTODETECT) {
break;
}
FALLTHROUGH;
case MAG_FAKE:
#ifdef USE_FAKE_MAG
if (fakeMagDetect(dev)) {

3
src/main/sensors/compass.h
View file

@ -42,7 +42,8 @@ typedef enum {
MAG_IST8308 = 10,
MAG_LIS3MDL = 11,
MAG_MSP = 12,
MAG_FAKE = 13,
MAG_RM3100 = 13,
MAG_FAKE = 14,
MAG_MAX = MAG_FAKE
} magSensor_e;

2
src/main/sensors/opflow.c
View file

@ -75,7 +75,7 @@ static float opflowCalibrationFlowAcc;
#define OPFLOW_UPDATE_TIMEOUT_US 200000 // At least 5Hz updates required
#define OPFLOW_CALIBRATE_TIME_MS 30000 // 30 second calibration time
PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLOW_CONFIG, 1);
PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLOW_CONFIG, 2);
PG_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig,
.opflow_hardware = OPFLOW_NONE,

7
src/main/sensors/opflow.h
View file

@ -30,10 +30,9 @@
typedef enum {
OPFLOW_NONE = 0,
OPFLOW_PMW3901 = 1,
OPFLOW_CXOF = 2,
OPFLOW_MSP = 3,
OPFLOW_FAKE = 4,
OPFLOW_CXOF = 1,
OPFLOW_MSP = 2,
OPFLOW_FAKE = 3,
} opticalFlowSensor_e;
typedef enum {

6
src/main/target/DALRCF405/target.c
View file

@ -28,9 +28,9 @@ const timerHardware_t timerHardware[] = {
DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 (2,2)
DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 (2,6)
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4 (2,1) (2.3 2.6)
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 (2,4) (2.2)
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 (1,2)
DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 (1,5)
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S5 (2,4) (2.2)
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S6 (1,2)
DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S7 (2,3)
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 (2,7)
DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED STRIP(1,0)

4
src/main/target/HGLRCF722/target.c
View file

@ -42,8 +42,8 @@ const timerHardware_t timerHardware[] = {
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP1-2 D(1, 2, 5)
DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2
DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP1-7 D(1, 6, 3)
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 UP1-6 D(1, 0, 2)
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8 UP1-6 D(1, 3, 2)
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 UP1-6 D(1, 0, 2)
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8 UP1-6 D(1, 3, 2)
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 2), // LED D(2, 6, 0)
DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM, RX2

1
src/main/target/MATEKF405CAN/CMakeLists.txt Normal file
View file

@ -0,0 +1 @@
target_stm32f405xg(MATEKF405CAN)

31
src/main/target/MATEKF405CAN/config.c Normal file
View file

@ -0,0 +1,31 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include "platform.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "io/serial.h"
void targetConfiguration(void)
{
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_GPS;
// serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].gps_baudrateIndex = BAUD_115200;
}

44
src/main/target/MATEKF405CAN/target.c Normal file
View file

@ -0,0 +1,44 @@
/*
* 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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <platform.h>
#include "drivers/io.h"
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
const timerHardware_t timerHardware[] = {
DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1), // S1 D(2,2,7)
DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1), // S2 D(2,3,7)
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1), // S3 D(2,4,7)
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4 D(2,7,7)
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S5 D(1,7,5)
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S6 D(1,2,5)
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S7 D(1,0,2)
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S8 D(1,3,2)
DEF_TIM(TIM1, CH1, PA8, TIM_USE_BEEPER, 0, 0), // BEEPER PWM
DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), //2812LED D(1,5,3)
DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2
DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

174
src/main/target/MATEKF405CAN/target.h Normal file
View file

@ -0,0 +1,174 @@
/*
* 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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#define TARGET_BOARD_IDENTIFIER "MF4C"
#define USBD_PRODUCT_STRING "Matek_F405CAN"
#define LED0 PA14 //Blue
#define LED1 PA13 //Green
#define BEEPER PA8
#define BEEPER_INVERTED
#define BEEPER_PWM
#define BEEPER_PWM_FREQUENCY 2500
// *************** SPI1 Gyro & ACC *******************
#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_IMU_MPU6500
#define IMU_MPU6500_ALIGN CW180_DEG_FLIP
#define MPU6500_CS_PIN PA4
#define MPU6500_SPI_BUS BUS_SPI1
#define USE_EXTI
#define GYRO_INT_EXTI PC4
#define USE_MPU_DATA_READY_SIGNAL
// *************** I2C /Baro/Mag *********************
#define USE_I2C
#define USE_I2C_DEVICE_2
#define I2C2_SCL PB10
#define I2C2_SDA PB11
#define USE_BARO
#define BARO_I2C_BUS BUS_I2C2
#define USE_BARO_BMP280
#define USE_BARO_MS5611
#define USE_BARO_DPS310
#define USE_MAG
#define MAG_I2C_BUS BUS_I2C2
#define USE_MAG_AK8963
#define USE_MAG_AK8975
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_MAG_IST8310
#define USE_MAG_MAG3110
#define USE_MAG_LIS3MDL
#define USE_RANGEFINDER
#define USE_RANGEFINDER_HCSR04_I2C
#define RANGEFINDER_I2C_BUS BUS_I2C2
#define PITOT_I2C_BUS BUS_I2C2
#define TEMPERATURE_I2C_BUS BUS_I2C2
#define PCA9685_I2C_BUS BUS_I2C2
// *************** SPI2 RM3100 **************************
#define USE_SPI_DEVICE_2
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_MAG_RM3100
#define MAG_RM3100_ALIGN CW0_DEG_FLIP
#define RM3100_CS_PIN PB12
#define RM3100_SPI_BUS BUS_SPI2
// *************** SPI3 SD Card ********************
#define USE_SDCARD
#define USE_SDCARD_SPI
#define SDCARD_SPI_BUS BUS_SPI3
#define SDCARD_CS_PIN PC14
#define USE_SPI_DEVICE_3
#define SPI3_SCK_PIN PB3
#define SPI3_MISO_PIN PB4
#define SPI3_MOSI_PIN PB5
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
// *************** UART *****************************
#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 PC10
#define UART3_RX_PIN PC11
#define USE_UART4
#define UART4_TX_PIN PA0
#define UART4_RX_PIN PA1
#define USE_UART5
#define UART5_TX_PIN PC12
#define UART5_RX_PIN PD2
#define USE_SOFTSERIAL1
#define SOFTSERIAL_1_TX_PIN PA2 //TX2 pad
#define SOFTSERIAL_1_RX_PIN PA2 //TX2 pad
#define SERIAL_PORT_COUNT 7
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART2
// *************** ADC ***************************
#define USE_ADC
#define ADC_INSTANCE ADC1
#define ADC1_DMA_STREAM DMA2_Stream0
#define ADC_CHANNEL_1_PIN PC0
#define ADC_CHANNEL_2_PIN PC1
#define ADC_CHANNEL_3_PIN PC2
#define ADC_CHANNEL_4_PIN PC3
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
#define RSSI_ADC_CHANNEL ADC_CHN_3
#define AIRSPEED_ADC_CHANNEL ADC_CHN_4
// *************** LED2812 ************************
#define USE_LED_STRIP
#define WS2811_PIN PA15
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST5_HANDLER
#define WS2811_DMA_STREAM DMA1_Stream5
#define WS2811_DMA_CHANNEL DMA_Channel_3
// *************** OTHERS *************************
#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_GPS | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL)
#define VBAT_SCALE 2100 //1K:20K
#define USE_SPEKTRUM_BIND
#define BIND_PIN PA3 // RX2
#define USE_DSHOT
#define USE_ESC_SENSOR
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#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

1
src/main/target/MATEKF411SE/CMakeLists.txt
View file

@ -1,2 +1,3 @@
target_stm32f411xe(MATEKF411SE)
target_stm32f411xe(MATEKF411SE_PINIO)
target_stm32f411xe(MATEKF411SE_FD_SFTSRL1)

3
src/main/target/MATEKF411SE/config.c
View file

@ -25,4 +25,7 @@
void targetConfiguration(void)
{
pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
#ifdef MATEKF411SE_PINIO
pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
#endif
}

2
src/main/target/MATEKF411SE/target.c
View file

@ -35,7 +35,9 @@ const timerHardware_t timerHardware[] = {
DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 pad -softserial_tx2
DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 Pad -PPM
#ifndef MATEKF411SE_PINIO
DEF_TIM(TIM2, CH3, PB10, TIM_USE_ANY, 0, 0), // softserial_rx1 - LED 2812 D(1,1,3)
#endif
};

5
src/main/target/MATEKF411SE/target.h
View file

@ -135,7 +135,7 @@
#define AIRSPEED_ADC_CHANNEL ADC_CHN_4
// *************** LED2812 ************************
#ifndef MATEKF411SE_FD_SFTSRL1
#if !defined(MATEKF411SE_PINIO) && !defined(MATEKF411SE_FD_SFTSRL1)
#define USE_LED_STRIP
#define WS2811_PIN PB10
#endif
@ -144,6 +144,9 @@
#define USE_PINIO
#define USE_PINIOBOX
#define PINIO1_PIN PA13 // Camera switcher
#ifdef MATEKF411SE_PINIO
#define PINIO2_PIN PB10 // External PINIO (LED pad)
#endif
// *************** OTHERS *************************
#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL )

6
src/main/target/common_hardware.c
View file

@ -238,6 +238,12 @@
#endif
BUSDEV_REGISTER_I2C(busdev_ist8308, DEVHW_IST8308, IST8308_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0);
#endif
#if defined(USE_MAG_RM3100)
#if defined(RM3100_SPI_BUS)
BUSDEV_REGISTER_SPI(busdev_rm3100, DEVHW_RM3100, RM3100_SPI_BUS, RM3100_CS_PIN, NONE, DEVFLAGS_NONE, 0);
#endif
#endif
#endif

6
src/main/telemetry/crsf.c
View file

@ -246,7 +246,11 @@ static void crsfFrameBatterySensor(sbuf_t *dst)
// use sbufWrite since CRC does not include frame length
sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
crsfSerialize8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
crsfSerialize16(dst, getBatteryVoltage() / 10); // vbat is in units of 0.01V
if (telemetryConfig()->report_cell_voltage) {
crsfSerialize16(dst, getBatteryAverageCellVoltage() / 10);
} else {
crsfSerialize16(dst, getBatteryVoltage() / 10); // vbat is in units of 0.01V
}
crsfSerialize16(dst, getAmperage() / 10);
const uint8_t batteryRemainingPercentage = calculateBatteryPercentage();
crsfSerialize8(dst, (getMAhDrawn() >> 16));

67
src/main/telemetry/ghst.c
View file

@ -43,6 +43,7 @@
#include "drivers/nvic.h"
#include "fc/config.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
@ -51,6 +52,8 @@
#include "io/gps.h"
#include "io/serial.h"
#include "navigation/navigation.h"
#include "rx/rx.h"
#include "rx/ghst.h"
#include "rx/ghst_protocol.h"
@ -67,6 +70,7 @@
#define GHST_CYCLETIME_US 100000 // 10x/sec
#define GHST_FRAME_PACK_PAYLOAD_SIZE 10
#define GHST_FRAME_GPS_PAYLOAD_SIZE 10
#define GHST_FRAME_LENGTH_CRC 1
#define GHST_FRAME_LENGTH_TYPE 1
@ -112,11 +116,51 @@ void ghstFramePackTelemetry(sbuf_t *dst)
sbufWriteU8(dst, 0x00); // tbd3
}
// GPS data, primary, positional data
void ghstFrameGpsPrimaryTelemetry(sbuf_t *dst)
{
// use sbufWrite since CRC does not include frame length
sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
sbufWriteU8(dst, GHST_DL_GPS_PRIMARY);
sbufWriteU32(dst, gpsSol.llh.lat);
sbufWriteU32(dst, gpsSol.llh.lon);
// constrain alt. from -32,000m to +32,000m, units of meters
const int16_t altitude = (constrain(getEstimatedActualPosition(Z), -32000 * 100, 32000 * 100) / 100);
sbufWriteU16(dst, altitude);
}
// GPS data, secondary, auxiliary data
void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst)
{
// use sbufWrite since CRC does not include frame length
sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
sbufWriteU8(dst, GHST_DL_GPS_SECONDARY);
sbufWriteU16(dst, gpsSol.groundSpeed); // speed in 0.1m/s
sbufWriteU16(dst, gpsSol.groundCourse); // degrees * 10
sbufWriteU8(dst, gpsSol.numSat);
sbufWriteU16(dst, (uint16_t) (GPS_distanceToHome / 10)); // use units of 10m to increase range of U16 to 655.36km
sbufWriteU16(dst, GPS_directionToHome);
uint8_t gpsFlags = 0;
if(STATE(GPS_FIX))
gpsFlags |= GPS_FLAGS_FIX;
if(STATE(GPS_FIX_HOME))
gpsFlags |= GPS_FLAGS_FIX_HOME;
sbufWriteU8(dst, gpsFlags);
}
// schedule array to decide how often each type of frame is sent
typedef enum {
GHST_FRAME_START_INDEX = 0,
GHST_FRAME_PACK_INDEX = GHST_FRAME_START_INDEX, // Battery (Pack) data
GHST_SCHEDULE_COUNT_MAX
GHST_FRAME_GPS_PRIMARY_INDEX, // GPS, primary values (Lat, Long, Alt)
GHST_FRAME_GPS_SECONDARY_INDEX, // GPS, secondary values (Sat Count, HDOP, etc.)
GHST_SCHEDULE_COUNT_MAX
} ghstFrameTypeIndex_e;
static uint8_t ghstScheduleCount;
@ -136,6 +180,19 @@ static void processGhst(void)
ghstFramePackTelemetry(dst);
ghstFinalize(dst);
}
if (currentSchedule & BIT(GHST_FRAME_GPS_PRIMARY_INDEX)) {
ghstInitializeFrame(dst);
ghstFrameGpsPrimaryTelemetry(dst);
ghstFinalize(dst);
}
if (currentSchedule & BIT(GHST_FRAME_GPS_SECONDARY_INDEX)) {
ghstInitializeFrame(dst);
ghstFrameGpsSecondaryTelemetry(dst);
ghstFinalize(dst);
}
ghstScheduleIndex = (ghstScheduleIndex + 1) % ghstScheduleCount;
}
@ -152,6 +209,14 @@ void initGhstTelemetry(void)
if (isBatteryVoltageConfigured() || isAmperageConfigured()) {
ghstSchedule[index++] = BIT(GHST_FRAME_PACK_INDEX);
}
#ifdef USE_GPS
if (feature(FEATURE_GPS)) {
ghstSchedule[index++] = BIT(GHST_FRAME_GPS_PRIMARY_INDEX);
ghstSchedule[index++] = BIT(GHST_FRAME_GPS_SECONDARY_INDEX);
}
#endif
ghstScheduleCount = (uint8_t)index;
}

2
src/test/unit/CMakeLists.txt
View file

@ -27,6 +27,8 @@ set_property(SOURCE telemetry_hott_unittest.cc PROPERTY depends
set_property(SOURCE time_unittest.cc PROPERTY depends "drivers/time.c")
set_property(SOURCE circular_queue_unittest.cc PROPERTY depends "common/circular_queue.c")
function(unit_test src)
get_filename_component(basename ${src} NAME)
string(REPLACE ".cc" "" name ${basename} )

166
src/test/unit/circular_queue_unittest.cc Normal file
View file

@ -0,0 +1,166 @@
#include "unittest_macros.h"
#include "gtest/gtest.h"
#include <stdint.h>
#include <stdlib.h>
#include <time.h>
extern "C" {
#include "common/circular_queue.h"
}
#include <queue>
#include <climits>
TEST(CircularQueue, ElementsAreCorrect){
srand (time(NULL));
circularBuffer_t buffer;
const size_t bufferSize = 16;
uint8_t buff[bufferSize];
circularBufferInit(&buffer, buff, bufferSize, sizeof(char));
const uint8_t testBurst = 3;
const uint8_t dataToInsertSize[testBurst] = {5, 10, 3};
const uint8_t dataToRemoveSize[testBurst] = {3, 7, 2};
std::queue <uint8_t>queue;
for(uint8_t j=0; j<testBurst; j++){
for(uint8_t i = 0;i < dataToInsertSize[j]; i++)
{
uint8_t value = rand() % 255;
queue.push(value);
circularBufferPushElement(&buffer, &value);
}
for(uint8_t i = 0;i < dataToRemoveSize[j]; i++)
{
uint8_t value;
circularBufferPopHead(&buffer, &value);
EXPECT_EQ(queue.front(),value);
queue.pop();
}
EXPECT_EQ(circularBufferCountElements(&buffer),queue.size());
}
EXPECT_EQ(circularBufferIsEmpty(&buffer),queue.empty());
}
TEST(CircularQueue, CheckIsEmptyAndIsFull16){ //check with uint16_t
srand (time(NULL));
circularBuffer_t buffer;
typedef uint16_t tested_type;
const size_t bufferSize = 16 * sizeof(tested_type);
uint8_t buff[bufferSize];
circularBufferInit(&buffer, buff, bufferSize, sizeof(tested_type));
const int testBurst = 3;
const int dataToInsertSize[testBurst] = {10, 8, 4};
const int dataToRemoveSize[testBurst] = {3, 3, 0};
//At the end we have 0 elements
std::queue <tested_type>queue;
EXPECT_EQ(circularBufferIsEmpty(&buffer),true);
EXPECT_EQ(circularBufferIsFull(&buffer),false);
for(uint8_t j=0; j<testBurst; j++){
for(uint8_t i = 0;i < dataToInsertSize[j]; i++)
{
tested_type value = rand() % SHRT_MAX;
queue.push(value);
circularBufferPushElement(&buffer, (uint8_t*)&value);
}
for(uint8_t i = 0;i < dataToRemoveSize[j]; i++)
{
tested_type value;
circularBufferPopHead(&buffer, (uint8_t*)&value);
EXPECT_EQ(queue.front(),value);
queue.pop();
}
EXPECT_EQ(circularBufferCountElements(&buffer),queue.size());
}
EXPECT_EQ(circularBufferIsFull(&buffer),true);
//Remove all elements
while(!circularBufferIsEmpty(&buffer))
{
tested_type value;
circularBufferPopHead(&buffer, (uint8_t*)&value);
EXPECT_EQ(queue.front(),value);
queue.pop();
}
EXPECT_EQ(circularBufferIsFull(&buffer),false);
EXPECT_EQ(circularBufferIsEmpty(&buffer),true);
EXPECT_EQ(circularBufferIsEmpty(&buffer),queue.empty());
}
TEST(CircularQueue, CheckIsEmptyAndIsFull32){ //check with uint32_t
srand (time(NULL));
circularBuffer_t buffer;
typedef uint32_t tested_type;
const size_t bufferSize = 16 * sizeof(tested_type);
uint8_t buff[bufferSize];
circularBufferInit(&buffer, buff, bufferSize, sizeof(tested_type));
const int testBurst = 3;
const int dataToInsertSize[testBurst] = {10, 8, 4};
const int dataToRemoveSize[testBurst] = {3, 3, 0};
//At the end we have 0 elements
std::queue <tested_type>queue;
EXPECT_EQ(circularBufferIsEmpty(&buffer),true);
EXPECT_EQ(circularBufferIsFull(&buffer),false);
for(uint8_t j=0; j<testBurst; j++){
for(uint8_t i = 0;i < dataToInsertSize[j]; i++)
{
tested_type value = rand() % INT_MAX;
queue.push(value);
circularBufferPushElement(&buffer, (uint8_t*)&value);
}
for(uint8_t i = 0;i < dataToRemoveSize[j]; i++)
{
tested_type value;
circularBufferPopHead(&buffer, (uint8_t*)&value);
EXPECT_EQ(queue.front(),value);
queue.pop();
}
EXPECT_EQ(circularBufferCountElements(&buffer),queue.size());
}
EXPECT_EQ(circularBufferIsFull(&buffer),true);
//Remove all elements
while(!circularBufferIsEmpty(&buffer))
{
tested_type value;
circularBufferPopHead(&buffer, (uint8_t*)&value);
EXPECT_EQ(queue.front(),value);
queue.pop();
}
EXPECT_EQ(circularBufferIsFull(&buffer),false);
EXPECT_EQ(circularBufferIsEmpty(&buffer),true);
EXPECT_EQ(circularBufferIsEmpty(&buffer),queue.empty());
}