format code properly
match the comment from pullrequest about spacing remains : some hand alignment for comment and wrong /** */ usage. Conflicts: src/board.h src/buzzer.c src/config.c src/drivers/serial_common.h src/drivers/system_common.c src/drv_gpio.h src/drv_pwm.c src/drv_timer.c src/drv_uart.c src/flight_imu.c src/mw.c src/serial_cli.c
This commit is contained in:
parent
ab2273f93e
commit
cabc57774c
|
@ -113,7 +113,7 @@ void beep_code(char first, char second, char third, char pause)
|
||||||
patternChar[1] = second;
|
patternChar[1] = second;
|
||||||
patternChar[2] = third;
|
patternChar[2] = third;
|
||||||
patternChar[3] = pause;
|
patternChar[3] = pause;
|
||||||
switch(patternChar[icnt]) {
|
switch (patternChar[icnt]) {
|
||||||
case 'M':
|
case 'M':
|
||||||
Duration = 100;
|
Duration = 100;
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -93,18 +93,18 @@ char a2i(char ch, char **src, int base, int *nump)
|
||||||
#ifndef HAVE_ITOA_FUNCTION
|
#ifndef HAVE_ITOA_FUNCTION
|
||||||
|
|
||||||
/*
|
/*
|
||||||
** The following two functions together make up an itoa()
|
** The following two functions together make up an itoa()
|
||||||
** implementation. Function i2a() is a 'private' function
|
** implementation. Function i2a() is a 'private' function
|
||||||
** called by the public itoa() function.
|
** called by the public itoa() function.
|
||||||
**
|
**
|
||||||
** itoa() takes three arguments:
|
** itoa() takes three arguments:
|
||||||
** 1) the integer to be converted,
|
** 1) the integer to be converted,
|
||||||
** 2) a pointer to a character conversion buffer,
|
** 2) a pointer to a character conversion buffer,
|
||||||
** 3) the radix for the conversion
|
** 3) the radix for the conversion
|
||||||
** which can range between 2 and 36 inclusive
|
** which can range between 2 and 36 inclusive
|
||||||
** range errors on the radix default it to base10
|
** range errors on the radix default it to base10
|
||||||
** Code from http://groups.google.com/group/comp.lang.c/msg/66552ef8b04fe1ab?pli=1
|
** Code from http://groups.google.com/group/comp.lang.c/msg/66552ef8b04fe1ab?pli=1
|
||||||
*/
|
*/
|
||||||
|
|
||||||
static char *_i2a(unsigned i, char *a, unsigned r)
|
static char *_i2a(unsigned i, char *a, unsigned r)
|
||||||
{
|
{
|
||||||
|
@ -120,7 +120,7 @@ char *itoa(int i, char *a, int r)
|
||||||
r = 10;
|
r = 10;
|
||||||
if (i < 0) {
|
if (i < 0) {
|
||||||
*a = '-';
|
*a = '-';
|
||||||
*_i2a(-(unsigned)i, a + 1, r) = 0;
|
*_i2a(-(unsigned) i, a + 1, r) = 0;
|
||||||
} else
|
} else
|
||||||
*_i2a(i, a, r) = 0;
|
*_i2a(i, a, r) = 0;
|
||||||
return a;
|
return a;
|
||||||
|
@ -141,7 +141,7 @@ char *ftoa(float x, char *floatString)
|
||||||
else
|
else
|
||||||
x -= 0.0005f;
|
x -= 0.0005f;
|
||||||
|
|
||||||
value = (int32_t) (x * 1000.0f); // Convert float * 1000 to an integer
|
value = (int32_t)(x * 1000.0f); // Convert float * 1000 to an integer
|
||||||
|
|
||||||
itoa(abs(value), intString1, 10); // Create string from abs of integer value
|
itoa(abs(value), intString1, 10); // Create string from abs of integer value
|
||||||
|
|
||||||
|
@ -176,7 +176,6 @@ char *ftoa(float x, char *floatString)
|
||||||
return floatString;
|
return floatString;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// Simple and fast atof (ascii to float) function.
|
// Simple and fast atof (ascii to float) function.
|
||||||
//
|
//
|
||||||
// - Executes about 5x faster than standard MSCRT library atof().
|
// - Executes about 5x faster than standard MSCRT library atof().
|
||||||
|
@ -194,7 +193,7 @@ float fastA2F(const char *p)
|
||||||
float sign, value, scale;
|
float sign, value, scale;
|
||||||
|
|
||||||
// Skip leading white space, if any.
|
// Skip leading white space, if any.
|
||||||
while (white_space(*p) ) {
|
while (white_space(*p)) {
|
||||||
p += 1;
|
p += 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -254,8 +253,14 @@ float fastA2F(const char *p)
|
||||||
|
|
||||||
// Calculate scaling factor.
|
// Calculate scaling factor.
|
||||||
// while (expon >= 50) { scale *= 1E50f; expon -= 50; }
|
// while (expon >= 50) { scale *= 1E50f; expon -= 50; }
|
||||||
while (expon >= 8) { scale *= 1E8f; expon -= 8; }
|
while (expon >= 8) {
|
||||||
while (expon > 0) { scale *= 10.0f; expon -= 1; }
|
scale *= 1E8f;
|
||||||
|
expon -= 8;
|
||||||
|
}
|
||||||
|
while (expon > 0) {
|
||||||
|
scale *= 10.0f;
|
||||||
|
expon -= 1;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return signed and scaled floating point result.
|
// Return signed and scaled floating point result.
|
||||||
|
|
59
src/config.c
59
src/config.c
|
@ -37,22 +37,21 @@
|
||||||
#include "config_master.h"
|
#include "config_master.h"
|
||||||
|
|
||||||
void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h
|
void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h
|
||||||
void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfig, gimbalConfig_t *gimbalConfigToUse);
|
void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse,
|
||||||
|
escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse,
|
||||||
|
airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfig, gimbalConfig_t *gimbalConfigToUse);
|
||||||
|
|
||||||
#ifndef FLASH_PAGE_COUNT
|
#ifndef FLASH_PAGE_COUNT
|
||||||
#define FLASH_PAGE_COUNT 128
|
#define FLASH_PAGE_COUNT 128
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define FLASH_PAGE_SIZE ((uint16_t)0x400)
|
#define FLASH_PAGE_SIZE ((uint16_t)0x400)
|
||||||
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 2)) // use the last 2 KB for storage
|
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 2)) // use the last 2 KB for storage
|
||||||
|
master_t masterConfig; // master config struct with data independent from profiles
|
||||||
master_t masterConfig; // master config struct with data independent from profiles
|
|
||||||
profile_t currentProfile; // profile config struct
|
profile_t currentProfile; // profile config struct
|
||||||
|
|
||||||
static const uint8_t EEPROM_CONF_VERSION = 66;
|
static const uint8_t EEPROM_CONF_VERSION = 66;
|
||||||
|
|
||||||
|
|
||||||
static void resetAccelerometerTrims(int16_flightDynamicsTrims_t *accelerometerTrims)
|
static void resetAccelerometerTrims(int16_flightDynamicsTrims_t *accelerometerTrims)
|
||||||
{
|
{
|
||||||
accelerometerTrims->trims.pitch = 0;
|
accelerometerTrims->trims.pitch = 0;
|
||||||
|
@ -168,10 +167,10 @@ static void resetConf(void)
|
||||||
featureSet(FEATURE_VBAT);
|
featureSet(FEATURE_VBAT);
|
||||||
|
|
||||||
// global settings
|
// global settings
|
||||||
masterConfig.current_profile_index = 0; // default profile
|
masterConfig.current_profile_index = 0; // default profile
|
||||||
masterConfig.gyro_cmpf_factor = 600; // default MWC
|
masterConfig.gyro_cmpf_factor = 600; // default MWC
|
||||||
masterConfig.gyro_cmpfm_factor = 250; // default MWC
|
masterConfig.gyro_cmpfm_factor = 250; // default MWC
|
||||||
masterConfig.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
|
masterConfig.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
|
||||||
|
|
||||||
resetAccelerometerTrims(&masterConfig.accZero);
|
resetAccelerometerTrims(&masterConfig.accZero);
|
||||||
|
|
||||||
|
@ -195,7 +194,7 @@ static void resetConf(void)
|
||||||
masterConfig.rxConfig.midrc = 1500;
|
masterConfig.rxConfig.midrc = 1500;
|
||||||
masterConfig.rxConfig.mincheck = 1100;
|
masterConfig.rxConfig.mincheck = 1100;
|
||||||
masterConfig.rxConfig.maxcheck = 1900;
|
masterConfig.rxConfig.maxcheck = 1900;
|
||||||
masterConfig.retarded_arm = 0; // disable arm/disarm on roll left/right
|
masterConfig.retarded_arm = 0; // disable arm/disarm on roll left/right
|
||||||
masterConfig.airplaneConfig.flaps_speed = 0;
|
masterConfig.airplaneConfig.flaps_speed = 0;
|
||||||
masterConfig.fixedwing_althold_dir = 1;
|
masterConfig.fixedwing_althold_dir = 1;
|
||||||
|
|
||||||
|
@ -250,10 +249,10 @@ static void resetConf(void)
|
||||||
currentProfile.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv
|
currentProfile.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv
|
||||||
|
|
||||||
// Failsafe Variables
|
// Failsafe Variables
|
||||||
currentProfile.failsafeConfig.failsafe_delay = 10; // 1sec
|
currentProfile.failsafeConfig.failsafe_delay = 10; // 1sec
|
||||||
currentProfile.failsafeConfig.failsafe_off_delay = 200; // 20sec
|
currentProfile.failsafeConfig.failsafe_off_delay = 200; // 20sec
|
||||||
currentProfile.failsafeConfig.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
|
currentProfile.failsafeConfig.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
|
||||||
currentProfile.failsafeConfig.failsafe_detect_threshold = 985; // any of first 4 channels below this value will trigger failsafe
|
currentProfile.failsafeConfig.failsafe_detect_threshold = 985; // any of first 4 channels below this value will trigger failsafe
|
||||||
|
|
||||||
// servos
|
// servos
|
||||||
for (i = 0; i < 8; i++) {
|
for (i = 0; i < 8; i++) {
|
||||||
|
@ -293,7 +292,7 @@ static uint8_t calculateChecksum(const uint8_t *data, uint32_t length)
|
||||||
|
|
||||||
static bool isEEPROMContentValid(void)
|
static bool isEEPROMContentValid(void)
|
||||||
{
|
{
|
||||||
const master_t *temp = (const master_t *)FLASH_WRITE_ADDR;
|
const master_t *temp = (const master_t *) FLASH_WRITE_ADDR;
|
||||||
uint8_t checksum = 0;
|
uint8_t checksum = 0;
|
||||||
|
|
||||||
// check version number
|
// check version number
|
||||||
|
@ -305,7 +304,7 @@ static bool isEEPROMContentValid(void)
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
// verify integrity of temporary copy
|
// verify integrity of temporary copy
|
||||||
checksum = calculateChecksum((const uint8_t *)temp, sizeof(master_t));
|
checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t));
|
||||||
if (checksum != 0)
|
if (checksum != 0)
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
|
@ -326,21 +325,22 @@ void activateConfig(void)
|
||||||
useFailsafeConfig(¤tProfile.failsafeConfig);
|
useFailsafeConfig(¤tProfile.failsafeConfig);
|
||||||
setAccelerationTrims(&masterConfig.accZero);
|
setAccelerationTrims(&masterConfig.accZero);
|
||||||
mixerUseConfigs(
|
mixerUseConfigs(
|
||||||
currentProfile.servoConf,
|
currentProfile.servoConf,
|
||||||
&masterConfig.flight3DConfig,
|
&masterConfig.flight3DConfig,
|
||||||
&masterConfig.escAndServoConfig,
|
&masterConfig.escAndServoConfig,
|
||||||
¤tProfile.mixerConfig,
|
¤tProfile.mixerConfig,
|
||||||
&masterConfig.airplaneConfig,
|
&masterConfig.airplaneConfig,
|
||||||
&masterConfig.rxConfig,
|
&masterConfig.rxConfig,
|
||||||
¤tProfile.gimbalConfig
|
¤tProfile.gimbalConfig
|
||||||
);
|
);
|
||||||
|
|
||||||
#ifdef BARO
|
#ifdef BARO
|
||||||
useBarometerConfig(¤tProfile.barometerConfig);
|
useBarometerConfig(¤tProfile.barometerConfig);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void validateAndFixConfig(void) {
|
void validateAndFixConfig(void)
|
||||||
|
{
|
||||||
if (!(feature(FEATURE_PARALLEL_PWM) || feature(FEATURE_PPM) || feature(FEATURE_SERIALRX))) {
|
if (!(feature(FEATURE_PARALLEL_PWM) || feature(FEATURE_PPM) || feature(FEATURE_SERIALRX))) {
|
||||||
featureSet(FEATURE_PARALLEL_PWM); // Consider changing the default to PPM
|
featureSet(FEATURE_PARALLEL_PWM); // Consider changing the default to PPM
|
||||||
}
|
}
|
||||||
|
@ -390,7 +390,7 @@ void readEEPROM(void)
|
||||||
failureMode(10);
|
failureMode(10);
|
||||||
|
|
||||||
// Read flash
|
// Read flash
|
||||||
memcpy(&masterConfig, (char *)FLASH_WRITE_ADDR, sizeof(master_t));
|
memcpy(&masterConfig, (char *) FLASH_WRITE_ADDR, sizeof(master_t));
|
||||||
// Copy current profile
|
// Copy current profile
|
||||||
if (masterConfig.current_profile_index > 2) // sanity check
|
if (masterConfig.current_profile_index > 2) // sanity check
|
||||||
masterConfig.current_profile_index = 0;
|
masterConfig.current_profile_index = 0;
|
||||||
|
@ -425,7 +425,7 @@ void writeEEPROM(void)
|
||||||
masterConfig.magic_be = 0xBE;
|
masterConfig.magic_be = 0xBE;
|
||||||
masterConfig.magic_ef = 0xEF;
|
masterConfig.magic_ef = 0xEF;
|
||||||
masterConfig.chk = 0; // erase checksum before recalculating
|
masterConfig.chk = 0; // erase checksum before recalculating
|
||||||
masterConfig.chk = calculateChecksum((const uint8_t *)&masterConfig, sizeof(master_t));
|
masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t));
|
||||||
|
|
||||||
// write it
|
// write it
|
||||||
FLASH_Unlock();
|
FLASH_Unlock();
|
||||||
|
@ -438,7 +438,8 @@ void writeEEPROM(void)
|
||||||
#endif
|
#endif
|
||||||
status = FLASH_ErasePage(FLASH_WRITE_ADDR);
|
status = FLASH_ErasePage(FLASH_WRITE_ADDR);
|
||||||
for (wordOffset = 0; wordOffset < sizeof(master_t) && status == FLASH_COMPLETE; wordOffset += 4) {
|
for (wordOffset = 0; wordOffset < sizeof(master_t) && status == FLASH_COMPLETE; wordOffset += 4) {
|
||||||
status = FLASH_ProgramWord(FLASH_WRITE_ADDR + wordOffset, *(uint32_t *) ((char *)&masterConfig + wordOffset));
|
status = FLASH_ProgramWord(FLASH_WRITE_ADDR + wordOffset,
|
||||||
|
*(uint32_t *) ((char *) &masterConfig + wordOffset));
|
||||||
}
|
}
|
||||||
if (status == FLASH_COMPLETE) {
|
if (status == FLASH_COMPLETE) {
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -66,7 +66,7 @@ bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc)
|
||||||
|
|
||||||
static void adxl345Init(void)
|
static void adxl345Init(void)
|
||||||
{
|
{
|
||||||
if (useFifo) {
|
if (useFifo) {
|
||||||
uint8_t fifoDepth = 16;
|
uint8_t fifoDepth = 16;
|
||||||
i2cWrite(ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS);
|
i2cWrite(ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS);
|
||||||
i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G);
|
i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G);
|
||||||
|
|
|
@ -2,19 +2,17 @@
|
||||||
|
|
||||||
extern uint16_t acc_1G;
|
extern uint16_t acc_1G;
|
||||||
|
|
||||||
typedef void (* sensorInitFuncPtr)(void); // sensor init prototype
|
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
|
||||||
typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
|
typedef void (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
|
||||||
|
|
||||||
typedef struct gyro_s
|
typedef struct gyro_s {
|
||||||
{
|
|
||||||
sensorInitFuncPtr init; // initialize function
|
sensorInitFuncPtr init; // initialize function
|
||||||
sensorReadFuncPtr read; // read 3 axis data function
|
sensorReadFuncPtr read; // read 3 axis data function
|
||||||
sensorReadFuncPtr temperature; // read temperature if available
|
sensorReadFuncPtr temperature; // read temperature if available
|
||||||
float scale; // scalefactor
|
float scale; // scalefactor
|
||||||
} gyro_t;
|
} gyro_t;
|
||||||
|
|
||||||
typedef struct acc_s
|
typedef struct acc_s {
|
||||||
{
|
|
||||||
sensorInitFuncPtr init; // initialize function
|
sensorInitFuncPtr init; // initialize function
|
||||||
sensorReadFuncPtr read; // read 3 axis data function
|
sensorReadFuncPtr read; // read 3 axis data function
|
||||||
char revisionCode; // a revision code for the sensor, if known
|
char revisionCode; // a revision code for the sensor, if known
|
||||||
|
|
|
@ -39,7 +39,7 @@ typedef struct {
|
||||||
int16_t md;
|
int16_t md;
|
||||||
} bmp085_smd500_calibration_param_t;
|
} bmp085_smd500_calibration_param_t;
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
bmp085_smd500_calibration_param_t cal_param;
|
bmp085_smd500_calibration_param_t cal_param;
|
||||||
uint8_t mode;
|
uint8_t mode;
|
||||||
uint8_t chip_id, ml_version, al_version;
|
uint8_t chip_id, ml_version, al_version;
|
||||||
|
@ -198,9 +198,9 @@ static int32_t bmp085_get_pressure(uint32_t up)
|
||||||
|
|
||||||
// *****calculate B4************
|
// *****calculate B4************
|
||||||
x1 = (bmp085.cal_param.ac3 * b6) >> 13;
|
x1 = (bmp085.cal_param.ac3 * b6) >> 13;
|
||||||
x2 = (bmp085.cal_param.b1 * ((b6 * b6) >> 12) ) >> 16;
|
x2 = (bmp085.cal_param.b1 * ((b6 * b6) >> 12)) >> 16;
|
||||||
x3 = ((x1 + x2) + 2) >> 2;
|
x3 = ((x1 + x2) + 2) >> 2;
|
||||||
b4 = (bmp085.cal_param.ac4 * (uint32_t) (x3 + 32768)) >> 15;
|
b4 = (bmp085.cal_param.ac4 * (uint32_t)(x3 + 32768)) >> 15;
|
||||||
|
|
||||||
b7 = ((uint32_t)(up - b3) * (50000 >> bmp085.oversampling_setting));
|
b7 = ((uint32_t)(up - b3) * (50000 >> bmp085.oversampling_setting));
|
||||||
if (b7 < 0x80000000) {
|
if (b7 < 0x80000000) {
|
||||||
|
@ -258,7 +258,7 @@ static void bmp085_get_up(void)
|
||||||
convOverrun++;
|
convOverrun++;
|
||||||
|
|
||||||
i2cRead(BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 3, data);
|
i2cRead(BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 3, data);
|
||||||
bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2]) >> (8 - bmp085.oversampling_setting);
|
bmp085_up = (((uint32_t)data[0] << 16) | ((uint32_t)data[1] << 8) | (uint32_t)data[2]) >> (8 - bmp085.oversampling_setting);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void bmp085_calculate(int32_t *pressure, int32_t *temperature)
|
static void bmp085_calculate(int32_t *pressure, int32_t *temperature)
|
||||||
|
|
|
@ -1,10 +1,9 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
typedef void (* baroOpFuncPtr)(void); // baro start operation
|
typedef void (*baroOpFuncPtr)(void); // baro start operation
|
||||||
typedef void (* baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature)
|
typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature)
|
||||||
|
|
||||||
typedef struct baro_t
|
typedef struct baro_t {
|
||||||
{
|
|
||||||
uint16_t ut_delay;
|
uint16_t ut_delay;
|
||||||
uint16_t up_delay;
|
uint16_t up_delay;
|
||||||
baroOpFuncPtr start_ut;
|
baroOpFuncPtr start_ut;
|
||||||
|
|
|
@ -165,11 +165,11 @@ static void ms5611_calculate(int32_t *pressure, int32_t *temperature)
|
||||||
{
|
{
|
||||||
uint32_t press;
|
uint32_t press;
|
||||||
int64_t temp;
|
int64_t temp;
|
||||||
int64_t delt;
|
int64_t delt;
|
||||||
int32_t dT = (int64_t)ms5611_ut - ((uint64_t)ms5611_c[5] * 256);
|
int32_t dT = (int64_t)ms5611_ut - ((uint64_t)ms5611_c[5] * 256);
|
||||||
int64_t off = ((int64_t)ms5611_c[2] << 16) + (((int64_t)ms5611_c[4] * dT) >> 7);
|
int64_t off = ((int64_t)ms5611_c[2] << 16) + (((int64_t)ms5611_c[4] * dT) >> 7);
|
||||||
int64_t sens = ((int64_t)ms5611_c[1] << 15) + (((int64_t)ms5611_c[3] * dT) >> 8);
|
int64_t sens = ((int64_t)ms5611_c[1] << 15) + (((int64_t)ms5611_c[3] * dT) >> 8);
|
||||||
temp = 2000 + ((dT * (int64_t)ms5611_c[6]) >> 23);
|
temp = 2000 + ((dT * (int64_t)ms5611_c[6]) >> 23);
|
||||||
|
|
||||||
if (temp < 2000) { // temperature lower than 20degC
|
if (temp < 2000) { // temperature lower than 20degC
|
||||||
delt = temp - 2000;
|
delt = temp - 2000;
|
||||||
|
@ -179,11 +179,11 @@ static void ms5611_calculate(int32_t *pressure, int32_t *temperature)
|
||||||
if (temp < -1500) { // temperature lower than -15degC
|
if (temp < -1500) { // temperature lower than -15degC
|
||||||
delt = temp + 1500;
|
delt = temp + 1500;
|
||||||
delt = delt * delt;
|
delt = delt * delt;
|
||||||
off -= 7 * delt;
|
off -= 7 * delt;
|
||||||
sens -= (11 * delt) >> 1;
|
sens -= (11 * delt) >> 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
press = ((((int64_t)ms5611_up * sens ) >> 21) - off) >> 15;
|
press = ((((int64_t)ms5611_up * sens) >> 21) - off) >> 15;
|
||||||
|
|
||||||
if (pressure)
|
if (pressure)
|
||||||
*pressure = press;
|
*pressure = press;
|
||||||
|
|
|
@ -77,7 +77,7 @@ void gpioPinRemapConfig(uint32_t remap, bool enable)
|
||||||
}
|
}
|
||||||
|
|
||||||
if (enable)
|
if (enable)
|
||||||
tmpreg |= (tmp << ((remap >> 0x15)*0x10));
|
tmpreg |= (tmp << ((remap >> 0x15) * 0x10));
|
||||||
|
|
||||||
if ((remap & 0x80000000) == 0x80000000)
|
if ((remap & 0x80000000) == 0x80000000)
|
||||||
AFIO->MAPR2 = tmpreg;
|
AFIO->MAPR2 = tmpreg;
|
||||||
|
|
|
@ -56,7 +56,7 @@ static struct PWM_State {
|
||||||
uint16_t capture;
|
uint16_t capture;
|
||||||
} Inputs[8] = { { 0, } };
|
} Inputs[8] = { { 0, } };
|
||||||
|
|
||||||
static TIM_ICInitTypeDef TIM_ICInitStructure = { 0, };
|
static TIM_ICInitTypeDef TIM_ICInitStructure = { 0, };
|
||||||
static bool usePPMFlag = false;
|
static bool usePPMFlag = false;
|
||||||
static uint8_t numOutputChannels = 0;
|
static uint8_t numOutputChannels = 0;
|
||||||
|
|
||||||
|
@ -158,11 +158,11 @@ static void pwmIRQHandler(TIM_TypeDef *tim)
|
||||||
static void pwmInitializeInput(bool usePPM)
|
static void pwmInitializeInput(bool usePPM)
|
||||||
{
|
{
|
||||||
GPIO_InitTypeDef GPIO_InitStructure = { 0, };
|
GPIO_InitTypeDef GPIO_InitStructure = { 0, };
|
||||||
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, };
|
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, };
|
||||||
NVIC_InitTypeDef NVIC_InitStructure = { 0, };
|
NVIC_InitTypeDef NVIC_InitStructure = { 0, };
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
|
|
||||||
// Input pins
|
// Input pins
|
||||||
if (usePPM) {
|
if (usePPM) {
|
||||||
// Configure TIM2_CH1 for PPM input
|
// Configure TIM2_CH1 for PPM input
|
||||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
|
||||||
|
@ -246,7 +246,7 @@ static void pwmInitializeInput(bool usePPM)
|
||||||
void pwmInit(drv_pwm_config_t *init)
|
void pwmInit(drv_pwm_config_t *init)
|
||||||
{
|
{
|
||||||
GPIO_InitTypeDef GPIO_InitStructure = { 0, };
|
GPIO_InitTypeDef GPIO_InitStructure = { 0, };
|
||||||
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, };
|
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, };
|
||||||
TIM_OCInitTypeDef TIM_OCInitStructure = { 0, };
|
TIM_OCInitTypeDef TIM_OCInitStructure = { 0, };
|
||||||
|
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
|
|
|
@ -14,7 +14,7 @@
|
||||||
|
|
||||||
#include "pwm_output.h"
|
#include "pwm_output.h"
|
||||||
|
|
||||||
typedef void (* pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors
|
typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
#ifdef STM32F303xC
|
#ifdef STM32F303xC
|
||||||
|
|
|
@ -7,17 +7,18 @@ typedef enum {
|
||||||
} serialInversion_e;
|
} serialInversion_e;
|
||||||
|
|
||||||
typedef enum portMode_t {
|
typedef enum portMode_t {
|
||||||
MODE_RX = 1 << 0,
|
MODE_RX = 1 << 0,
|
||||||
MODE_TX = 1 << 1,
|
MODE_TX = 1 << 1,
|
||||||
MODE_RXTX = MODE_RX | MODE_TX,
|
MODE_RXTX = MODE_RX | MODE_TX,
|
||||||
MODE_SBUS = 1 << 2,
|
MODE_SBUS = 1 << 2,
|
||||||
} portMode_t;
|
} portMode_t;
|
||||||
|
|
||||||
typedef void (* serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app
|
typedef void (*serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app
|
||||||
|
|
||||||
typedef struct serialPort {
|
typedef struct serialPort {
|
||||||
|
|
||||||
const struct serialPortVTable *vTable;
|
const struct serialPortVTable *vTable;
|
||||||
|
|
||||||
uint8_t identifier;
|
uint8_t identifier;
|
||||||
portMode_t mode;
|
portMode_t mode;
|
||||||
serialInversion_e inversion;
|
serialInversion_e inversion;
|
||||||
|
@ -38,14 +39,14 @@ typedef struct serialPort {
|
||||||
|
|
||||||
struct serialPortVTable {
|
struct serialPortVTable {
|
||||||
void (*serialWrite)(serialPort_t *instance, uint8_t ch);
|
void (*serialWrite)(serialPort_t *instance, uint8_t ch);
|
||||||
|
|
||||||
uint8_t (*serialTotalBytesWaiting)(serialPort_t *instance);
|
uint8_t (*serialTotalBytesWaiting)(serialPort_t *instance);
|
||||||
|
|
||||||
uint8_t (*serialRead)(serialPort_t *instance);
|
uint8_t (*serialRead)(serialPort_t *instance);
|
||||||
|
|
||||||
// Specified baud rate may not be allowed by an implementation, use serialGetBaudRate to determine actual baud rate in use.
|
// Specified baud rate may not be allowed by an implementation, use serialGetBaudRate to determine actual baud rate in use.
|
||||||
void (*serialSetBaudRate)(serialPort_t *instance, uint32_t baudRate);
|
void (*serialSetBaudRate)(serialPort_t *instance, uint32_t baudRate);
|
||||||
|
|
||||||
bool (*isSerialTransmitBufferEmpty)(serialPort_t *instance);
|
bool (*isSerialTransmitBufferEmpty)(serialPort_t *instance);
|
||||||
|
|
||||||
void (*setMode)(serialPort_t *instance, portMode_t mode);
|
void (*setMode)(serialPort_t *instance, portMode_t mode);
|
||||||
|
|
|
@ -91,7 +91,7 @@ static void serialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t
|
||||||
|
|
||||||
static void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
|
static void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
|
||||||
{
|
{
|
||||||
TIM_ICInitTypeDef TIM_ICInitStructure;
|
TIM_ICInitTypeDef TIM_ICInitStructure;
|
||||||
|
|
||||||
TIM_ICStructInit(&TIM_ICInitStructure);
|
TIM_ICStructInit(&TIM_ICInitStructure);
|
||||||
TIM_ICInitStructure.TIM_Channel = channel;
|
TIM_ICInitStructure.TIM_Channel = channel;
|
||||||
|
@ -231,7 +231,7 @@ void applyChangedBits(softSerial_t *softSerial)
|
||||||
{
|
{
|
||||||
if (softSerial->rxEdge == TRAILING) {
|
if (softSerial->rxEdge == TRAILING) {
|
||||||
uint8_t bitToSet;
|
uint8_t bitToSet;
|
||||||
for (bitToSet = softSerial->rxLastLeadingEdgeAtBitIndex; bitToSet < softSerial->rxBitIndex ; bitToSet++) {
|
for (bitToSet = softSerial->rxLastLeadingEdgeAtBitIndex; bitToSet < softSerial->rxBitIndex; bitToSet++) {
|
||||||
softSerial->internalRxBuffer |= 1 << bitToSet;
|
softSerial->internalRxBuffer |= 1 << bitToSet;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -8,7 +8,7 @@
|
||||||
// FIXME this is a uart_t really. Move the generic properties into a separate structure (serialPort_t) and update the code to use it
|
// FIXME this is a uart_t really. Move the generic properties into a separate structure (serialPort_t) and update the code to use it
|
||||||
typedef struct {
|
typedef struct {
|
||||||
serialPort_t port;
|
serialPort_t port;
|
||||||
|
|
||||||
// FIXME these are uart specific and do not belong in here
|
// FIXME these are uart specific and do not belong in here
|
||||||
DMA_Channel_TypeDef *rxDMAChannel;
|
DMA_Channel_TypeDef *rxDMAChannel;
|
||||||
DMA_Channel_TypeDef *txDMAChannel;
|
DMA_Channel_TypeDef *txDMAChannel;
|
||||||
|
|
|
@ -69,7 +69,7 @@ void hcsr04_init(sonar_config_t config)
|
||||||
// enable AFIO for EXTI support - already done is drv_system.c
|
// enable AFIO for EXTI support - already done is drv_system.c
|
||||||
// RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph, ENABLE);
|
// RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph, ENABLE);
|
||||||
|
|
||||||
switch(config) {
|
switch (config) {
|
||||||
case sonar_pwm56:
|
case sonar_pwm56:
|
||||||
trigger_pin = Pin_8; // PWM5 (PB8) - 5v tolerant
|
trigger_pin = Pin_8; // PWM5 (PB8) - 5v tolerant
|
||||||
echo_pin = Pin_9; // PWM6 (PB9) - 5v tolerant
|
echo_pin = Pin_9; // PWM6 (PB9) - 5v tolerant
|
||||||
|
|
|
@ -13,7 +13,7 @@
|
||||||
|
|
||||||
#ifdef BUZZER
|
#ifdef BUZZER
|
||||||
|
|
||||||
void (* systemBeepPtr)(bool onoff) = NULL;
|
void (*systemBeepPtr)(bool onoff) = NULL;
|
||||||
|
|
||||||
static void beepNormal(bool onoff)
|
static void beepNormal(bool onoff)
|
||||||
{
|
{
|
||||||
|
|
|
@ -17,17 +17,17 @@ int16_t heading, magHold;
|
||||||
int16_t axisPID[3];
|
int16_t axisPID[3];
|
||||||
uint8_t dynP8[3], dynI8[3], dynD8[3];
|
uint8_t dynP8[3], dynI8[3], dynD8[3];
|
||||||
|
|
||||||
|
|
||||||
static int32_t errorGyroI[3] = { 0, 0, 0 };
|
static int32_t errorGyroI[3] = { 0, 0, 0 };
|
||||||
static int32_t errorAngleI[2] = { 0, 0 };
|
static int32_t errorAngleI[2] = { 0, 0 };
|
||||||
|
|
||||||
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim);
|
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||||
|
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim);
|
||||||
|
|
||||||
typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); // pid controller function prototype
|
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||||
|
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); // pid controller function prototype
|
||||||
|
|
||||||
pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
|
pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
|
||||||
|
|
||||||
|
|
||||||
void mwDisarm(void)
|
void mwDisarm(void)
|
||||||
{
|
{
|
||||||
if (f.ARMED)
|
if (f.ARMED)
|
||||||
|
@ -53,7 +53,8 @@ void resetErrorGyro(void)
|
||||||
errorGyroI[YAW] = 0;
|
errorGyroI[YAW] = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
|
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||||
|
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
|
||||||
{
|
{
|
||||||
int axis, prop;
|
int axis, prop;
|
||||||
int32_t error, errorAngle;
|
int32_t error, errorAngle;
|
||||||
|
@ -68,7 +69,8 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
||||||
for (axis = 0; axis < 3; axis++) {
|
for (axis = 0; axis < 3; axis++) {
|
||||||
if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
|
if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
|
||||||
// 50 degrees max inclination
|
// 50 degrees max inclination
|
||||||
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis];
|
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
|
||||||
|
+max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis];
|
||||||
PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
|
PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
|
||||||
PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
|
PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
|
||||||
|
|
||||||
|
@ -76,7 +78,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
||||||
ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12;
|
ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12;
|
||||||
}
|
}
|
||||||
if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == FD_YAW) { // MODE relying on GYRO or YAW axis
|
if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == FD_YAW) { // MODE relying on GYRO or YAW axis
|
||||||
error = (int32_t)rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
|
error = (int32_t) rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
|
||||||
error -= gyroData[axis];
|
error -= gyroData[axis];
|
||||||
|
|
||||||
PTermGYRO = rcCommand[axis];
|
PTermGYRO = rcCommand[axis];
|
||||||
|
@ -99,7 +101,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
|
PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
|
||||||
delta = gyroData[axis] - lastGyro[axis];
|
delta = gyroData[axis] - lastGyro[axis];
|
||||||
lastGyro[axis] = gyroData[axis];
|
lastGyro[axis] = gyroData[axis];
|
||||||
deltaSum = delta1[axis] + delta2[axis] + delta;
|
deltaSum = delta1[axis] + delta2[axis] + delta;
|
||||||
|
@ -112,7 +114,8 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
||||||
|
|
||||||
#define GYRO_I_MAX 256
|
#define GYRO_I_MAX 256
|
||||||
|
|
||||||
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
|
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
|
||||||
|
rollAndPitchTrims_t *angleTrim)
|
||||||
{
|
{
|
||||||
int32_t errorAngle = 0;
|
int32_t errorAngle = 0;
|
||||||
int axis;
|
int axis;
|
||||||
|
@ -127,13 +130,14 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
|
||||||
// -----Get the desired angle rate depending on flight mode
|
// -----Get the desired angle rate depending on flight mode
|
||||||
if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) { // MODE relying on ACC
|
if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) { // MODE relying on ACC
|
||||||
// calculate error and limit the angle to max configured inclination
|
// calculate error and limit the angle to max configured inclination
|
||||||
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis]; // 16 bits is ok here
|
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int) max_angle_inclination),
|
||||||
|
+max_angle_inclination) - inclination.rawAngles[axis] + angleTrim->raw[axis]; // 16 bits is ok here
|
||||||
}
|
}
|
||||||
if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
|
if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
|
||||||
AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[2]) >> 5);
|
AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[2]) >> 5);
|
||||||
} else {
|
} else {
|
||||||
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
|
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
|
||||||
AngleRateTmp = ((int32_t) (controlRateConfig->rollPitchRate + 27) * rcCommand[axis]) >> 4;
|
AngleRateTmp = ((int32_t)(controlRateConfig->rollPitchRate + 27) * rcCommand[axis]) >> 4;
|
||||||
if (f.HORIZON_MODE) {
|
if (f.HORIZON_MODE) {
|
||||||
// mix up angle error to desired AngleRateTmp to add a little auto-level feel
|
// mix up angle error to desired AngleRateTmp to add a little auto-level feel
|
||||||
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL]) >> 8;
|
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL]) >> 8;
|
||||||
|
@ -160,16 +164,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
|
||||||
|
|
||||||
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
|
// 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
|
// I coefficient (I8) moved before integration to make limiting independent from PID settings
|
||||||
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t)-GYRO_I_MAX << 13, (int32_t)+GYRO_I_MAX << 13);
|
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
|
||||||
ITerm = errorGyroI[axis] >> 13;
|
ITerm = errorGyroI[axis] >> 13;
|
||||||
|
|
||||||
//-----calculate D-term
|
//-----calculate D-term
|
||||||
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
||||||
lastError[axis] = RateError;
|
lastError[axis] = RateError;
|
||||||
|
|
||||||
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
|
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
|
||||||
// would be scaled by different dt each time. Division by dT fixes that.
|
// would be scaled by different dt each time. Division by dT fixes that.
|
||||||
delta = (delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6;
|
delta = (delta * ((uint16_t) 0xFFFF / (cycleTime >> 4))) >> 6;
|
||||||
// add moving average here to reduce noise
|
// add moving average here to reduce noise
|
||||||
deltaSum = delta1[axis] + delta2[axis] + delta;
|
deltaSum = delta1[axis] + delta2[axis] + delta;
|
||||||
delta2[axis] = delta1[axis];
|
delta2[axis] = delta1[axis];
|
||||||
|
|
|
@ -458,6 +458,7 @@ int getEstimatedAltitude(void)
|
||||||
vario = applyDeadband(vel_tmp, 5);
|
vario = applyDeadband(vel_tmp, 5);
|
||||||
|
|
||||||
BaroPID = calculateBaroPid(vel_tmp, accZ_tmp, accZ_old);
|
BaroPID = calculateBaroPid(vel_tmp, accZ_tmp, accZ_old);
|
||||||
|
|
||||||
accZ_old = accZ_tmp;
|
accZ_old = accZ_tmp;
|
||||||
|
|
||||||
return 1;
|
return 1;
|
||||||
|
|
|
@ -478,7 +478,7 @@ void mixTable(void)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MULTITYPE_DUALCOPTER:
|
case MULTITYPE_DUALCOPTER:
|
||||||
for (i = 4; i < 6; i++ ) {
|
for (i = 4; i < 6; i++) {
|
||||||
servo[i] = axisPID[5 - i] * servoDirection(i, 1); // mix and setup direction
|
servo[i] = axisPID[5 - i] * servoDirection(i, 1); // mix and setup direction
|
||||||
servo[i] += determineServoMiddleOrForwardFromChannel(i);
|
servo[i] += determineServoMiddleOrForwardFromChannel(i);
|
||||||
}
|
}
|
||||||
|
|
|
@ -118,10 +118,10 @@ typedef struct gpsData_t {
|
||||||
int errors; // gps error counter - crc error/lost of data/sync etc. reset on each reinit.
|
int errors; // gps error counter - crc error/lost of data/sync etc. reset on each reinit.
|
||||||
uint32_t lastMessage; // last time valid GPS data was received (millis)
|
uint32_t lastMessage; // last time valid GPS data was received (millis)
|
||||||
uint32_t lastLastMessage; // last-last valid GPS message. Used to calculate delta.
|
uint32_t lastLastMessage; // last-last valid GPS message. Used to calculate delta.
|
||||||
|
|
||||||
uint32_t state_position; // incremental variable for loops
|
uint32_t state_position; // incremental variable for loops
|
||||||
uint32_t state_ts; // timestamp for last state_position increment
|
uint32_t state_ts; // timestamp for last state_position increment
|
||||||
|
|
||||||
} gpsData_t;
|
} gpsData_t;
|
||||||
|
|
||||||
static gpsData_t gpsData;
|
static gpsData_t gpsData;
|
||||||
|
@ -495,7 +495,7 @@ static void gpsNewData(uint16_t c)
|
||||||
#endif
|
#endif
|
||||||
// dTnav calculation
|
// dTnav calculation
|
||||||
// Time for calculating x,y speed and navigation pids
|
// Time for calculating x,y speed and navigation pids
|
||||||
dTnav = (float) (millis() - nav_loopTimer) / 1000.0f;
|
dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
|
||||||
nav_loopTimer = millis();
|
nav_loopTimer = millis();
|
||||||
// prevent runup from bad GPS
|
// prevent runup from bad GPS
|
||||||
dTnav = min(dTnav, 1.0f);
|
dTnav = min(dTnav, 1.0f);
|
||||||
|
@ -684,7 +684,7 @@ static bool check_missed_wp(void)
|
||||||
static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing)
|
static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing)
|
||||||
{
|
{
|
||||||
float dLat = *lat2 - *lat1; // difference of latitude in 1/10 000 000 degrees
|
float dLat = *lat2 - *lat1; // difference of latitude in 1/10 000 000 degrees
|
||||||
float dLon = (float) (*lon2 - *lon1) * GPS_scaleLonDown;
|
float dLon = (float)(*lon2 - *lon1) * GPS_scaleLonDown;
|
||||||
*dist = sqrtf(sq(dLat) + sq(dLon)) * 1.113195f;
|
*dist = sqrtf(sq(dLat) + sq(dLon)) * 1.113195f;
|
||||||
|
|
||||||
*bearing = 9000.0f + atan2f(-dLat, dLon) * 5729.57795f; // Convert the output radians to 100xdeg
|
*bearing = 9000.0f + atan2f(-dLat, dLon) * 5729.57795f; // Convert the output radians to 100xdeg
|
||||||
|
@ -716,8 +716,8 @@ static void GPS_calc_velocity(void)
|
||||||
|
|
||||||
if (init) {
|
if (init) {
|
||||||
float tmp = 1.0f / dTnav;
|
float tmp = 1.0f / dTnav;
|
||||||
actual_speed[GPS_X] = (float) (GPS_coord[LON] - last[LON]) * GPS_scaleLonDown * tmp;
|
actual_speed[GPS_X] = (float)(GPS_coord[LON] - last[LON]) * GPS_scaleLonDown * tmp;
|
||||||
actual_speed[GPS_Y] = (float) (GPS_coord[LAT] - last[LAT]) * tmp;
|
actual_speed[GPS_Y] = (float)(GPS_coord[LAT] - last[LAT]) * tmp;
|
||||||
|
|
||||||
actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
|
actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
|
||||||
actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
|
actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
|
||||||
|
@ -742,7 +742,7 @@ static void GPS_calc_velocity(void)
|
||||||
//
|
//
|
||||||
static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, int32_t *gps_lat, int32_t *gps_lng)
|
static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, int32_t *gps_lat, int32_t *gps_lng)
|
||||||
{
|
{
|
||||||
error[LON] = (float) (*target_lng - *gps_lng) * GPS_scaleLonDown; // X Error
|
error[LON] = (float)(*target_lng - *gps_lng) * GPS_scaleLonDown; // X Error
|
||||||
error[LAT] = *target_lat - *gps_lat; // Y Error
|
error[LAT] = *target_lat - *gps_lat; // Y Error
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -770,7 +770,7 @@ static void GPS_calc_poshold(void)
|
||||||
d = 0;
|
d = 0;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
nav[axis] +=d;
|
nav[axis] += d;
|
||||||
nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
|
nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
|
||||||
navPID[axis].integrator = poshold_ratePID[axis].integrator;
|
navPID[axis].integrator = poshold_ratePID[axis].integrator;
|
||||||
}
|
}
|
||||||
|
@ -846,7 +846,7 @@ static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow)
|
||||||
// limit the ramp up of the speed
|
// limit the ramp up of the speed
|
||||||
// waypoint_speed_gov is reset to 0 at each new WP command
|
// waypoint_speed_gov is reset to 0 at each new WP command
|
||||||
if (max_speed > waypoint_speed_gov) {
|
if (max_speed > waypoint_speed_gov) {
|
||||||
waypoint_speed_gov += (int) (100.0f * dTnav); // increase at .5/ms
|
waypoint_speed_gov += (int)(100.0f * dTnav); // increase at .5/ms
|
||||||
max_speed = waypoint_speed_gov;
|
max_speed = waypoint_speed_gov;
|
||||||
}
|
}
|
||||||
return max_speed;
|
return max_speed;
|
||||||
|
@ -970,7 +970,7 @@ static uint32_t grab_fields(char *src, uint8_t mult)
|
||||||
tmp *= 10;
|
tmp *= 10;
|
||||||
if (src[i] >= '0' && src[i] <= '9')
|
if (src[i] >= '0' && src[i] <= '9')
|
||||||
tmp += src[i] - '0';
|
tmp += src[i] - '0';
|
||||||
if (i >= 15)
|
if (i >= 15)
|
||||||
return 0; // out of bounds
|
return 0; // out of bounds
|
||||||
}
|
}
|
||||||
return tmp;
|
return tmp;
|
||||||
|
@ -1128,8 +1128,7 @@ typedef struct {
|
||||||
uint32_t heading_accuracy;
|
uint32_t heading_accuracy;
|
||||||
} ubx_nav_velned;
|
} ubx_nav_velned;
|
||||||
|
|
||||||
typedef struct
|
typedef struct {
|
||||||
{
|
|
||||||
uint8_t chn; // Channel number, 255 for SVx not assigned to channel
|
uint8_t chn; // Channel number, 255 for SVx not assigned to channel
|
||||||
uint8_t svid; // Satellite ID
|
uint8_t svid; // Satellite ID
|
||||||
uint8_t flags; // Bitmask
|
uint8_t flags; // Bitmask
|
||||||
|
@ -1140,8 +1139,7 @@ typedef struct
|
||||||
int32_t prRes; // Pseudo range residual in centimetres
|
int32_t prRes; // Pseudo range residual in centimetres
|
||||||
} ubx_nav_svinfo_channel;
|
} ubx_nav_svinfo_channel;
|
||||||
|
|
||||||
typedef struct
|
typedef struct {
|
||||||
{
|
|
||||||
uint32_t time; // GPS Millisecond time of week
|
uint32_t time; // GPS Millisecond time of week
|
||||||
uint8_t numCh; // Number of channels
|
uint8_t numCh; // Number of channels
|
||||||
uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
|
uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
|
||||||
|
@ -1252,7 +1250,7 @@ static bool gpsNewFrameUBLOX(uint8_t data)
|
||||||
case 5:
|
case 5:
|
||||||
_step++;
|
_step++;
|
||||||
_ck_b += (_ck_a += data); // checksum byte
|
_ck_b += (_ck_a += data); // checksum byte
|
||||||
_payload_length += (uint16_t) (data << 8);
|
_payload_length += (uint16_t)(data << 8);
|
||||||
if (_payload_length > 512) {
|
if (_payload_length > 512) {
|
||||||
_payload_length = 0;
|
_payload_length = 0;
|
||||||
_step = 0;
|
_step = 0;
|
||||||
|
|
6
src/mw.c
6
src/mw.c
|
@ -68,7 +68,7 @@ uint16_t rssi; // range: [0;1023]
|
||||||
extern uint8_t dynP8[3], dynI8[3], dynD8[3];
|
extern uint8_t dynP8[3], dynI8[3], dynD8[3];
|
||||||
extern failsafe_t *failsafe;
|
extern failsafe_t *failsafe;
|
||||||
|
|
||||||
typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *accelerometerTrims);
|
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *accelerometerTrims);
|
||||||
|
|
||||||
extern pidControllerFuncPtr pid_controller;
|
extern pidControllerFuncPtr pid_controller;
|
||||||
|
|
||||||
|
@ -224,7 +224,7 @@ static void mwVario(void)
|
||||||
{
|
{
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void loop(void)
|
void loop(void)
|
||||||
{
|
{
|
||||||
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
|
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
|
||||||
|
@ -395,7 +395,7 @@ void loop(void)
|
||||||
if (rcOptions[BOXCALIB]) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
|
if (rcOptions[BOXCALIB]) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
|
||||||
if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
|
if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
|
||||||
InflightcalibratingA = 50;
|
InflightcalibratingA = 50;
|
||||||
AccInflightCalibrationActive = true;
|
AccInflightCalibrationActive = true;
|
||||||
} else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
|
} else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
|
||||||
AccInflightCalibrationMeasurementDone = false;
|
AccInflightCalibrationMeasurementDone = false;
|
||||||
AccInflightCalibrationSavetoEEProm = true;
|
AccInflightCalibrationSavetoEEProm = true;
|
||||||
|
|
|
@ -38,7 +38,7 @@ typedef struct rxRuntimeConfig_s {
|
||||||
|
|
||||||
extern rxRuntimeConfig_t rxRuntimeConfig;
|
extern rxRuntimeConfig_t rxRuntimeConfig;
|
||||||
|
|
||||||
typedef uint16_t (* rcReadRawDataPtr)(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
|
typedef uint16_t (*rcReadRawDataPtr)(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
|
||||||
|
|
||||||
void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
|
void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
|
||||||
|
|
||||||
|
|
|
@ -43,8 +43,7 @@ bool sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa
|
||||||
return sBusPort != NULL;
|
return sBusPort != NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
struct sbus_dat
|
struct sbus_dat {
|
||||||
{
|
|
||||||
unsigned int chan0 : 11;
|
unsigned int chan0 : 11;
|
||||||
unsigned int chan1 : 11;
|
unsigned int chan1 : 11;
|
||||||
unsigned int chan2 : 11;
|
unsigned int chan2 : 11;
|
||||||
|
@ -59,8 +58,7 @@ struct sbus_dat
|
||||||
unsigned int chan11 : 11;
|
unsigned int chan11 : 11;
|
||||||
} __attribute__ ((__packed__));
|
} __attribute__ ((__packed__));
|
||||||
|
|
||||||
typedef union
|
typedef union {
|
||||||
{
|
|
||||||
uint8_t in[SBUS_FRAME_SIZE];
|
uint8_t in[SBUS_FRAME_SIZE];
|
||||||
struct sbus_dat msg;
|
struct sbus_dat msg;
|
||||||
} sbus_msg;
|
} sbus_msg;
|
||||||
|
@ -72,7 +70,7 @@ static void sbusDataReceive(uint16_t c)
|
||||||
{
|
{
|
||||||
uint32_t sbusTime;
|
uint32_t sbusTime;
|
||||||
static uint32_t sbusTimeLast;
|
static uint32_t sbusTimeLast;
|
||||||
static uint8_t sbusFramePosition;
|
static uint8_t sbusFramePosition;
|
||||||
|
|
||||||
sbusTime = micros();
|
sbusTime = micros();
|
||||||
if ((sbusTime - sbusTimeLast) > 2500) // sbus2 fast timing
|
if ((sbusTime - sbusTimeLast) > 2500) // sbus2 fast timing
|
||||||
|
|
|
@ -49,17 +49,17 @@ static void sumdDataReceive(uint16_t c)
|
||||||
static uint8_t sumdIndex;
|
static uint8_t sumdIndex;
|
||||||
|
|
||||||
sumdTime = micros();
|
sumdTime = micros();
|
||||||
if ((sumdTime - sumdTimeLast) > 4000)
|
if ((sumdTime - sumdTimeLast) > 4000)
|
||||||
sumdIndex = 0;
|
sumdIndex = 0;
|
||||||
sumdTimeLast = sumdTime;
|
sumdTimeLast = sumdTime;
|
||||||
|
|
||||||
if (sumdIndex == 0) {
|
if (sumdIndex == 0) {
|
||||||
if (c != SUMD_SYNCBYTE)
|
if (c != SUMD_SYNCBYTE)
|
||||||
return;
|
return;
|
||||||
else
|
else
|
||||||
sumdFrameDone = false; // lazy main loop didnt fetch the stuff
|
sumdFrameDone = false; // lazy main loop didnt fetch the stuff
|
||||||
}
|
}
|
||||||
if (sumdIndex == 2)
|
if (sumdIndex == 2)
|
||||||
sumdSize = (uint8_t)c;
|
sumdSize = (uint8_t)c;
|
||||||
if (sumdIndex < SUMD_BUFFSIZE)
|
if (sumdIndex < SUMD_BUFFSIZE)
|
||||||
sumd[sumdIndex] = (uint8_t)c;
|
sumd[sumdIndex] = (uint8_t)c;
|
||||||
|
|
|
@ -303,7 +303,7 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value);
|
||||||
static void cliPrintVar(const clivalue_t *var, uint32_t full);
|
static void cliPrintVar(const clivalue_t *var, uint32_t full);
|
||||||
static void cliPrint(const char *str);
|
static void cliPrint(const char *str);
|
||||||
static void cliWrite(uint8_t ch);
|
static void cliWrite(uint8_t ch);
|
||||||
|
|
||||||
static void cliPrompt(void)
|
static void cliPrompt(void)
|
||||||
{
|
{
|
||||||
cliPrint("\r\n# ");
|
cliPrint("\r\n# ");
|
||||||
|
|
|
@ -202,14 +202,14 @@ uint8_t read8(void)
|
||||||
uint16_t read16(void)
|
uint16_t read16(void)
|
||||||
{
|
{
|
||||||
uint16_t t = read8();
|
uint16_t t = read8();
|
||||||
t += (uint16_t) read8() << 8;
|
t += (uint16_t)read8() << 8;
|
||||||
return t;
|
return t;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint32_t read32(void)
|
uint32_t read32(void)
|
||||||
{
|
{
|
||||||
uint32_t t = read16();
|
uint32_t t = read16();
|
||||||
t += (uint32_t) read16() << 16;
|
t += (uint32_t)read16() << 16;
|
||||||
return t;
|
return t;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -112,13 +112,13 @@ void hottV4FormatAndSendGPSResponse(void)
|
||||||
void hottV4GPSUpdate(void)
|
void hottV4GPSUpdate(void)
|
||||||
{
|
{
|
||||||
// Number of Satelites
|
// Number of Satelites
|
||||||
HoTTV4GPSModule.GPSNumSat=GPS_numSat;
|
HoTTV4GPSModule.GPSNumSat = GPS_numSat;
|
||||||
if (f.GPS_FIX > 0) {
|
if (f.GPS_FIX > 0) {
|
||||||
// GPS fix
|
// GPS fix
|
||||||
HoTTV4GPSModule.GPS_fix = 0x66; // Displays a 'f' for fix
|
HoTTV4GPSModule.GPS_fix = 0x66; // Displays a 'f' for fix
|
||||||
|
|
||||||
// latitude
|
// latitude
|
||||||
HoTTV4GPSModule.LatitudeNS=(GPS_coord[LAT]<0);
|
HoTTV4GPSModule.LatitudeNS = (GPS_coord[LAT] < 0);
|
||||||
uint8_t deg = GPS_coord[LAT] / 100000;
|
uint8_t deg = GPS_coord[LAT] / 100000;
|
||||||
uint32_t sec = (GPS_coord[LAT] - (deg * 100000)) * 6;
|
uint32_t sec = (GPS_coord[LAT] - (deg * 100000)) * 6;
|
||||||
uint8_t min = sec / 10000;
|
uint8_t min = sec / 10000;
|
||||||
|
@ -130,7 +130,7 @@ void hottV4GPSUpdate(void)
|
||||||
HoTTV4GPSModule.LatitudeSecHigh = sec >> 8;
|
HoTTV4GPSModule.LatitudeSecHigh = sec >> 8;
|
||||||
|
|
||||||
// longitude
|
// longitude
|
||||||
HoTTV4GPSModule.longitudeEW=(GPS_coord[LON]<0);
|
HoTTV4GPSModule.longitudeEW = (GPS_coord[LON] < 0);
|
||||||
deg = GPS_coord[LON] / 100000;
|
deg = GPS_coord[LON] / 100000;
|
||||||
sec = (GPS_coord[LON] - (deg * 100000)) * 6;
|
sec = (GPS_coord[LON] - (deg * 100000)) * 6;
|
||||||
min = sec / 10000;
|
min = sec / 10000;
|
||||||
|
@ -312,7 +312,8 @@ void handleHoTTTelemetry(void)
|
||||||
|
|
||||||
switch (c) {
|
switch (c) {
|
||||||
case 0x8A:
|
case 0x8A:
|
||||||
if (sensors(SENSOR_GPS)) hottV4FormatAndSendGPSResponse();
|
if (sensors(SENSOR_GPS))
|
||||||
|
hottV4FormatAndSendGPSResponse();
|
||||||
break;
|
break;
|
||||||
case 0x8E:
|
case 0x8E:
|
||||||
hottV4FormatAndSendEAMResponse();
|
hottV4FormatAndSendEAMResponse();
|
||||||
|
|
Loading…
Reference in New Issue