Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 00:05:33 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'refs/remotes/betaflight/master' into betaflight

Browse source
This commit is contained in:
tianbin4279 2016-07-26 12:53:15 +08:00
commit c8d1da1c6e
165 changed files with 4789 additions and 3999 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/main/blackbox/blackbox.c
View file

@ -632,7 +632,7 @@ static void writeInterframe(void)
arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
/*
/*
* The PID I field changes very slowly, most of the time +-2, so use an encoding
* that can pack all three fields into one byte in that situation.
*/
@ -1140,6 +1140,7 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("Firmware type:%s", "Cleanflight");
BLACKBOX_PRINT_HEADER_LINE("Firmware revision:Betaflight %s (%s) %s", FC_VERSION_STRING, shortGitRevision, targetName);
BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate, buildTime);
BLACKBOX_PRINT_HEADER_LINE("Craft name:%s", masterConfig.name);
BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", masterConfig.escAndServoConfig.maxthrottle);
@ -1210,7 +1211,7 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit);
BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count);
BLACKBOX_PRINT_HEADER_LINE("dynamic_pid:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pid);
BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.vbatPidCompensation);
BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate);
BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f));
@ -1218,12 +1219,13 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d", (int)(masterConfig.gyro_soft_notch_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_q:%d", (int)(masterConfig.gyro_soft_notch_q * 10.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.acc_hardware);
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware);
BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", masterConfig.mag_hardware);
BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm);
BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation);
BLACKBOX_PRINT_HEADER_LINE("rc_smooth_interval:%d", masterConfig.rxConfig.rcSmoothInterval);
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold);
BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures);
@ -1314,7 +1316,7 @@ static void blackboxCheckAndLogFlightMode()
}
}
/*
/*
* Use the user's num/denom settings to decide if the P-frame of the given index should be logged, allowing the user to control
* the portion of logged loop iterations.
*/

2
src/main/blackbox/blackbox_io.c
View file

@ -507,7 +507,7 @@ void blackboxWriteFloat(float value)
/**
* If there is data waiting to be written to the blackbox device, attempt to write (a portion of) that now.
*
*
* Intended to be called regularly for the blackbox device to perform housekeeping.
*/
void blackboxDeviceFlush(void)

41
src/main/common/filter.c
View file

@ -56,24 +56,39 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
}
/* sets up a biquad Filter */
void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate)
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
{
biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
}
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, filterType_e filterType)
{
const float sampleRate = 1 / ((float)refreshRate * 0.000001f);
// setup variables
const float omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate;
const float sampleRate = 1 / ((float)refreshRate * 0.000001f);
const float omega = 2 * M_PI_FLOAT * filterFreq / sampleRate;
const float sn = sinf(omega);
const float cs = cosf(omega);
//this is wrong, should be hyperbolic sine
//alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);
const float alpha = sn / (2 * BIQUAD_Q);
const float alpha = sn / (2 * Q);
const float b0 = (1 - cs) / 2;
const float b1 = 1 - cs;
const float b2 = (1 - cs) / 2;
const float a0 = 1 + alpha;
const float a1 = -2 * cs;
const float a2 = 1 - alpha;
float b0, b1, b2, a0, a1, a2;
switch (filterType) {
case FILTER_LPF:
b0 = (1 - cs) / 2;
b1 = 1 - cs;
b2 = (1 - cs) / 2;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;
break;
case FILTER_NOTCH:
b0 = 1;
b1 = -2 * cs;
b2 = 1;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;
break;
}
// precompute the coefficients
filter->b0 = b0 / a0;

7
src/main/common/filter.h
View file

@ -29,8 +29,13 @@ typedef struct biquadFilter_s {
float d1, d2;
} biquadFilter_t;
typedef enum {
FILTER_LPF,
FILTER_NOTCH
} filterType_e;
void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate);
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, filterType_e filterType);
float biquadFilterApply(biquadFilter_t *filter, float input);
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);

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

@ -231,7 +231,7 @@ int32_t quickMedianFilter5(int32_t * v)
QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[4]); QMF_SORT(p[0], p[3]);
QMF_SORT(p[1], p[4]); QMF_SORT(p[1], p[2]); QMF_SORT(p[2], p[3]);
QMF_SORT(p[1], p[2]);
QMF_SORT(p[1], p[2]);
return p[2];
}
@ -279,7 +279,7 @@ float quickMedianFilter5f(float * v)
QMF_SORTF(p[0], p[1]); QMF_SORTF(p[3], p[4]); QMF_SORTF(p[0], p[3]);
QMF_SORTF(p[1], p[4]); QMF_SORTF(p[1], p[2]); QMF_SORTF(p[2], p[3]);
QMF_SORTF(p[1], p[2]);
QMF_SORTF(p[1], p[2]);
return p[2];
}

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

@ -56,7 +56,7 @@ typedef void (*putcf) (void *, char);
static putcf stdout_putf;
static void *stdout_putp;
// print bf, padded from left to at least n characters.
// print bf, padded from left to at least n characters.
// padding is zero ('0') if z!=0, space (' ') otherwise
static int putchw(void *putp, putcf putf, int n, char z, char *bf)
{

11
src/main/common/utils.h
View file

@ -71,4 +71,15 @@ http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html
static inline int16_t cmp16(uint16_t a, uint16_t b) { return a-b; }
static inline int32_t cmp32(uint32_t a, uint32_t b) { return a-b; }
// using memcpy_fn will force memcpy function call, instead of inlining it. In most cases function call takes fewer instructions
// than inlined version (inlining is cheaper for very small moves < 8 bytes / 2 store instructions)
#ifdef UNIT_TEST
// Call memcpy when building unittest - this is easier that asm symbol name mangling (symbols start with _underscore on win32)
#include <string.h>
static inline void memcpy_fn ( void * destination, const void * source, size_t num ) { memcpy(destination, source, num); };
#else
void * memcpy_fn ( void * destination, const void * source, size_t num ) asm("memcpy");
#endif
#endif

46
src/main/config/config.c
View file

@ -159,7 +159,7 @@ size_t custom_flash_memory_address = 0;
#define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address)
#else
// use the last flash pages for storage
#ifndef CONFIG_START_FLASH_ADDRESS
#ifndef CONFIG_START_FLASH_ADDRESS
#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG))
#endif
#endif
@ -182,12 +182,7 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
void resetPidProfile(pidProfile_t *pidProfile)
{
#if (defined(STM32F10X))
pidProfile->pidController = PID_CONTROLLER_INTEGER;
#else
pidProfile->pidController = PID_CONTROLLER_FLOAT;
#endif
pidProfile->pidController = PID_CONTROLLER_BETAFLIGHT;
pidProfile->P8[ROLL] = 45;
pidProfile->I8[ROLL] = 40;
@ -220,11 +215,19 @@ void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->yaw_lpf_hz = 80;
pidProfile->rollPitchItermIgnoreRate = 180;
pidProfile->yawItermIgnoreRate = 35;
pidProfile->rollPitchItermIgnoreRate = 200;
pidProfile->yawItermIgnoreRate = 50;
pidProfile->dterm_lpf_hz = 100; // filtering ON by default
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
pidProfile->dynamic_pid = 1;
pidProfile->vbatPidCompensation = 0;
pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF;
// Betaflight PID controller parameters
pidProfile->toleranceBand = 15;
pidProfile->toleranceBandReduction = 35;
pidProfile->zeroCrossAllowanceCount = 3;
pidProfile->accelerationLimitPercent = 20;
pidProfile->itermThrottleGain = 10;
#ifdef GTUNE
pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune.
@ -278,7 +281,6 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig)
#endif
escAndServoConfig->mincommand = 1000;
escAndServoConfig->servoCenterPulse = 1500;
escAndServoConfig->escDesyncProtection = 0;
}
void resetFlight3DConfig(flight3DConfig_t *flight3DConfig)
@ -311,7 +313,6 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig)
batteryConfig->vbatmincellvoltage = 33;
batteryConfig->vbatwarningcellvoltage = 35;
batteryConfig->vbathysteresis = 1;
batteryConfig->vbatPidCompensation = 0;
batteryConfig->currentMeterOffset = 0;
batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A)
batteryConfig->batteryCapacity = 0;
@ -349,7 +350,7 @@ void resetSerialConfig(serialConfig_t *serialConfig)
serialConfig->reboot_character = 'R';
}
static void resetControlRateConfig(controlRateConfig_t *controlRateConfig)
static void resetControlRateConfig(controlRateConfig_t *controlRateConfig)
{
controlRateConfig->rcRate8 = 100;
controlRateConfig->rcYawRate8 = 100;
@ -366,7 +367,7 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig)
}
void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig)
void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig)
{
rcControlsConfig->deadband = 0;
rcControlsConfig->yaw_deadband = 0;
@ -374,7 +375,7 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig)
rcControlsConfig->alt_hold_fast_change = 1;
}
void resetMixerConfig(mixerConfig_t *mixerConfig)
void resetMixerConfig(mixerConfig_t *mixerConfig)
{
mixerConfig->yaw_motor_direction = 1;
#ifdef USE_SERVOS
@ -441,6 +442,7 @@ static void resetConf(void)
featureSet(FEATURE_VBAT);
#endif
masterConfig.version = EEPROM_CONF_VERSION;
masterConfig.mixerMode = MIXER_QUADX;
@ -451,12 +453,14 @@ static void resetConf(void)
masterConfig.gyro_lpf = 0; // 256HZ default
#ifdef STM32F10X
masterConfig.gyro_sync_denom = 8;
masterConfig.pid_process_denom = 1;
#else
masterConfig.gyro_sync_denom = 4;
masterConfig.pid_process_denom = 2;
#endif
masterConfig.gyro_soft_lpf_hz = 100;
masterConfig.pid_process_denom = 2;
masterConfig.gyro_soft_notch_hz = 0;
masterConfig.gyro_soft_notch_q = 5;
masterConfig.debug_mode = 0;
@ -533,6 +537,7 @@ static void resetConf(void)
#ifdef BRUSHED_MOTORS
masterConfig.motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE;
masterConfig.motor_pwm_protocol = PWM_TYPE_BRUSHED;
masterConfig.use_unsyncedPwm = true;
#else
masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE;
masterConfig.motor_pwm_protocol = PWM_TYPE_ONESHOT125;
@ -612,8 +617,10 @@ static void resetConf(void)
}
#ifdef LED_STRIP
applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT);
applyDefaultColors(masterConfig.colors);
applyDefaultLedStripConfig(masterConfig.ledConfigs);
applyDefaultModeColors(masterConfig.modeColors);
applyDefaultSpecialColors(&(masterConfig.specialColors));
masterConfig.ledstrip_visual_beeper = 0;
#endif
@ -657,6 +664,7 @@ static void resetConf(void)
targetConfiguration();
#endif
// copy first profile into remaining profile
for (int i = 1; i < MAX_PROFILE_COUNT; i++) {
memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t));
@ -715,7 +723,7 @@ void activateConfig(void)
&currentProfile->pidProfile
);
gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz);
gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_q);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);

4
src/main/config/config.h
View file

@ -24,6 +24,8 @@
#endif
#define MAX_RATEPROFILES 3
#define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500
#define MAX_NAME_LENGTH 16
typedef enum {
FEATURE_RX_PPM = 1 << 0,
@ -52,6 +54,8 @@ typedef enum {
FEATURE_SUPEREXPO_RATES = 1 << 23,
FEATURE_OSD = 1 << 24,
FEATURE_VTX = 1 << 25,
FEATURE_RX_NRF24 = 1 << 26,
FEATURE_SOFTSPI = 1 << 27,
} features_e;
void latchActiveFeatures(void);

17
src/main/config/config_master.h
View file

@ -56,9 +56,11 @@ typedef struct master_t {
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint8_t gyro_sync_denom; // Gyro sample divider
uint8_t gyro_soft_lpf_hz; // Biqyad gyro lpf hz
uint8_t gyro_soft_lpf_hz; // Biquad gyro lpf hz
uint16_t gyro_soft_notch_hz; // Biquad gyro notch hz
uint8_t gyro_soft_notch_q; // Biquad gyro notch quality
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)
@ -117,8 +119,10 @@ typedef struct master_t {
telemetryConfig_t telemetryConfig;
#ifdef LED_STRIP
ledConfig_t ledConfigs[MAX_LED_STRIP_LENGTH];
hsvColor_t colors[CONFIGURABLE_COLOR_COUNT];
ledConfig_t ledConfigs[LED_MAX_STRIP_LENGTH];
hsvColor_t colors[LED_CONFIGURABLE_COLOR_COUNT];
modeColorIndexes_t modeColors[LED_MODE_COUNT];
specialColorIndexes_t specialColors;
uint8_t ledstrip_visual_beeper; // suppress LEDLOW mode if beeper is on
#endif
@ -137,7 +141,7 @@ typedef struct master_t {
profile_t profile[MAX_PROFILE_COUNT];
uint8_t current_profile_index;
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT];
@ -161,6 +165,9 @@ typedef struct master_t {
uint8_t magic_ef; // magic number, should be 0xEF
uint8_t chk; // XOR checksum
char name[MAX_NAME_LENGTH+1];
} master_t;
extern master_t masterConfig;

1
src/main/debug.h
View file

@ -50,5 +50,6 @@ typedef enum {
DEBUG_MIXER,
DEBUG_AIRMODE,
DEBUG_PIDLOOP,
DEBUG_NOTCH,
DEBUG_COUNT
} debugType_e;

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

@ -252,7 +252,7 @@ void mpuIntExtiInit(void)
EXTIEnable(mpuIntIO, true);
#endif
mpuExtiInitDone = true;
mpuExtiInitDone = true;
}
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)

4
src/main/drivers/accgyro_mpu.h
View file

@ -18,7 +18,7 @@
#pragma once
#include "exti.h"
// MPU6050
#define MPU_RA_WHO_AM_I 0x75
#define MPU_RA_WHO_AM_I_LEGACY 0x00
@ -119,7 +119,7 @@
typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data);
typedef void(*mpuResetFuncPtr)(void);
typedef void(*mpuResetFuncPtr)(void);
typedef struct mpuConfiguration_s {
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each

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

@ -104,7 +104,7 @@ static void mpu6050GyroInit(uint8_t lpf)
delay(100);
ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
ack = mpuConfiguration.write(MPU_RA_CONFIG, lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
ack = mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec

4
src/main/drivers/accgyro_spi_mpu6000.c
View file

@ -158,7 +158,7 @@ bool mpu6000SpiDetect(void)
uint8_t in;
uint8_t attemptsRemaining = 5;
#ifdef MPU6000_CS_PIN
#ifdef MPU6000_CS_PIN
mpuSpi6000CsPin = IOGetByTag(IO_TAG(MPU6000_CS_PIN));
#endif
IOInit(mpuSpi6000CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
@ -253,7 +253,7 @@ static void mpu6000AccAndGyroInit(void) {
delayMicroseconds(15);
#endif
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST);
delayMicroseconds(1);
mpuSpi6000InitDone = true;

8
src/main/drivers/accgyro_spi_mpu9250.c
View file

@ -54,7 +54,7 @@ static IO_t mpuSpi9250CsPin = IO_NONE;
#define DISABLE_MPU9250 IOHi(mpuSpi9250CsPin)
#define ENABLE_MPU9250 IOLo(mpuSpi9250CsPin)
void mpu9250ResetGyro(void)
void mpu9250ResetGyro(void)
{
// Device Reset
mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
@ -123,7 +123,7 @@ void mpu9250SpiAccInit(acc_t *acc)
acc->acc_1G = 512 * 8;
}
bool verifympu9250WriteRegister(uint8_t reg, uint8_t data)
bool verifympu9250WriteRegister(uint8_t reg, uint8_t data)
{
uint8_t in;
uint8_t attemptsRemaining = 20;
@ -176,7 +176,7 @@ static void mpu9250AccAndGyroInit(uint8_t lpf) {
verifympu9250WriteRegister(MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
#endif
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
mpuSpi9250InitDone = true; //init done
}
@ -208,7 +208,7 @@ bool mpu9250SpiDetect(void)
}
} while (attemptsRemaining--);
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
return true;
}

6
src/main/drivers/adc_impl.h
View file

@ -24,14 +24,14 @@
#define ADC_TAG_MAP_COUNT 16
#elif defined(STM32F3)
#define ADC_TAG_MAP_COUNT 39
#else
#else
#define ADC_TAG_MAP_COUNT 10
#endif
#endif
typedef enum ADCDevice {
ADCINVALID = -1,
ADCDEV_1 = 0,
#if defined(STM32F3)
#if defined(STM32F3)
ADCDEV_2,
ADCDEV_MAX = ADCDEV_2,
#elif defined(STM32F4)

20
src/main/drivers/adc_stm32f10x.c
View file

@ -24,9 +24,6 @@
#include "build_config.h"
#include "system.h"
#include "sensors/sensors.h" // FIXME dependency into the main code
#include "sensor.h"
#include "accgyro.h"
#include "adc.h"
@ -38,13 +35,13 @@
#define ADC_INSTANCE ADC1
#endif
const adcDevice_t adcHardware[] = {
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 }
const adcDevice_t adcHardware[] = {
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 }
};
ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
{
if (instance == ADC1)
if (instance == ADC1)
return ADCDEV_1;
/* TODO -- ADC2 available on large 10x devices.
@ -54,7 +51,7 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
return ADCINVALID;
}
const adcTagMap_t adcTagMap[] = {
const adcTagMap_t adcTagMap[] = {
{ DEFIO_TAG_E__PA0, ADC_Channel_0 }, // ADC12
{ DEFIO_TAG_E__PA1, ADC_Channel_1 }, // ADC12
{ DEFIO_TAG_E__PA2, ADC_Channel_2 }, // ADC12
@ -64,7 +61,7 @@ const adcTagMap_t adcTagMap[] = {
{ DEFIO_TAG_E__PA6, ADC_Channel_6 }, // ADC12
{ DEFIO_TAG_E__PA7, ADC_Channel_7 }, // ADC12
{ DEFIO_TAG_E__PB0, ADC_Channel_8 }, // ADC12
{ DEFIO_TAG_E__PB1, ADC_Channel_9 }, // ADC12
{ DEFIO_TAG_E__PB1, ADC_Channel_9 }, // ADC12
};
// Driver for STM32F103CB onboard ADC
@ -84,7 +81,6 @@ void adcInit(drv_adc_config_t *init)
UNUSED(init);
#endif
uint8_t i;
uint8_t configuredAdcChannels = 0;
memset(&adcConfig, 0, sizeof(adcConfig));
@ -117,9 +113,9 @@ void adcInit(drv_adc_config_t *init)
if (device == ADCINVALID)
return;
adcDevice_t adc = adcHardware[device];
const adcDevice_t adc = adcHardware[device];
for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) {
for (int i = 0; i < ADC_CHANNEL_COUNT; i++) {
if (!adcConfig[i].tag)
continue;
@ -163,7 +159,7 @@ void adcInit(drv_adc_config_t *init)
ADC_Init(adc.ADCx, &ADC_InitStructure);
uint8_t rank = 1;
for (i = 0; i < ADC_CHANNEL_COUNT; i++) {
for (int i = 0; i < ADC_CHANNEL_COUNT; i++) {
if (!adcConfig[i].enabled) {
continue;
}

20
src/main/drivers/adc_stm32f30x.c
View file

@ -21,7 +21,6 @@
#include "platform.h"
#include "system.h"
#include "common/utils.h"
#include "gpio.h"
#include "sensor.h"
@ -32,16 +31,18 @@
#include "io.h"
#include "rcc.h"
#include "common/utils.h"
#ifndef ADC_INSTANCE
#define ADC_INSTANCE ADC1
#endif
const adcDevice_t adcHardware[] = {
{ .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 },
{ .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA2), .DMAy_Channelx = DMA2_Channel1 }
const adcDevice_t adcHardware[] = {
{ .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 },
{ .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA2), .DMAy_Channelx = DMA2_Channel1 }
};
const adcTagMap_t adcTagMap[] = {
const adcTagMap_t adcTagMap[] = {
{ DEFIO_TAG_E__PA0, ADC_Channel_1 }, // ADC1
{ DEFIO_TAG_E__PA1, ADC_Channel_2 }, // ADC1
{ DEFIO_TAG_E__PA2, ADC_Channel_3 }, // ADC1
@ -85,7 +86,7 @@ const adcTagMap_t adcTagMap[] = {
ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
{
if (instance == ADC1)
if (instance == ADC1)
return ADCDEV_1;
if (instance == ADC2)
@ -100,7 +101,6 @@ void adcInit(drv_adc_config_t *init)
ADC_InitTypeDef ADC_InitStructure;
DMA_InitTypeDef DMA_InitStructure;
uint8_t i;
uint8_t adcChannelCount = 0;
memset(&adcConfig, 0, sizeof(adcConfig));
@ -133,9 +133,9 @@ void adcInit(drv_adc_config_t *init)
if (device == ADCINVALID)
return;
adcDevice_t adc = adcHardware[device];
adcDevice_t adc = adcHardware[device];
for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) {
for (int i = 0; i < ADC_CHANNEL_COUNT; i++) {
if (!adcConfig[i].tag)
continue;
@ -203,7 +203,7 @@ void adcInit(drv_adc_config_t *init)
ADC_Init(adc.ADCx, &ADC_InitStructure);
uint8_t rank = 1;
for (i = 0; i < ADC_CHANNEL_COUNT; i++) {
for (int i = 0; i < ADC_CHANNEL_COUNT; i++) {
if (!adcConfig[i].enabled) {
continue;
}

14
src/main/drivers/adc_stm32f4xx.c
View file

@ -26,8 +26,6 @@
#include "io_impl.h"
#include "rcc.h"
#include "sensors/sensors.h" // FIXME dependency into the main code
#include "sensor.h"
#include "accgyro.h"
@ -38,13 +36,13 @@
#define ADC_INSTANCE ADC1
#endif
const adcDevice_t adcHardware[] = {
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 },
//{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 }
const adcDevice_t adcHardware[] = {
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 },
//{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 }
};
/* note these could be packed up for saving space */
const adcTagMap_t adcTagMap[] = {
const adcTagMap_t adcTagMap[] = {
/*
{ DEFIO_TAG_E__PF3, ADC_Channel_9 },
{ DEFIO_TAG_E__PF4, ADC_Channel_14 },
@ -75,7 +73,7 @@ const adcTagMap_t adcTagMap[] = {
ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
{
if (instance == ADC1)
if (instance == ADC1)
return ADCDEV_1;
/*
if (instance == ADC2) // TODO add ADC2 and 3
@ -128,7 +126,7 @@ void adcInit(drv_adc_config_t *init)
if (device == ADCINVALID)
return;
adcDevice_t adc = adcHardware[device];
adcDevice_t adc = adcHardware[device];
for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) {
if (!adcConfig[i].tag)

8
src/main/drivers/barometer_bmp085.c
View file

@ -35,7 +35,7 @@
#ifdef BARO
#if defined(BARO_EOC_GPIO)
#if defined(BARO_EOC_GPIO)
static IO_t eocIO;
@ -49,7 +49,7 @@ void bmp085_extiHandler(extiCallbackRec_t* cb)
isConversionComplete = true;
}
bool bmp085TestEOCConnected(const bmp085Config_t *config);
bool bmp085TestEOCConnected(const bmp085Config_t *config);
# endif
typedef struct {
@ -139,7 +139,7 @@ static IO_t xclrIO;
#endif
void bmp085InitXclrIO(const bmp085Config_t *config)
void bmp085InitXclrIO(const bmp085Config_t *config)
{
if (!xclrIO && config && config->xclrIO) {
xclrIO = IOGetByTag(config->xclrIO);
@ -184,7 +184,7 @@ bool bmp085Detect(const bmp085Config_t *config, baro_t *baro)
delay(20); // datasheet says 10ms, we'll be careful and do 20.
ack = i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
ack = i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
if (ack) {
bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
bmp085.oversampling_setting = 3;

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

@ -19,7 +19,7 @@
typedef struct bmp085Config_s {
ioTag_t xclrIO;
ioTag_t eocIO;
ioTag_t eocIO;
} bmp085Config_t;
bool bmp085Detect(const bmp085Config_t *config, baro_t *baro);

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

@ -26,7 +26,7 @@
#ifndef I2C_DEVICE
#define I2C_DEVICE I2CINVALID
#endif
#endif
typedef enum I2CDevice {
I2CINVALID = -1,

44
src/main/drivers/bus_i2c_stm32f10x.c
View file

@ -47,35 +47,35 @@ static void i2cUnstick(IO_t scl, IO_t sda);
#define IOCFG_I2C IOCFG_AF_OD
#endif
#ifndef I2C1_SCL
#define I2C1_SCL PB8
#ifndef I2C1_SCL
#define I2C1_SCL PB8
#endif
#ifndef I2C1_SDA
#define I2C1_SDA PB9
#ifndef I2C1_SDA
#define I2C1_SDA PB9
#endif
#else
#ifndef I2C1_SCL
#define I2C1_SCL PB6
#ifndef I2C1_SCL
#define I2C1_SCL PB6
#endif
#ifndef I2C1_SDA
#define I2C1_SDA PB7
#ifndef I2C1_SDA
#define I2C1_SDA PB7
#endif
#endif
#ifndef I2C2_SCL
#define I2C2_SCL PB10
#ifndef I2C2_SCL
#define I2C2_SCL PB10
#endif
#ifndef I2C2_SDA
#ifndef I2C2_SDA
#define I2C2_SDA PB11
#endif
#ifdef STM32F4
#ifndef I2C3_SCL
#ifndef I2C3_SCL
#define I2C3_SCL PA8
#endif
#ifndef I2C3_SDA
#define I2C3_SDA PB4
#ifndef I2C3_SDA
#define I2C3_SDA PB4
#endif
#endif
@ -93,7 +93,7 @@ static i2cState_t i2cState[] = {
{ false, false, 0, 0, 0, 0, 0, 0, 0 },
{ false, false, 0, 0, 0, 0, 0, 0, 0 },
{ false, false, 0, 0, 0, 0, 0, 0, 0 }
};
};
void I2C1_ER_IRQHandler(void) {
i2c_er_handler(I2CDEV_1);
@ -121,7 +121,7 @@ void I2C3_EV_IRQHandler(void) {
}
#endif
static bool i2cHandleHardwareFailure(I2CDevice device)
static bool i2cHandleHardwareFailure(I2CDevice device)
{
i2cErrorCount++;
// reinit peripheral + clock out garbage
@ -129,7 +129,7 @@ static bool i2cHandleHardwareFailure(I2CDevice device)
return false;
}
bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data)
bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data)
{
if (device == I2CINVALID)
@ -138,7 +138,7 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_,
uint32_t timeout = I2C_DEFAULT_TIMEOUT;
I2C_TypeDef *I2Cx;
I2Cx = i2cHardwareMap[device].dev;
I2Cx = i2cHardwareMap[device].dev;
i2cState_t *state;
state = &(i2cState[device]);
@ -171,12 +171,12 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_,
return !(state->error);
}
bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data)
bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data)
{
return i2cWriteBuffer(device, addr_, reg_, 1, &data);
}
bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
{
if (device == I2CINVALID)
return false;
@ -369,7 +369,7 @@ void i2c_ev_handler(I2CDevice device) {
}
}
void i2cInit(I2CDevice device)
void i2cInit(I2CDevice device)
{
if (device == I2CINVALID)
return;
@ -392,7 +392,7 @@ void i2cInit(I2CDevice device)
I2C_ITConfig(i2c->dev, I2C_IT_EVT | I2C_IT_ERR, DISABLE);
i2cUnstick(scl, sda);
// Init pins
#ifdef STM32F4
IOConfigGPIOAF(scl, IOCFG_I2C, GPIO_AF_I2C);

24
src/main/drivers/bus_i2c_stm32f30x.c
View file

@ -39,18 +39,18 @@
#define I2C_SHORT_TIMEOUT ((uint32_t)0x1000)
#define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT))
#define I2C_GPIO_AF GPIO_AF_4
#define I2C_GPIO_AF GPIO_AF_4
#ifndef I2C1_SCL
#define I2C1_SCL PB6
#ifndef I2C1_SCL
#define I2C1_SCL PB6
#endif
#ifndef I2C1_SDA
#define I2C1_SDA PB7
#ifndef I2C1_SDA
#define I2C1_SDA PB7
#endif
#ifndef I2C2_SCL
#define I2C2_SCL PF4
#ifndef I2C2_SCL
#define I2C2_SCL PF4
#endif
#ifndef I2C2_SDA
#ifndef I2C2_SDA
#define I2C2_SDA PA10
#endif
@ -82,7 +82,7 @@ void i2cInit(I2CDevice device)
I2C_TypeDef *I2Cx;
I2Cx = i2c->dev;
IO_t scl = IOGetByTag(i2c->scl);
IO_t sda = IOGetByTag(i2c->sda);
@ -108,7 +108,7 @@ void i2cInit(I2CDevice device)
I2C_Init(I2Cx, &i2cInit);
I2C_StretchClockCmd(I2Cx, ENABLE);
I2C_Cmd(I2Cx, ENABLE);
}
@ -122,7 +122,7 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data)
addr_ <<= 1;
I2C_TypeDef *I2Cx;
I2Cx = i2cHardwareMap[device].dev;
I2Cx = i2cHardwareMap[device].dev;
/* Test on BUSY Flag */
i2cTimeout = I2C_LONG_TIMEOUT;
@ -188,7 +188,7 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t*
addr_ <<= 1;
I2C_TypeDef *I2Cx;
I2Cx = i2cHardwareMap[device].dev;
I2Cx = i2cHardwareMap[device].dev;
/* Test on BUSY Flag */
i2cTimeout = I2C_LONG_TIMEOUT;

4
src/main/drivers/bus_spi.c
View file

@ -36,7 +36,7 @@
#ifndef GPIO_AF_SPI3
#define GPIO_AF_SPI3 GPIO_AF_6
#endif
#endif
#endif
#ifndef SPI1_SCK_PIN
#define SPI1_NSS_PIN PA4
@ -84,7 +84,7 @@ static spiDevice_t spiHardwareMap[] = {
SPIDevice spiDeviceByInstance(SPI_TypeDef *instance)
{
if (instance == SPI1)
if (instance == SPI1)
return SPIDEV_1;
if (instance == SPI2)

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

@ -31,8 +31,6 @@
#define SPI_IO_AF_MOSI_CFG IO_CONFIG(GPIO_Mode_AF_PP, GPIO_Speed_50MHz)
#define SPI_IO_AF_MISO_CFG IO_CONFIG(GPIO_Mode_IN_FLOATING, GPIO_Speed_50MHz)
#define SPI_IO_CS_CFG IO_CONFIG(GPIO_Mode_Out_PP, GPIO_Speed_50MHz)
#else
#error "Unknown processor"
#endif
/*

6
src/main/drivers/dma.h
View file

@ -22,13 +22,15 @@ typedef void (*dmaCallbackHandlerFuncPtr)(struct dmaChannelDescriptor_s *channel
#ifdef STM32F4
typedef enum {
DMA1_ST1_HANDLER = 0,
DMA1_ST0_HANDLER = 0,
DMA1_ST1_HANDLER,
DMA1_ST2_HANDLER,
DMA1_ST3_HANDLER,
DMA1_ST4_HANDLER,
DMA1_ST5_HANDLER,
DMA1_ST6_HANDLER,
DMA1_ST7_HANDLER,
DMA2_ST0_HANDLER,
DMA2_ST1_HANDLER,
DMA2_ST2_HANDLER,
DMA2_ST3_HANDLER,
@ -63,7 +65,7 @@ typedef struct dmaChannelDescriptor_s {
#define DMA_IT_TEIF ((uint32_t)0x00000008)
#define DMA_IT_DMEIF ((uint32_t)0x00000004)
#define DMA_IT_FEIF ((uint32_t)0x00000001)
#else
typedef enum {

6
src/main/drivers/dma_stm32f4xx.c
View file

@ -45,12 +45,12 @@ static dmaChannelDescriptor_t dmaDescriptors[] = {
DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream5, 38, DMA2_Stream5_IRQn, RCC_AHB1Periph_DMA2),
DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream6, 48, DMA2_Stream6_IRQn, RCC_AHB1Periph_DMA2),
DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream7, 54, DMA2_Stream7_IRQn, RCC_AHB1Periph_DMA2),
};
/*
* DMA IRQ Handlers
*/
DEFINE_DMA_IRQ_HANDLER(1, 0, DMA1_ST0_HANDLER)
DEFINE_DMA_IRQ_HANDLER(1, 1, DMA1_ST1_HANDLER)
DEFINE_DMA_IRQ_HANDLER(1, 2, DMA1_ST2_HANDLER)
DEFINE_DMA_IRQ_HANDLER(1, 3, DMA1_ST3_HANDLER)
@ -58,12 +58,14 @@ DEFINE_DMA_IRQ_HANDLER(1, 4, DMA1_ST4_HANDLER)
DEFINE_DMA_IRQ_HANDLER(1, 5, DMA1_ST5_HANDLER)
DEFINE_DMA_IRQ_HANDLER(1, 6, DMA1_ST6_HANDLER)
DEFINE_DMA_IRQ_HANDLER(1, 7, DMA1_ST7_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 0, DMA2_ST0_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 1, DMA2_ST1_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 2, DMA2_ST2_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 3, DMA2_ST3_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 4, DMA2_ST4_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_ST5_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 6, DMA2_ST6_HANDLER)
DEFINE_DMA_IRQ_HANDLER(2, 7, DMA2_ST7_HANDLER)
void dmaInit(void)
{

20
src/main/drivers/exti.c
View file

@ -24,22 +24,22 @@ static uint8_t extiGroupPriority[EXTI_IRQ_GROUPS];
#if defined(STM32F1) || defined(STM32F4)
static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = {
EXTI0_IRQn,
EXTI1_IRQn,
EXTI2_IRQn,
EXTI3_IRQn,
EXTI0_IRQn,
EXTI1_IRQn,
EXTI2_IRQn,
EXTI3_IRQn,
EXTI4_IRQn,
EXTI9_5_IRQn,
EXTI9_5_IRQn,
EXTI15_10_IRQn
};
#elif defined(STM32F3)
static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = {
EXTI0_IRQn,
EXTI1_IRQn,
EXTI2_TS_IRQn,
EXTI3_IRQn,
EXTI0_IRQn,
EXTI1_IRQn,
EXTI2_TS_IRQn,
EXTI3_IRQn,
EXTI4_IRQn,
EXTI9_5_IRQn,
EXTI9_5_IRQn,
EXTI15_10_IRQn
};
#else

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

@ -199,7 +199,7 @@ static bool m25p16_readIdentification()
bool m25p16_init()
{
#ifdef M25P16_CS_PIN
#ifdef M25P16_CS_PIN
m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN));
#endif
IOInit(m25p16CsPin, OWNER_FLASH, RESOURCE_SPI_CS, 0);

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

@ -20,7 +20,7 @@
#include "platform.h"
#ifdef INVERTER
#ifdef INVERTER
#include "io.h"
#include "io_impl.h"

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

@ -52,7 +52,7 @@ typedef uint8_t ioConfig_t; // packed IO configuration
#define IO_CONFIG(mode, speed, otype, pupd) ((mode) | ((speed) << 2) | ((otype) << 4) | ((pupd) << 5))
#define IOCFG_OUT_PP IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL) // TODO
#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_25MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL)
#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_25MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL)
#define IOCFG_OUT_OD IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)
#define IOCFG_AF_PP IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL)
#define IOCFG_AF_PP_PD IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_DOWN)

4
src/main/drivers/io_impl.h
View file

@ -12,7 +12,7 @@ typedef struct ioRec_s {
GPIO_TypeDef *gpio;
uint16_t pin;
resourceOwner_t owner;
resourceType_t resource;
resourceType_t resource;
uint8_t index;
} ioRec_t;
@ -26,7 +26,7 @@ int IO_GPIO_PortSource(IO_t io);
#if defined(STM32F3) || defined(STM32F4)
int IO_EXTI_PortSourceGPIO(IO_t io);
int IO_EXTI_PinSource(IO_t io);
int IO_EXTI_PinSource(IO_t io);
#endif
GPIO_TypeDef* IO_GPIO(IO_t io);

15
src/main/drivers/light_led.c
View file

@ -27,12 +27,12 @@ static const IO_t leds[] = {
DEFIO_IO(LED0),
#else
DEFIO_IO(NONE),
#endif
#endif
#ifdef LED1
DEFIO_IO(LED1),
#else
DEFIO_IO(NONE),
#endif
#endif
#ifdef LED2
DEFIO_IO(LED2),
#else
@ -78,15 +78,14 @@ uint8_t ledPolarity = 0
#endif
;
uint8_t ledOffset = 0;
static uint8_t ledOffset = 0;
void ledInit(bool alternative_led)
{
uint32_t i;
#if defined(LED0_A) || defined(LED1_A) || defined(LED2_A)
if (alternative_led)
if (alternative_led) {
ledOffset = LED_NUMBER;
}
#else
UNUSED(alternative_led);
#endif
@ -95,7 +94,7 @@ void ledInit(bool alternative_led)
LED1_OFF;
LED2_OFF;
for (i = 0; i < LED_NUMBER; i++) {
for (int i = 0; i < LED_NUMBER; i++) {
if (leds[i + ledOffset]) {
IOInit(leds[i + ledOffset], OWNER_LED, RESOURCE_OUTPUT, RESOURCE_INDEX(i));
IOConfigGPIO(leds[i + ledOffset], IOCFG_OUT_PP);
@ -114,6 +113,6 @@ void ledToggle(int led)
void ledSet(int led, bool on)
{
bool inverted = (1 << (led + ledOffset)) & ledPolarity;
const bool inverted = (1 << (led + ledOffset)) & ledPolarity;
IOWrite(leds[led + ledOffset], on ? inverted : !inverted);
}

3
src/main/drivers/light_ws2811strip.h
View file

@ -53,6 +53,3 @@ bool isWS2811LedStripReady(void);
extern uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE];
extern volatile uint8_t ws2811LedDataTransferInProgress;
extern const hsvColor_t hsv_white;
extern const hsvColor_t hsv_black;

1
src/main/drivers/light_ws2811strip_stm32f10x.c
View file

@ -105,6 +105,7 @@ void ws2811LedStripHardwareInit(void)
DMA_ITConfig(DMA1_Channel6, DMA_IT_TC, ENABLE);
const hsvColor_t hsv_white = { 0, 255, 255};
ws2811Initialised = true;
setStripColor(&hsv_white);
ws2811UpdateStrip();

1
src/main/drivers/light_ws2811strip_stm32f30x.c
View file

@ -111,6 +111,7 @@ void ws2811LedStripHardwareInit(void)
DMA_ITConfig(WS2811_DMA_CHANNEL, DMA_IT_TC, ENABLE);
const hsvColor_t hsv_white = { 0, 255, 255};
ws2811Initialised = true;
setStripColor(&hsv_white);
ws2811UpdateStrip();

5
src/main/drivers/light_ws2811strip_stm32f4xx.c
View file

@ -31,8 +31,8 @@
#ifdef LED_STRIP
#if !defined(WS2811_PIN)
#define WS2811_PIN PA0
#if !defined(WS2811_PIN)
#define WS2811_PIN PA0
#define WS2811_TIMER TIM5
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER
#define WS2811_DMA_STREAM DMA1_Stream2
@ -154,6 +154,7 @@ void ws2811LedStripHardwareInit(void)
dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0);
const hsvColor_t hsv_white = { 0, 255, 255};
ws2811Initialised = true;
setStripColor(&hsv_white);
ws2811UpdateStrip();

20
src/main/drivers/max7456.c
View file

@ -75,6 +75,7 @@ static void max7456_send_dma(void* tx_buffer, void* rx_buffer, uint16_t buffer_s
#endif
// Common to both channels
DMA_StructInit(&DMA_InitStructure);
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)(&(MAX7456_SPI_INSTANCE->DR));
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
@ -82,22 +83,31 @@ static void max7456_send_dma(void* tx_buffer, void* rx_buffer, uint16_t buffer_s
DMA_InitStructure.DMA_BufferSize = buffer_size;
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
DMA_InitStructure.DMA_Priority = DMA_Priority_Low;
DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
#ifdef MAX7456_DMA_CHANNEL_RX
// Rx Channel
#ifdef STM32F4
DMA_InitStructure.DMA_Memory0BaseAddr = rx_buffer ? (uint32_t)rx_buffer : (uint32_t)(dummy);
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
#else
DMA_InitStructure.DMA_MemoryBaseAddr = rx_buffer ? (uint32_t)rx_buffer : (uint32_t)(dummy);
DMA_InitStructure.DMA_MemoryInc = rx_buffer ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
#endif
DMA_InitStructure.DMA_MemoryInc = rx_buffer ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable;
DMA_Init(MAX7456_DMA_CHANNEL_RX, &DMA_InitStructure);
DMA_Cmd(MAX7456_DMA_CHANNEL_RX, ENABLE);
#endif
// Tx channel
#ifdef STM32F4
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)tx_buffer; //max7456_screen;
DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
#else
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)tx_buffer; //max7456_screen;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
#endif
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_Init(MAX7456_DMA_CHANNEL_TX, &DMA_InitStructure);
DMA_Cmd(MAX7456_DMA_CHANNEL_TX, ENABLE);
@ -147,7 +157,7 @@ void max7456_dma_irq_handler(dmaChannelDescriptor_t* descriptor) {
}
#endif
void max7456_init(uint8_t video_system)
void max7456_init(uint8_t video_system)
{
uint8_t max_screen_rows;
uint8_t srdata = 0;
@ -247,7 +257,7 @@ void max7456_draw_screen(void) {
max7456_send(MAX7456ADD_DMM, 1);
for (xx = 0; xx < max_screen_size; ++xx) {
max7456_send(MAX7456ADD_DMDI, SCREEN_BUFFER[xx]);
SCREEN_BUFFER[xx] = MAX7456_CHAR(0);
SCREEN_BUFFER[xx] = MAX7456_CHAR(' ');
}
max7456_send(MAX7456ADD_DMDI, 0xFF);
max7456_send(MAX7456ADD_DMM, 0);

17
src/main/drivers/pwm_mapping.c
View file

@ -30,11 +30,6 @@
#include "pwm_rx.h"
#include "pwm_mapping.h"
void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate);
void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse);
void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint16_t idlePulse);
void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
/*
Configuration maps
@ -100,7 +95,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
int channelIndex = 0;
memset(&pwmOutputConfiguration, 0, sizeof(pwmOutputConfiguration));
// this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ]
if (init->airplane)
i = 2; // switch to air hardware config
@ -265,7 +260,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
if (init->useChannelForwarding && !init->airplane) {
#if defined(NAZE) && defined(WS2811_TIMER)
// if LED strip is active, PWM5-8 are unavailable, so map AUX1+AUX2 to PWM13+PWM14
if (init->useLEDStrip) {
if (init->useLEDStrip) {
if (timerIndex >= PWM13 && timerIndex <= PWM14) {
type = MAP_TO_SERVO_OUTPUT;
}
@ -313,14 +308,14 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
}
#endif
if (init->useFastPwm) {
pwmFastPwmMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->pwmProtocolType, init->motorPwmRate, init->idlePulse);
pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_ONESHOT|PWM_PF_OUTPUT_PROTOCOL_PWM ;
pwmFastPwmMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->pwmProtocolType);
pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM | PWM_PF_OUTPUT_PROTOCOL_ONESHOT;
} else if (init->pwmProtocolType == PWM_TYPE_BRUSHED) {
pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate);
pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_MOTOR_MODE_BRUSHED | PWM_PF_OUTPUT_PROTOCOL_PWM;
pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM | PWM_PF_MOTOR_MODE_BRUSHED;
} else {
pwmBrushlessMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse);
pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM ;
pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM;
}
pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].index = pwmOutputConfiguration.motorCount;
pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].timerHardware = timerHardwarePtr;

18
src/main/drivers/pwm_mapping.h
View file

@ -30,24 +30,8 @@
#error Invalid motor/servo/port configuration
#endif
#define PULSE_1MS (1000) // 1ms pulse width
#define MAX_INPUTS 8
#define PWM_TIMER_MHZ 1
#if defined(STM32F40_41xxx) // must be multiples of timer clock
#define ONESHOT42_TIMER_MHZ 21
#define ONESHOT125_TIMER_MHZ 12
#define PWM_BRUSHED_TIMER_MHZ 21
#define MULTISHOT_TIMER_MHZ 84
#else
#define PWM_BRUSHED_TIMER_MHZ 24
#define MULTISHOT_TIMER_MHZ 72
#define ONESHOT42_TIMER_MHZ 24
#define ONESHOT125_TIMER_MHZ 8
#endif
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
typedef struct sonarIOConfig_s {
ioTag_t triggerTag;
@ -133,7 +117,7 @@ enum {
PWM13,
PWM14,
PWM15,
PWM16,
PWM16,
PWM17,
PWM18,
PWM19,

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

@ -17,21 +17,31 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "platform.h"
#include "io.h"
#include "timer.h"
#include "flight/failsafe.h" // FIXME dependency into the main code from a driver
#include "pwm_mapping.h"
#include "pwm_output.h"
#if defined(STM32F40_41xxx) // must be multiples of timer clock
#define ONESHOT125_TIMER_MHZ 12
#define ONESHOT42_TIMER_MHZ 21
#define MULTISHOT_TIMER_MHZ 84
#define PWM_BRUSHED_TIMER_MHZ 21
#else
#define ONESHOT125_TIMER_MHZ 8
#define ONESHOT42_TIMER_MHZ 24
#define MULTISHOT_TIMER_MHZ 72
#define PWM_BRUSHED_TIMER_MHZ 24
#endif
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors
typedef struct {
@ -52,35 +62,43 @@ static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
static uint8_t allocatedOutputPortCount = 0;
static bool pwmMotorsEnabled = true;
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t ouputPolarity)
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
{
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_OCStructInit(&TIM_OCInitStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
if (output & TIMER_OUTPUT_N_CHANNEL) {
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
}
else {
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
}
TIM_OCInitStructure.TIM_Pulse = value;
TIM_OCInitStructure.TIM_OCPolarity = ouputPolarity ? TIM_OCPolarity_High : TIM_OCPolarity_Low;
TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_High : TIM_OCPolarity_Low;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
switch (channel) {
case TIM_Channel_1:
TIM_OC1Init(tim, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable);
break;
case TIM_Channel_2:
TIM_OC2Init(tim, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable);
break;
case TIM_Channel_3:
TIM_OC3Init(tim, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable);
break;
case TIM_Channel_4:
TIM_OC4Init(tim, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable);
break;
case TIM_Channel_1:
TIM_OC1Init(tim, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable);
break;
case TIM_Channel_2:
TIM_OC2Init(tim, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable);
break;
case TIM_Channel_3:
TIM_OC3Init(tim, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable);
break;
case TIM_Channel_4:
TIM_OC4Init(tim, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable);
break;
}
}
@ -90,11 +108,11 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8
configTimeBase(timerHardware->tim, period, mhz);
IO_t io = IOGetByTag(timerHardware->tag);
const IO_t io = IOGetByTag(timerHardware->tag);
IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount);
IOConfigGPIO(io, IOCFG_AF_PP);
pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output & TIMER_OUTPUT_INVERTED);
pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output);
if (timerHardware->output & TIMER_OUTPUT_ENABLED) {
TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE);
@ -102,24 +120,24 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8
TIM_Cmd(timerHardware->tim, ENABLE);
switch (timerHardware->channel) {
case TIM_Channel_1:
p->ccr = &timerHardware->tim->CCR1;
break;
case TIM_Channel_2:
p->ccr = &timerHardware->tim->CCR2;
break;
case TIM_Channel_3:
p->ccr = &timerHardware->tim->CCR3;
break;
case TIM_Channel_4:
p->ccr = &timerHardware->tim->CCR4;
break;
case TIM_Channel_1:
p->ccr = &timerHardware->tim->CCR1;
break;
case TIM_Channel_2:
p->ccr = &timerHardware->tim->CCR2;
break;
case TIM_Channel_3:
p->ccr = &timerHardware->tim->CCR3;
break;
case TIM_Channel_4:
p->ccr = &timerHardware->tim->CCR4;
break;
}
p->period = period;
p->tim = timerHardware->tim;
*p->ccr = 0;
return p;
}
@ -133,16 +151,16 @@ static void pwmWriteStandard(uint8_t index, uint16_t value)
*motors[index]->ccr = value;
}
static void pwmWriteOneShot42(uint8_t index, uint16_t value)
{
*motors[index]->ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f));
}
static void pwmWriteOneShot125(uint8_t index, uint16_t value)
{
*motors[index]->ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f));
}
static void pwmWriteOneShot42(uint8_t index, uint16_t value)
{
*motors[index]->ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f));
}
static void pwmWriteMultiShot(uint8_t index, uint16_t value)
{
*motors[index]->ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW);
@ -150,15 +168,14 @@ static void pwmWriteMultiShot(uint8_t index, uint16_t value)
void pwmWriteMotor(uint8_t index, uint16_t value)
{
if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled)
if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) {
motors[index]->pwmWritePtr(index, value);
}
}
void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
{
uint8_t index;
for(index = 0; index < motorCount; index++){
for(int index = 0; index < motorCount; index++){
// Set the compare register to 0, which stops the output pulsing if the timer overflows
*motors[index]->ccr = 0;
}
@ -176,17 +193,14 @@ void pwmEnableMotors(void)
void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
{
uint8_t index;
TIM_TypeDef *lastTimerPtr = NULL;
for (index = 0; index < motorCount; index++) {
for (int index = 0; index < motorCount; index++) {
// Force the timer to overflow if it's the first motor to output, or if we change timers
if (motors[index]->tim != lastTimerPtr) {
lastTimerPtr = motors[index]->tim;
timerForceOverflow(motors[index]->tim);
}
// Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
// This compare register will be set to the output value on the next main loop.
*motors[index]->ccr = 0;
@ -195,44 +209,46 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate)
{
uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000;
const uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000;
motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, 0);
motors[motorIndex]->pwmWritePtr = pwmWriteBrushed;
}
void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse)
{
uint32_t hz = PWM_TIMER_MHZ * 1000000;
const uint32_t hz = PWM_TIMER_MHZ * 1000000;
motors[motorIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, hz / motorPwmRate, idlePulse);
motors[motorIndex]->pwmWritePtr = pwmWriteStandard;
}
void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint16_t idlePulse)
void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t fastPwmProtocolType)
{
uint32_t timerMhzCounter;
pwmWriteFuncPtr pwmWritePtr;
switch (fastPwmProtocolType) {
default:
case (PWM_TYPE_ONESHOT125):
timerMhzCounter = ONESHOT125_TIMER_MHZ;
break;
case (PWM_TYPE_ONESHOT42):
timerMhzCounter = ONESHOT42_TIMER_MHZ;
break;
case (PWM_TYPE_MULTISHOT):
timerMhzCounter = MULTISHOT_TIMER_MHZ;
default:
case (PWM_TYPE_ONESHOT125):
timerMhzCounter = ONESHOT125_TIMER_MHZ;
pwmWritePtr = pwmWriteOneShot125;
break;
case (PWM_TYPE_ONESHOT42):
timerMhzCounter = ONESHOT42_TIMER_MHZ;
pwmWritePtr = pwmWriteOneShot42;
break;
case (PWM_TYPE_MULTISHOT):
timerMhzCounter = MULTISHOT_TIMER_MHZ;
pwmWritePtr = pwmWriteMultiShot;
break;
}
if (motorPwmRate > 0) {
uint32_t hz = timerMhzCounter * 1000000;
const uint32_t hz = timerMhzCounter * 1000000;
motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse);
} else {
motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, 0xFFFF, 0);
}
motors[motorIndex]->pwmWritePtr = (fastPwmProtocolType == PWM_TYPE_MULTISHOT) ? pwmWriteMultiShot :
((fastPwmProtocolType == PWM_TYPE_ONESHOT125) ? pwmWriteOneShot125 :
pwmWriteOneShot42);
motors[motorIndex]->pwmWritePtr = pwmWritePtr;
}
#ifdef USE_SERVOS
@ -243,7 +259,8 @@ void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, ui
void pwmWriteServo(uint8_t index, uint16_t value)
{
if (servos[index] && index < MAX_SERVOS)
if (servos[index] && index < MAX_SERVOS) {
*servos[index]->ccr = value;
}
}
#endif

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

@ -25,6 +25,12 @@ typedef enum {
PWM_TYPE_BRUSHED
} motorPwmProtocolTypes_e;
struct timerHardware_s;
void pwmBrushedMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate);
void pwmBrushlessMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse);
void pwmFastPwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t fastPwmProtocolType);
void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
void pwmWriteMotor(uint8_t index, uint16_t value);
void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
void pwmCompleteOneshotMotorUpdate(uint8_t motorCount);

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

@ -1,7 +1,7 @@
#pragma once
#define RESOURCE_INDEX(x) x + 1
#define RESOURCE_INDEX(x) (x + 1)
typedef enum {
OWNER_FREE = 0,

9
src/main/drivers/sdcard.c
View file

@ -126,7 +126,7 @@ void sdcardInsertionDetectDeinit(void)
#ifdef SDCARD_DETECT_PIN
sdCardDetectPin = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN));
IOInit(sdCardDetectPin, OWNER_FREE, RESOURCE_NONE, 0);
IOConfigGPIO(sdCardDetectPin, IOCFG_IN_FLOATING);
IOConfigGPIO(sdCardDetectPin, IOCFG_IN_FLOATING);
#endif
}
@ -135,7 +135,7 @@ void sdcardInsertionDetectInit(void)
#ifdef SDCARD_DETECT_PIN
sdCardDetectPin = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN));
IOInit(sdCardDetectPin, OWNER_SDCARD, RESOURCE_INPUT, 0);
IOConfigGPIO(sdCardDetectPin, IOCFG_IPU);
IOConfigGPIO(sdCardDetectPin, IOCFG_IPU);
#endif
}
@ -192,6 +192,11 @@ static void sdcard_deselect(void)
*/
static void sdcard_reset(void)
{
if (!sdcard_isInserted()) {
sdcard.state = SDCARD_STATE_NOT_PRESENT;
return;
}
if (sdcard.state >= SDCARD_STATE_READY) {
spiSetDivisor(SDCARD_SPI_INSTANCE, SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER);
}

4
src/main/drivers/serial_uart_stm32f30x.c
View file

@ -114,8 +114,8 @@ static void handleUsartTxDma(dmaChannelDescriptor_t* descriptor)
void serialUARTInit(IO_t tx, IO_t rx, portMode_t mode, portOptions_t options, uint8_t af, uint8_t index)
{
if (options & SERIAL_BIDIR) {
ioConfig_t ioCfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz,
(options & SERIAL_INVERTED) ? GPIO_OType_PP : GPIO_OType_OD,
ioConfig_t ioCfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz,
(options & SERIAL_INVERTED) ? GPIO_OType_PP : GPIO_OType_OD,
(options & SERIAL_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP
);

75
src/main/drivers/serial_uart_stm32f4xx.c
View file

@ -64,17 +64,17 @@ typedef struct uartDevice_s {
//static uartPort_t uartPort[MAX_UARTS];
#ifdef USE_UART1
static uartDevice_t uart1 =
{
static uartDevice_t uart1 =
{
.DMAChannel = DMA_Channel_4,
.txDMAStream = DMA2_Stream7,
#ifdef USE_UART1_RX_DMA
.rxDMAStream = DMA2_Stream5,
#endif
.dev = USART1,
.rx = IO_TAG(UART1_RX_PIN),
.tx = IO_TAG(UART1_TX_PIN),
.af = GPIO_AF_USART1,
.dev = USART1,
.rx = IO_TAG(UART1_RX_PIN),
.tx = IO_TAG(UART1_TX_PIN),
.af = GPIO_AF_USART1,
#ifdef UART1_AHB1_PERIPHERALS
.rcc_ahb1 = UART1_AHB1_PERIPHERALS,
#endif
@ -87,17 +87,17 @@ static uartDevice_t uart1 =
#endif
#ifdef USE_UART2
static uartDevice_t uart2 =
{
static uartDevice_t uart2 =
{
.DMAChannel = DMA_Channel_4,
#ifdef USE_UART2_RX_DMA
.rxDMAStream = DMA1_Stream5,
#endif
.txDMAStream = DMA1_Stream6,
.dev = USART2,
.rx = IO_TAG(UART2_RX_PIN),
.tx = IO_TAG(UART2_TX_PIN),
.af = GPIO_AF_USART2,
.dev = USART2,
.rx = IO_TAG(UART2_RX_PIN),
.tx = IO_TAG(UART2_TX_PIN),
.af = GPIO_AF_USART2,
#ifdef UART2_AHB1_PERIPHERALS
.rcc_ahb1 = UART2_AHB1_PERIPHERALS,
#endif
@ -110,17 +110,17 @@ static uartDevice_t uart2 =
#endif
#ifdef USE_UART3
static uartDevice_t uart3 =
{
static uartDevice_t uart3 =
{
.DMAChannel = DMA_Channel_4,
#ifdef USE_UART3_RX_DMA
.rxDMAStream = DMA1_Stream1,
#endif
.txDMAStream = DMA1_Stream3,
.dev = USART3,
.rx = IO_TAG(UART3_RX_PIN),
.tx = IO_TAG(UART3_TX_PIN),
.af = GPIO_AF_USART3,
.dev = USART3,
.rx = IO_TAG(UART3_RX_PIN),
.tx = IO_TAG(UART3_TX_PIN),
.af = GPIO_AF_USART3,
#ifdef UART3_AHB1_PERIPHERALS
.rcc_ahb1 = UART3_AHB1_PERIPHERALS,
#endif
@ -133,17 +133,17 @@ static uartDevice_t uart3 =
#endif
#ifdef USE_UART4
static uartDevice_t uart4 =
{
static uartDevice_t uart4 =
{
.DMAChannel = DMA_Channel_4,
#ifdef USE_UART1_RX_DMA
.rxDMAStream = DMA1_Stream2,
#endif
.txDMAStream = DMA1_Stream4,
.dev = UART4,
.rx = IO_TAG(UART4_RX_PIN),
.tx = IO_TAG(UART4_TX_PIN),
.af = GPIO_AF_UART4,
.dev = UART4,
.rx = IO_TAG(UART4_RX_PIN),
.tx = IO_TAG(UART4_TX_PIN),
.af = GPIO_AF_UART4,
#ifdef UART4_AHB1_PERIPHERALS
.rcc_ahb1 = UART4_AHB1_PERIPHERALS,
#endif
@ -156,17 +156,17 @@ static uartDevice_t uart4 =
#endif
#ifdef USE_UART5
static uartDevice_t uart5 =
{
static uartDevice_t uart5 =
{
.DMAChannel = DMA_Channel_4,
#ifdef USE_UART1_RX_DMA
.rxDMAStream = DMA1_Stream0,
#endif
.txDMAStream = DMA2_Stream7,
.dev = UART5,
.rx = IO_TAG(UART5_RX_PIN),
.tx = IO_TAG(UART5_TX_PIN),
.af = GPIO_AF_UART5,
.dev = UART5,
.rx = IO_TAG(UART5_RX_PIN),
.tx = IO_TAG(UART5_TX_PIN),
.af = GPIO_AF_UART5,
#ifdef UART5_AHB1_PERIPHERALS
.rcc_ahb1 = UART5_AHB1_PERIPHERALS,
#endif
@ -179,17 +179,17 @@ static uartDevice_t uart5 =
#endif
#ifdef USE_UART6
static uartDevice_t uart6 =
{
static uartDevice_t uart6 =
{
.DMAChannel = DMA_Channel_5,
#ifdef USE_UART6_RX_DMA
.rxDMAStream = DMA2_Stream1,
#endif
.txDMAStream = DMA2_Stream6,
.dev = USART6,
.rx = IO_TAG(UART6_RX_PIN),
.tx = IO_TAG(UART6_TX_PIN),
.af = GPIO_AF_USART6,
.dev = USART6,
.rx = IO_TAG(UART6_RX_PIN),
.tx = IO_TAG(UART6_TX_PIN),
.af = GPIO_AF_USART6,
#ifdef UART6_AHB1_PERIPHERALS
.rcc_ahb1 = UART6_AHB1_PERIPHERALS,
#endif
@ -344,7 +344,7 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po
IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af);
}
if (mode & MODE_RX) {
if (mode & MODE_RX) {
IOInit(rx, OWNER_SERIAL, RESOURCE_UART_RX, RESOURCE_INDEX(device));
IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->af);
}
@ -376,6 +376,7 @@ void USART1_IRQHandler(void)
uartPort_t *s = &(uartHardwareMap[UARTDEV_1]->port);
uartIrqHandler(s);
}
#endif
#ifdef USE_UART2

33
src/main/drivers/serial_usb_vcp.c
View file

@ -77,27 +77,25 @@ static uint8_t usbVcpRead(serialPort_t *instance)
uint8_t buf[1];
uint32_t rxed = 0;
while (rxed < 1) {
rxed += CDC_Receive_DATA((uint8_t*)buf + rxed, 1 - rxed);
while (true) {
if (CDC_Receive_DATA(buf, 1))
return buf[0];
}
return buf[0];
}
static void usbVcpWriteBuf(serialPort_t *instance, void *data, int count)
{
UNUSED(instance);
if (!(usbIsConnected() && usbIsConfigured())) {
return;
}
uint32_t start = millis();
for (uint8_t *p = data; count > 0; ) {
uint32_t txed = CDC_Send_DATA(p, count);
uint8_t *p = data;
uint32_t txed = 0;
while (count > 0) {
txed = CDC_Send_DATA(p, count);
count -= txed;
p += txed;
@ -120,14 +118,19 @@ static bool usbVcpFlush(vcpPort_t *port)
return false;
}
uint32_t txed;
uint32_t start = millis();
uint8_t *p = port->txBuf;
uint32_t txed = 0;
while (count > 0) {
txed = CDC_Send_DATA(p, count);
count -= txed;
p += txed;
do {
txed = CDC_Send_DATA(port->txBuf, count);
} while (txed != count && (millis() - start < USB_TIMEOUT));
return txed == count;
if (millis() - start > USB_TIMEOUT) {
break;
}
}
return count == 0;
}
static void usbVcpWrite(serialPort_t *instance, uint8_t c)

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

@ -36,7 +36,7 @@ void systemReset(void)
SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
}
void systemResetToBootloader(void)
void systemResetToBootloader(void)
{
// 1FFFF000 -> 20000200 -> SP
// 1FFFF004 -> 1FFFF021 -> PC

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

@ -34,7 +34,7 @@ void systemReset(void)
SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
}
void systemResetToBootloader(void)
void systemResetToBootloader(void)
{
// 1FFFF000 -> 20000200 -> SP
// 1FFFF004 -> 1FFFF021 -> PC

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

@ -41,7 +41,7 @@ void systemReset(void)
NVIC_SystemReset();
}
void systemResetToBootloader(void)
void systemResetToBootloader(void)
{
if (mpuConfiguration.reset)
mpuConfiguration.reset();

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

@ -253,7 +253,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
}
#else
TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / ((uint32_t)mhz * 1000000)) - 1;
#endif
#endif
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;

5
src/main/drivers/timer.h
View file

@ -87,8 +87,9 @@ typedef struct timerHardware_s {
} timerHardware_t;
enum {
TIMER_OUTPUT_ENABLED = 0x01,
TIMER_OUTPUT_INVERTED = 0x02
TIMER_OUTPUT_ENABLED = 0x01,
TIMER_OUTPUT_INVERTED = 0x02,
TIMER_OUTPUT_N_CHANNEL= 0x04
};
#ifdef STM32F1

4
src/main/drivers/timer_stm32f10x.c
View file

@ -23,10 +23,10 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
{ .TIMx = TIM8, .rcc = RCC_APB1(TIM8) },
{ .TIMx = TIM9, .rcc = RCC_APB2(TIM9) },
{ .TIMx = TIM10, .rcc = RCC_APB2(TIM10) },
{ .TIMx = TIM11, .rcc = RCC_APB2(TIM11) },
{ .TIMx = TIM11, .rcc = RCC_APB2(TIM11) },
{ .TIMx = TIM12, .rcc = RCC_APB1(TIM12) },
{ .TIMx = TIM13, .rcc = RCC_APB1(TIM13) },
{ .TIMx = TIM14, .rcc = RCC_APB1(TIM14) },
{ .TIMx = TIM14, .rcc = RCC_APB1(TIM14) },
#endif
};

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

@ -54,7 +54,7 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
*/
#define CCMR_OFFSET ((uint16_t)0x0018)
#define CCMR_OC13M_MASK ((uint32_t)0xFFFEFF8F)
#define CCMR_OC24M_MASK ((uint32_t)0xFEFF8FFF)
#define CCMR_OC24M_MASK ((uint32_t)0xFEFF8FFF)
void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint32_t TIM_OCMode)
{

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

@ -66,7 +66,7 @@ bool usbCableIsInserted(void)
void usbGenerateDisconnectPulse(void)
{
/* Pull down PA12 to create USB disconnect pulse */
IO_t usbPin = IOGetByTag(IO_TAG(PA12));
IO_t usbPin = IOGetByTag(IO_TAG(PA12));
IOConfigGPIO(usbPin, IOCFG_OUT_OD);
IOHi(usbPin);

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

@ -179,7 +179,7 @@ void rtc6705SetFreq(uint16_t freq)
uint32_t val_a = ((((uint64_t)freq*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) % RTC6705_SET_FDIV) / RTC6705_SET_NDIV; //Casts required to make sure correct math (large numbers)
uint32_t val_n = (((uint64_t)freq*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) / RTC6705_SET_FDIV; //Casts required to make sure correct math (large numbers)
val_hex |= RTC6705_SET_WRITE;
val_hex |= (val_a << 5);
val_hex |= (val_n << 12);

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

@ -65,13 +65,11 @@ static flight3DConfig_t *flight3DConfig;
static escAndServoConfig_t *escAndServoConfig;
static airplaneConfig_t *airplaneConfig;
static rxConfig_t *rxConfig;
static bool syncPwm = false;
static bool syncPwmWithPidLoop = false;
static mixerMode_e currentMixerMode;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
float errorLimiter = 1.0f;
#ifdef USE_SERVOS
static uint8_t servoRuleCount = 0;
static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
@ -427,7 +425,7 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
motorCount = 0;
servoCount = pwmOutputConfiguration->servoCount;
syncPwm = use_unsyncedPwm;
syncPwmWithPidLoop = !use_unsyncedPwm;
if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
// load custom mixer into currentMixer
@ -528,9 +526,12 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
customMixers = initialCustomMixers;
}
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration)
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration, bool use_unsyncedPwm)
{
UNUSED(pwmOutputConfiguration);
syncPwmWithPidLoop = !use_unsyncedPwm;
motorCount = 4;
#ifdef USE_SERVOS
servoCount = 0;
@ -644,7 +645,7 @@ void writeMotors(void)
for (i = 0; i < motorCount; i++)
pwmWriteMotor(i, motor[i]);
if (syncPwm) {
if (syncPwmWithPidLoop) {
pwmCompleteOneshotMotorUpdate(motorCount);
}
}
@ -752,13 +753,14 @@ STATIC_UNIT_TESTED void servoMixer(void)
#endif
void mixTable(void)
void mixTable(void *pidProfilePtr)
{
uint32_t i = 0;
fix12_t vbatCompensationFactor = 0;
static fix12_t mixReduction;
bool use_vbat_compensation = false;
if (batteryConfig && batteryConfig->vbatPidCompensation) {
pidProfile_t *pidProfile = (pidProfile_t *) pidProfilePtr;
if (batteryConfig && pidProfile->vbatPidCompensation) {
use_vbat_compensation = true;
vbatCompensationFactor = calculateVbatPidCompensation();
}
@ -827,21 +829,40 @@ void mixTable(void)
rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]);
}
// Get the maximum correction by setting offset to center
if (!escAndServoConfig->escDesyncProtection) throttleMin = throttleMax = throttleMin + (throttleRange / 2);
throttleMin = throttleMax = throttleMin + (throttleRange / 2);
} else {
throttleMin = throttleMin + (rollPitchYawMixRange / 2);
throttleMax = throttleMax - (rollPitchYawMixRange / 2);
}
// adjust feedback to scale PID error inputs to our limitations.
errorLimiter = constrainf(((float)throttleRange / rollPitchYawMixRange), 0.1f, 1.0f);
if (debugMode == DEBUG_AIRMODE) debug[1] = errorLimiter * 100;
// Keep track for motor update timing // Only for Betaflight pid controller to keep legacy pidc basic and free from additional float math
float motorDtms;
if (pidProfile->accelerationLimitPercent && pidProfile->pidController == PID_CONTROLLER_BETAFLIGHT) {
static uint32_t previousMotorTime;
uint32_t currentMotorTime = micros();
motorDtms = (float) (currentMotorTime - previousMotorTime) / 1000.0f;
previousMotorTime = currentMotorTime;
}
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
for (i = 0; i < motorCount; i++) {
motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax);
// Accel limit. Prevent PID controller to output huge ramps to the motors. Only limiting acceleration. Only for Betaflight pid controller to keep legacy pidc basic
if (pidProfile->accelerationLimitPercent && pidProfile->pidController == PID_CONTROLLER_BETAFLIGHT) {
static int16_t lastFilteredMotor[MAX_SUPPORTED_MOTORS];
// acceleration limit
float delta = motor[i] - lastFilteredMotor[i];
const float maxDeltaPerMs = throttleRange * ((float)pidProfile->accelerationLimitPercent / 100.0f);
float maxDelta = maxDeltaPerMs * motorDtms;
if (delta > maxDelta) { // accelerating too hard
motor[i] = lastFilteredMotor[i] + maxDelta;
}
lastFilteredMotor[i] = motor[i];
}
if (isFailsafeActive) {
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle);
} else if (feature(FEATURE_3D)) {
@ -860,19 +881,6 @@ void mixTable(void)
motor[i] = escAndServoConfig->mincommand;
}
}
// Experimental Code. Anti Desync feature for ESC's
if (escAndServoConfig->escDesyncProtection) {
const int16_t maxThrottleStep = constrain(escAndServoConfig->escDesyncProtection / (1000 / targetPidLooptime), 5, 10000);
// Only makes sense when it's within the range
if (maxThrottleStep < throttleRange) {
static int16_t motorPrevious[MAX_SUPPORTED_MOTORS];
motor[i] = constrain(motor[i], motorPrevious[i] - maxThrottleStep, motorPrevious[i] + maxThrottleStep);
motorPrevious[i] = motor[i];
}
}
}
// Disarmed mode
@ -956,7 +964,7 @@ void filterServos(void)
if (mixerConfig->servo_lowpass_enable) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
if (!servoFilterIsSet) {
biquadFilterInit(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime);
biquadFilterInitLPF(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime);
servoFilterIsSet = true;
}

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

@ -211,7 +211,7 @@ void loadCustomServoMixer(void);
int servoDirection(int servoIndex, int fromChannel);
#endif
void mixerResetDisarmedMotors(void);
void mixTable(void);
void mixTable(void *pidProfilePtr);
void syncMotors(bool enabled);
void writeMotors(void);
void stopMotors(void);

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

@ -49,9 +49,10 @@
extern uint8_t motorCount;
uint32_t targetPidLooptime;
extern float errorLimiter;
extern float angleRate[3], angleRateSmooth[3];
static bool pidStabilisationEnabled;
int16_t axisPID[3];
#ifdef BLACKBOX
@ -62,46 +63,31 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
uint8_t PIDweight[3];
static int32_t errorGyroI[3];
#ifndef SKIP_PID_LUXFLOAT
static float errorGyroIf[3];
#endif
static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
pidControllerFuncPtr pid_controller = pidInteger; // which pid controller are we using
pidControllerFuncPtr pid_controller = pidBetaflight; // which pid controller are we using
void setTargetPidLooptime(uint8_t pidProcessDenom)
void setTargetPidLooptime(uint8_t pidProcessDenom)
{
targetPidLooptime = gyro.targetLooptime * pidProcessDenom;
}
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate)
{
uint16_t dynamicKi;
uint16_t resetRate;
resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
uint16_t dynamicFactor = (1 << 8) - constrain((ABS(angleRate) << 6) / resetRate, 0, 1 << 8);
dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8;
return dynamicKi;
}
void pidResetErrorGyroState(void)
{
int axis;
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
errorGyroI[axis] = 0;
#ifndef SKIP_PID_LUXFLOAT
errorGyroIf[axis] = 0.0f;
#endif
}
}
void pidStabilisationState(pidStabilisationState_e pidControllerState)
{
pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false;
}
float getdT (void)
{
static float dT;
@ -115,8 +101,8 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static pt1Filter_t deltaFilter[3];
static pt1Filter_t yawFilter;
#ifndef SKIP_PID_LUXFLOAT
static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
// Experimental betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage
static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
float RateError = 0, gyroRate = 0, RateErrorSmooth = 0;
@ -129,9 +115,9 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
// Scaling factors for Pids for better tunable range in configurator
static const float luxPTermScale = 1.0f / 128;
static const float luxITermScale = 1000000.0f / 0x1000000;
static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 508;
static const float PTermScale = 0.032029f;
static const float ITermScale = 0.244381f;
static const float DTermScale = 0.000529f;
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
@ -147,6 +133,25 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
}
}
// Yet Highly experimental and under test and development
// Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols)
static float kiThrottleGain = 1.0f;
if (pidProfile->itermThrottleGain) {
const uint16_t maxLoopCount = 20000 / targetPidLooptime;
const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f;
static int16_t previousThrottle;
static uint16_t loopIncrement;
if (loopIncrement >= maxLoopCount) {
kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5
previousThrottle = rcCommand[THROTTLE];
loopIncrement = 0;
} else {
loopIncrement++;
}
}
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
@ -154,23 +159,23 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
// calculate error angle and limit the angle to the max inclination
#ifdef GPS
const float errorAngle = (constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
const float errorAngle = (constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
#else
const float errorAngle = (constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
const float errorAngle = (constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
angleRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
angleRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f;
} else {
// HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel
angleRate[axis] = angleRateSmooth[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f);
angleRate[axis] = angleRateSmooth[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f);
}
}
gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled for easier int conversion
gyroRate = gyroADCf[axis] / 16.4f; // gyro output deg/sec
// --------low-level gyro-based PID. ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
@ -178,6 +183,34 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = angleRate[axis] - gyroRate;
float dynReduction = tpaFactor;
// Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount
if (pidProfile->toleranceBand) {
const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f;
static uint8_t zeroCrossCount[3];
static uint8_t currentErrorPolarity[3];
if (ABS(RateError) < pidProfile->toleranceBand) {
if (zeroCrossCount[axis]) {
if (currentErrorPolarity[axis] == POSITIVE_ERROR) {
if (RateError < 0 ) {
zeroCrossCount[axis]--;
currentErrorPolarity[axis] = NEGATIVE_ERROR;
}
} else {
if (RateError > 0 ) {
zeroCrossCount[axis]--;
currentErrorPolarity[axis] = POSITIVE_ERROR;
}
}
} else {
dynReduction *= constrainf(ABS(RateError) / pidProfile->toleranceBand, minReduction, 1.0f);
}
} else {
zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount;
currentErrorPolarity[axis] = (RateError > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR;
}
}
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
// Smoothed Error for Derivative when delta from error selected
if (flightModeFlags && axis != YAW)
@ -187,7 +220,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
}
// -----calculate P component
PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * tpaFactor;
PTerm = PTermScale * RateError * pidProfile->P8[axis] * dynReduction;
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
@ -195,9 +228,11 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
}
// -----calculate I component.
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, (int32_t)angleRate[axis]) : pidProfile->I8[axis];
// Prevent strong Iterm accumulation during stick inputs
float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
float antiWindupScaler = constrainf(1.0f - (1.5f * (ABS(angleRate[axis]) / accumulationThreshold)), 0.0f, 1.0f);
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * errorLimiter * RateError * getdT() * kI, -250.0f, 250.0f);
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * RateError * getdT() * pidProfile->I8[axis] * antiWindupScaler * kiThrottleGain, -250.0f, 250.0f);
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
@ -232,12 +267,14 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
// Filter delta
if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f);
DTerm = constrainf(DTermScale * delta * (float)pidProfile->D8[axis] * dynReduction, -300.0f, 300.0f);
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
}
if (!pidStabilisationEnabled) axisPID[axis] = 0;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
@ -251,9 +288,9 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
#endif
}
}
#endif
static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future
static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
int axis;
@ -330,11 +367,12 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048.
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, AngleRateTmp) : pidProfile->I8[axis];
// Prevent Accumulation
uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8);
uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8;
int32_t rateErrorLimited = errorLimiter * RateError;
errorGyroI[axis] = errorGyroI[axis] + ((rateErrorLimited * (uint16_t)targetPidLooptime) >> 11) * kI;
errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi;
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
@ -377,6 +415,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin
axisPID[axis] = PTerm + ITerm + DTerm;
}
if (!pidStabilisationEnabled) axisPID[axis] = 0;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
@ -396,13 +435,11 @@ void pidSetController(pidControllerType_e type)
{
switch (type) {
default:
case PID_CONTROLLER_INTEGER:
pid_controller = pidInteger;
case PID_CONTROLLER_LEGACY:
pid_controller = pidLegacy;
break;
#ifndef SKIP_PID_LUXFLOAT
case PID_CONTROLLER_FLOAT:
pid_controller = pidFloat;
#endif
case PID_CONTROLLER_BETAFLIGHT:
pid_controller = pidBetaflight;
}
}

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

@ -41,8 +41,8 @@ typedef enum {
} pidIndex_e;
typedef enum {
PID_CONTROLLER_INTEGER = 1, // Integer math to gain some more performance from F1 targets
PID_CONTROLLER_FLOAT,
PID_CONTROLLER_LEGACY = 0, // Legacy PID controller. Old INT / Rewrite with 2.9 status. Fastest performance....least math. Will stay same in the future
PID_CONTROLLER_BETAFLIGHT, // Betaflight PID controller. Old luxfloat -> float evolution. More math added and maintained in the future
PID_COUNT
} pidControllerType_e;
@ -57,10 +57,18 @@ typedef enum {
SUPEREXPO_YAW_ALWAYS
} pidSuperExpoYaw_e;
#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
typedef enum {
NEGATIVE_ERROR = 0,
POSITIVE_ERROR
} pidErrorPolarity_e;
typedef enum {
PID_STABILISATION_OFF = 0,
PID_STABILISATION_ON
} pidStabilisationState_e;
typedef struct pidProfile_s {
uint8_t pidController; // 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Luggi09s new baseflight pid
uint8_t pidController; // 1 = rewrite betaflight evolved from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Betaflight PIDc (Evolved Luxfloat)
uint8_t P8[PID_ITEM_COUNT];
uint8_t I8[PID_ITEM_COUNT];
@ -73,7 +81,15 @@ typedef struct pidProfile_s {
uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates
uint16_t yaw_p_limit;
uint8_t dterm_average_count; // Configurable delta count for dterm
uint8_t dynamic_pid; // Dynamic PID implementation (currently only P and I)
uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage
uint8_t zeroThrottleStabilisation; // Disable/Enable zero throttle stabilisation. Normally even without airmode P and D would be active.
// Betaflight PID controller parameters
uint8_t toleranceBand; // Error tolerance area where toleranceBandReduction is applied under certain circumstances
uint8_t toleranceBandReduction; // Lowest possible P and D reduction in percentage
uint8_t zeroCrossAllowanceCount; // Amount of bouncebacks within tolerance band allowed before reduction kicks in
uint16_t accelerationLimitPercent; // Percentage that motor is allowed to increase or decrease in a period of 1ms
uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm
#ifdef GTUNE
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune
@ -97,5 +113,6 @@ extern uint32_t targetPidLooptime;
void pidSetController(pidControllerType_e type);
void pidResetErrorGyroState(void);
void pidStabilisationState(pidStabilisationState_e pidControllerState);
void setTargetPidLooptime(uint8_t pidProcessDenom);

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

@ -41,7 +41,7 @@ typedef enum {
BEEPER_USB, // Some boards have beeper powered USB connected
BEEPER_ALL, // Turn ON or OFF all beeper conditions
BEEPER_PREFERENCE, // Save prefered beeper configuration
BEEPER_PREFERENCE, // Save preferred beeper configuration
// BEEPER_ALL and BEEPER_PREFERENCE must remain at the bottom of this enum
} beeperMode_e;

1
src/main/io/escservo.h
View file

@ -24,5 +24,4 @@ typedef struct escAndServoConfig_s {
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500.
uint16_t escDesyncProtection; // Value that a motor is allowed to increase or decrease in a period of 1ms
} escAndServoConfig_t;

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

@ -1069,7 +1069,7 @@ static void gpsHandlePassthrough(uint8_t data)
#endif
}
void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
{

1601
src/main/io/ledstrip.c
View file

File diff suppressed because it is too large Load diff

206
src/main/io/ledstrip.h
View file

@ -17,81 +17,163 @@
#pragma once
#define MAX_LED_STRIP_LENGTH 32
#define CONFIGURABLE_COLOR_COUNT 16
#define LED_MAX_STRIP_LENGTH 32
#define LED_CONFIGURABLE_COLOR_COUNT 16
#define LED_MODE_COUNT 6
#define LED_DIRECTION_COUNT 6
#define LED_BASEFUNCTION_COUNT 7
#define LED_OVERLAY_COUNT 6
#define LED_SPECIAL_COLOR_COUNT 11
#define LED_POS_OFFSET 0
#define LED_FUNCTION_OFFSET 8
#define LED_OVERLAY_OFFSET 12
#define LED_COLOR_OFFSET 18
#define LED_DIRECTION_OFFSET 22
#define LED_PARAMS_OFFSET 28
#define LED_POS_BITCNT 8
#define LED_FUNCTION_BITCNT 4
#define LED_OVERLAY_BITCNT 6
#define LED_COLOR_BITCNT 4
#define LED_DIRECTION_BITCNT 6
#define LED_PARAMS_BITCNT 4
#define LED_FLAG_OVERLAY_MASK ((1 << LED_OVERLAY_BITCNT) - 1)
#define LED_FLAG_DIRECTION_MASK ((1 << LED_DIRECTION_BITCNT) - 1)
#define LED_MOV_POS(pos) ((pos) << LED_POS_OFFSET)
#define LED_MOV_FUNCTION(func) ((func) << LED_FUNCTION_OFFSET)
#define LED_MOV_OVERLAY(overlay) ((overlay) << LED_OVERLAY_OFFSET)
#define LED_MOV_COLOR(colorId) ((colorId) << LED_COLOR_OFFSET)
#define LED_MOV_DIRECTION(direction) ((direction) << LED_DIRECTION_OFFSET)
#define LED_MOV_PARAMS(param) ((param) << LED_PARAMS_OFFSET)
#define LED_BIT_MASK(len) ((1 << (len)) - 1)
#define LED_POS_MASK LED_MOV_POS(((1 << LED_POS_BITCNT) - 1))
#define LED_FUNCTION_MASK LED_MOV_FUNCTION(((1 << LED_FUNCTION_BITCNT) - 1))
#define LED_OVERLAY_MASK LED_MOV_OVERLAY(LED_FLAG_OVERLAY_MASK)
#define LED_COLOR_MASK LED_MOV_COLOR(((1 << LED_COLOR_BITCNT) - 1))
#define LED_DIRECTION_MASK LED_MOV_DIRECTION(LED_FLAG_DIRECTION_MASK)
#define LED_PARAMS_MASK LED_MOV_PARAMS(((1 << LED_PARAMS_BITCNT) - 1))
#define LED_FLAG_OVERLAY(id) (1 << (id))
#define LED_FLAG_DIRECTION(id) (1 << (id))
#define LED_X_BIT_OFFSET 4
#define LED_Y_BIT_OFFSET 0
#define LED_XY_MASK (0x0F)
#define GET_LED_X(ledConfig) ((ledConfig->xy >> LED_X_BIT_OFFSET) & LED_XY_MASK)
#define GET_LED_Y(ledConfig) ((ledConfig->xy >> LED_Y_BIT_OFFSET) & LED_XY_MASK)
#define CALCULATE_LED_X(x) ((x & LED_XY_MASK) << LED_X_BIT_OFFSET)
#define CALCULATE_LED_Y(y) ((y & LED_XY_MASK) << LED_Y_BIT_OFFSET)
#define CALCULATE_LED_XY(x,y) (CALCULATE_LED_X(x) | CALCULATE_LED_Y(y))
#define LED_XY_MASK 0x0F
#define CALCULATE_LED_XY(x, y) ((((x) & LED_XY_MASK) << LED_X_BIT_OFFSET) | (((y) & LED_XY_MASK) << LED_Y_BIT_OFFSET))
typedef enum {
LED_DISABLED = 0,
LED_DIRECTION_NORTH = (1 << 0),
LED_DIRECTION_EAST = (1 << 1),
LED_DIRECTION_SOUTH = (1 << 2),
LED_DIRECTION_WEST = (1 << 3),
LED_DIRECTION_UP = (1 << 4),
LED_DIRECTION_DOWN = (1 << 5),
LED_FUNCTION_INDICATOR = (1 << 6),
LED_FUNCTION_WARNING = (1 << 7),
LED_FUNCTION_FLIGHT_MODE = (1 << 8),
LED_FUNCTION_ARM_STATE = (1 << 9),
LED_FUNCTION_THROTTLE = (1 << 10),
LED_FUNCTION_THRUST_RING = (1 << 11),
LED_FUNCTION_COLOR = (1 << 12),
} ledFlag_e;
LED_MODE_ORIENTATION = 0,
LED_MODE_HEADFREE,
LED_MODE_HORIZON,
LED_MODE_ANGLE,
LED_MODE_MAG,
LED_MODE_BARO,
LED_SPECIAL
} ledModeIndex_e;
#define LED_DIRECTION_BIT_OFFSET 0
#define LED_DIRECTION_MASK ( \
LED_DIRECTION_NORTH | \
LED_DIRECTION_EAST | \
LED_DIRECTION_SOUTH | \
LED_DIRECTION_WEST | \
LED_DIRECTION_UP | \
LED_DIRECTION_DOWN \
)
#define LED_FUNCTION_BIT_OFFSET 6
#define LED_FUNCTION_MASK ( \
LED_FUNCTION_INDICATOR | \
LED_FUNCTION_WARNING | \
LED_FUNCTION_FLIGHT_MODE | \
LED_FUNCTION_ARM_STATE | \
LED_FUNCTION_THROTTLE | \
LED_FUNCTION_THRUST_RING | \
LED_FUNCTION_COLOR \
)
typedef enum {
LED_SCOLOR_DISARMED = 0,
LED_SCOLOR_ARMED,
LED_SCOLOR_ANIMATION,
LED_SCOLOR_BACKGROUND,
LED_SCOLOR_BLINKBACKGROUND,
LED_SCOLOR_GPSNOSATS,
LED_SCOLOR_GPSNOLOCK,
LED_SCOLOR_GPSLOCKED
} ledSpecialColorIds_e;
typedef enum {
LED_DIRECTION_NORTH = 0,
LED_DIRECTION_EAST,
LED_DIRECTION_SOUTH,
LED_DIRECTION_WEST,
LED_DIRECTION_UP,
LED_DIRECTION_DOWN
} ledDirectionId_e;
typedef enum {
LED_FUNCTION_COLOR,
LED_FUNCTION_FLIGHT_MODE,
LED_FUNCTION_ARM_STATE,
LED_FUNCTION_BATTERY,
LED_FUNCTION_RSSI,
LED_FUNCTION_GPS,
LED_FUNCTION_THRUST_RING,
} ledBaseFunctionId_e;
typedef enum {
LED_OVERLAY_THROTTLE,
LED_OVERLAY_LARSON_SCANNER,
LED_OVERLAY_BLINK,
LED_OVERLAY_LANDING_FLASH,
LED_OVERLAY_INDICATOR,
LED_OVERLAY_WARNING,
} ledOverlayId_e;
typedef struct modeColorIndexes_s {
uint8_t color[LED_DIRECTION_COUNT];
} modeColorIndexes_t;
typedef struct specialColorIndexes_s {
uint8_t color[LED_SPECIAL_COLOR_COUNT];
} specialColorIndexes_t;
typedef uint32_t ledConfig_t;
typedef struct ledCounts_s {
uint8_t count;
uint8_t ring;
uint8_t larson;
uint8_t ringSeqLen;
} ledCounts_t;
typedef struct ledConfig_s {
uint8_t xy; // see LED_X/Y_MASK defines
uint8_t color; // see colors (config_master)
uint16_t flags; // see ledFlag_e
} ledConfig_t;
ledConfig_t *ledConfigs;
hsvColor_t *colors;
modeColorIndexes_t *modeColors;
specialColorIndexes_t specialColors;
extern uint8_t ledCount;
extern uint8_t ledsInRingCount;
#define DEFINE_LED(x, y, col, dir, func, ol, params) (LED_MOV_POS(CALCULATE_LED_XY(x, y)) | LED_MOV_COLOR(col) | LED_MOV_DIRECTION(dir) | LED_MOV_FUNCTION(func) | LED_MOV_OVERLAY(ol) | LED_MOV_PARAMS(params))
static inline uint8_t ledGetXY(const ledConfig_t *lcfg) { return ((*lcfg >> LED_POS_OFFSET) & LED_BIT_MASK(LED_POS_BITCNT)); }
static inline uint8_t ledGetX(const ledConfig_t *lcfg) { return ((*lcfg >> (LED_POS_OFFSET + LED_X_BIT_OFFSET)) & LED_XY_MASK); }
static inline uint8_t ledGetY(const ledConfig_t *lcfg) { return ((*lcfg >> (LED_POS_OFFSET + LED_Y_BIT_OFFSET)) & LED_XY_MASK); }
static inline uint8_t ledGetFunction(const ledConfig_t *lcfg) { return ((*lcfg >> LED_FUNCTION_OFFSET) & LED_BIT_MASK(LED_FUNCTION_BITCNT)); }
static inline uint8_t ledGetOverlay(const ledConfig_t *lcfg) { return ((*lcfg >> LED_OVERLAY_OFFSET) & LED_BIT_MASK(LED_OVERLAY_BITCNT)); }
static inline uint8_t ledGetColor(const ledConfig_t *lcfg) { return ((*lcfg >> LED_COLOR_OFFSET) & LED_BIT_MASK(LED_COLOR_BITCNT)); }
static inline uint8_t ledGetDirection(const ledConfig_t *lcfg) { return ((*lcfg >> LED_DIRECTION_OFFSET) & LED_BIT_MASK(LED_DIRECTION_BITCNT)); }
static inline uint8_t ledGetParams(const ledConfig_t *lcfg) { return ((*lcfg >> LED_PARAMS_OFFSET) & LED_BIT_MASK(LED_PARAMS_BITCNT)); }
static inline bool ledGetOverlayBit(const ledConfig_t *lcfg, int id) { return ((ledGetOverlay(lcfg) >> id) & 1); }
static inline bool ledGetDirectionBit(const ledConfig_t *lcfg, int id) { return ((ledGetDirection(lcfg) >> id) & 1); }
/*
PG_DECLARE_ARR(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs);
PG_DECLARE_ARR(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors);
PG_DECLARE_ARR(modeColorIndexes_t, LED_MODE_COUNT, modeColors);
PG_DECLARE(specialColorIndexes_t, specialColors);
*/
bool parseColor(int index, const char *colorConfig);
bool parseLedStripConfig(uint8_t ledIndex, const char *config);
bool parseLedStripConfig(int ledIndex, const char *config);
void generateLedConfig(int ledIndex, char *ledConfigBuffer, size_t bufferSize);
void reevaluateLedConfig(void);
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse);
void ledStripEnable(void);
void updateLedStrip(void);
void updateLedRing(void);
bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex);
extern uint16_t rssi; // FIXME dependency on mw.c
void applyDefaultLedStripConfig(ledConfig_t *ledConfig);
void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize);
void applyDefaultColors(hsvColor_t *colors);
void applyDefaultModeColors(modeColorIndexes_t *modeColors);
void applyDefaultSpecialColors(specialColorIndexes_t *specialColors);
bool parseColor(uint8_t index, const char *colorConfig);
void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount);
void ledStripEnable(void);
void reevaluateLedConfig(void);

297
src/main/io/msp_protocol.h Normal file
View file

@ -0,0 +1,297 @@
/*
* 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/>.
*/
/**
* MSP Guidelines, emphasis is used to clarify.
*
* Each FlightController (FC, Server) MUST change the API version when any MSP command is added, deleted, or changed.
*
* If you fork the FC source code and release your own version, you MUST change the Flight Controller Identifier.
*
* NEVER release a modified copy of this code that shares the same Flight controller IDENT and API version
* if the API doesn't match EXACTLY.
*
* Consumers of the API (API clients) SHOULD first attempt to get a response from the MSP_API_VERSION command.
* If no response is obtained then client MAY try the legacy MSP_IDENT command.
*
* API consumers should ALWAYS handle communication failures gracefully and attempt to continue
* without the information if possible. Clients MAY log/display a suitable message.
*
* API clients should NOT attempt any communication if they can't handle the returned API MAJOR VERSION.
*
* API clients SHOULD attempt communication if the API MINOR VERSION has increased from the time
* the API client was written and handle command failures gracefully. Clients MAY disable
* functionality that depends on the commands while still leaving other functionality intact.
* that the newer API version may cause problems before using API commands that change FC state.
*
* It is for this reason that each MSP command should be specific as possible, such that changes
* to commands break as little functionality as possible.
*
* API client authors MAY use a compatibility matrix/table when determining if they can support
* a given command from a given flight controller at a given api version level.
*
* Developers MUST NOT create new MSP commands that do more than one thing.
*
* Failure to follow these guidelines will likely invoke the wrath of developers trying to write tools
* that use the API and the users of those tools.
*/
#pragma once
/* Protocol numbers used both by the wire format, config system, and
field setters.
*/
#define MSP_PROTOCOL_VERSION 0
#define API_VERSION_MAJOR 1 // increment when major changes are made
#define API_VERSION_MINOR 20 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_LENGTH 2
#define MULTIWII_IDENTIFIER "MWII";
#define BASEFLIGHT_IDENTIFIER "BAFL";
#define BETAFLIGHT_IDENTIFIER "BTFL"
#define CLEANFLIGHT_IDENTIFIER "CLFL"
#define INAV_IDENTIFIER "INAV"
#define RACEFLIGHT_IDENTIFIER "RCFL"
#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4
#define FLIGHT_CONTROLLER_VERSION_LENGTH 3
#define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF
#define BOARD_IDENTIFIER_LENGTH 4 // 4 UPPER CASE alpha numeric characters that identify the board being used.
#define BOARD_HARDWARE_REVISION_LENGTH 2
// These are baseflight specific flags but they are useless now since MW 2.3 uses the upper 4 bits for the navigation version.
#define CAP_PLATFORM_32BIT ((uint32_t)1 << 31)
#define CAP_BASEFLIGHT_CONFIG ((uint32_t)1 << 30)
// MW 2.3 stores NAVI_VERSION in the top 4 bits of the capability mask.
#define CAP_NAVI_VERSION_BIT_4_MSB ((uint32_t)1 << 31)
#define CAP_NAVI_VERSION_BIT_3 ((uint32_t)1 << 30)
#define CAP_NAVI_VERSION_BIT_2 ((uint32_t)1 << 29)
#define CAP_NAVI_VERSION_BIT_1_LSB ((uint32_t)1 << 28)
#define CAP_DYNBALANCE ((uint32_t)1 << 2)
#define CAP_FLAPS ((uint32_t)1 << 3)
#define CAP_NAVCAP ((uint32_t)1 << 4)
#define CAP_EXTAUX ((uint32_t)1 << 5)
#define MSP_API_VERSION 1 //out message
#define MSP_FC_VARIANT 2 //out message
#define MSP_FC_VERSION 3 //out message
#define MSP_BOARD_INFO 4 //out message
#define MSP_BUILD_INFO 5 //out message
#define MSP_NAME 10 //out message Returns user set board name - betaflight
#define MSP_SET_NAME 11 //in message Sets board name - betaflight
//
// MSP commands for Cleanflight original features
//
#define MSP_MODE_RANGES 34 //out message Returns all mode ranges
#define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range
#define MSP_FEATURE 36
#define MSP_SET_FEATURE 37
#define MSP_BOARD_ALIGNMENT 38
#define MSP_SET_BOARD_ALIGNMENT 39
#define MSP_CURRENT_METER_CONFIG 40
#define MSP_SET_CURRENT_METER_CONFIG 41
#define MSP_MIXER 42
#define MSP_SET_MIXER 43
#define MSP_RX_CONFIG 44
#define MSP_SET_RX_CONFIG 45
#define MSP_LED_COLORS 46
#define MSP_SET_LED_COLORS 47
#define MSP_LED_STRIP_CONFIG 48
#define MSP_SET_LED_STRIP_CONFIG 49
#define MSP_RSSI_CONFIG 50
#define MSP_SET_RSSI_CONFIG 51
#define MSP_ADJUSTMENT_RANGES 52
#define MSP_SET_ADJUSTMENT_RANGE 53
// private - only to be used by the configurator, the commands are likely to change
#define MSP_CF_SERIAL_CONFIG 54
#define MSP_SET_CF_SERIAL_CONFIG 55
#define MSP_VOLTAGE_METER_CONFIG 56
#define MSP_SET_VOLTAGE_METER_CONFIG 57
#define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm]
#define MSP_PID_CONTROLLER 59
#define MSP_SET_PID_CONTROLLER 60
#define MSP_ARMING_CONFIG 61 //out message Returns auto_disarm_delay and disarm_kill_switch parameters
#define MSP_SET_ARMING_CONFIG 62 //in message Sets auto_disarm_delay and disarm_kill_switch parameters
//
// Baseflight MSP commands (if enabled they exist in Cleanflight)
//
#define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total)
#define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP
// FIXME - Provided for backwards compatibility with configurator code until configurator is updated.
// DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead.
#define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere
#define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save
#define MSP_REBOOT 68 //in message reboot settings
// DEPRECATED - Use MSP_BUILD_INFO instead
#define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion
#define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip
#define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip
#define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip
#define MSP_LOOP_TIME 73 //out message Returns FC cycle time i.e looptime parameter
#define MSP_SET_LOOP_TIME 74 //in message Sets FC cycle time i.e looptime parameter
#define MSP_FAILSAFE_CONFIG 75 //out message Returns FC Fail-Safe settings
#define MSP_SET_FAILSAFE_CONFIG 76 //in message Sets FC Fail-Safe settings
#define MSP_RXFAIL_CONFIG 77 //out message Returns RXFAIL settings
#define MSP_SET_RXFAIL_CONFIG 78 //in message Sets RXFAIL settings
#define MSP_SDCARD_SUMMARY 79 //out message Get the state of the SD card
#define MSP_BLACKBOX_CONFIG 80 //out message Get blackbox settings
#define MSP_SET_BLACKBOX_CONFIG 81 //in message Set blackbox settings
#define MSP_TRANSPONDER_CONFIG 82 //out message Get transponder settings
#define MSP_SET_TRANSPONDER_CONFIG 83 //in message Set transponder settings
#define MSP_OSD_CONFIG 84 //out message Get osd settings - betaflight
#define MSP_SET_OSD_CONFIG 85 //in message Set osd settings - betaflight
#define MSP_OSD_CHAR_READ 86 //out message Get osd settings - betaflight
#define MSP_OSD_CHAR_WRITE 87 //in message Set osd settings - betaflight
#define MSP_VTX_CONFIG 88 //out message Get vtx settings - betaflight
#define MSP_SET_VTX_CONFIG 89 //in message Set vtx settings - betaflight
// Betaflight Additional Commands
#define MSP_PID_ADVANCED_CONFIG 90
#define MSP_SET_PID_ADVANCED_CONFIG 91
#define MSP_FILTER_CONFIG 92
#define MSP_SET_FILTER_CONFIG 93
#define MSP_ADVANCED_TUNING 94
#define MSP_SET_ADVANCED_TUNING 95
#define MSP_SENSOR_CONFIG 96
#define MSP_SET_SENSOR_CONFIG 97
#define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility
#define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility
//
// OSD specific
//
#define MSP_OSD_VIDEO_CONFIG 180
#define MSP_SET_OSD_VIDEO_CONFIG 181
//
// Multwii original MSP commands
//
// DEPRECATED - See MSP_API_VERSION and MSP_MIXER
#define MSP_IDENT 100 //out message mixerMode + multiwii version + protocol version + capability variable
#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
#define MSP_RAW_IMU 102 //out message 9 DOF
#define MSP_SERVO 103 //out message servos
#define MSP_MOTOR 104 //out message motors
#define MSP_RC 105 //out message rc channels and more
#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course
#define MSP_COMP_GPS 107 //out message distance home, direction home
#define MSP_ATTITUDE 108 //out message 2 angles 1 heading
#define MSP_ALTITUDE 109 //out message altitude, variometer
#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX
#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
#define MSP_PID 112 //out message P I D coeff (9 are used currently)
#define MSP_BOX 113 //out message BOX setup (number is dependant of your setup)
#define MSP_MISC 114 //out message powermeter trig
#define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI
#define MSP_BOXNAMES 116 //out message the aux switch names
#define MSP_PIDNAMES 117 //out message the PID names
#define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold
#define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes
#define MSP_SERVO_CONFIGURATIONS 120 //out message All servo configurations.
#define MSP_NAV_STATUS 121 //out message Returns navigation status
#define MSP_NAV_CONFIG 122 //out message Returns navigation parameters
#define MSP_3D 124 //out message Settings needed for reversible ESCs
#define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll
#define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag
#define MSP_LED_STRIP_MODECOLOR 127 //out message Get LED strip mode_color settings
#define MSP_SET_RAW_RC 200 //in message 8 rc chan
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
#define MSP_SET_PID 202 //in message P I D coeff (9 are used currently)
#define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup)
#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo
#define MSP_ACC_CALIBRATION 205 //in message no param
#define MSP_MAG_CALIBRATION 206 //in message no param
#define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use
#define MSP_RESET_CONF 208 //in message no param
#define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags)
#define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2)
#define MSP_SET_HEAD 211 //in message define a new heading hold direction
#define MSP_SET_SERVO_CONFIGURATION 212 //in message Servo settings
#define MSP_SET_MOTOR 214 //in message PropBalance function
#define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom
#define MSP_SET_3D 217 //in message Settings needed for reversible ESCs
#define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll
#define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults
#define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag
#define MSP_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings
// #define MSP_BIND 240 //in message no param
// #define MSP_ALARMS 242
#define MSP_EEPROM_WRITE 250 //in message no param
#define MSP_RESERVE_1 251 //reserved for system usage
#define MSP_RESERVE_2 252 //reserved for system usage
#define MSP_DEBUGMSG 253 //out message debug string buffer
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
#define MSP_RESERVE_3 255 //reserved for system usage
// Additional commands that are not compatible with MultiWii
#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc
#define MSP_UID 160 //out message Unique device ID
#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
#define MSP_GPSSTATISTICS 166 //out message get GPS debugging data
#define MSP_ACC_TRIM 240 //out message get acc angle trim values
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration
#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface

4
src/main/io/rc_controls.c
View file

@ -672,8 +672,8 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rx
applyStepAdjustment(controlRateConfig, adjustmentFunction, delta);
} else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
applySelectAdjustment(adjustmentFunction, position);
}

1
src/main/io/rc_controls.h
View file

@ -50,6 +50,7 @@ typedef enum {
BOXFAILSAFE,
BOXAIRMODE,
BOX3DDISABLESWITCH,
BOXFPVANGLEMIX,
CHECKBOX_ITEM_COUNT
} boxId_e;

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

@ -465,7 +465,7 @@ static void nopConsumer(uint8_t data)
arbitrary serial passthrough "proxy". Optional callbacks can be given to allow
for specialized data processing.
*/
void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer
void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer
*leftC, serialConsumer *rightC)
{
waitForSerialPortToFinishTransmitting(left);

1018
src/main/io/serial_4way.c
View file

File diff suppressed because it is too large Load diff

131
src/main/io/serial_4way.h
View file

@ -15,127 +15,36 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
* Author: 4712
*/
#pragma once
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
#include "serial_4way_impl.h"
#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER
#define USE_SERIAL_4WAY_SK_BOOTLOADER
typedef enum {
imC2 = 0,
imSIL_BLB = 1,
imATM_BLB = 2,
imSK = 3,
} esc4wayInterfaceMode_e;
#define imC2 0
#define imSIL_BLB 1
#define imATM_BLB 2
#define imSK 3
typedef enum {
// Test Interface still present
cmd_InterfaceTestAlive = 0x30, // '0' alive
// RETURN: ACK
extern uint8_t selected_esc;
// get Protocol Version Number 01..255
cmd_ProtocolGetVersion = 0x31, // '1' version
// RETURN: uint8_t VersionNumber + ACK
extern ioMem_t ioMem;
// get Version String
cmd_InterfaceGetName = 0x32, // '2' name
// RETURN: String + ACK
typedef union __attribute__ ((packed)) {
uint8_t bytes[2];
uint16_t word;
} uint8_16_u;
//get Version Number 01..255
cmd_InterfaceGetVersion = 0x33, // '3' version
// RETURN: uint16_t VersionNumber + ACK
typedef union __attribute__ ((packed)) {
uint8_t bytes[4];
uint16_t words[2];
uint32_t dword;
} uint8_32_u;
// Exit / Restart Interface - can be used to switch to Box Mode
cmd_InterfaceExit = 0x34, // '4' exit
// RETURN: ACK
//extern uint8_32_u DeviceInfo;
// Reset the Device connected to the Interface
cmd_DeviceReset = 0x35, // '5' reset
// PARAM: uint8_t escId
// RETURN: ACK
// Get the Device ID connected
// cmd_DeviceGetID = 0x36, // '6' device id; removed since 06/106
// RETURN: uint8_t DeviceID + ACK
// Initialize Flash Access for Device connected
// Autodetects interface protocol; retruns device signature and protocol
cmd_DeviceInitFlash = 0x37, // '7' init flash access
// PARAM: uint8_t escId
// RETURN: uint8_t deviceInfo[4] + ACK
// Erase the whole Device Memory of connected Device
cmd_DeviceEraseAll = 0x38, // '8' erase all
// RETURN: ACK
// Erase one Page of Device Memory of connected Device
cmd_DevicePageErase = 0x39, // '9' page erase
// PARAM: uint8_t PageNumber (512B pages)
// RETURN: APageNumber ACK
// Read to Buffer from FLASH Memory of connected Device
// Buffer Len is Max 256 Bytes, 0 means 256 Bytes
cmd_DeviceRead = 0x3A, // ':' read Device
// PARAM: [ADRESS] uint8_t BuffLen
// RETURN: [ADRESS, len] Buffer[0..256] ACK
// Write Buffer to FLASH Memory of connected Device
// Buffer Len is Max 256 Bytes, 0 means 256 Bytes
cmd_DeviceWrite = 0x3B, // ';' write
// PARAM: [ADRESS + BuffLen] Buffer[1..256]
// RETURN: ACK
// Set C2CK low infinite - permanent Reset state (unimplemented)
cmd_DeviceC2CK_LOW = 0x3C, // '<'
// RETURN: ACK
// Read to Buffer from EEPROM Memory of connected Device
// Buffer Len is Max 256 Bytes, 0 means 256 Bytes
cmd_DeviceReadEEprom = 0x3D, // '=' read Device
// PARAM: [ADRESS] uint8_t BuffLen
// RETURN: [ADRESS + BuffLen] + Buffer[1..256] ACK
// Write Buffer to EEPROM Memory of connected Device
// Buffer Len is Max 256 Bytes, 0 means 256 Bytes
cmd_DeviceWriteEEprom = 0x3E, // '>' write
// PARAM: [ADRESS + BuffLen] Buffer[1..256]
// RETURN: ACK
// Set Interface Mode
cmd_InterfaceSetMode = 0x3F, // '?'
// PARAM: uint8_t Mode (interfaceMode_e)
// RETURN: ACK
} esc4wayCmd_e;
// responses
typedef enum {
esc4wayAck_OK = 0x00,
// esc4wayAck_I_UNKNOWN_ERROR = 0x01,
esc4wayAck_I_INVALID_CMD = 0x02,
esc4wayAck_I_INVALID_CRC = 0x03,
esc4wayAck_I_VERIFY_ERROR = 0x04,
// esc4wayAck_D_INVALID_COMMAND = 0x05,
// esc4wayAck_D_COMMAND_FAILED = 0x06,
// esc4wayAck_D_UNKNOWN_ERROR = 0x07,
esc4wayAck_I_INVALID_CHANNEL = 0x08,
esc4wayAck_I_INVALID_PARAM = 0x09,
esc4wayAck_D_GENERAL_ERROR = 0x0f,
} esc4wayAck_e;
typedef struct escDeviceInfo_s {
uint16_t signature; // lower 16 bit of signature
uint8_t signature2; // top 8 bit of signature for SK / BootMsg last char from BL
uint8_t interfaceMode;
} escDeviceInfo_t;
extern bool esc4wayExitRequested; // flag that exit was requested. Set by esc4wayProcessCmd, used internally by esc4wayProcess
int esc4wayInit(void);
void esc4wayStart(void);
bool isMcuConnected(void);
uint8_t esc4wayInit(void);
void esc4wayProcess(serialPort_t *mspPort);
void esc4wayRelease(void);
void esc4wayProcess(serialPort_t *serial);
esc4wayAck_e esc4wayProcessCmd(esc4wayCmd_e command, uint16_t addr, uint8_t *data, int inLen, int *outLen);
#endif

329
src/main/io/serial_4way_avrootloader.c
View file

@ -24,271 +24,289 @@
#include <stdlib.h>
#include "platform.h"
#include "common/utils.h"
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/buf_writer.h"
#include "drivers/pwm_mapping.h"
#include "drivers/gpio.h"
#include "io/serial.h"
#include "io/serial_msp.h"
#include "io/serial_4way.h"
#include "io/serial_4way_impl.h"
#include "io/serial_4way_avrootloader.h"
#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
#if defined(USE_SERIAL_4WAY_BLHELI_INTERFACE) && defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER)
// Bootloader commands
// RunCmd
#define RestartBootloader 0
#define ExitBootloader 1
#define ExitBootloader 1
#define CMD_RUN 0x00
#define CMD_PROG_FLASH 0x01
#define CMD_ERASE_FLASH 0x02
#define CMD_RUN 0x00
#define CMD_PROG_FLASH 0x01
#define CMD_ERASE_FLASH 0x02
#define CMD_READ_FLASH_SIL 0x03
#define CMD_VERIFY_FLASH 0x03
#define CMD_READ_EEPROM 0x04
#define CMD_PROG_EEPROM 0x05
#define CMD_READ_SRAM 0x06
#define CMD_VERIFY_FLASH 0x03
#define CMD_READ_EEPROM 0x04
#define CMD_PROG_EEPROM 0x05
#define CMD_READ_SRAM 0x06
#define CMD_READ_FLASH_ATM 0x07
#define CMD_KEEP_ALIVE 0xFD
#define CMD_SET_ADDRESS 0xFF
#define CMD_SET_BUFFER 0xFE
#define CMD_KEEP_ALIVE 0xFD
#define CMD_SET_ADDRESS 0xFF
#define CMD_SET_BUFFER 0xFE
#define CMD_BOOTINIT 0x07
#define CMD_BOOTSIGN 0x08
#define CMD_BOOTINIT 0x07
#define CMD_BOOTSIGN 0x08
// Bootloader result codes
#define BR_SUCCESS 0x30
#define BR_ERRORCOMMAND 0xC1
#define BR_ERRORCRC 0xC2
#define BR_NONE 0xFF
#define brSUCCESS 0x30
#define brERRORCOMMAND 0xC1
#define brERRORCRC 0xC2
#define brNONE 0xFF
#define START_BIT_TIMEOUT 2000 // 2ms
#define BIT_TIME 52 // 52uS
#define BIT_TIME_HALVE (BIT_TIME >> 1) // 26uS
#define BIT_TIME_3_4 (BIT_TIME_HALVE + (BIT_TIME_HALVE >> 1)) // 39uS
#define START_BIT_TIME (BIT_TIME_3_4)
#define START_BIT_TIMEOUT_MS 2
static int suart_getc(void)
#define BIT_TIME (52) //52uS
#define BIT_TIME_HALVE (BIT_TIME >> 1) //26uS
#define START_BIT_TIME (BIT_TIME_HALVE + 1)
//#define STOP_BIT_TIME ((BIT_TIME * 9) + BIT_TIME_HALVE)
static uint8_t suart_getc_(uint8_t *bt)
{
uint32_t btime;
uint32_t start_time;
uint32_t wait_time = micros() + START_BIT_TIMEOUT;
uint32_t wait_time = millis() + START_BIT_TIMEOUT_MS;
while (ESC_IS_HI) {
// check for startbit begin
if (micros() >= wait_time) {
return -1;
if (millis() >= wait_time) {
return 0;
}
}
// start bit
start_time = micros();
btime = start_time + START_BIT_TIME;
uint16_t bitmask = 0;
for(int bit = 0; bit < 10; bit++) {
while (cmp32(micros(), btime) < 0);
uint8_t bit = 0;
while (micros() < btime);
while(1) {
if (ESC_IS_HI)
{
bitmask |= (1 << bit);
}
btime = btime + BIT_TIME;
bit++;
if (bit == 10) break;
while (micros() < btime);
}
// check start bit and stop bit
if ((bitmask & (1 << 0)) || (!(bitmask & (1 << 9)))) {
return -1;
if ((bitmask & 1) || (!(bitmask & (1 << 9)))) {
return 0;
}
return bitmask >> 1;
*bt = bitmask >> 1;
return 1;
}
static void suart_putc(uint8_t byte)
static void suart_putc_(uint8_t *tx_b)
{
// send one idle bit first (stopbit from previous byte)
uint16_t bitmask = (byte << 2) | (1 << 0) | (1 << 10);
// shift out stopbit first
uint16_t bitmask = (*tx_b << 2) | 1 | (1 << 10);
uint32_t btime = micros();
while(1) {
if(bitmask & 1)
if(bitmask & 1) {
ESC_SET_HI; // 1
else
}
else {
ESC_SET_LO; // 0
}
btime = btime + BIT_TIME;
bitmask >>= 1;
if (bitmask == 0)
break; // stopbit shifted out - but don't wait
while (cmp32(micros(), btime) < 0);
bitmask = (bitmask >> 1);
if (bitmask == 0) break; // stopbit shifted out - but don't wait
while (micros() < btime);
}
}
static uint16_t crc16Byte(uint16_t from, uint8_t byte)
static uint8_16_u CRC_16;
static uint8_16_u LastCRC_16;
static void ByteCrc(uint8_t *bt)
{
uint16_t crc16 = from;
for (int i = 0; i < 8; i++) {
if (((byte & 0x01) ^ (crc16 & 0x0001)) != 0) {
crc16 >>= 1;
crc16 ^= 0xA001;
uint8_t xb = *bt;
for (uint8_t i = 0; i < 8; i++)
{
if (((xb & 0x01) ^ (CRC_16.word & 0x0001)) !=0 ) {
CRC_16.word = CRC_16.word >> 1;
CRC_16.word = CRC_16.word ^ 0xA001;
} else {
crc16 >>= 1;
CRC_16.word = CRC_16.word >> 1;
}
byte >>= 1;
xb = xb >> 1;
}
return crc16;
}
static uint8_t BL_ReadBuf(uint8_t *pstring, int len, bool checkCrc)
static uint8_t BL_ReadBuf(uint8_t *pstring, uint8_t len)
{
int crc = 0;
int c;
// len 0 means 256
CRC_16.word = 0;
LastCRC_16.word = 0;
uint8_t LastACK = brNONE;
do {
if(!suart_getc_(pstring)) goto timeout;
ByteCrc(pstring);
pstring++;
len--;
} while(len > 0);
uint8_t lastACK = BR_NONE;
for(int i = 0; i < len; i++) {
int c;
if ((c = suart_getc()) < 0) goto timeout;
crc = crc16Byte(crc, c);
pstring[i] = c;
}
if(checkCrc) {
// With CRC read 3 more
for(int i = 0; i < 2; i++) { // checksum 2 CRC bytes
if ((c = suart_getc()) < 0) goto timeout;
crc = crc16Byte(crc, c);
if(isMcuConnected()) {
//With CRC read 3 more
if(!suart_getc_(&LastCRC_16.bytes[0])) goto timeout;
if(!suart_getc_(&LastCRC_16.bytes[1])) goto timeout;
if(!suart_getc_(&LastACK)) goto timeout;
if (CRC_16.word != LastCRC_16.word) {
LastACK = brERRORCRC;
}
if((c = suart_getc()) < 0) goto timeout;
lastACK = c;
if (crc != 0) // CRC of correct message is 0
lastACK = BR_ERRORCRC;
} else {
if((c = suart_getc()) < 0) goto timeout;
lastACK = c;
if(!suart_getc_(&LastACK)) goto timeout;
}
timeout:
return (lastACK == BR_SUCCESS);
return (LastACK == brSUCCESS);
}
static void BL_SendBuf(uint8_t *pstring, int len, bool appendCrc)
static void BL_SendBuf(uint8_t *pstring, uint8_t len)
{
ESC_OUTPUT;
uint16_t crc = 0;
for(int i = 0; i < len; i++) {
suart_putc(pstring[i]);
crc = crc16Byte(crc, pstring[i]);
}
if (appendCrc) {
suart_putc(crc & 0xff);
suart_putc(crc >> 8);
CRC_16.word=0;
do {
suart_putc_(pstring);
ByteCrc(pstring);
pstring++;
len--;
} while (len > 0);
if (isMcuConnected()) {
suart_putc_(&CRC_16.bytes[0]);
suart_putc_(&CRC_16.bytes[1]);
}
ESC_INPUT;
}
uint8_t BL_ConnectEx(escDeviceInfo_t *pDeviceInfo)
uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo)
{
#define BOOT_MSG_LEN 4
#define DevSignHi (BOOT_MSG_LEN)
#define DevSignLo (BOOT_MSG_LEN + 1)
#define BootMsgLen 4
#define DevSignHi (BootMsgLen)
#define DevSignLo (BootMsgLen+1)
memset(pDeviceInfo, 0, sizeof(*pDeviceInfo));
uint8_t bootInfo[BOOT_MSG_LEN + 4];
static const uint8_t bootMsgCheck[BOOT_MSG_LEN - 1] = "471";
//DeviceInfo.dword=0; is set before
uint8_t BootInfo[9];
uint8_t BootMsg[BootMsgLen-1] = "471";
// x * 0 + 9
#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
// SK message was sent during autodetection, use longer preamble
uint8_t bootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D};
uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D};
BL_SendBuf(BootInit, 21);
#else
uint8_t bootInit[] = { 0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D};
uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D};
BL_SendBuf(BootInit, 17);
#endif
BL_SendBuf(bootInit, sizeof(bootInit), false);
if (!BL_ReadBuf(bootInfo, sizeof(bootInfo), false))
if (!BL_ReadBuf(BootInfo, BootMsgLen + 4)) {
return 0;
}
// BootInfo has no CRC (ACK byte already analyzed... )
// Format = BootMsg("471c") SIGNATURE_001, SIGNATURE_002, BootVersion (always 6), BootPages (,ACK)
if(memcmp(bootInfo, bootMsgCheck, sizeof(bootMsgCheck)) != 0) // Check only the first 3 letters -> 471x OK
return 0;
for (uint8_t i = 0; i < (BootMsgLen - 1); i++) { // Check only the first 3 letters -> 471x OK
if (BootInfo[i] != BootMsg[i]) {
return (0);
}
}
pDeviceInfo->signature2 = bootInfo[BOOT_MSG_LEN - 1]; // taken from bootloaderMsg part, ascii 'c' now
pDeviceInfo->signature = (bootInfo[DevSignHi] << 8) | bootInfo[DevSignLo]; // SIGNATURE_001, SIGNATURE_002
return 1;
//only 2 bytes used $1E9307 -> 0x9307
pDeviceInfo->bytes[2] = BootInfo[BootMsgLen - 1];
pDeviceInfo->bytes[1] = BootInfo[DevSignHi];
pDeviceInfo->bytes[0] = BootInfo[DevSignLo];
return (1);
}
static uint8_t BL_GetACK(int timeout)
static uint8_t BL_GetACK(uint32_t Timeout)
{
int c;
while ((c = suart_getc()) < 0)
if(--timeout < 0) // timeout=1 -> 1 retry
return BR_NONE;
return c;
uint8_t LastACK = brNONE;
while (!(suart_getc_(&LastACK)) && (Timeout)) {
Timeout--;
} ;
return (LastACK);
}
uint8_t BL_SendCMDKeepAlive(void)
uint8_t BL_SendCMDKeepAlive(void)
{
uint8_t sCMD[] = {CMD_KEEP_ALIVE, 0};
BL_SendBuf(sCMD, sizeof(sCMD), true);
if (BL_GetACK(1) != BR_ERRORCOMMAND)
BL_SendBuf(sCMD, 2);
if (BL_GetACK(1) != brERRORCOMMAND) {
return 0;
}
return 1;
}
void BL_SendCMDRunRestartBootloader(void)
void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo)
{
uint8_t sCMD[] = {RestartBootloader, 0};
BL_SendBuf(sCMD, sizeof(sCMD), true); // sends simply 4 x 0x00 (CRC = 00)
pDeviceInfo->bytes[0] = 1;
BL_SendBuf(sCMD, 2); //sends simply 4 x 0x00 (CRC =00)
return;
}
static uint8_t BL_SendCMDSetAddress(ioMem_t *pMem) //supports only 16 bit Adr
{
// skip if adr == 0xFFFF
if(pMem->addr == 0xffff)
return 1;
uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->addr >> 8, pMem->addr & 0xff };
BL_SendBuf(sCMD, sizeof(sCMD), true);
return BL_GetACK(2) == BR_SUCCESS;
if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1;
uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->D_FLASH_ADDR_H, pMem->D_FLASH_ADDR_L };
BL_SendBuf(sCMD, 4);
return (BL_GetACK(2) == brSUCCESS);
}
static uint8_t BL_SendCMDSetBuffer(ioMem_t *pMem)
{
uint16_t len = pMem->len;
uint8_t sCMD[] = {CMD_SET_BUFFER, 0, len >> 8, len & 0xff};
BL_SendBuf(sCMD, sizeof(sCMD), true);
if (BL_GetACK(2) != BR_NONE)
return 0;
BL_SendBuf(pMem->data, len, true);
return BL_GetACK(40) == BR_SUCCESS;
uint8_t sCMD[] = {CMD_SET_BUFFER, 0, 0, pMem->D_NUM_BYTES};
if (pMem->D_NUM_BYTES == 0) {
// set high byte
sCMD[2] = 1;
}
BL_SendBuf(sCMD, 4);
if (BL_GetACK(2) != brNONE) return 0;
BL_SendBuf(pMem->D_PTR_I, pMem->D_NUM_BYTES);
return (BL_GetACK(40) == brSUCCESS);
}
static uint8_t BL_ReadA(uint8_t cmd, ioMem_t *pMem)
{
if(!BL_SendCMDSetAddress(pMem))
return 0;
unsigned len = pMem->len;
uint8_t sCMD[] = {cmd, len & 0xff}; // 0x100 is sent a 0x00 here
BL_SendBuf(sCMD, sizeof(sCMD), true);
return BL_ReadBuf(pMem->data, len, true);
if (BL_SendCMDSetAddress(pMem)) {
uint8_t sCMD[] = {cmd, pMem->D_NUM_BYTES};
BL_SendBuf(sCMD, 2);
return (BL_ReadBuf(pMem->D_PTR_I, pMem->D_NUM_BYTES ));
}
return 0;
}
static uint8_t BL_WriteA(uint8_t cmd, ioMem_t *pMem, uint32_t timeout)
{
if(!BL_SendCMDSetAddress(pMem))
return 0;
if (!BL_SendCMDSetBuffer(pMem))
return 0;
uint8_t sCMD[] = {cmd, 0x01};
BL_SendBuf(sCMD, sizeof(sCMD), true);
return BL_GetACK(timeout) == BR_SUCCESS;
if (BL_SendCMDSetAddress(pMem)) {
if (!BL_SendCMDSetBuffer(pMem)) return 0;
uint8_t sCMD[] = {cmd, 0x01};
BL_SendBuf(sCMD, 2);
return (BL_GetACK(timeout) == brSUCCESS);
}
return 0;
}
uint8_t BL_ReadFlashATM(ioMem_t *pMem)
uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem)
{
return BL_ReadA(CMD_READ_FLASH_ATM, pMem);
if(interface_mode == imATM_BLB) {
return BL_ReadA(CMD_READ_FLASH_ATM, pMem);
} else {
return BL_ReadA(CMD_READ_FLASH_SIL, pMem);
}
}
uint8_t BL_ReadFlashSIL(ioMem_t *pMem)
{
return BL_ReadA(CMD_READ_FLASH_SIL, pMem);
}
uint8_t BL_ReadEEprom(ioMem_t *pMem)
{
return BL_ReadA(CMD_READ_EEPROM, pMem);
@ -296,22 +314,23 @@ uint8_t BL_ReadEEprom(ioMem_t *pMem)
uint8_t BL_PageErase(ioMem_t *pMem)
{
if(!BL_SendCMDSetAddress(pMem))
return 0;
uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01};
BL_SendBuf(sCMD, sizeof(sCMD), true);
return BL_GetACK(40 * 1000 / START_BIT_TIMEOUT) == BR_SUCCESS;
if (BL_SendCMDSetAddress(pMem)) {
uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01};
BL_SendBuf(sCMD, 2);
return (BL_GetACK((40 / START_BIT_TIMEOUT_MS)) == brSUCCESS);
}
return 0;
}
uint8_t BL_WriteEEprom(ioMem_t *pMem)
{
return BL_WriteA(CMD_PROG_EEPROM, pMem, 3000 * 1000 / START_BIT_TIMEOUT);
return BL_WriteA(CMD_PROG_EEPROM, pMem, (3000 / START_BIT_TIMEOUT_MS));
}
uint8_t BL_WriteFlash(ioMem_t *pMem)
{
return BL_WriteA(CMD_PROG_FLASH, pMem, 40 * 1000 / START_BIT_TIMEOUT);
return BL_WriteA(CMD_PROG_FLASH, pMem, (40 / START_BIT_TIMEOUT_MS));
}
#endif
#endif

15
src/main/io/serial_4way_avrootloader.h
View file

@ -20,17 +20,12 @@
#pragma once
#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
void BL_SendBootInit(void);
uint8_t BL_ConnectEx(escDeviceInfo_t *pDeviceInfo);
uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo);
uint8_t BL_SendCMDKeepAlive(void);
uint8_t BL_WriteEEprom(ioMem_t *pMem);
uint8_t BL_ReadEEprom(ioMem_t *pMem);
uint8_t BL_PageErase(ioMem_t *pMem);
uint8_t BL_ReadEEprom(ioMem_t *pMem);
uint8_t BL_WriteEEprom(ioMem_t *pMem);
uint8_t BL_WriteFlash(ioMem_t *pMem);
uint8_t BL_ReadFlashATM(ioMem_t *pMem);
uint8_t BL_ReadFlashSIL(ioMem_t *pMem);
void BL_SendCMDRunRestartBootloader(void);
#endif
uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem);
void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo);

25
src/main/io/serial_4way_impl.h
View file

@ -15,12 +15,15 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
* Author: 4712
*/
#pragma once
#include "drivers/io.h"
typedef struct {
IO_t io;
} escHardware_t;
extern uint8_t escSelected;
extern uint8_t selected_esc;
bool isEscHi(uint8_t selEsc);
bool isEscLo(uint8_t selEsc);
@ -29,15 +32,17 @@ void setEscLo(uint8_t selEsc);
void setEscInput(uint8_t selEsc);
void setEscOutput(uint8_t selEsc);
#define ESC_IS_HI isEscHi(escSelected)
#define ESC_IS_LO isEscLo(escSelected)
#define ESC_SET_HI setEscHi(escSelected)
#define ESC_SET_LO setEscLo(escSelected)
#define ESC_INPUT setEscInput(escSelected)
#define ESC_OUTPUT setEscOutput(escSelected)
#define ESC_IS_HI isEscHi(selected_esc)
#define ESC_IS_LO isEscLo(selected_esc)
#define ESC_SET_HI setEscHi(selected_esc)
#define ESC_SET_LO setEscLo(selected_esc)
#define ESC_INPUT setEscInput(selected_esc)
#define ESC_OUTPUT setEscOutput(selected_esc)
typedef struct ioMem_s {
uint16_t len;
uint16_t addr;
uint8_t *data;
uint8_t D_NUM_BYTES;
uint8_t D_FLASH_ADDR_H;
uint8_t D_FLASH_ADDR_L;
uint8_t *D_PTR_I;
} ioMem_t;

313
src/main/io/serial_4way_stk500v2.c
View file

@ -16,45 +16,40 @@
* Author: 4712
* have a look at https://github.com/sim-/tgy/blob/master/boot.inc
* for info about the stk500v2 implementation
*/
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include "platform.h"
#include "common/utils.h"
#include "drivers/gpio.h"
#include "drivers/buf_writer.h"
#include "drivers/pwm_mapping.h"
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
#include "drivers/serial.h"
#include "drivers/system.h"
#include "config/config.h"
#include "io/serial.h"
#include "io/serial_msp.h"
#include "io/serial_4way.h"
#include "io/serial_4way_impl.h"
#include "io/serial_4way_stk500v2.h"
#include "drivers/system.h"
#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
#if defined(USE_SERIAL_4WAY_BLHELI_INTERFACE) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER)
#define BIT_LO_US (32) //32uS
#define BIT_HI_US (2*BIT_LO_US)
static uint8_t StkInBuf[16];
#define BIT_LO_US 32 //32uS
#define BIT_HI_US (2 * BIT_LO_US)
static uint8_t stkInBuf[16];
#define STK_BIT_TIMEOUT 250 // micro seconds
#define STK_BIT_TIMEOUT 250 // micro seconds
#define STK_WAIT_TICKS (1000 / STK_BIT_TIMEOUT) // per ms
#define STK_WAITCYLCES (STK_WAIT_TICKS * 35) // 35ms
#define STK_WAITCYLCES_START (STK_WAIT_TICKS / 2) // 0.5ms
#define STK_WAITCYLCES_EXT (STK_WAIT_TICKS * 5000) // 5s
#define STK_WAITCYLCES_START (STK_WAIT_TICKS / 2) // 0.5 ms
#define STK_WAITCYLCES_EXT (STK_WAIT_TICKS * 5000) //5 s
#define WaitPinLo while (ESC_IS_HI) { if (cmp32(micros(), timeout_timer) > 0) goto timeout; }
#define WaitPinHi while (ESC_IS_LO) { if (cmp32(micros(), timeout_timer) > 0) goto timeout; }
#define WaitPinLo while (ESC_IS_HI) {if (micros() > timeout_timer) goto timeout;}
#define WaitPinHi while (ESC_IS_LO) {if (micros() > timeout_timer) goto timeout;}
static uint32_t lastBitTime;
static uint32_t hiLoTsh;
static uint32_t LastBitTime;
static uint32_t HiLoTsh;
static uint8_t SeqNumber;
static uint8_t StkCmd;
@ -77,10 +72,10 @@ static uint8_t ckSumOut;
#define STATUS_CMD_OK 0x00
#define CmdFlashEepromRead 0xA0
#define EnterIspCmd1 0xAC
#define EnterIspCmd2 0x53
#define SPI_SIGNATURE_READ 0x30
#define CmdFlashEepromRead 0xA0
#define EnterIspCmd1 0xAC
#define EnterIspCmd2 0x53
#define signature_r 0x30
#define delay_us(x) delayMicroseconds(x)
#define IRQ_OFF // dummy
@ -89,7 +84,7 @@ static uint8_t ckSumOut;
static void StkSendByte(uint8_t dat)
{
ckSumOut ^= dat;
for (uint8_t i = 0; i < 8; i++) {
for (uint8_t i = 0; i < 8; i++) {
if (dat & 0x01) {
// 1-bits are encoded as 64.0us high, 72.8us low (135.8us total).
ESC_SET_HI;
@ -111,7 +106,7 @@ static void StkSendByte(uint8_t dat)
}
}
static void StkSendPacketHeader(uint16_t len)
static void StkSendPacketHeader(void)
{
IRQ_OFF;
ESC_OUTPUT;
@ -121,9 +116,6 @@ static void StkSendPacketHeader(uint16_t len)
ckSumOut = 0;
StkSendByte(MESSAGE_START);
StkSendByte(++SeqNumber);
StkSendByte(len >> 8);
StkSendByte(len & 0xff);
StkSendByte(TOKEN);
}
static void StkSendPacketFooter(void)
@ -135,14 +127,16 @@ static void StkSendPacketFooter(void)
IRQ_ON;
}
static int8_t ReadBit(void)
{
uint32_t btimer = micros();
uint32_t timeout_timer = btimer + STK_BIT_TIMEOUT;
WaitPinLo;
WaitPinHi;
lastBitTime = micros() - btimer;
if (lastBitTime <= hiLoTsh) {
LastBitTime = micros() - btimer;
if (LastBitTime <= HiLoTsh) {
timeout_timer = timeout_timer + STK_BIT_TIMEOUT;
WaitPinLo;
WaitPinHi;
@ -155,27 +149,30 @@ timeout:
return -1;
}
static int ReadByte(void)
static uint8_t ReadByte(uint8_t *bt)
{
uint8_t byte = 0;
for (int i = 0; i < 8; i++) {
*bt = 0;
for (uint8_t i = 0; i < 8; i++) {
int8_t bit = ReadBit();
if (bit < 0)
return -1; // timeout
byte |= bit << i;
if (bit == -1) goto timeout;
if (bit == 1) {
*bt |= (1 << i);
}
}
ckSumIn ^= byte;
return byte;
ckSumIn ^=*bt;
return 1;
timeout:
return 0;
}
static uint8_t StkReadLeader(void)
{
// Reset learned timing
hiLoTsh = BIT_HI_US + BIT_LO_US;
HiLoTsh = BIT_HI_US + BIT_LO_US;
// Wait for the first bit
int waitcycl; //250uS each
uint32_t waitcycl; //250uS each
if((StkCmd == CMD_PROGRAM_EEPROM_ISP) || (StkCmd == CMD_CHIP_ERASE_ISP)) {
waitcycl = STK_WAITCYLCES_EXT;
@ -184,54 +181,57 @@ static uint8_t StkReadLeader(void)
} else {
waitcycl= STK_WAITCYLCES;
}
while(ReadBit() < 0 && --waitcycl > 0);
if (waitcycl <= 0)
goto timeout;
// Skip the first bits
for (int i = 0; i < 10; i++) {
if (ReadBit() < 0)
goto timeout;
for ( ; waitcycl >0 ; waitcycl--) {
//check is not timeout
if (ReadBit() >- 1) break;
}
// learn timing (0.75 * lastBitTime)
hiLoTsh = (lastBitTime >> 1) + (lastBitTime >> 2);
//Skip the first bits
if (waitcycl == 0){
goto timeout;
}
for (uint8_t i = 0; i < 10; i++) {
if (ReadBit() == -1) goto timeout;
}
// learn timing
HiLoTsh = (LastBitTime >> 1) + (LastBitTime >> 2);
// Read until we get a 0 bit
int bit;
int8_t bit;
do {
bit = ReadBit();
if (bit < 0)
goto timeout;
if (bit == -1) goto timeout;
} while (bit > 0);
return 1;
timeout:
return 0;
}
static uint8_t StkRcvPacket(uint8_t *pstring, int maxLen)
static uint8_t StkRcvPacket(uint8_t *pstring)
{
int byte;
int len;
uint8_t bt = 0;
uint8_16_u Len;
IRQ_OFF;
if (!StkReadLeader()) goto Err;
ckSumIn=0;
if ((byte = ReadByte()) < 0 || (byte != MESSAGE_START)) goto Err;
if ((byte = ReadByte()) < 0 || (byte != SeqNumber)) goto Err;
len = ReadByte() << 8;
len |= ReadByte();
if(len < 1 || len >= 256 + 4) // will catch timeout too; limit length to max expected size
goto Err;
if ((byte = ReadByte()) < 0 || (byte != TOKEN)) goto Err;
if ((byte = ReadByte()) < 0 || (byte != StkCmd)) goto Err;
if ((byte = ReadByte()) < 0 || (byte != STATUS_CMD_OK)) goto Err;
for (int i = 0; i < len - 2; i++) {
if ((byte = ReadByte()) < 0) goto Err;
if(i < maxLen) // limit saved length (buffer is only 256B, but memory read reply contains additional status + 1 unknown byte)
pstring[i] = byte;
if (!ReadByte(&bt) || (bt != MESSAGE_START)) goto Err;
if (!ReadByte(&bt) || (bt != SeqNumber)) goto Err;
ReadByte(&Len.bytes[1]);
if (Len.bytes[1] > 1) goto Err;
ReadByte(&Len.bytes[0]);
if (Len.bytes[0] < 1) goto Err;
if (!ReadByte(&bt) || (bt != TOKEN)) goto Err;
if (!ReadByte(&bt) || (bt != StkCmd)) goto Err;
if (!ReadByte(&bt) || (bt != STATUS_CMD_OK)) goto Err;
for (uint16_t i = 0; i < (Len.word - 2); i++)
{
if (!ReadByte(pstring)) goto Err;
pstring++;
}
ReadByte(); // read checksum
ReadByte(&bt);
if (ckSumIn != 0) goto Err;
IRQ_ON;
return 1;
@ -240,26 +240,27 @@ Err:
return 0;
}
static uint8_t _CMD_SPI_MULTI_EX(uint8_t * resByte, uint8_t subcmd, uint16_t addr)
static uint8_t _CMD_SPI_MULTI_EX(volatile uint8_t * ResByte,uint8_t Cmd,uint8_t AdrHi,uint8_t AdrLo)
{
StkCmd = CMD_SPI_MULTI;
StkSendPacketHeader(8);
StkCmd= CMD_SPI_MULTI;
StkSendPacketHeader();
StkSendByte(0); // hi byte Msg len
StkSendByte(8); // lo byte Msg len
StkSendByte(TOKEN);
StkSendByte(CMD_SPI_MULTI);
StkSendByte(4); // NumTX
StkSendByte(4); // NumRX
StkSendByte(0); // RxStartAdr
StkSendByte(subcmd); // {TxData} Cmd
StkSendByte(addr >> 8); // {TxData} AdrHi
StkSendByte(addr & 0xff); // {TxData} AdrLow
StkSendByte(0); // {TxData} 0
StkSendByte(4); // NumTX
StkSendByte(4); // NumRX
StkSendByte(0); // RxStartAdr
StkSendByte(Cmd); // {TxData} Cmd
StkSendByte(AdrHi); // {TxData} AdrHi
StkSendByte(AdrLo); // {TxData} AdrLoch
StkSendByte(0); // {TxData} 0
StkSendPacketFooter();
if (StkRcvPacket(stkInBuf, sizeof(stkInBuf))) { // NumRX + 3
if ((stkInBuf[0] == 0x00)
&& ((stkInBuf[1] == subcmd) || (stkInBuf[1] == 0x00 /* ignore zero returns */))
&& (stkInBuf[2] == 0x00)) {
*resByte = stkInBuf[3];
}
return 1;
if (StkRcvPacket((void *)StkInBuf)) { // NumRX + 3
if ((StkInBuf[0] == 0x00) && ((StkInBuf[1] == Cmd)||(StkInBuf[1] == 0x00)/* ignore zero returns */) &&(StkInBuf[2] == 0x00)) {
*ResByte = StkInBuf[3];
}
return 1;
}
return 0;
}
@ -267,37 +268,61 @@ static uint8_t _CMD_SPI_MULTI_EX(uint8_t * resByte, uint8_t subcmd, uint16_t add
static uint8_t _CMD_LOAD_ADDRESS(ioMem_t *pMem)
{
// ignore 0xFFFF
// assume address is set before and we read or write the immediately following memory
if((pMem->addr == 0xffff))
return 1;
// assume address is set before and we read or write the immediately following package
if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1;
StkCmd = CMD_LOAD_ADDRESS;
StkSendPacketHeader(5);
StkSendPacketHeader();
StkSendByte(0); // hi byte Msg len
StkSendByte(5); // lo byte Msg len
StkSendByte(TOKEN);
StkSendByte(CMD_LOAD_ADDRESS);
StkSendByte(0);
StkSendByte(0);
StkSendByte(pMem->addr >> 8);
StkSendByte(pMem->addr & 0xff);
StkSendByte(pMem->D_FLASH_ADDR_H);
StkSendByte(pMem->D_FLASH_ADDR_L);
StkSendPacketFooter();
return StkRcvPacket(stkInBuf, sizeof(stkInBuf));
return (StkRcvPacket((void *)StkInBuf));
}
static uint8_t _CMD_READ_MEM_ISP(ioMem_t *pMem)
{
StkSendPacketHeader(4);
uint8_t LenHi;
if (pMem->D_NUM_BYTES>0) {
LenHi=0;
} else {
LenHi=1;
}
StkSendPacketHeader();
StkSendByte(0); // hi byte Msg len
StkSendByte(4); // lo byte Msg len
StkSendByte(TOKEN);
StkSendByte(StkCmd);
StkSendByte(pMem->len >> 8);
StkSendByte(pMem->len & 0xff);
StkSendByte(LenHi);
StkSendByte(pMem->D_NUM_BYTES);
StkSendByte(CmdFlashEepromRead);
StkSendPacketFooter();
return StkRcvPacket(pMem->data, pMem->len);
return (StkRcvPacket(pMem->D_PTR_I));
}
static uint8_t _CMD_PROGRAM_MEM_ISP(ioMem_t *pMem)
{
StkSendPacketHeader(pMem->len + 10);
uint8_16_u Len;
uint8_t LenLo = pMem->D_NUM_BYTES;
uint8_t LenHi;
if (LenLo) {
LenHi = 0;
Len.word = LenLo + 10;
} else {
LenHi = 1;
Len.word = 256 + 10;
}
StkSendPacketHeader();
StkSendByte(Len.bytes[1]); // high byte Msg len
StkSendByte(Len.bytes[0]); // low byte Msg len
StkSendByte(TOKEN);
StkSendByte(StkCmd);
StkSendByte(pMem->len >> 8);
StkSendByte(pMem->len & 0xff);
StkSendByte(LenHi);
StkSendByte(LenLo);
StkSendByte(0); // mode
StkSendByte(0); // delay
StkSendByte(0); // cmd1
@ -305,82 +330,92 @@ static uint8_t _CMD_PROGRAM_MEM_ISP(ioMem_t *pMem)
StkSendByte(0); // cmd3
StkSendByte(0); // poll1
StkSendByte(0); // poll2
for(int i = 0; i < pMem->len; i++)
StkSendByte(pMem->data[i]);
do {
StkSendByte(*pMem->D_PTR_I);
pMem->D_PTR_I++;
LenLo--;
} while (LenLo);
StkSendPacketFooter();
return StkRcvPacket(stkInBuf, sizeof(stkInBuf));
return StkRcvPacket((void *)StkInBuf);
}
uint8_t Stk_SignOn(void)
{
StkCmd = CMD_SIGN_ON;
StkSendPacketHeader(1);
StkCmd=CMD_SIGN_ON;
StkSendPacketHeader();
StkSendByte(0); // hi byte Msg len
StkSendByte(1); // lo byte Msg len
StkSendByte(TOKEN);
StkSendByte(CMD_SIGN_ON);
StkSendPacketFooter();
return StkRcvPacket(stkInBuf, sizeof(stkInBuf));
return (StkRcvPacket((void *) StkInBuf));
}
uint8_t Stk_ConnectEx(escDeviceInfo_t *pDeviceInfo)
uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo)
{
if (!Stk_SignOn())
return 0;
uint8_t signature[3]; // device signature, MSB first
for(unsigned i = 0; i < sizeof(signature); i++) {
if (!_CMD_SPI_MULTI_EX(&signature[i], SPI_SIGNATURE_READ, i))
return 0;
if (Stk_SignOn()) {
if (_CMD_SPI_MULTI_EX(&pDeviceInfo->bytes[1], signature_r,0,1)) {
if (_CMD_SPI_MULTI_EX(&pDeviceInfo->bytes[0], signature_r,0,2)) {
return 1;
}
}
}
// convert signature to little endian
pDeviceInfo->signature = (signature[1] << 8) | signature[2];
pDeviceInfo->signature2 = signature[0];
return 1;
return 0;
}
uint8_t Stk_Chip_Erase(void)
{
StkCmd = CMD_CHIP_ERASE_ISP;
StkSendPacketHeader(7);
StkSendByte(StkCmd);
StkSendPacketHeader();
StkSendByte(0); // high byte Msg len
StkSendByte(7); // low byte Msg len
StkSendByte(TOKEN);
StkSendByte(CMD_CHIP_ERASE_ISP);
StkSendByte(20); // ChipErase_eraseDelay atmega8
StkSendByte(0); // ChipErase_pollMethod atmega8
StkSendByte(0); // ChipErase_pollMethod atmega8
StkSendByte(0xAC);
StkSendByte(0x88);
StkSendByte(0x13);
StkSendByte(0x76);
StkSendPacketFooter();
return StkRcvPacket(stkInBuf, sizeof(stkInBuf));
return (StkRcvPacket(StkInBuf));
}
uint8_t Stk_ReadFlash(ioMem_t *pMem)
{
if (!_CMD_LOAD_ADDRESS(pMem))
return 0;
StkCmd = CMD_READ_FLASH_ISP;
return _CMD_READ_MEM_ISP(pMem);
if (_CMD_LOAD_ADDRESS(pMem)) {
StkCmd = CMD_READ_FLASH_ISP;
return (_CMD_READ_MEM_ISP(pMem));
}
return 0;
}
uint8_t Stk_ReadEEprom(ioMem_t *pMem)
{
if (!_CMD_LOAD_ADDRESS(pMem))
return 0;
StkCmd = CMD_READ_EEPROM_ISP;
return _CMD_READ_MEM_ISP(pMem);
if (_CMD_LOAD_ADDRESS(pMem)) {
StkCmd = CMD_READ_EEPROM_ISP;
return (_CMD_READ_MEM_ISP(pMem));
}
return 0;
}
uint8_t Stk_WriteFlash(ioMem_t *pMem)
{
if (!_CMD_LOAD_ADDRESS(pMem))
return 0;
StkCmd = CMD_PROGRAM_FLASH_ISP;
return _CMD_PROGRAM_MEM_ISP(pMem);
if (_CMD_LOAD_ADDRESS(pMem)) {
StkCmd = CMD_PROGRAM_FLASH_ISP;
return (_CMD_PROGRAM_MEM_ISP(pMem));
}
return 0;
}
uint8_t Stk_WriteEEprom(ioMem_t *pMem)
{
if (!_CMD_LOAD_ADDRESS(pMem))
return 0;
StkCmd = CMD_PROGRAM_EEPROM_ISP;
return _CMD_PROGRAM_MEM_ISP(pMem);
if (_CMD_LOAD_ADDRESS(pMem)) {
StkCmd = CMD_PROGRAM_EEPROM_ISP;
return (_CMD_PROGRAM_MEM_ISP(pMem));
}
return 0;
}
#endif
#endif

5
src/main/io/serial_4way_stk500v2.h
View file

@ -17,14 +17,11 @@
*/
#pragma once
#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER
uint8_t Stk_SignOn(void);
uint8_t Stk_ConnectEx(escDeviceInfo_t *pDeviceInfo);
uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo);
uint8_t Stk_ReadEEprom(ioMem_t *pMem);
uint8_t Stk_WriteEEprom(ioMem_t *pMem);
uint8_t Stk_ReadFlash(ioMem_t *pMem);
uint8_t Stk_WriteFlash(ioMem_t *pMem);
uint8_t Stk_Chip_Erase(void);
#endif

159
src/main/io/serial_cli.c
View file

@ -112,13 +112,14 @@ static void cliRxFail(char *cmdline);
static void cliAdjustmentRange(char *cmdline);
static void cliMotorMix(char *cmdline);
static void cliDefaults(char *cmdline);
void cliDfu(char *cmdLine);
void cliDfu(char *cmdLine);
static void cliDump(char *cmdLine);
void cliDumpProfile(uint8_t profileIndex);
void cliDumpRateProfile(uint8_t rateProfileIndex) ;
static void cliExit(char *cmdline);
static void cliFeature(char *cmdline);
static void cliMotor(char *cmdline);
static void cliName(char *cmdline);
static void cliPlaySound(char *cmdline);
static void cliProfile(char *cmdline);
static void cliRateProfile(char *cmdline);
@ -155,6 +156,7 @@ static void cliMap(char *cmdline);
#ifdef LED_STRIP
static void cliLed(char *cmdline);
static void cliColor(char *cmdline);
static void cliModeColor(char *cmdline);
#endif
#ifndef USE_QUAD_MIXER_ONLY
@ -263,6 +265,7 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux),
#ifdef LED_STRIP
CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor),
CLI_COMMAND_DEF("mode_color", "configure mode and special colors", NULL, cliModeColor),
#endif
CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults),
CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu),
@ -340,6 +343,7 @@ const clicmd_t cmdTable[] = {
#ifdef VTX
CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx),
#endif
CLI_COMMAND_DEF("name", "Name of craft", NULL, cliName),
};
#define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t))
@ -387,7 +391,7 @@ static const char * const lookupTableBlackboxDevice[] = {
static const char * const lookupTablePidController[] = {
"UNUSED", "INT", "FLOAT"
"LEGACY", "BETAFLIGHT"
};
static const char * const lookupTableSerialRX[] = {
@ -451,6 +455,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
"MIXER",
"AIRMODE",
"PIDLOOP",
"NOTCH",
};
#ifdef OSD
static const char * const lookupTableOsdType[] = {
@ -486,7 +491,7 @@ typedef enum {
TABLE_GPS_SBAS_MODE,
#endif
#ifdef BLACKBOX
TABLE_BLACKBOX_DEVICE,
TABLE_BLACKBOX_DEVICE,
#endif
TABLE_CURRENT_SENSOR,
TABLE_GIMBAL_MODE,
@ -598,7 +603,6 @@ const clivalue_t valueTable[] = {
{ "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "anti_desync_power_step", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.escDesyncProtection, .config.minmax = { 0, 10000 } },
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently
@ -677,7 +681,6 @@ const clivalue_t valueTable[] = {
{ "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmincellvoltage, .config.minmax = { 10, 50 } },
{ "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatwarningcellvoltage, .config.minmax = { 10, 50 } },
{ "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbathysteresis, .config.minmax = { 0, 250 } },
{ "vbat_pid_compensation", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } },
{ "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -10000, 10000 } },
{ "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { 0, 3300 } },
{ "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } },
@ -696,6 +699,8 @@ const clivalue_t valueTable[] = {
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
{ "gyro_notch_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz, .config.minmax = { 0, 500 } },
{ "gyro_notch_q", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_notch_q, .config.minmax = { 1, 100 } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } },
@ -762,8 +767,15 @@ const clivalue_t valueTable[] = {
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } },
{ "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dynamic_iterm", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pid, .config.lookup = { TABLE_OFF_ON } },
{ "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
{ "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } },
{ "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } },
{ "motor_accel_limit_percent", VAR_UINT16 | MASTER_VALUE, &masterConfig.profile[0].pidProfile.accelerationLimitPercent, .config.minmax = { 0, 10000 } },
{ "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } },
{ "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } },
{ "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } },
{ "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } },
{ "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
{ "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } },
@ -812,7 +824,7 @@ const clivalue_t valueTable[] = {
{ "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } },
#endif
#ifdef USE_RTC6705
{ "vtx_channel", VAR_INT16 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } },
{ "vtx_channel", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } },
{ "vtx_power", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_power, .config.minmax = { 0, 1 } },
#endif
#ifdef OSD
@ -1399,20 +1411,20 @@ static void cliLed(char *cmdline)
char ledConfigBuffer[20];
if (isEmpty(cmdline)) {
for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) {
for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
generateLedConfig(i, ledConfigBuffer, sizeof(ledConfigBuffer));
cliPrintf("led %u %s\r\n", i, ledConfigBuffer);
}
} else {
ptr = cmdline;
i = atoi(ptr);
if (i < MAX_LED_STRIP_LENGTH) {
if (i < LED_MAX_STRIP_LENGTH) {
ptr = strchr(cmdline, ' ');
if (!parseLedStripConfig(i, ++ptr)) {
cliShowParseError();
}
} else {
cliShowArgumentRangeError("index", 0, MAX_LED_STRIP_LENGTH - 1);
cliShowArgumentRangeError("index", 0, LED_MAX_STRIP_LENGTH - 1);
}
}
}
@ -1423,7 +1435,7 @@ static void cliColor(char *cmdline)
char *ptr;
if (isEmpty(cmdline)) {
for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) {
for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
cliPrintf("color %u %d,%u,%u\r\n",
i,
masterConfig.colors[i].h,
@ -1434,16 +1446,57 @@ static void cliColor(char *cmdline)
} else {
ptr = cmdline;
i = atoi(ptr);
if (i < CONFIGURABLE_COLOR_COUNT) {
if (i < LED_CONFIGURABLE_COLOR_COUNT) {
ptr = strchr(cmdline, ' ');
if (!parseColor(i, ++ptr)) {
cliShowParseError();
}
} else {
cliShowArgumentRangeError("index", 0, CONFIGURABLE_COLOR_COUNT - 1);
cliShowArgumentRangeError("index", 0, LED_CONFIGURABLE_COLOR_COUNT - 1);
}
}
}
static void cliModeColor(char *cmdline)
{
if (isEmpty(cmdline)) {
for (int i = 0; i < LED_MODE_COUNT; i++) {
for (int j = 0; j < LED_DIRECTION_COUNT; j++) {
int colorIndex = modeColors[i].color[j];
cliPrintf("mode_color %u %u %u\r\n", i, j, colorIndex);
}
}
for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) {
int colorIndex = specialColors.color[j];
cliPrintf("mode_color %u %u %u\r\n", LED_SPECIAL, j, colorIndex);
}
} else {
enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT};
int args[ARGS_COUNT];
int argNo = 0;
char* ptr = strtok(cmdline, " ");
while (ptr && argNo < ARGS_COUNT) {
args[argNo++] = atoi(ptr);
ptr = strtok(NULL, " ");
}
if (ptr != NULL || argNo != ARGS_COUNT) {
cliShowParseError();
return;
}
int modeIdx = args[MODE];
int funIdx = args[FUNCTION];
int color = args[COLOR];
if(!setModeColor(modeIdx, funIdx, color)) {
cliShowParseError();
return;
}
// values are validated
cliPrintf("mode_color %u %u %u\r\n", modeIdx, funIdx, color);
}
}
#endif
#ifdef USE_SERVOS
@ -1918,7 +1971,6 @@ typedef enum {
DUMP_ALL = (1 << 3),
} dumpFlags_e;
static const char* const sectionBreak = "\r\n";
#define printSectionBreak() cliPrintf((char *)sectionBreak)
@ -1941,7 +1993,7 @@ static void cliDump(char *cmdline)
dumpMask = DUMP_PROFILE; // only
}
if (strcasecmp(cmdline, "rates") == 0) {
dumpMask = DUMP_RATES;
dumpMask = DUMP_RATES;
}
if (strcasecmp(cmdline, "all") == 0) {
@ -1953,7 +2005,10 @@ static void cliDump(char *cmdline)
cliPrint("\r\n# version\r\n");
cliVersion(NULL);
cliPrint("\r\n# dump master\r\n");
printSectionBreak();
cliPrint("\r\n# name\r\n");
cliName(NULL);
cliPrint("\r\n# mixer\r\n");
#ifndef USE_QUAD_MIXER_ONLY
@ -2014,8 +2069,7 @@ static void cliDump(char *cmdline)
#endif
#endif
cliPrint("\r\n\r\n# feature\r\n");
cliPrint("\r\n# feature\r\n");
mask = featureMask();
for (i = 0; ; i++) { // disable all feature first
if (featureNames[i] == NULL)
@ -2035,10 +2089,8 @@ static void cliDump(char *cmdline)
#endif
}
#ifdef BEEPER
cliPrint("\r\n\r\n# beeper\r\n");
cliPrint("\r\n# beeper\r\n");
uint8_t beeperCount = beeperTableEntryCount();
mask = getBeeperOffMask();
for (int i = 0; i < (beeperCount-2); i++) {
@ -2049,40 +2101,37 @@ static void cliDump(char *cmdline)
}
#endif
cliPrint("\r\n\r\n# map\r\n");
cliPrint("\r\n# map\r\n");
for (i = 0; i < 8; i++)
buf[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i];
buf[i] = '\0';
cliPrintf("map %s\r\n", buf);
cliPrint("\r\n\r\n# serial\r\n");
cliPrint("\r\n# serial\r\n");
cliSerial("");
#ifdef LED_STRIP
cliPrint("\r\n\r\n# led\r\n");
cliPrint("\r\n# led\r\n");
cliLed("");
cliPrint("\r\n\r\n# color\r\n");
cliPrint("\r\n# color\r\n");
cliColor("");
cliPrint("\r\n# mode_color\r\n");
cliModeColor("");
#endif
cliPrint("\r\n# aux\r\n");
cliAux("");
cliPrint("\r\n# adjrange\r\n");
cliAdjustmentRange("");
cliPrintf("\r\n# rxrange\r\n");
cliRxRange("");
#ifdef USE_SERVOS
cliPrint("\r\n# servo\r\n");
cliServo("");
// print servo directions
@ -2102,11 +2151,10 @@ static void cliDump(char *cmdline)
#ifdef VTX
cliPrint("\r\n# vtx\r\n");
cliVtx("");
#endif
printSectionBreak();
cliPrint("\r\n# master\r\n");
dumpValues(MASTER_VALUE);
cliPrint("\r\n# rxfail\r\n");
@ -2124,7 +2172,6 @@ static void cliDump(char *cmdline)
cliDumpRateProfile(rateCount);
cliPrint("\r\n# restore original rateprofile selection\r\n");
changeControlRateProfile(currentRateIndex);
cliRateProfile("");
#ifdef USE_SLOW_SERIAL_CLI
@ -2133,7 +2180,6 @@ static void cliDump(char *cmdline)
}
cliPrint("\r\n# restore original profile selection\r\n");
changeProfile(activeProfile);
cliProfile("");
printSectionBreak();
@ -2156,30 +2202,23 @@ static void cliDump(char *cmdline)
void cliDumpProfile(uint8_t profileIndex)
{
if (profileIndex >= MAX_PROFILE_COUNT) // Faulty values
return;
changeProfile(profileIndex);
cliPrint("\r\n# profile\r\n");
cliProfile("");
cliPrintf("############################# PROFILE VALUES ####################################\r\n");
printSectionBreak();
dumpValues(PROFILE_VALUE);
cliRateProfile("");
if (profileIndex >= MAX_PROFILE_COUNT) // Faulty values
return;
changeProfile(profileIndex);
cliPrint("\r\n# profile\r\n");
cliProfile("");
printSectionBreak();
dumpValues(PROFILE_VALUE);
}
void cliDumpRateProfile(uint8_t rateProfileIndex)
{
if (rateProfileIndex >= MAX_RATEPROFILES) // Faulty values
return;
changeControlRateProfile(rateProfileIndex);
cliPrint("\r\n# rateprofile\r\n");
cliRateProfile("");
printSectionBreak();
dumpValues(PROFILE_RATE_VALUE);
}
@ -2494,6 +2533,18 @@ static void cliMotor(char *cmdline)
cliPrintf("motor %d: %d\r\n", motor_index, motor_disarmed[motor_index]);
}
static void cliName(char *cmdline)
{
uint32_t len = strlen(cmdline);
if (len > 0) {
memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name));
strncpy(masterConfig.name, cmdline, MIN(len, MAX_NAME_LENGTH));
}
cliPrintf("name %s\r\n", strlen(masterConfig.name) > 0 ? masterConfig.name : "-");
return;
}
static void cliPlaySound(char *cmdline)
{
#if FLASH_SIZE <= 64
@ -2681,7 +2732,7 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full)
break;
}
}
static void cliPrintVarRange(const clivalue_t *var)
static void cliPrintVarRange(const clivalue_t *var)
{
switch (var->type & VALUE_MODE_MASK) {
case (MODE_DIRECT): {
@ -2693,7 +2744,7 @@ static void cliPrintVarRange(const clivalue_t *var)
cliPrint("Allowed values:");
uint8_t i;
for (i = 0; i < tableEntry->valueCount ; i++) {
if (i > 0)
if (i > 0)
cliPrint(",");
cliPrintf(" %s", tableEntry->values[i]);
}
@ -2904,6 +2955,10 @@ static void cliStatus(char *cmdline)
#endif
cliPrintf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cErrorCounter, sizeof(master_t));
#ifdef USE_SDCARD
cliSdInfo(NULL);
#endif
}
#ifndef SKIP_TASK_STATISTICS

214
src/main/io/serial_msp.c
View file

@ -59,6 +59,7 @@
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/osd.h"
#include "io/vtx.h"
#include "io/msp_protocol.h"
#include "telemetry/telemetry.h"
@ -104,77 +105,10 @@ extern uint16_t cycleTime; // FIXME dependency on mw.c
extern uint16_t rssi; // FIXME dependency on mw.c
extern void resetPidProfile(pidProfile_t *pidProfile);
void setGyroSamplingSpeed(uint16_t looptime) {
uint16_t gyroSampleRate = 1000;
uint8_t maxDivider = 1;
if (looptime != gyro.targetLooptime || looptime == 0) {
if (looptime == 0) looptime = gyro.targetLooptime; // needed for pid controller changes
#ifdef STM32F303xC
if (looptime < 1000) {
masterConfig.gyro_lpf = 0;
gyroSampleRate = 125;
maxDivider = 8;
masterConfig.pid_process_denom = 1;
masterConfig.acc_hardware = ACC_DEFAULT;
masterConfig.baro_hardware = BARO_DEFAULT;
masterConfig.mag_hardware = MAG_DEFAULT;
if (looptime < 250) {
masterConfig.acc_hardware = ACC_NONE;
masterConfig.baro_hardware = BARO_NONE;
masterConfig.mag_hardware = MAG_NONE;
masterConfig.pid_process_denom = 2;
} else if (looptime < 375) {
masterConfig.acc_hardware = CONFIG_FASTLOOP_PREFERRED_ACC;
masterConfig.baro_hardware = BARO_NONE;
masterConfig.mag_hardware = MAG_NONE;
masterConfig.pid_process_denom = 2;
}
masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider);
} else {
masterConfig.gyro_lpf = 0;
masterConfig.gyro_sync_denom = 8;
masterConfig.acc_hardware = ACC_DEFAULT;
masterConfig.baro_hardware = BARO_DEFAULT;
masterConfig.mag_hardware = MAG_DEFAULT;
}
#else
if (looptime < 1000) {
masterConfig.gyro_lpf = 0;
masterConfig.acc_hardware = ACC_NONE;
masterConfig.baro_hardware = BARO_NONE;
masterConfig.mag_hardware = MAG_NONE;
gyroSampleRate = 125;
maxDivider = 8;
masterConfig.pid_process_denom = 1;
if (currentProfile->pidProfile.pidController == PID_CONTROLLER_FLOAT) {
masterConfig.pid_process_denom = 2;
}
if (looptime < 250) {
masterConfig.pid_process_denom = 4;
} else if (looptime < 375) {
if (currentProfile->pidProfile.pidController == PID_CONTROLLER_FLOAT) {
masterConfig.pid_process_denom = 3;
} else {
masterConfig.pid_process_denom = 2;
}
}
masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider);
} else {
masterConfig.gyro_lpf = 0;
masterConfig.gyro_sync_denom = 8;
masterConfig.acc_hardware = ACC_DEFAULT;
masterConfig.baro_hardware = BARO_DEFAULT;
masterConfig.mag_hardware = MAG_DEFAULT;
masterConfig.pid_process_denom = 1;
}
#endif
}
}
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
typedef struct box_e {
const uint8_t boxId; // see boxId_e
@ -214,6 +148,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXFAILSAFE, "FAILSAFE;", 27 },
{ BOXAIRMODE, "AIR MODE;", 28 },
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 29},
{ BOXFPVANGLEMIX, "FPV ANGLE MIX;", 30},
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -253,6 +188,8 @@ STATIC_UNIT_TESTED mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
STATIC_UNIT_TESTED mspPort_t *currentPort;
STATIC_UNIT_TESTED bufWriter_t *writer;
#define RATEPROFILE_MASK (1 << 7)
static void serialize8(uint8_t a)
{
bufWriterAppend(writer, a);
@ -542,6 +479,8 @@ void mspInit(serialConfig_t *serialConfig)
if (!feature(FEATURE_AIRMODE)) activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH;
activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX;
if (sensors(SENSOR_BARO)) {
activeBoxIds[activeBoxIdCount++] = BOXBARO;
}
@ -659,7 +598,7 @@ static uint32_t packFlightModeFlags(void)
static bool processOutCommand(uint8_t cmdMSP)
{
uint32_t i;
uint8_t len;
#ifdef GPS
uint8_t wp_no;
int32_t lat = 0, lon = 0;
@ -737,7 +676,7 @@ static bool processOutCommand(uint8_t cmdMSP)
break;
case MSP_STATUS_EX:
headSerialReply(12);
headSerialReply(15);
serialize16(cycleTime);
#ifdef USE_I2C
serialize16(i2cGetErrorCounter());
@ -746,8 +685,18 @@ static bool processOutCommand(uint8_t cmdMSP)
#endif
serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
serialize32(packFlightModeFlags());
serialize8(masterConfig.current_profile_index);
serialize8(getCurrentProfile());
serialize16(constrain(averageSystemLoadPercent, 0, 100));
serialize8(MAX_PROFILE_COUNT);
serialize8(getCurrentControlRateProfile());
break;
case MSP_NAME:
len = strlen(masterConfig.name);
headSerialReply(len);
for (uint8_t i=0; i<len; i++) {
serialize8(masterConfig.name[i]);
}
break;
case MSP_STATUS:
@ -1142,8 +1091,8 @@ static bool processOutCommand(uint8_t cmdMSP)
#ifdef LED_STRIP
case MSP_LED_COLORS:
headSerialReply(CONFIGURABLE_COLOR_COUNT * 4);
for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) {
headSerialReply(LED_CONFIGURABLE_COLOR_COUNT * 4);
for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
hsvColor_t *color = &masterConfig.colors[i];
serialize16(color->h);
serialize8(color->s);
@ -1152,14 +1101,27 @@ static bool processOutCommand(uint8_t cmdMSP)
break;
case MSP_LED_STRIP_CONFIG:
headSerialReply(MAX_LED_STRIP_LENGTH * 7);
for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) {
headSerialReply(LED_MAX_STRIP_LENGTH * 4);
for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
ledConfig_t *ledConfig = &masterConfig.ledConfigs[i];
serialize16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET);
serialize16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET);
serialize8(GET_LED_X(ledConfig));
serialize8(GET_LED_Y(ledConfig));
serialize8(ledConfig->color);
serialize32(*ledConfig);
}
break;
case MSP_LED_STRIP_MODECOLOR:
headSerialReply(((LED_MODE_COUNT * LED_DIRECTION_COUNT) + LED_SPECIAL_COLOR_COUNT) * 3);
for (int i = 0; i < LED_MODE_COUNT; i++) {
for (int j = 0; j < LED_DIRECTION_COUNT; j++) {
serialize8(i);
serialize8(j);
serialize8(masterConfig.modeColors[i].color[j]);
}
}
for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) {
serialize8(LED_MODE_COUNT);
serialize8(j);
serialize8(masterConfig.specialColors.color[j]);
}
break;
#endif
@ -1237,18 +1199,18 @@ static bool processOutCommand(uint8_t cmdMSP)
break;
case MSP_3D:
headSerialReply(2 * 4);
headSerialReply(2 * 3);
serialize16(masterConfig.flight3DConfig.deadband3d_low);
serialize16(masterConfig.flight3DConfig.deadband3d_high);
serialize16(masterConfig.flight3DConfig.neutral3d);
serialize16(masterConfig.flight3DConfig.deadband3d_throttle);
break;
case MSP_RC_DEADBAND:
headSerialReply(3);
headSerialReply(5);
serialize8(masterConfig.rcControlsConfig.deadband);
serialize8(masterConfig.rcControlsConfig.yaw_deadband);
serialize8(masterConfig.rcControlsConfig.alt_hold_deadband);
serialize16(masterConfig.flight3DConfig.deadband3d_throttle);
break;
case MSP_SENSOR_ALIGNMENT:
headSerialReply(3);
@ -1276,14 +1238,14 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize16(currentProfile->pidProfile.yawItermIgnoreRate);
serialize16(currentProfile->pidProfile.yaw_p_limit);
serialize8(currentProfile->pidProfile.deltaMethod);
serialize8(masterConfig.batteryConfig.vbatPidCompensation);
serialize8(currentProfile->pidProfile.vbatPidCompensation);
break;
case MSP_SPECIAL_PARAMETERS:
headSerialReply(1 + 2 + 1 + 2);
serialize8(currentControlRateProfile->rcYawRate8);
serialize16(masterConfig.rxConfig.airModeActivateThreshold);
serialize8(masterConfig.rxConfig.rcSmoothInterval);
serialize16(masterConfig.escAndServoConfig.escDesyncProtection);
serialize16(currentProfile->pidProfile.accelerationLimitPercent);
break;
case MSP_SENSOR_CONFIG:
headSerialReply(3);
@ -1302,8 +1264,7 @@ static bool processInCommand(void)
{
uint32_t i;
uint16_t tmp;
uint8_t rate;
uint8_t oldPid;
uint8_t value;
#ifdef GPS
uint8_t wp_no;
int32_t lat = 0, lon = 0, alt = 0;
@ -1313,14 +1274,23 @@ static bool processInCommand(void)
#endif
switch (currentPort->cmdMSP) {
case MSP_SELECT_SETTING:
if (!ARMING_FLAG(ARMED)) {
masterConfig.current_profile_index = read8();
if (masterConfig.current_profile_index > 1) {
masterConfig.current_profile_index = 0;
value = read8();
if ((value & RATEPROFILE_MASK) == 0) {
if (!ARMING_FLAG(ARMED)) {
if (value >= MAX_PROFILE_COUNT) {
value = 0;
}
changeProfile(value);
}
writeEEPROM();
readEEPROM();
} else {
value = value & ~RATEPROFILE_MASK;
if (value >= MAX_RATEPROFILES) {
value = 0;
}
changeControlRateProfile(value);
}
break;
case MSP_SET_HEAD:
magHold = read16();
@ -1350,13 +1320,11 @@ static bool processInCommand(void)
masterConfig.disarm_kill_switch = read8();
break;
case MSP_SET_LOOP_TIME:
setGyroSamplingSpeed(read16());
read16();
break;
case MSP_SET_PID_CONTROLLER:
oldPid = currentProfile->pidProfile.pidController;
currentProfile->pidProfile.pidController = constrain(read8(), 1, 2);
currentProfile->pidProfile.pidController = constrain(read8(), 0, 1);
pidSetController(currentProfile->pidProfile.pidController);
if (oldPid != currentProfile->pidProfile.pidController) setGyroSamplingSpeed(0); // recalculate looptimes for new PID
break;
case MSP_SET_PID:
for (i = 0; i < PID_ITEM_COUNT; i++) {
@ -1410,11 +1378,11 @@ static bool processInCommand(void)
currentControlRateProfile->rcRate8 = read8();
currentControlRateProfile->rcExpo8 = read8();
for (i = 0; i < 3; i++) {
rate = read8();
currentControlRateProfile->rates[i] = MIN(rate, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
value = read8();
currentControlRateProfile->rates[i] = MIN(value, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX);
}
rate = read8();
currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX);
value = read8();
currentControlRateProfile->dynThrPID = MIN(value, CONTROL_RATE_CONFIG_TPA_MAX);
currentControlRateProfile->thrMid8 = read8();
currentControlRateProfile->thrExpo8 = read8();
currentControlRateProfile->tpa_breakpoint = read16();
@ -1774,7 +1742,7 @@ static bool processInCommand(void)
#ifdef LED_STRIP
case MSP_SET_LED_COLORS:
for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) {
for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
hsvColor_t *color = &masterConfig.colors[i];
color->h = read16();
color->s = read8();
@ -1785,31 +1753,26 @@ static bool processInCommand(void)
case MSP_SET_LED_STRIP_CONFIG:
{
i = read8();
if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != (1 + 7)) {
if (i >= LED_MAX_STRIP_LENGTH || currentPort->dataSize != (1 + 4)) {
headSerialError(0);
break;
}
ledConfig_t *ledConfig = &masterConfig.ledConfigs[i];
uint16_t mask;
// currently we're storing directions and functions in a uint16 (flags)
// the msp uses 2 x uint16_t to cater for future expansion
mask = read16();
ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK;
mask = read16();
ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK;
mask = read8();
ledConfig->xy = CALCULATE_LED_X(mask);
mask = read8();
ledConfig->xy |= CALCULATE_LED_Y(mask);
ledConfig->color = read8();
*ledConfig = read32();
reevaluateLedConfig();
}
break;
case MSP_SET_LED_STRIP_MODECOLOR:
{
ledModeIndex_e modeIdx = read8();
int funIdx = read8();
int color = read8();
if (!setModeColor(modeIdx, funIdx, color))
return false;
}
break;
#endif
case MSP_REBOOT:
isRebootScheduled = true;
@ -1855,19 +1818,26 @@ static bool processInCommand(void)
currentProfile->pidProfile.yawItermIgnoreRate = read16();
currentProfile->pidProfile.yaw_p_limit = read16();
currentProfile->pidProfile.deltaMethod = read8();
masterConfig.batteryConfig.vbatPidCompensation = read8();
currentProfile->pidProfile.vbatPidCompensation = read8();
break;
case MSP_SET_SPECIAL_PARAMETERS:
currentControlRateProfile->rcYawRate8 = read8();
masterConfig.rxConfig.airModeActivateThreshold = read16();
masterConfig.rxConfig.rcSmoothInterval = read8();
masterConfig.escAndServoConfig.escDesyncProtection = read16();
currentProfile->pidProfile.accelerationLimitPercent = read16();
break;
case MSP_SET_SENSOR_CONFIG:
masterConfig.acc_hardware = read8();
masterConfig.baro_hardware = read8();
masterConfig.mag_hardware = read8();
break;
case MSP_SET_NAME:
memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name));
for (i = 0; i < MIN(MAX_NAME_LENGTH, currentPort->dataSize); i++) {
masterConfig.name[i] = read8();
}
break;
default:
// we do not know how to handle the (valid) message, indicate error MSP $M!
return false;

261
src/main/io/serial_msp.h
View file

@ -20,267 +20,6 @@
#include "io/serial.h"
#include "drivers/serial.h"
/**
* MSP Guidelines, emphasis is used to clarify.
*
* Each FlightController (FC, Server) MUST change the API version when any MSP command is added, deleted, or changed.
*
* If you fork the FC source code and release your own version, you MUST change the Flight Controller Identifier.
*
* NEVER release a modified copy of this code that shares the same Flight controller IDENT and API version
* if the API doesn't match EXACTLY.
*
* Consumers of the API (API clients) SHOULD first attempt to get a response from the MSP_API_VERSION command.
* If no response is obtained then client MAY try the legacy MSP_IDENT command.
*
* API consumers should ALWAYS handle communication failures gracefully and attempt to continue
* without the information if possible. Clients MAY log/display a suitable message.
*
* API clients should NOT attempt any communication if they can't handle the returned API MAJOR VERSION.
*
* API clients SHOULD attempt communication if the API MINOR VERSION has increased from the time
* the API client was written and handle command failures gracefully. Clients MAY disable
* functionality that depends on the commands while still leaving other functionality intact.
* Clients SHOULD operate in READ-ONLY mode and SHOULD present a warning to the user to state
* that the newer API version may cause problems before using API commands that change FC state.
*
* It is for this reason that each MSP command should be specific as possible, such that changes
* to commands break as little client functionality as possible.
*
* API client authors MAY use a compatibility matrix/table when determining if they can support
* a given command from a given flight controller at a given api version level.
*
* Developers MUST NOT create new MSP commands that do more than one thing.
*
* Failure to follow these guidelines will likely invoke the wrath of developers trying to write tools
* that use the API and the users of those tools.
*/
#define MSP_PROTOCOL_VERSION 0
#define API_VERSION_MAJOR 1 // increment when major changes are made
#define API_VERSION_MINOR 17 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_LENGTH 2
#define MULTIWII_IDENTIFIER "MWII";
#define CLEANFLIGHT_IDENTIFIER "CLFL"
#define BETAFLIGHT_IDENTIFIER "BTFL"
#define BASEFLIGHT_IDENTIFIER "BAFL";
#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4
extern const char * const flightControllerIdentifier;
#define FLIGHT_CONTROLLER_VERSION_LENGTH 3
#define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF
static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
#define BOARD_IDENTIFIER_LENGTH 4 // 4 UPPER CASE alpha numeric characters that identify the board being used.
#define BOARD_HARDWARE_REVISION_LENGTH 2
// These are baseflight specific flags but they are useless now since MW 2.3 uses the upper 4 bits for the navigation version.
#define CAP_PLATFORM_32BIT ((uint32_t)1 << 31)
#define CAP_BASEFLIGHT_CONFIG ((uint32_t)1 << 30)
// MW 2.3 stores NAVI_VERSION in the top 4 bits of the capability mask.
#define CAP_NAVI_VERSION_BIT_4_MSB ((uint32_t)1 << 31)
#define CAP_NAVI_VERSION_BIT_3 ((uint32_t)1 << 30)
#define CAP_NAVI_VERSION_BIT_2 ((uint32_t)1 << 29)
#define CAP_NAVI_VERSION_BIT_1_LSB ((uint32_t)1 << 28)
#define CAP_DYNBALANCE ((uint32_t)1 << 2)
#define CAP_FLAPS ((uint32_t)1 << 3)
#define CAP_NAVCAP ((uint32_t)1 << 4)
#define CAP_EXTAUX ((uint32_t)1 << 5)
#define MSP_API_VERSION 1 //out message
#define MSP_FC_VARIANT 2 //out message
#define MSP_FC_VERSION 3 //out message
#define MSP_BOARD_INFO 4 //out message
#define MSP_BUILD_INFO 5 //out message
//
// MSP commands for Cleanflight original features
//
#define MSP_MODE_RANGES 34 //out message Returns all mode ranges
#define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range
#define MSP_FEATURE 36
#define MSP_SET_FEATURE 37
#define MSP_BOARD_ALIGNMENT 38
#define MSP_SET_BOARD_ALIGNMENT 39
#define MSP_CURRENT_METER_CONFIG 40
#define MSP_SET_CURRENT_METER_CONFIG 41
#define MSP_MIXER 42
#define MSP_SET_MIXER 43
#define MSP_RX_CONFIG 44
#define MSP_SET_RX_CONFIG 45
#define MSP_LED_COLORS 46
#define MSP_SET_LED_COLORS 47
#define MSP_LED_STRIP_CONFIG 48
#define MSP_SET_LED_STRIP_CONFIG 49
#define MSP_RSSI_CONFIG 50
#define MSP_SET_RSSI_CONFIG 51
#define MSP_ADJUSTMENT_RANGES 52
#define MSP_SET_ADJUSTMENT_RANGE 53
// private - only to be used by the configurator, the commands are likely to change
#define MSP_CF_SERIAL_CONFIG 54
#define MSP_SET_CF_SERIAL_CONFIG 55
#define MSP_VOLTAGE_METER_CONFIG 56
#define MSP_SET_VOLTAGE_METER_CONFIG 57
#define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm]
#define MSP_PID_CONTROLLER 59
#define MSP_SET_PID_CONTROLLER 60
#define MSP_ARMING_CONFIG 61 //out message Returns auto_disarm_delay and disarm_kill_switch parameters
#define MSP_SET_ARMING_CONFIG 62 //in message Sets auto_disarm_delay and disarm_kill_switch parameters
#define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip
#define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip
#define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip
#define MSP_LOOP_TIME 73 //out message Returns FC cycle time i.e looptime parameter
#define MSP_SET_LOOP_TIME 74 //in message Sets FC cycle time i.e looptime parameter
#define MSP_FAILSAFE_CONFIG 75 //out message Returns FC Fail-Safe settings
#define MSP_SET_FAILSAFE_CONFIG 76 //in message Sets FC Fail-Safe settings
#define MSP_RXFAIL_CONFIG 77 //out message Returns RXFAIL settings
#define MSP_SET_RXFAIL_CONFIG 78 //in message Sets RXFAIL settings
#define MSP_SDCARD_SUMMARY 79 //out message Get the state of the SD card
#define MSP_BLACKBOX_CONFIG 80 //out message Get blackbox settings
#define MSP_SET_BLACKBOX_CONFIG 81 //in message Set blackbox settings
#define MSP_TRANSPONDER_CONFIG 82 //in message Get transponder settings
#define MSP_SET_TRANSPONDER_CONFIG 83 //out message Set transponder settings
#define MSP_OSD_CONFIG 84 //in message Get osd settings
#define MSP_SET_OSD_CONFIG 85 //out message Set osd settings
#define MSP_OSD_CHAR_READ 86 //in message Get osd settings
#define MSP_OSD_CHAR_WRITE 87 //out message Set osd settings
#define MSP_VTX_CONFIG 88 //in message Get vtx settings
#define MSP_SET_VTX_CONFIG 89 //out message Set vtx settings
//
// Baseflight MSP commands (if enabled they exist in Cleanflight)
//
#define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total)
#define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP
// FIXME - Provided for backwards compatibility with configurator code until configurator is updated.
// DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead.
#define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere
#define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save
#define MSP_REBOOT 68 //in message reboot settings
// DEPRECATED - Use MSP_BUILD_INFO instead
#define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion
// Betaflight Additional Commands
#define MSP_PID_ADVANCED_CONFIG 90
#define MSP_SET_PID_ADVANCED_CONFIG 91
#define MSP_FILTER_CONFIG 92
#define MSP_SET_FILTER_CONFIG 93
#define MSP_ADVANCED_TUNING 94
#define MSP_SET_ADVANCED_TUNING 95
#define MSP_SENSOR_CONFIG 96
#define MSP_SET_SENSOR_CONFIG 97
#define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility
#define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility
//
// Multwii original MSP commands
//
// DEPRECATED - See MSP_API_VERSION and MSP_MIXER
#define MSP_IDENT 100 //out message mixerMode + multiwii version + protocol version + capability variable
#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
#define MSP_RAW_IMU 102 //out message 9 DOF
#define MSP_SERVO 103 //out message servos
#define MSP_MOTOR 104 //out message motors
#define MSP_RC 105 //out message rc channels and more
#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course
#define MSP_COMP_GPS 107 //out message distance home, direction home
#define MSP_ATTITUDE 108 //out message 2 angles 1 heading
#define MSP_ALTITUDE 109 //out message altitude, variometer
#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX
#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
#define MSP_PID 112 //out message P I D coeff (9 are used currently)
#define MSP_BOX 113 //out message BOX setup (number is dependant of your setup)
#define MSP_MISC 114 //out message powermeter trig
#define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI
#define MSP_BOXNAMES 116 //out message the aux switch names
#define MSP_PIDNAMES 117 //out message the PID names
#define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold
#define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes
#define MSP_SERVO_CONFIGURATIONS 120 //out message All servo configurations.
#define MSP_NAV_STATUS 121 //out message Returns navigation status
#define MSP_NAV_CONFIG 122 //out message Returns navigation parameters
#define MSP_3D 124 //out message Settings needed for reversible ESCs
#define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll
#define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag
#define MSP_SET_RAW_RC 200 //in message 8 rc chan
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
#define MSP_SET_PID 202 //in message P I D coeff (9 are used currently)
#define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup)
#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo
#define MSP_ACC_CALIBRATION 205 //in message no param
#define MSP_MAG_CALIBRATION 206 //in message no param
#define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use
#define MSP_RESET_CONF 208 //in message no param
#define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags)
#define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2)
#define MSP_SET_HEAD 211 //in message define a new heading hold direction
#define MSP_SET_SERVO_CONFIGURATION 212 //in message Servo settings
#define MSP_SET_MOTOR 214 //in message PropBalance function
#define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom
#define MSP_SET_3D 217 //in message Settings needed for reversible ESCs
#define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll
#define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults
#define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag
// #define MSP_BIND 240 //in message no param
// #define MSP_ALARMS 242
#define MSP_EEPROM_WRITE 250 //in message no param
#define MSP_DEBUGMSG 253 //out message debug string buffer
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
// Additional commands that are not compatible with MultiWii
#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc
#define MSP_UID 160 //out message Unique device ID
#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
#define MSP_ACC_TRIM 240 //out message get acc angle trim values
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration
#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface
// Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports.
#define MAX_MSP_PORT_COUNT 2

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

@ -14,7 +14,7 @@
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define VTX_BAND_MIN 1

11
src/main/main.c
View file

@ -134,7 +134,7 @@ void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
void imuInit(void);
void displayInit(rxConfig_t *intialRxConfig);
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse);
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse);
void spektrumBind(rxConfig_t *rxConfig);
const sonarHardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor);
void osdInit(void);
@ -310,12 +310,12 @@ void init(void)
featureClear(FEATURE_ONESHOT125);
}
bool use_unsyncedPwm = masterConfig.use_unsyncedPwm;
bool use_unsyncedPwm = masterConfig.use_unsyncedPwm || masterConfig.motor_pwm_protocol == PWM_TYPE_CONVENTIONAL || masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED;
// Configurator feature abused for enabling Fast PWM
pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED);
pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED);
pwm_params.pwmProtocolType = masterConfig.motor_pwm_protocol;
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.motorPwmRate = use_unsyncedPwm ? masterConfig.motor_pwm_rate : 0;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
@ -323,7 +323,6 @@ void init(void)
if (masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED) {
featureClear(FEATURE_3D);
pwm_params.idlePulse = 0; // brushed motors
use_unsyncedPwm = false;
}
#ifdef CC3D
pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false;
@ -544,7 +543,7 @@ void init(void)
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors);
ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors);
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();

66
src/main/mw.c
View file

@ -184,8 +184,28 @@ float calculateRate(int axis, int16_t rc) {
angleRate = (float)((currentControlRateProfile->rates[axis] + 27) * rc) / 16.0f;
}
if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY)
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
else
return constrainf(angleRate / 4.1f, -1997.0f, 1997.0f); // Rate limit protection (deg/sec)
}
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
void scaleRcCommandToFpvCamAngle(void) {
//recalculate sin/cos only when masterConfig.rxConfig.fpvCamAngleDegrees changed
static uint8_t lastFpvCamAngleDegrees = 0;
static float cosFactor = 1.0;
static float sinFactor = 0.0;
if (lastFpvCamAngleDegrees != masterConfig.rxConfig.fpvCamAngleDegrees){
lastFpvCamAngleDegrees = masterConfig.rxConfig.fpvCamAngleDegrees;
cosFactor = cos_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
}
int16_t roll = angleRate[ROLL];
int16_t yaw = angleRate[YAW];
angleRate[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500);
angleRate[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500);
}
void processRcCommand(void)
@ -208,6 +228,11 @@ void processRcCommand(void)
if (isRXDataNew) {
for (axis = 0; axis < 3; axis++) angleRate[axis] = calculateRate(axis, rcCommand[axis]);
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
scaleRcCommandToFpvCamAngle();
}
for (int channel=0; channel < 4; channel++) {
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel];
@ -230,24 +255,6 @@ void processRcCommand(void)
}
}
void scaleRcCommandToFpvCamAngle(void) {
//recalculate sin/cos only when masterConfig.rxConfig.fpvCamAngleDegrees changed
static uint8_t lastFpvCamAngleDegrees = 0;
static float cosFactor = 1.0;
static float sinFactor = 0.0;
if (lastFpvCamAngleDegrees != masterConfig.rxConfig.fpvCamAngleDegrees){
lastFpvCamAngleDegrees = masterConfig.rxConfig.fpvCamAngleDegrees;
cosFactor = cos_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
}
int16_t roll = rcCommand[ROLL];
int16_t yaw = rcCommand[YAW];
rcCommand[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500);
rcCommand[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500);
}
static void updateRcCommands(void)
{
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
@ -310,11 +317,6 @@ static void updateRcCommands(void)
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
// experimental scaling of RC command to FPV cam angle
if (masterConfig.rxConfig.fpvCamAngleDegrees && !FLIGHT_MODE(HEADFREE_MODE)) {
scaleRcCommandToFpvCamAngle();
}
}
static void updateLEDs(void)
@ -483,7 +485,7 @@ void updateMagHold(void)
void processRx(void)
{
static bool armedBeeperOn = false;
static bool wasAirmodeIsActivated;
static bool airmodeIsActivated;
calculateRxChannelsAndUpdateFailsafe(currentTime);
@ -507,15 +509,21 @@ void processRx(void)
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) wasAirmodeIsActivated = true; // Prevent Iterm from being reset
if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
} else {
wasAirmodeIsActivated = false;
airmodeIsActivated = false;
}
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
if (throttleStatus == THROTTLE_LOW && !wasAirmodeIsActivated) {
if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) {
pidResetErrorGyroState();
if (currentProfile->pidProfile.zeroThrottleStabilisation)
pidStabilisationState(PID_STABILISATION_ON);
else
pidStabilisationState(PID_STABILISATION_OFF);
} else {
pidStabilisationState(PID_STABILISATION_ON);
}
// When armed and motors aren't spinning, do beeps and then disarm
@ -763,7 +771,7 @@ void subTaskMotorUpdate(void)
previousMotorUpdateTime = startTime;
}
mixTable();
mixTable(&currentProfile->pidProfile);
#ifdef USE_SERVOS
filterServos();

4
src/main/rx/xbus.c
View file

@ -226,7 +226,7 @@ static void xBusUnpackRJ01Frame(void)
uint8_t outerCrc = 0;
uint8_t i = 0;
// When using the Align RJ01 receiver with
// When using the Align RJ01 receiver with
// a MODE B setting in the radio (XG14 tested)
// the MODE_B -frame is packed within some
// at the moment unknown bytes before and after:
@ -234,7 +234,7 @@ static void xBusUnpackRJ01Frame(void)
// Compared to a standard MODE B frame that only
// contains the "middle" package.
// Hence, at the moment, the unknown header and footer
// of the RJ01 MODEB packages are discarded.
// of the RJ01 MODEB packages are discarded.
// However, the LAST byte (CRC_OUTER) is infact an 8-bit
// CRC for the whole package, using the Dallas-One-Wire CRC
// method.

2
src/main/scheduler/scheduler_tasks.c
View file

@ -168,7 +168,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
[TASK_LEDSTRIP] = {
.taskName = "LEDSTRIP",
.taskFunc = taskLedStrip,
.desiredPeriod = 1000000 / 10, // 10 Hz
.desiredPeriod = 1000000 / 100, // 100 Hz
.staticPriority = TASK_PRIORITY_LOW,
},
#endif

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

@ -190,7 +190,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
if (!accFilterInitialised) {
if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime);
}
accFilterInitialised = true;
}

8
src/main/sensors/battery.c
View file

@ -70,7 +70,7 @@ static void updateBatteryVoltage(void)
if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
if (!vbatFilterIsInitialised) {
biquadFilterInit(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
vbatFilterIsInitialised = true;
}
vbatSample = biquadFilterApply(&vbatFilter, vbatSample);
@ -91,7 +91,7 @@ void updateBattery(void)
/* Actual battery state is calculated below, this is really BATTERY_PRESENT */
batteryState = BATTERY_OK;
/* wait for VBatt to stabilise then we can calc number of cells
(using the filtered value takes a long time to ramp up)
(using the filtered value takes a long time to ramp up)
We only do this on the ground so don't care if we do block, not
worse than original code anyway*/
delay(VBATTERY_STABLE_DELAY);
@ -212,7 +212,7 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea
fix12_t calculateVbatPidCompensation(void) {
fix12_t batteryScaler;
if (batteryConfig->vbatPidCompensation && feature(FEATURE_VBAT) && batteryCellCount > 1) {
if (feature(FEATURE_VBAT) && batteryCellCount > 1) {
uint16_t maxCalculatedVoltage = batteryConfig->vbatmaxcellvoltage * batteryCellCount;
batteryScaler = qConstruct(maxCalculatedVoltage, constrain(vbat, maxCalculatedVoltage - batteryConfig->vbatmaxcellvoltage, maxCalculatedVoltage));
} else {
@ -224,7 +224,7 @@ fix12_t calculateVbatPidCompensation(void) {
uint8_t calculateBatteryPercentage(void)
{
return (((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount);
return constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100);
}
uint8_t calculateBatteryCapacityRemainingPercentage(void)

1
src/main/sensors/battery.h
View file

@ -43,7 +43,6 @@ typedef struct batteryConfig_s {
uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery critical alarm, in 0.1V units, default is 33 (3.3V)
uint8_t vbatwarningcellvoltage; // warning voltage per cell, this triggers battery warning alarm, in 0.1V units, default is 35 (3.5V)
uint8_t vbathysteresis; // hysteresis for alarm, default 1 = 0.1V
uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage
int16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
uint16_t currentMeterOffset; // offset of the current sensor in millivolt steps

30
src/main/sensors/gyro.c
View file

@ -27,12 +27,14 @@
#include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/system.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h"
#include "io/beeper.h"
#include "io/statusindicator.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
gyro_t gyro; // gyro access functions
@ -43,21 +45,27 @@ float gyroADCf[XYZ_AXIS_COUNT];
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
static const gyroConfig_t *gyroConfig;
static biquadFilter_t gyroFilter[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT];
static uint16_t gyroSoftNotchHz;
static uint8_t gyroSoftNotchQ;
static uint8_t gyroSoftLpfHz;
static uint16_t calibratingG = 0;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz)
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q)
{
gyroConfig = gyroConfigToUse;
gyroSoftLpfHz = gyro_soft_lpf_hz;
gyroSoftNotchHz = gyro_soft_notch_hz;
gyroSoftNotchQ = gyro_soft_notch_q;
}
void gyroInit(void)
{
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilter[axis], gyroSoftLpfHz, gyro.targetLooptime);
biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, ((float) gyroSoftNotchQ) / 10, FILTER_NOTCH);
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
}
}
}
@ -157,7 +165,17 @@ void gyroUpdate(void)
if (gyroSoftLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADCf[axis] = biquadFilterApply(&gyroFilter[axis], (float)gyroADC[axis]);
float sample = (float) gyroADC[axis];
if (gyroSoftNotchHz) {
sample = biquadFilterApply(&gyroFilterNotch[axis], sample);
}
if (debugMode == DEBUG_NOTCH && axis < 2){
debug[axis*2 + 0] = gyroADC[axis];
debug[axis*2 + 1] = lrintf(sample);
}
gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample);;
gyroADC[axis] = lrintf(gyroADCf[axis]);
}
} else {

2
src/main/sensors/gyro.h
View file

@ -40,7 +40,7 @@ typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
} gyroConfig_t;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz);
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q);
void gyroSetCalibrationCycles(void);
void gyroInit(void);
void gyroUpdate(void);

25
src/main/sensors/initialisation.c
View file

@ -89,7 +89,7 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
#if defined(MPU_INT_EXTI)
static const extiConfig_t mpuIntExtiConfig = { .tag = IO_TAG(MPU_INT_EXTI) };
return &mpuIntExtiConfig;
#elif defined(USE_HARDWARE_REVISION_DETECTION)
#elif defined(USE_HARDWARE_REVISION_DETECTION)
return selectMPUIntExtiConfigByHardwareRevision();
#else
return NULL;
@ -118,9 +118,15 @@ static bool fakeGyroReadTemp(int16_t *tempData)
return true;
}
static bool fakeGyroInitStatus(void) {
return true;
}
bool fakeGyroDetect(gyro_t *gyro)
{
gyro->init = fakeGyroInit;
gyro->intStatus = fakeGyroInitStatus;
gyro->read = fakeGyroRead;
gyro->temperature = fakeGyroReadTemp;
gyro->scale = 1.0f / 16.4f;
@ -143,6 +149,7 @@ bool fakeAccDetect(acc_t *acc)
{
acc->init = fakeAccInit;
acc->read = fakeAccRead;
acc->acc_1G = 512*8;
acc->revisionCode = 0;
return true;
}
@ -584,10 +591,14 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
}
}
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator)
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
uint8_t accHardwareToUse,
uint8_t magHardwareToUse,
uint8_t baroHardwareToUse,
int16_t magDeclinationFromConfig,
uint8_t gyroLpf,
uint8_t gyroSyncDenominator)
{
int16_t deg, min;
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
@ -605,7 +616,6 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a
detectAcc(accHardwareToUse);
detectBaro(baroHardwareToUse);
// Now time to init things, acc first
if (sensors(SENSOR_ACC)) {
acc.acc_1G = 256; // set default
@ -623,9 +633,8 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a
// FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
if (sensors(SENSOR_MAG)) {
// calculate magnetic declination
deg = magDeclinationFromConfig / 100;
min = magDeclinationFromConfig % 100;
const int16_t deg = magDeclinationFromConfig / 100;
const int16_t min = magDeclinationFromConfig % 100;
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
} else {
magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.

70
src/main/target/AIR32/target.h
View file

@ -21,18 +21,14 @@
#define USE_CLI
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
#define LED0 PB5 // Blue LED - PB5
#define LED0 PB5 // Blue LED - PB5
#define BEEPER PA0
#define USABLE_TIMER_CHANNEL_COUNT 9
#define BEEPER PA0
// MPU6050 interrupts
#define USE_EXTI
#define MPU_INT_EXTI PA15
#define MPU_INT_EXTI PA15
#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready
#define USE_MPU_DATA_READY_SIGNAL
//#define ENSURE_MPU_DATA_READY_IS_LOW
@ -41,20 +37,18 @@
#define ACC
#define USE_GYRO_MPU6050
#define GYRO_MPU6050_ALIGN CW180_DEG
#define GYRO_MPU6050_ALIGN CW180_DEG
#define USE_ACC_MPU6050
#define ACC_MPU6050_ALIGN CW180_DEG
#define ACC_MPU6050_ALIGN CW180_DEG
#define USE_GYRO_SPI_MPU6000
#define GYRO_MPU6000_ALIGN CW180_DEG
#define GYRO_MPU6000_ALIGN CW180_DEG
#define USE_ACC_SPI_MPU6000
#define ACC_MPU6000_ALIGN CW180_DEG
#define ACC_MPU6000_ALIGN CW180_DEG
#define MPU6000_CS_GPIO GPIOB
#define MPU6000_CS_PIN PB12
#define MPU6000_SPI_INSTANCE SPI2
#define MPU6000_CS_PIN PB12
#define MPU6000_SPI_INSTANCE SPI2
//#define BARO
//#define USE_BARO_MS5611
@ -66,31 +60,30 @@
#define USE_UART1
#define USE_UART2
#define USE_UART3
#define SERIAL_PORT_COUNT 4
#define SERIAL_PORT_COUNT 4
#define UART1_TX_PIN PB6
#define UART1_RX_PIN PB7
#define UART1_TX_PIN PB6
#define UART1_RX_PIN PB7
#define UART2_TX_PIN PB3
#define UART2_RX_PIN PB4
#define UART2_TX_PIN PB3
#define UART2_RX_PIN PB4
#define UART3_TX_PIN PB10 //(AF7)
#define UART3_RX_PIN PB11 //(AF7)
#define UART3_TX_PIN PB10 //(AF7)
#define UART3_RX_PIN PB11 //(AF7)
#define USE_I2C
#define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4)
#define I2C2_SCL PA9
#define I2C2_SDA PA10
#define I2C2_SCL PA9
#define I2C2_SDA PA10
#define USE_SPI
#define USE_SPI_DEVICE_2
#define M25P16_CS_PIN PB12
#define M25P16_SPI_INSTANCE SPI2
#define M25P16_CS_PIN PB12
#define M25P16_SPI_INSTANCE SPI2
//#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG)
#define SENSORS_SET (SENSOR_ACC)
#define SENSORS_SET (SENSOR_ACC)
#undef GPS
#define DISPLAY
@ -99,10 +92,10 @@
#define USE_ADC
#define BOARD_HAS_VOLTAGE_DIVIDER
#define ADC_INSTANCE ADC2
#define VBAT_ADC_PIN PA5
//#define CURRENT_METER_ADC_PIN PA5
#define RSSI_ADC_PIN PB2
#define ADC_INSTANCE ADC2
#define VBAT_ADC_PIN PA5
//#define CURRENT_METER_ADC_PIN PA5
#define RSSI_ADC_PIN PB2
#define LED_STRIP
#if 1
@ -135,17 +128,18 @@
#define SPEKTRUM_BIND
// USART2, PB4
#define BIND_PIN PB4
#define BIND_PIN PB4
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
// IO - stm32f303cc in 48pin package
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15))
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15))
// #define TARGET_IO_PORTF (BIT(0)|BIT(1))
// !!TODO - check the following line is correct
#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4))
#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4))
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17))
#define USABLE_TIMER_CHANNEL_COUNT 9
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17))

11
src/main/target/ALIENFLIGHTF3/config.c
View file

@ -84,9 +84,14 @@ void targetConfiguration(void) {
masterConfig.motor_pwm_rate = 32000;
masterConfig.failsafeConfig.failsafe_delay = 2;
masterConfig.failsafeConfig.failsafe_off_delay = 0;
currentControlRateProfile->rates[FD_PITCH] = 40;
currentControlRateProfile->rates[FD_ROLL] = 40;
currentControlRateProfile->rates[FD_YAW] = 40;
masterConfig.gyro_sync_denom = 2;
masterConfig.pid_process_denom = 1;
currentProfile->pidProfile.P8[ROLL] = 90;
currentProfile->pidProfile.I8[ROLL] = 44;
currentProfile->pidProfile.D8[ROLL] = 60;
currentProfile->pidProfile.P8[PITCH] = 90;
currentProfile->pidProfile.I8[PITCH] = 44;
currentProfile->pidProfile.D8[PITCH] = 60;
parseRcChannels("TAER1234", &masterConfig.rxConfig);
masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R

2
src/main/target/ALIENFLIGHTF3/hardware_revision.h
View file

@ -15,7 +15,7 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "drivers/exti.h"
typedef enum awf3HardwareRevision_t {

68
src/main/target/ALIENFLIGHTF3/target.h
View file

@ -23,17 +23,17 @@
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
#define USE_HARDWARE_REVISION_DETECTION
#define HW_PIN PB2
#define HW_PIN PB2
// LED's V1
#define LED0 PB4 // LED - PB4
#define LED1 PB5 // LED - PB5
#define LED0 PB4
#define LED1 PB5
// LED's V2
#define LED0_A PB8 // LED - PB8
#define LED1_A PB9 // LED - PB9
#define LED0_A PB8
#define LED1_A PB9
#define BEEPER PA5 // LED - PA5
#define BEEPER PA5
#define USABLE_TIMER_CHANNEL_COUNT 11
@ -48,15 +48,15 @@
#define USE_GYRO_MPU6050
#define USE_GYRO_SPI_MPU6500
#define GYRO_MPU6050_ALIGN CW270_DEG
#define GYRO_MPU6500_ALIGN CW270_DEG
#define GYRO_MPU6050_ALIGN CW270_DEG
#define GYRO_MPU6500_ALIGN CW270_DEG
#define ACC
#define USE_ACC_MPU6050
#define USE_ACC_SPI_MPU6500
#define ACC_MPU6050_ALIGN CW270_DEG
#define ACC_MPU6500_ALIGN CW270_DEG
#define ACC_MPU6050_ALIGN CW270_DEG
#define ACC_MPU6500_ALIGN CW270_DEG
// No baro support.
//#define BARO
@ -65,29 +65,28 @@
// option to use MPU9150 or MPU9250 integrated AK89xx Mag
#define MAG
#define USE_MAG_AK8963
#define MAG_AK8963_ALIGN CW0_DEG_FLIP
#define MAG_AK8963_ALIGN CW0_DEG_FLIP
#define USE_VCP
#define USE_UART1 // Not connected - TX (PB6) RX PB7 (AF7)
#define USE_UART2 // Receiver - RX (PA3)
#define USE_UART3 // Not connected - 10/RX (PB11) 11/TX (PB10)
#define SERIAL_PORT_COUNT 4
#define SERIAL_PORT_COUNT 4
#define UART1_TX_PIN PB6 // PB6
#define UART1_RX_PIN PB7 // PB7
#define UART1_TX_PIN PB6
#define UART1_RX_PIN PB7
#define UART2_TX_PIN PA2 // PA2
#define UART2_RX_PIN PA3 // PA3
#define UART2_TX_PIN PA2
#define UART2_RX_PIN PA3
#define UART3_TX_PIN PB10 // PB10 (AF7)
#define UART3_RX_PIN PB11 // PB11 (AF7)
#define UART3_TX_PIN PB10
#define UART3_RX_PIN PB11
#define USE_I2C
#define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4)
#define I2C2_SCL PA9
#define I2C2_SDA PA10
#define I2C2_SCL PA9
#define I2C2_SDA PA10
// SPI3
// PA15 38 SPI3_NSS
@ -98,22 +97,22 @@
#define USE_SPI
#define USE_SPI_DEVICE_3
#define MPU6500_CS_PIN PA15
#define MPU6500_SPI_INSTANCE SPI3
#define MPU6500_CS_PIN PA15
#define MPU6500_SPI_INSTANCE SPI3
#define USE_ADC
#define ADC_INSTANCE ADC2
#define VBAT_ADC_PIN PA4
#define VBAT_SCALE_DEFAULT 20
#define ADC_INSTANCE ADC2
#define VBAT_ADC_PIN PA4
#define VBAT_SCALE_DEFAULT 20
#define SPEKTRUM_BIND
// USART2, PA3
#define BIND_PIN PA3
#define BIND_PIN PA3
#define HARDWARE_BIND_PLUG
// Hardware bind plug at PB12 (Pin 25)
#define BINDPLUG_PIN PB12
#define BINDPLUG_PIN PB12
#define BRUSHED_MOTORS
#define DEFAULT_FEATURES FEATURE_MOTOR_STOP
@ -124,12 +123,11 @@
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
// IO - assuming 303 in 64pin package, TODO
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))
#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4))
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))
#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4))
#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17) )
#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17) )

11
src/main/target/ALIENFLIGHTF4/config.c
View file

@ -84,9 +84,14 @@ void targetConfiguration(void) {
masterConfig.motor_pwm_rate = 32000;
masterConfig.failsafeConfig.failsafe_delay = 2;
masterConfig.failsafeConfig.failsafe_off_delay = 0;
currentControlRateProfile->rates[FD_PITCH] = 40;
currentControlRateProfile->rates[FD_ROLL] = 40;
currentControlRateProfile->rates[FD_YAW] = 40;
masterConfig.gyro_sync_denom = 1;
masterConfig.pid_process_denom = 1;
currentProfile->pidProfile.P8[ROLL] = 90;
currentProfile->pidProfile.I8[ROLL] = 44;
currentProfile->pidProfile.D8[ROLL] = 60;
currentProfile->pidProfile.P8[PITCH] = 90;
currentProfile->pidProfile.I8[PITCH] = 44;
currentProfile->pidProfile.D8[PITCH] = 60;
parseRcChannels("TAER1234", &masterConfig.rxConfig);
masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R

105
src/main/target/ALIENFLIGHTF4/target.h
View file

@ -23,39 +23,39 @@
#define USBD_PRODUCT_STRING "AlienFlight F4"
#define LED0 PC12
#define LED1 PD2
#define LED0 PC12
#define LED1 PD2
#define BEEPER PC13
#define BEEPER PC13
#define INVERTER PC15
#define INVERTER_USART USART2
#define INVERTER PC15
#define INVERTER_USART USART2
// MPU interrupt
//#define DEBUG_MPU_DATA_READY_INTERRUPT
#define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW
#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready
#define MPU_INT_EXTI PC14
#define MPU_INT_EXTI PC14
#define USE_EXTI
#define MPU6500_CS_PIN PA4
#define MPU6500_SPI_INSTANCE SPI1
#define MPU6500_CS_PIN PA4
#define MPU6500_SPI_INSTANCE SPI1
#define ACC
#define USE_ACC_SPI_MPU6500
#define ACC_MPU6500_ALIGN CW270_DEG
#define ACC_MPU6500_ALIGN CW270_DEG
#define GYRO
#define USE_GYRO_SPI_MPU6500
#define GYRO_MPU6500_ALIGN CW270_DEG
#define GYRO_MPU6500_ALIGN CW270_DEG
#define MAG
#define USE_MAG_HMC5883
#define USE_MAG_AK8963
#define MAG_HMC5883_ALIGN CW180_DEG
#define MAG_AK8963_ALIGN CW270_DEG
#define MAG_HMC5883_ALIGN CW180_DEG
#define MAG_AK8963_ALIGN CW270_DEG
#define BARO
#define USE_BARO_MS5611
@ -65,14 +65,14 @@
//#define SDCARD_DETECT_INVERTED
#define SDCARD_DETECT_PIN PB10
#define SDCARD_DETECT_EXTI_LINE EXTI_Line10
#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource10
#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB
#define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn
#define SDCARD_DETECT_PIN PB10
#define SDCARD_DETECT_EXTI_LINE EXTI_Line10
#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource10
#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB
#define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn
#define SDCARD_SPI_INSTANCE SPI2
#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN
#define SDCARD_SPI_INSTANCE SPI2
#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN
// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init:
#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz
@ -93,57 +93,53 @@
//#define USE_FLASHFS
//#define USE_FLASH_M25P16
#define USABLE_TIMER_CHANNEL_COUNT 13
#define USE_VCP
#define USE_UART1
#define UART1_RX_PIN PA10
#define UART1_TX_PIN PA9
#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2
#define UART1_RX_PIN PA10
#define UART1_TX_PIN PA9
#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2
#define USE_UART2
#define UART2_RX_PIN PA3
#define UART2_TX_PIN PA2 //inverter
#define UART2_RX_PIN PA3
#define UART2_TX_PIN PA2 //inverter
//#define USE_UART3
//#define UART3_RX_PIN PB11
//#define UART3_TX_PIN PB10
//#define UART3_RX_PIN PB11
//#define UART3_TX_PIN PB10
#define USE_UART4
#define UART4_RX_PIN PC10
#define UART4_TX_PIN PC11
#define UART4_RX_PIN PC10
#define UART4_TX_PIN PC11
//#define USE_UART5
//#define UART5_RX_PIN PD2
//#define UART5_TX_PIN PC12
//#define UART5_RX_PIN PD2
//#define UART5_TX_PIN PC12
#define SERIAL_PORT_COUNT 4
#define SERIAL_PORT_COUNT 4
#define USE_SPI
#define USE_SPI_DEVICE_1
#define USE_SPI_DEVICE_2
#define SPI2_NSS_PIN PB12
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PC2
#define SPI2_MOSI_PIN PC3
#define SPI2_NSS_PIN PB12
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PC2
#define SPI2_MOSI_PIN PC3
#define USE_SPI_DEVICE_3
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1)
//#define I2C_DEVICE_EXT (I2CDEV_2)
#define I2C1_SCL PB6
#define I2C1_SDA PB7
#define I2C_DEVICE (I2CDEV_1)
//#define I2C_DEVICE_EXT (I2CDEV_2)
#define I2C1_SCL PB6
#define I2C1_SDA PB7
#define USE_ADC
//#define BOARD_HAS_VOLTAGE_DIVIDER
#define VBAT_ADC_PIN PC0
#define CURRENT_METER_ADC_PIN PC1
#define RSSI_ADC_PIN PC4
#define EXTERNAL1_ADC_GPIO_PIN PC5
#define VBAT_ADC_PIN PC0
#define CURRENT_METER_ADC_PIN PC1
#define RSSI_ADC_PIN PC4
#define EXTERNAL1_ADC_GPIO_PIN PC5
// LED strip configuration using RC5 pin.
//#define LED_STRIP
@ -156,11 +152,11 @@
#define SPEKTRUM_BIND
// USART2, PA3
#define BIND_PIN PA3
#define BIND_PIN PA3
#define HARDWARE_BIND_PLUG
// Hardware bind plug at PB2 (Pin 28)
#define BINDPLUG_PIN PB2
#define BINDPLUG_PIN PB2
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
@ -172,10 +168,11 @@
#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 TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))
#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) )
#define USABLE_TIMER_CHANNEL_COUNT 13
#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) )

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

@ -33,7 +33,7 @@ const uint16_t multiPPM[] = {
};
const uint16_t multiPWM[] = {
PWM1 | (MAP_TO_PWM_INPUT << 8),
PWM1 | (MAP_TO_PWM_INPUT << 8),
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8),
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8),

65
src/main/target/BLUEJAYF4/target.h
View file

@ -20,40 +20,40 @@
#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8)
#define USBD_PRODUCT_STRING "BlueJayF4"
#define USBD_PRODUCT_STRING "BlueJayF4"
#define BOARD_HAS_VOLTAGE_DIVIDER
#define USE_EXTI
#define INVERTER PB15
#define INVERTER_USART USART6
#define LED0 PB6
#define LED1 PB5
#define LED2 PB4
#define BEEPER PB7
#define BEEPER PB7
#define BEEPER_INVERTED
#define LED0 PB6
#define LED1 PB5
#define LED2 PB4
#define INVERTER PB15
#define INVERTER_USART USART6
#define MPU6500_CS_PIN PC4
#define MPU6500_SPI_INSTANCE SPI1
#define MPU6500_CS_PIN PC4
#define MPU6500_SPI_INSTANCE SPI1
#define ACC
#define USE_ACC_MPU6500
#define USE_ACC_SPI_MPU6500
#define ACC_MPU6500_ALIGN CW180_DEG
#define ACC_MPU6500_ALIGN CW180_DEG
#define GYRO
#define USE_GYRO_MPU6500
#define USE_GYRO_SPI_MPU6500
#define GYRO_MPU6500_ALIGN CW180_DEG
#define GYRO_MPU6500_ALIGN CW180_DEG
//#define MAG
//#define USE_MAG_AK8963
#define BARO
#define USE_BARO_MS5611
#define MS5611_I2C_INSTANCE I2CDEV_1
#define MS5611_I2C_INSTANCE I2CDEV_1
#define USE_SDCARD
@ -76,8 +76,8 @@
// Performance logging for SD card operations:
// #define AFATFS_USE_INTROSPECTIVE_LOGGING
//#define M25P16_CS_PIN PB3
//#define M25P16_SPI_INSTANCE SPI3
//#define M25P16_CS_PIN PB3
//#define M25P16_SPI_INSTANCE SPI3
//#define USE_FLASHFS
//#define USE_FLASH_M25P16
@ -96,22 +96,21 @@
//#define VBUS_SENSING_ENABLED
#define USE_UART1
#define UART1_RX_PIN PA10
#define UART1_TX_PIN PA9
#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2
#define UART1_RX_PIN PA10
#define UART1_TX_PIN PA9
#define USE_UART3
#define UART3_RX_PIN PB11
#define UART3_TX_PIN PB10
#define UART3_RX_PIN PB11
#define UART3_TX_PIN PB10
#define USE_UART6
#define UART6_RX_PIN PC7
#define UART6_TX_PIN PC6
#define UART6_RX_PIN PC7
#define UART6_TX_PIN PC6
#define USE_SOFTSERIAL1
#define SERIAL_PORT_COUNT 5
#define SERIAL_PORT_COUNT 5
#define SOFTSERIAL_1_TIMER TIM3
#define SOFTSERIAL_1_TIMER TIM3
#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5
#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6
@ -134,15 +133,15 @@
#define SPI3_MOSI_PIN PC12
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1)
#define I2C_DEVICE (I2CDEV_1)
#define USE_I2C_PULLUP
#define USE_ADC
#define VBAT_ADC_PIN PC3
#define VBAT_ADC_PIN PC3
#define LED_STRIP
// LED Strip can run off Pin 6 (PB1) of the ESC outputs.
#define WS2811_PIN PB1
#define WS2811_PIN PB1
#define WS2811_TIMER TIM3
#define WS2811_TIMER_CHANNEL TIM_Channel_4
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER
@ -155,15 +154,15 @@
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define DEFAULT_FEATURES FEATURE_BLACKBOX
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#define DEFAULT_FEATURES FEATURE_BLACKBOX
#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 TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))
#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(9))
#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(9))

Some files were not shown because too many files have changed in this diff Show more