Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +03:00
Code Issues Wiki Activity

synced with mwc 2.1. it's suprising how many "new" things in 2.1 didn't actually matter on a real platform.

Browse source 
removed camtrig stuff since it wasnt possible. somewhat replaced with aux forwarding (see below)
2.1 buzzer code changed, untested.
removed flying wing mixer. nobody used that. 
added alt_hold_throttle_neutral, nav_slew_rate and looptime configuration to cli. default looptime set to 3000. changed default gyro_cmpf to 400 to sync with 2.1.
increased bmp085 oversampling
added gimbal_flags (bit 4 set) flag which, in PPM mode, forwards AUX1..4 to the lower 4 PWM outputs instead of using them as motors. set gimbal_flags=8 to test it out. output is fixed to 50Hz.
merged 2.1 gps changes (not many)
casting in gyro smoothing (nobody uses that anyway)
calibrate accel in gimbal mode, set smallangle in gyro-only mode
vtail4 mixer fix
flight tested on quadx w/ppm.


git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@182 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop 2012-07-20 14:53:15 +00:00
parent e70d7b5d16
commit c98113b82c
19 changed files with 5363 additions and 5696 deletions
Download patch file Download diff file Expand all files Collapse all files

4952
obj/baseflight.hex
View file

File diff suppressed because it is too large Load diff

5307
obj/baseflight_fy90q.hex
View file

File diff suppressed because it is too large Load diff

15
src/board.h
View file

@ -15,7 +15,7 @@
#include "core_cm3.h" #include "core_cm3.h"
#ifndef M_PI #ifndef M_PI
#define M_PI 3.14159265358979323846 #define M_PI 3.14159265358979323846f
#endif /* M_PI */ #endif /* M_PI */
typedef enum { typedef enum {
@ -41,13 +41,12 @@ typedef enum {
FEATURE_SPEKTRUM = 1 << 3, FEATURE_SPEKTRUM = 1 << 3,
FEATURE_MOTOR_STOP = 1 << 4, FEATURE_MOTOR_STOP = 1 << 4,
FEATURE_SERVO_TILT = 1 << 5, FEATURE_SERVO_TILT = 1 << 5,
FEATURE_CAMTRIG = 1 << 6, FEATURE_GYRO_SMOOTHING = 1 << 6,
FEATURE_GYRO_SMOOTHING = 1 << 7, FEATURE_LED_RING = 1 << 7,
FEATURE_LED_RING = 1 << 8, FEATURE_GPS = 1 << 8,
FEATURE_GPS = 1 << 9, FEATURE_FAILSAFE = 1 << 9,
FEATURE_FAILSAFE = 1 << 10, FEATURE_SONAR = 1 << 10,
FEATURE_SONAR = 1 << 11, FEATURE_TELEMETRY = 1 << 11,
FEATURE_TELEMETRY = 1 << 12,
} AvailableFeatures; } AvailableFeatures;
typedef void (* sensorInitFuncPtr)(void); // sensor init prototype typedef void (* sensorInitFuncPtr)(void); // sensor init prototype

172
src/buzzer.c
View file

@ -1,22 +1,18 @@
#include "board.h" #include "board.h"
#include "mw.h" #include "mw.h"
static uint8_t buzzerIsOn = 0, beepDone = 0;
static uint32_t buzzerLastToggleTime;
static void beep(uint16_t pulse);
static void beep_code(char first, char second, char third, char pause);
void buzzer(uint8_t warn_vbat) void buzzer(uint8_t warn_vbat)
{ {
static uint16_t ontime, offtime, beepcount, repeat, repeatcounter; static uint8_t beeperOnBox;
static uint32_t buzzerLastToggleTime; static uint8_t warn_noGPSfix = 0;
static uint8_t buzzerIsOn = 0, activateBuzzer, beeperOnBox, i, last_rcOptions[CHECKBOXITEMS], warn_noGPSfix = 0, warn_failsafe = 0; static uint8_t warn_failsafe = 0;
static uint8_t warn_runtime = 0;
//===================== Beeps for changing rcOptions =====================
#if defined(RCOPTIONSBEEP)
if (last_rcOptions[i] != rcOptions[i]) {
toggleBeep = 1;
}
last_rcOptions[i] = rcOptions[i];
i++;
if (i >= CHECKBOXITEMS)
i = 0;
#endif
//===================== BeeperOn via rcOptions ===================== //===================== BeeperOn via rcOptions =====================
if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch
beeperOnBox = 1; beeperOnBox = 1;
@ -38,79 +34,95 @@ void buzzer(uint8_t warn_vbat)
//===================== GPS fix notification handling ===================== //===================== GPS fix notification handling =====================
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && !f.GPS_FIX) { //if no fix and gps funtion is activated: do warning beeps. if ((rcOptions[BOXGPSHOME] || rcOptions[BOXGPSHOLD]) && !f.GPS_FIX) { // if no fix and gps funtion is activated: do warning beeps
warn_noGPSfix = 1; warn_noGPSfix = 1;
} else { } else {
warn_noGPSfix = 0; warn_noGPSfix = 0;
} }
} }
//===================== Main Handling Block =====================
repeat = 1; // set repeat to default
ontime = 100; // set offtime to default
//the order of the below is the priority from high to low, the last entry has the lowest priority, only one option can be active at the same time //===================== Priority driven Handling =====================
if (warn_failsafe == 2) { // beepcode(length1,length2,length3,pause)
activateBuzzer = 1; // D: Double, L: Long, M: Middle, S: Short, N: None
offtime = 2000; if (warn_failsafe == 2)
ontime = 300; beep_code('L','N','N','D'); // failsafe "find me" signal
repeat = 1; else if (warn_failsafe == 1)
} //failsafe "find me" signal beep_code('S','M','L','M'); // failsafe landing active
else if (warn_failsafe == 1) { else if (warn_noGPSfix == 1)
activateBuzzer = 1; beep_code('S','S','N','M');
offtime = 50; else if (beeperOnBox == 1)
} //failsafe landing active beep_code('S','S','S','M'); // beeperon
else if (warn_noGPSfix == 1) { else if (warn_vbat == 4)
activateBuzzer = 1; beep_code('S','M','M','D');
offtime = 10; else if (warn_vbat == 2)
} else if (beeperOnBox == 1) { beep_code('S','S','M','D');
activateBuzzer = 1; else if (warn_vbat == 1)
offtime = 50; beep_code('S','M','N','D');
} //beeperon else if (warn_runtime == 1 && f.ARMED)
else if (warn_vbat == 4) { beep_code('S','S','M','N'); // Runtime warning
activateBuzzer = 1; else if (toggleBeep > 0)
offtime = 500; beep(50); // fast confirmation beep
repeat = 3; else {
} else if (warn_vbat == 2) {
activateBuzzer = 1;
offtime = 1000;
repeat = 2;
} else if (warn_vbat == 1) {
activateBuzzer = 1;
offtime = 2000;
} else if (toggleBeep > 0) {
activateBuzzer = 1;
ontime = 50;
offtime = 50;
} //fast confirmation beep
else {
activateBuzzer = 0;
}
if (activateBuzzer) {
if (repeatcounter > 1 && !buzzerIsOn && (millis() >= (buzzerLastToggleTime + 80))) { // if the buzzer is off and there is a short pause neccessary (multipe buzzes)
buzzerIsOn = 1;
BEEP_ON;
buzzerLastToggleTime = millis(); // save the time the buzer turned on
repeatcounter--;
} else if (!buzzerIsOn && (millis() >= (buzzerLastToggleTime + offtime))) { // Buzzer is off and long pause time is up -> turn it on
buzzerIsOn = 1;
BEEP_ON;
buzzerLastToggleTime = millis(); // save the time the buzer turned on
repeatcounter = repeat; //set the amount of repeats
} else if (buzzerIsOn && (millis() >= buzzerLastToggleTime + ontime)) { //Buzzer is on and time is up -> turn it off
buzzerIsOn = 0;
BEEP_OFF;
buzzerLastToggleTime = millis(); // save the time the buzer turned on
if (toggleBeep > 0)
beepcount++; // only increment if confirmation beep, the rest is endless while the condition is given
}
if (beepcount >= toggleBeep) { //confirmation flag is 0,1 or 2
beepcount = 0; //reset the counter for the next time
toggleBeep = 0; //reset the flag after all beeping is done
}
} else { //no beeping neccessary:reset everything (just in case)
beepcount = 0; //reset the counter for the next time
BEEP_OFF;
buzzerIsOn = 0; buzzerIsOn = 0;
BEEP_OFF;
}
}
void beep_code(char first, char second, char third, char pause)
{
char patternChar[4];
uint16_t Duration;
static uint8_t icnt = 0;
patternChar[0] = first;
patternChar[1] = second;
patternChar[2] = third;
patternChar[3] = pause;
switch(patternChar[icnt]) {
case 'M':
Duration = 100;
break;
case 'L':
Duration = 200;
break;
case 'D':
Duration = 2000;
break;
case 'N':
Duration = 0;
break;
default:
Duration = 50;
break;
}
if (icnt < 3 && Duration != 0)
beep(Duration);
if (icnt >= 3 && (buzzerLastToggleTime < millis() - Duration)) {
icnt = 0;
toggleBeep = 0;
}
if (beepDone == 1 || Duration == 0) {
if (icnt < 3)
icnt++;
beepDone = 0;
buzzerIsOn = 0;
BEEP_OFF;
}
}
static void beep(uint16_t pulse)
{
if (!buzzerIsOn && (millis() >= (buzzerLastToggleTime + 50))) { // Buzzer is off and long pause time is up -> turn it on
buzzerIsOn = 1;
BEEP_ON;
buzzerLastToggleTime = millis(); // save the time the buzer turned on
} else if (buzzerIsOn && (millis() >= buzzerLastToggleTime + pulse)) { // Buzzer is on and time is up -> turn it off
buzzerIsOn = 0;
BEEP_OFF;
buzzerLastToggleTime = millis();
if (toggleBeep >0)
toggleBeep--;
beepDone = 1;
} }
} }

7
src/cli.c
View file

@ -36,7 +36,7 @@ const char *mixerNames[] = {
// sync this with AvailableFeatures enum from board.h // sync this with AvailableFeatures enum from board.h
const char *featureNames[] = { const char *featureNames[] = {
"PPM", "VBAT", "INFLIGHT_ACC_CAL", "SPEKTRUM", "MOTOR_STOP", "PPM", "VBAT", "INFLIGHT_ACC_CAL", "SPEKTRUM", "MOTOR_STOP",
"SERVO_TILT", "CAMTRIG", "GYRO_SMOOTHING", "LED_RING", "GPS", "SERVO_TILT", "GYRO_SMOOTHING", "LED_RING", "GPS",
"FAILSAFE", "SONAR", "TELEMETRY", "FAILSAFE", "SONAR", "TELEMETRY",
NULL NULL
}; };
@ -91,6 +91,7 @@ typedef struct {
const clivalue_t valueTable[] = { const clivalue_t valueTable[] = {
{ "deadband", VAR_UINT8, &cfg.deadband, 0, 32 }, { "deadband", VAR_UINT8, &cfg.deadband, 0, 32 },
{ "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 }, { "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 },
{ "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 },
{ "midrc", VAR_UINT16, &cfg.midrc, 1200, 1700 }, { "midrc", VAR_UINT16, &cfg.midrc, 1200, 1700 },
{ "minthrottle", VAR_UINT16, &cfg.minthrottle, 0, 2000 }, { "minthrottle", VAR_UINT16, &cfg.minthrottle, 0, 2000 },
{ "maxthrottle", VAR_UINT16, &cfg.maxthrottle, 0, 2000 }, { "maxthrottle", VAR_UINT16, &cfg.maxthrottle, 0, 2000 },
@ -110,8 +111,6 @@ const clivalue_t valueTable[] = {
{ "vbatmaxcellvoltage", VAR_UINT8, &cfg.vbatmaxcellvoltage, 10, 50 }, { "vbatmaxcellvoltage", VAR_UINT8, &cfg.vbatmaxcellvoltage, 10, 50 },
{ "vbatmincellvoltage", VAR_UINT8, &cfg.vbatmincellvoltage, 10, 50 }, { "vbatmincellvoltage", VAR_UINT8, &cfg.vbatmincellvoltage, 10, 50 },
{ "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 }, { "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 },
{ "wing_left_mid", VAR_UINT16, &cfg.wing_left_mid, 0, 2000 },
{ "wing_right_mid", VAR_UINT16, &cfg.wing_right_mid, 0, 2000 },
{ "tri_yaw_middle", VAR_UINT16, &cfg.tri_yaw_middle, 0, 2000 }, { "tri_yaw_middle", VAR_UINT16, &cfg.tri_yaw_middle, 0, 2000 },
{ "tri_yaw_min", VAR_UINT16, &cfg.tri_yaw_min, 0, 2000 }, { "tri_yaw_min", VAR_UINT16, &cfg.tri_yaw_min, 0, 2000 },
{ "tri_yaw_max", VAR_UINT16, &cfg.tri_yaw_max, 0, 2000 }, { "tri_yaw_max", VAR_UINT16, &cfg.tri_yaw_max, 0, 2000 },
@ -142,6 +141,8 @@ const clivalue_t valueTable[] = {
{ "nav_controls_heading", VAR_UINT8, &cfg.nav_controls_heading, 0, 1 }, { "nav_controls_heading", VAR_UINT8, &cfg.nav_controls_heading, 0, 1 },
{ "nav_speed_min", VAR_UINT16, &cfg.nav_speed_min, 10, 2000 }, { "nav_speed_min", VAR_UINT16, &cfg.nav_speed_min, 10, 2000 },
{ "nav_speed_max", VAR_UINT16, &cfg.nav_speed_max, 10, 2000 }, { "nav_speed_max", VAR_UINT16, &cfg.nav_speed_max, 10, 2000 },
{ "nav_slew_rate", VAR_UINT8, &cfg.nav_slew_rate, 0, 100 },
{ "looptime", VAR_UINT16, &cfg.looptime, 1500, 9000 },
{ "p_pitch", VAR_UINT8, &cfg.P8[PITCH], 0, 200 }, { "p_pitch", VAR_UINT8, &cfg.P8[PITCH], 0, 200 },
{ "i_pitch", VAR_UINT8, &cfg.I8[PITCH], 0, 200 }, { "i_pitch", VAR_UINT8, &cfg.I8[PITCH], 0, 200 },
{ "d_pitch", VAR_UINT8, &cfg.D8[PITCH], 0, 200 }, { "d_pitch", VAR_UINT8, &cfg.D8[PITCH], 0, 200 },

23
src/config.c
View file

@ -13,7 +13,7 @@ config_t cfg;
const char rcChannelLetters[] = "AERT1234"; const char rcChannelLetters[] = "AERT1234";
static uint32_t enabledSensors = 0; static uint32_t enabledSensors = 0;
uint8_t checkNewConf = 22; uint8_t checkNewConf = 24;
void parseRcChannels(const char *input) void parseRcChannels(const char *input)
{ {
@ -47,8 +47,6 @@ void readEEPROM(void)
lookupThrottleRC[i] = cfg.minthrottle + (int32_t) (cfg.maxthrottle - cfg.minthrottle) * lookupThrottleRC[i] / 1000; // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE] lookupThrottleRC[i] = cfg.minthrottle + (int32_t) (cfg.maxthrottle - cfg.minthrottle) * lookupThrottleRC[i] / 1000; // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE]
} }
cfg.wing_left_mid = constrain(cfg.wing_left_mid, WING_LEFT_MIN, WING_LEFT_MAX); //LEFT
cfg.wing_right_mid = constrain(cfg.wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
} }
@ -89,8 +87,9 @@ void checkFirstTime(bool reset)
cfg.version = checkNewConf; cfg.version = checkNewConf;
cfg.mixerConfiguration = MULTITYPE_QUADX; cfg.mixerConfiguration = MULTITYPE_QUADX;
featureClearAll(); featureClearAll();
featureSet(FEATURE_VBAT); // | FEATURE_PPM); // sadly, this is for hackers only featureSet(FEATURE_VBAT);
cfg.looptime = 3000;
cfg.P8[ROLL] = 40; cfg.P8[ROLL] = 40;
cfg.I8[ROLL] = 30; cfg.I8[ROLL] = 30;
cfg.D8[ROLL] = 23; cfg.D8[ROLL] = 23;
@ -112,13 +111,13 @@ void checkFirstTime(bool reset)
cfg.P8[PIDNAVR] = 14; // NAV_P * 10; cfg.P8[PIDNAVR] = 14; // NAV_P * 10;
cfg.I8[PIDNAVR] = 20; // NAV_I * 100; cfg.I8[PIDNAVR] = 20; // NAV_I * 100;
cfg.D8[PIDNAVR] = 80; // NAV_D * 1000; cfg.D8[PIDNAVR] = 80; // NAV_D * 1000;
cfg.P8[PIDVEL] = 0;
cfg.I8[PIDVEL] = 0;
cfg.D8[PIDVEL] = 0;
cfg.P8[PIDLEVEL] = 70; cfg.P8[PIDLEVEL] = 70;
cfg.I8[PIDLEVEL] = 10; cfg.I8[PIDLEVEL] = 10;
cfg.D8[PIDLEVEL] = 20; cfg.D8[PIDLEVEL] = 20;
cfg.P8[PIDMAG] = 40; cfg.P8[PIDMAG] = 40;
cfg.P8[PIDVEL] = 0;
cfg.I8[PIDVEL] = 0;
cfg.D8[PIDVEL] = 0;
cfg.rcRate8 = 90; cfg.rcRate8 = 90;
cfg.rcExpo8 = 65; cfg.rcExpo8 = 65;
cfg.rollPitchRate = 0; cfg.rollPitchRate = 0;
@ -128,15 +127,15 @@ void checkFirstTime(bool reset)
cfg.thrExpo8 = 0; cfg.thrExpo8 = 0;
for (i = 0; i < CHECKBOXITEMS; i++) for (i = 0; i < CHECKBOXITEMS; i++)
cfg.activate[i] = 0; cfg.activate[i] = 0;
cfg.accTrim[0] = 0; cfg.angleTrim[0] = 0;
cfg.accTrim[1] = 0; cfg.angleTrim[1] = 0;
cfg.accZero[0] = 0; cfg.accZero[0] = 0;
cfg.accZero[1] = 0; cfg.accZero[1] = 0;
cfg.accZero[2] = 0; cfg.accZero[2] = 0;
cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
cfg.acc_hardware = ACC_DEFAULT; // default/autodetect cfg.acc_hardware = ACC_DEFAULT; // default/autodetect
cfg.acc_lpf_factor = 4; cfg.acc_lpf_factor = 4;
cfg.gyro_cmpf_factor = 310; // default MWC cfg.gyro_cmpf_factor = 400; // default MWC
cfg.gyro_lpf = 42; cfg.gyro_lpf = 42;
cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
cfg.vbatscale = 110; cfg.vbatscale = 110;
@ -147,6 +146,7 @@ void checkFirstTime(bool reset)
parseRcChannels("AETR1234"); parseRcChannels("AETR1234");
cfg.deadband = 0; cfg.deadband = 0;
cfg.yawdeadband = 0; cfg.yawdeadband = 0;
cfg.alt_hold_throttle_neutral = 20;
cfg.spektrum_hires = 0; cfg.spektrum_hires = 0;
cfg.midrc = 1500; cfg.midrc = 1500;
cfg.mincheck = 1100; cfg.mincheck = 1100;
@ -167,8 +167,6 @@ void checkFirstTime(bool reset)
// servos // servos
cfg.yaw_direction = 1; cfg.yaw_direction = 1;
cfg.wing_left_mid = 1500;
cfg.wing_right_mid = 1500;
cfg.tri_yaw_middle = 1500; cfg.tri_yaw_middle = 1500;
cfg.tri_yaw_min = 1020; cfg.tri_yaw_min = 1020;
cfg.tri_yaw_max = 2000; cfg.tri_yaw_max = 2000;
@ -188,6 +186,7 @@ void checkFirstTime(bool reset)
cfg.gps_baudrate = 9600; cfg.gps_baudrate = 9600;
cfg.gps_wp_radius = 200; cfg.gps_wp_radius = 200;
cfg.gps_lpf = 20; cfg.gps_lpf = 20;
cfg.nav_slew_rate = 30;
cfg.nav_controls_heading = 1; cfg.nav_controls_heading = 1;
cfg.nav_speed_min = 100; cfg.nav_speed_min = 100;
cfg.nav_speed_max = 300; cfg.nav_speed_max = 300;

2
src/drv_bmp085.c
View file

@ -125,7 +125,7 @@ bool bmp085Init(void)
p_bmp085->dev_addr = BMP085_I2C_ADDR; /* preset BMP085 I2C_addr */ p_bmp085->dev_addr = BMP085_I2C_ADDR; /* preset BMP085 I2C_addr */
i2cRead(p_bmp085->dev_addr, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */ i2cRead(p_bmp085->dev_addr, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
p_bmp085->chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID); p_bmp085->chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
p_bmp085->oversampling_setting = 2; p_bmp085->oversampling_setting = 3;
if (p_bmp085->chip_id == BMP085_CHIP_ID) { /* get bitslice */ if (p_bmp085->chip_id == BMP085_CHIP_ID) { /* get bitslice */
p_bmp085->sensortype = BOSCH_PRESSURE_BMP085; p_bmp085->sensortype = BOSCH_PRESSURE_BMP085;

2
src/drv_i2c.c
View file

@ -350,4 +350,4 @@ static void i2cUnstick(void)
GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_Init(GPIOB, &GPIO_InitStructure);
} }
#endif #endif

3
src/drv_i2c_soft.c
View file

@ -19,9 +19,8 @@
static void I2C_delay(void) static void I2C_delay(void)
{ {
volatile int i = 7; volatile int i = 7;
while (i) { while (i)
i--; i--;
}
} }
static bool I2C_Start(void) static bool I2C_Start(void)

5
src/drv_pwm.c
View file

@ -366,7 +366,8 @@ bool pwmInit(drv_pwm_config_t *init)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_Init(GPIOB, &GPIO_InitStructure);
TIM_TimeBaseStructure.TIM_Period = (1000000 / init->motorPwmRate) - 1; // when in extra servos mode, init lower 4 channels as 50Hz outputs
TIM_TimeBaseStructure.TIM_Period = (1000000 / (init->extraServos ? 50 : init->motorPwmRate)) - 1;
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
@ -388,7 +389,7 @@ bool pwmInit(drv_pwm_config_t *init)
TIM_Cmd(TIM3, ENABLE); TIM_Cmd(TIM3, ENABLE);
TIM_CtrlPWMOutputs(TIM3, ENABLE); TIM_CtrlPWMOutputs(TIM3, ENABLE);
// configure number of PWM outputs, in PPM/spektrum mode, we use bottom 4 channels more more motors // configure number of PWM outputs, in PPM/spektrum mode, we use bottom 4 channels more more motors
numOutputChannels = 10; numOutputChannels = init->extraServos ? 6 : 10;
} }
#if 0 #if 0

1
src/drv_pwm.h
View file

@ -4,6 +4,7 @@ typedef struct drv_pwm_config_t {
bool enableInput; bool enableInput;
bool usePPM; bool usePPM;
bool useServos; bool useServos;
bool extraServos; // configure additional 4 channels in PPM mode as servos, not motors
uint16_t motorPwmRate; uint16_t motorPwmRate;
uint16_t servoPwmRate; uint16_t servoPwmRate;
} drv_pwm_config_t; } drv_pwm_config_t;

77
src/gps.c
View file

@ -12,7 +12,11 @@ void gpsInit(uint32_t baudrate)
{ {
GPS_set_pids(); GPS_set_pids();
uart2Init(baudrate, GPS_NewData); uart2Init(baudrate, GPS_NewData);
sensorsSet(SENSOR_GPS);
// catch some GPS frames. TODO check this
delay(500);
if (GPS_Present)
sensorsSet(SENSOR_GPS);
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------
@ -42,7 +46,7 @@ static void GPS_calc_nav_rate(int max_speed);
static void GPS_update_crosstrack(void); static void GPS_update_crosstrack(void);
static bool GPS_newFrame(char c); static bool GPS_newFrame(char c);
static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow); static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow);
static int32_t wrap_18000(int32_t error); int32_t wrap_18000(int32_t error);
static int32_t wrap_36000(int32_t angle); static int32_t wrap_36000(int32_t angle);
typedef struct { typedef struct {
@ -140,7 +144,6 @@ static AC_PID pid_nav[2];
#define RADX100 0.000174532925f #define RADX100 0.000174532925f
#define CROSSTRACK_GAIN 1 #define CROSSTRACK_GAIN 1
#define NAV_SPEED_MAX 300 // cm/sec
#define NAV_SLOW_NAV true #define NAV_SLOW_NAV true
#define NAV_BANK_MAX 3000 // 30deg max banking when navigating (just for security and testing) #define NAV_BANK_MAX 3000 // 30deg max banking when navigating (just for security and testing)
@ -201,7 +204,7 @@ static int16_t nav_takeoff_bearing;
void GPS_NewData(uint16_t c) void GPS_NewData(uint16_t c)
{ {
uint8_t axis; int axis;
static uint32_t nav_loopTimer; static uint32_t nav_loopTimer;
uint32_t dist; uint32_t dist;
int32_t dir; int32_t dir;
@ -225,7 +228,7 @@ void GPS_NewData(uint16_t c)
} }
// Apply moving average filter to GPS data // Apply moving average filter to GPS data
#if defined(GPS_FILTERING) #if defined(GPS_FILTERING)
GPS_filter_index = ++GPS_filter_index % GPS_FILTER_VECTOR_LENGTH; GPS_filter_index = (GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH;
for (axis = 0; axis < 2; axis++) { for (axis = 0; axis < 2; axis++) {
GPS_read[axis] = GPS_coord[axis]; // latest unfiltered data is in GPS_latitude and GPS_longitude GPS_read[axis] = GPS_coord[axis]; // latest unfiltered data is in GPS_latitude and GPS_longitude
GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t
@ -300,14 +303,19 @@ void GPS_NewData(uint16_t c)
void GPS_reset_home_position(void) void GPS_reset_home_position(void)
{ {
GPS_home[LAT] = GPS_coord[LAT]; if (f.GPS_FIX && GPS_numSat >= 5) {
GPS_home[LON] = GPS_coord[LON]; GPS_home[LAT] = GPS_coord[LAT];
GPS_home[LON] = GPS_coord[LON];
nav_takeoff_bearing = heading; //save takeoff heading
//Set ground altitude
f.GPS_FIX_HOME = 1;
}
} }
//reset navigation (stop the navigation processor, and clear nav) //reset navigation (stop the navigation processor, and clear nav)
void GPS_reset_nav(void) void GPS_reset_nav(void)
{ {
uint8_t i; int i;
for (i = 0; i < 2; i++) { for (i = 0; i < 2; i++) {
GPS_angle[i] = 0; GPS_angle[i] = 0;
@ -454,12 +462,12 @@ static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng,
//////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////
// Calculate nav_lat and nav_lon from the x and y error and the speed // Calculate nav_lat and nav_lon from the x and y error and the speed
// //
static void GPS_calc_poshold() static void GPS_calc_poshold(void)
{ {
int32_t p, i, d; int32_t p, i, d;
int32_t output; int32_t output;
int32_t target_speed; int32_t target_speed;
uint8_t axis; int axis;
for (axis = 0; axis < 2; axis++) { for (axis = 0; axis < 2; axis++) {
target_speed = AC_PID_get_p(&pi_poshold[axis], error[axis], &posholdPID); // calculate desired speed from lon error target_speed = AC_PID_get_p(&pi_poshold[axis], error[axis], &posholdPID); // calculate desired speed from lon error
@ -488,7 +496,7 @@ static void GPS_calc_nav_rate(int max_speed)
{ {
float trig[2]; float trig[2];
float temp; float temp;
uint8_t axis; int axis;
// push us towards the original track // push us towards the original track
GPS_update_crosstrack(); GPS_update_crosstrack();
@ -518,7 +526,7 @@ static void GPS_update_crosstrack(void)
{ {
if (abs(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following if (abs(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following
float temp = (target_bearing - original_target_bearing) * RADX100; float temp = (target_bearing - original_target_bearing) * RADX100;
crosstrack_error = sin(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line crosstrack_error = sinf(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line
nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000); nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
nav_bearing = wrap_36000(nav_bearing); nav_bearing = wrap_36000(nav_bearing);
} else { } else {
@ -560,7 +568,7 @@ static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow)
//////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////
// Utilities // Utilities
// //
static int32_t wrap_18000(int32_t error) int32_t wrap_18000(int32_t error)
{ {
if (error > 18000) if (error > 18000)
error -= 36000; error -= 36000;
@ -592,7 +600,6 @@ static int32_t wrap_36000(int32_t angle)
EOS increased the precision here, even if we think that the gps is not precise enough, with 10e5 precision it has 76cm resolution EOS increased the precision here, even if we think that the gps is not precise enough, with 10e5 precision it has 76cm resolution
with 10e7 it's around 1 cm now. Increasing it further is irrelevant, since even 1cm resolution is unrealistic, however increased with 10e7 it's around 1 cm now. Increasing it further is irrelevant, since even 1cm resolution is unrealistic, however increased
resolution also increased precision of nav calculations resolution also increased precision of nav calculations
*/
static uint32_t GPS_coord_to_degrees(char *s) static uint32_t GPS_coord_to_degrees(char *s)
{ {
char *p = s, *d = s; char *p = s, *d = s;
@ -617,6 +624,46 @@ static uint32_t GPS_coord_to_degrees(char *s)
min = *(d - 1) - '0' + (*(d - 2) - '0') * 10; // convert minutes : 2 previous char before '.' min = *(d - 1) - '0' + (*(d - 2) - '0') * 10; // convert minutes : 2 previous char before '.'
return deg * 10000000UL + (min * 100000UL + frac) * 10UL / 6; return deg * 10000000UL + (min * 100000UL + frac) * 10UL / 6;
} }
*/
#define DIGIT_TO_VAL(_x) (_x - '0')
uint32_t GPS_coord_to_degrees(char* s)
{
char *p, *q;
uint8_t deg = 0, min = 0;
unsigned int frac_min = 0;
int i;
// scan for decimal point or end of field
for (p = s; isdigit(*p); p++)
;
q = s;
// convert degrees
while ((p - q) > 2) {
if (deg)
deg *= 10;
deg += DIGIT_TO_VAL(*q++);
}
// convert minutes
while (p > q) {
if (min)
min *= 10;
min += DIGIT_TO_VAL(*q++);
}
// convert fractional minutes
// expect up to four digits, result is in
// ten-thousandths of a minute
if (*p == '.') {
q = p + 1;
for (i = 0; i < 4; i++) {
frac_min *= 10;
if (isdigit(*q))
frac_min += *q++ - '0';
}
}
return deg * 10000000UL + (min * 1000000UL + frac_min * 100UL) / 6;
}
// helper functions // helper functions
static uint32_t grab_fields(char *src, uint8_t mult) static uint32_t grab_fields(char *src, uint8_t mult)
@ -699,7 +746,7 @@ static bool GPS_newFrame(char c)
} }
} else if (frame == FRAME_RMC) { } else if (frame == FRAME_RMC) {
if (param == 7) { if (param == 7) {
GPS_speed = ((uint32_t) grab_fields(string, 1) * 514444L) / 100000L; // speed in cm/s added by Mis GPS_speed = ((uint32_t) grab_fields(string, 1) * 5144L) / 1000L; // speed in cm/s added by Mis
} }
} }
param++; param++;

12
src/imu.c
View file

@ -72,7 +72,7 @@ void computeIMU(void)
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
gyroADCinter[axis] = gyroADC[axis] + gyroADCp[axis]; gyroADCinter[axis] = gyroADC[axis] + gyroADCp[axis];
// empirical, we take a weighted value of the current and the previous values // empirical, we take a weighted value of the current and the previous values
gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis] + 1) / 3; gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis]) / 3;
gyroADCprevious[axis] = gyroADCinter[axis] / 2; gyroADCprevious[axis] = gyroADCinter[axis] / 2;
if (!sensors(SENSOR_ACC)) if (!sensors(SENSOR_ACC))
accADC[axis] = 0; accADC[axis] = 0;
@ -89,11 +89,11 @@ void computeIMU(void)
Smoothing[YAW] = (cfg.gyro_smoothing_factor) & 0xff; Smoothing[YAW] = (cfg.gyro_smoothing_factor) & 0xff;
} }
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
gyroData[axis] = (gyroSmooth[axis] * (Smoothing[axis] - 1) + gyroData[axis] + 1) / Smoothing[axis]; gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroData[axis] + 1 ) / Smoothing[axis]);
gyroSmooth[axis] = gyroData[axis]; gyroSmooth[axis] = gyroData[axis];
} }
} else if (cfg.mixerConfiguration == MULTITYPE_TRI) { } else if (cfg.mixerConfiguration == MULTITYPE_TRI) {
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW] + 1) / 3; gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW]) / 3;
gyroYawSmooth = gyroData[YAW]; gyroYawSmooth = gyroData[YAW];
} }
} }
@ -127,12 +127,6 @@ void computeIMU(void)
/* Default WMC value: n/a*/ /* Default WMC value: n/a*/
//#define MG_LPF_FACTOR 4 //#define MG_LPF_FACTOR 4
/* Set the Gyro Weight for Gyro/Acc complementary filter */
/* Increasing this value would reduce and delay Acc influence on the output of the filter*/
/* Default WMC value: 300*/
// #define GYR_CMPF_FACTOR 310.0f
// #define GYR_CMPF_FACTOR 500.0f
/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */ /* Set the Gyro Weight for Gyro/Magnetometer complementary filter */
/* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/ /* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
/* Default WMC value: n/a*/ /* Default WMC value: n/a*/

4
src/main.c
View file

@ -79,6 +79,7 @@ int main(void)
pwm_params.usePPM = feature(FEATURE_PPM); pwm_params.usePPM = feature(FEATURE_PPM);
pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
pwm_params.useServos = useServo; pwm_params.useServos = useServo;
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = cfg.motor_pwm_rate; pwm_params.motorPwmRate = cfg.motor_pwm_rate;
pwm_params.servoPwmRate = cfg.servo_pwm_rate; pwm_params.servoPwmRate = cfg.servo_pwm_rate;
@ -127,7 +128,10 @@ int main(void)
} }
previousTime = micros(); previousTime = micros();
if (cfg.mixerConfiguration == MULTITYPE_GIMBAL)
calibratingA = 400;
calibratingG = 1000; calibratingG = 1000;
f.SMALL_ANGLES_25 = 1;
// loopy // loopy
while (1) { while (1) {

51
src/mixer.c
View file

@ -12,7 +12,7 @@ void mixerInit(void)
if (cfg.mixerConfiguration == MULTITYPE_BI || cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_GIMBAL || cfg.mixerConfiguration == MULTITYPE_FLYING_WING) if (cfg.mixerConfiguration == MULTITYPE_BI || cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_GIMBAL || cfg.mixerConfiguration == MULTITYPE_FLYING_WING)
useServo = 1; useServo = 1;
// if we want camstab/trig, that also enabled servos. this is kinda lame. maybe rework feature bits later. // if we want camstab/trig, that also enabled servos. this is kinda lame. maybe rework feature bits later.
if (feature(FEATURE_SERVO_TILT) || feature(FEATURE_CAMTRIG)) if (feature(FEATURE_SERVO_TILT))
useServo = 1; useServo = 1;
switch (cfg.mixerConfiguration) { switch (cfg.mixerConfiguration) {
@ -98,12 +98,9 @@ void mixTable(void)
{ {
int16_t maxMotor; int16_t maxMotor;
uint32_t i; uint32_t i;
static uint8_t camCycle = 0;
static uint8_t camState = 0;
static uint32_t camTime = 0;
if (numberMotor > 3) { if (numberMotor > 3) {
//prevent "yaw jump" during yaw correction // prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW])); axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));
} }
@ -214,9 +211,9 @@ void mixTable(void)
break; break;
case MULTITYPE_VTAIL4: case MULTITYPE_VTAIL4:
motor[0] = PIDMIX(+0, +1, -1 / 2); //REAR_R motor[0] = PIDMIX(+0, +1, +1); //REAR_R
motor[1] = PIDMIX(-1, -1, +0); //FRONT_R motor[1] = PIDMIX(-1, -1, +0); //FRONT_R
motor[2] = PIDMIX(+0, +1, +1 / 2); //REAR_L motor[2] = PIDMIX(+0, +1, -1); //REAR_L
motor[3] = PIDMIX(+1, -1, -0); //FRONT_L motor[3] = PIDMIX(+1, -1, -0); //FRONT_L
break; break;
@ -224,19 +221,6 @@ void mixTable(void)
servo[0] = constrain(cfg.gimbal_pitch_mid + cfg.gimbal_pitch_gain * angle[PITCH] / 16 + rcCommand[PITCH], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max); servo[0] = constrain(cfg.gimbal_pitch_mid + cfg.gimbal_pitch_gain * angle[PITCH] / 16 + rcCommand[PITCH], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max);
servo[1] = constrain(cfg.gimbal_roll_mid + cfg.gimbal_roll_gain * angle[ROLL] / 16 + rcCommand[ROLL], cfg.gimbal_roll_min, cfg.gimbal_roll_max); servo[1] = constrain(cfg.gimbal_roll_mid + cfg.gimbal_roll_gain * angle[ROLL] / 16 + rcCommand[ROLL], cfg.gimbal_roll_min, cfg.gimbal_roll_max);
break; break;
case MULTITYPE_FLYING_WING:
motor[0] = rcCommand[THROTTLE];
if (f.PASSTHRU_MODE) { // do not use sensors for correction, simple 2 channel mixing
servo[0] = PITCH_DIRECTION_L * (rcData[PITCH] - cfg.midrc) + ROLL_DIRECTION_L * (rcData[ROLL] - cfg.midrc);
servo[1] = PITCH_DIRECTION_R * (rcData[PITCH] - cfg.midrc) + ROLL_DIRECTION_R * (rcData[ROLL] - cfg.midrc);
} else { // use sensors to correct (gyro only or gyro+acc according to aux1/aux2 configuration
servo[0] = PITCH_DIRECTION_L * axisPID[PITCH] + ROLL_DIRECTION_L * axisPID[ROLL];
servo[1] = PITCH_DIRECTION_R * axisPID[PITCH] + ROLL_DIRECTION_R * axisPID[ROLL];
}
servo[0] = constrain(servo[0] + cfg.wing_left_mid , WING_LEFT_MIN, WING_LEFT_MAX);
servo[1] = constrain(servo[1] + cfg.wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX);
break;
} }
// do camstab // do camstab
@ -260,29 +244,10 @@ void mixTable(void)
servo[1] = constrain(servo[1], cfg.gimbal_roll_min, cfg.gimbal_roll_max); servo[1] = constrain(servo[1], cfg.gimbal_roll_min, cfg.gimbal_roll_max);
} }
// do camtrig (this doesn't actually work) if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) {
if (feature(FEATURE_CAMTRIG)) { for (i = 0; i < 4; i++)
if (camCycle == 1) { pwmWrite(6 + i, rcData[AUX1 + i]);
if (camState == 0) { }
servo[2] = CAM_SERVO_HIGH;
camState = 1;
camTime = millis();
} else if (camState == 1) {
if ((millis() - camTime) > CAM_TIME_HIGH) {
servo[2] = CAM_SERVO_LOW;
camState = 2;
camTime = millis();
}
} else { //camState ==2
if ((millis() - camTime) > CAM_TIME_LOW) {
camState = 0;
camCycle = 0;
}
}
}
if (rcOptions[BOXCAMTRIG])
camCycle = 1;
}
maxMotor = motor[0]; maxMotor = motor[0];
for (i = 1; i < numberMotor; i++) for (i = 1; i < numberMotor; i++)

294
src/mw.c
View file

@ -1,11 +1,10 @@
#include "board.h" #include "board.h"
#include "mw.h" #include "mw.h"
// June 2012 V2-dev // July 2012 V2.1
flags_t f; flags_t f;
int16_t debug[4]; int16_t debug[4];
uint8_t buzzerState = 0;
uint8_t toggleBeep = 0; uint8_t toggleBeep = 0;
uint32_t currentTime = 0; uint32_t currentTime = 0;
uint32_t previousTime = 0; uint32_t previousTime = 0;
@ -36,15 +35,17 @@ int32_t GPS_coord[2];
int32_t GPS_home[2]; int32_t GPS_home[2];
int32_t GPS_hold[2]; int32_t GPS_hold[2];
uint8_t GPS_numSat; uint8_t GPS_numSat;
uint16_t GPS_distanceToHome, GPS_distanceToHold; // distance to home or hold point in meters uint16_t GPS_distanceToHome; // distance to home point in meters
int16_t GPS_directionToHome, GPS_directionToHold; // direction to home or hol point in degrees int16_t GPS_directionToHome; // direction to home or hol point in degrees
uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s
uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for GPS correction int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for GPS correction
uint16_t GPS_ground_course = 0; // degrees*10 uint16_t GPS_ground_course = 0; // degrees*10
uint8_t GPS_Present = 0; // Checksum from Gps serial uint8_t GPS_Present = 0; // Checksum from Gps serial
uint8_t GPS_Enable = 0;
int16_t nav[2]; int16_t nav[2];
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
int8_t nav_mode = NAV_MODE_NONE; // Navigation mode
//Automatic ACC Offset Calibration //Automatic ACC Offset Calibration
// ********************** // **********************
@ -80,8 +81,8 @@ void annexCode(void)
{ {
static uint32_t calibratedAccTime; static uint32_t calibratedAccTime;
uint16_t tmp, tmp2; uint16_t tmp, tmp2;
static uint8_t vbatTimer = 0;
static uint8_t buzzerFreq; //delay between buzzer ring static uint8_t buzzerFreq; //delay between buzzer ring
static uint8_t vbatTimer = 0;
uint8_t axis, prop1, prop2; uint8_t axis, prop1, prop2;
static uint8_t ind = 0; static uint8_t ind = 0;
uint16_t vbatRaw = 0; uint16_t vbatRaw = 0;
@ -152,12 +153,8 @@ void annexCode(void)
vbatRaw += vbatRawArray[i]; vbatRaw += vbatRawArray[i];
vbat = batteryAdcToVoltage(vbatRaw / 8); vbat = batteryAdcToVoltage(vbatRaw / 8);
} }
if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch if ((vbat > batteryWarningVoltage) || (vbat < cfg.vbatmincellvoltage)) { // VBAT ok, buzzer off
buzzerFreq = 7;
} else if ((vbat > batteryWarningVoltage) || (vbat < cfg.vbatmincellvoltage)) { //VBAT ok, buzzer off
buzzerFreq = 0; buzzerFreq = 0;
buzzerState = 0;
BEEP_OFF;
} else } else
buzzerFreq = 4; // low battery buzzerFreq = 4; // low battery
} }
@ -167,9 +164,8 @@ void annexCode(void)
if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis
LED0_TOGGLE; LED0_TOGGLE;
} else { } else {
if (f.ACC_CALIBRATED) { if (f.ACC_CALIBRATED)
LED0_OFF; LED0_OFF;
}
if (f.ARMED) if (f.ARMED)
LED0_ON; LED0_ON;
// This will switch to/from 9600 or 115200 baud depending on state. Of course, it should only do it on changes. // This will switch to/from 9600 or 115200 baud depending on state. Of course, it should only do it on changes.
@ -267,10 +263,8 @@ void loop(void)
static int16_t errorAngleI[2] = { 0, 0 }; static int16_t errorAngleI[2] = { 0, 0 };
static uint32_t rcTime = 0; static uint32_t rcTime = 0;
static int16_t initialThrottleHold; static int16_t initialThrottleHold;
static uint32_t loopTime;
#ifdef TIMINGDEBUG uint16_t auxState = 0;
trollTime = micros();
#endif
// this will return false if spektrum is disabled. shrug. // this will return false if spektrum is disabled. shrug.
if (spektrumFrameComplete()) if (spektrumFrameComplete())
@ -356,28 +350,28 @@ void loop(void)
f.CALIBRATE_MAG = 1; // MAG calibration request f.CALIBRATE_MAG = 1; // MAG calibration request
rcDelayCommand++; rcDelayCommand++;
} else if (rcData[PITCH] > cfg.maxcheck) { } else if (rcData[PITCH] > cfg.maxcheck) {
cfg.accTrim[PITCH] += 2; cfg.angleTrim[PITCH] += 2;
writeParams(1); writeParams(1);
#ifdef LEDRING #ifdef LEDRING
if (feature(FEATURE_LED_RING)) if (feature(FEATURE_LED_RING))
ledringBlink(); ledringBlink();
#endif #endif
} else if (rcData[PITCH] < cfg.mincheck) { } else if (rcData[PITCH] < cfg.mincheck) {
cfg.accTrim[PITCH] -= 2; cfg.angleTrim[PITCH] -= 2;
writeParams(1); writeParams(1);
#ifdef LEDRING #ifdef LEDRING
if (feature(FEATURE_LED_RING)) if (feature(FEATURE_LED_RING))
ledringBlink(); ledringBlink();
#endif #endif
} else if (rcData[ROLL] > cfg.maxcheck) { } else if (rcData[ROLL] > cfg.maxcheck) {
cfg.accTrim[ROLL] += 2; cfg.angleTrim[ROLL] += 2;
writeParams(1); writeParams(1);
#ifdef LEDRING #ifdef LEDRING
if (feature(FEATURE_LED_RING)) if (feature(FEATURE_LED_RING))
ledringBlink(); ledringBlink();
#endif #endif
} else if (rcData[ROLL] < cfg.mincheck) { } else if (rcData[ROLL] < cfg.mincheck) {
cfg.accTrim[ROLL] -= 2; cfg.angleTrim[ROLL] -= 2;
writeParams(1); writeParams(1);
#ifdef LEDRING #ifdef LEDRING
if (feature(FEATURE_LED_RING)) if (feature(FEATURE_LED_RING))
@ -393,22 +387,19 @@ void loop(void)
InflightcalibratingA = 50; InflightcalibratingA = 50;
AccInflightCalibrationArmed = 0; AccInflightCalibrationArmed = 0;
} }
if (rcOptions[BOXPASSTHRU]) { // Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored if (rcOptions[BOXPASSTHRU]) { // Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
if (!AccInflightCalibrationArmed) { if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
AccInflightCalibrationArmed = 1;
InflightcalibratingA = 50; InflightcalibratingA = 50;
}
} else if (AccInflightCalibrationMeasurementDone && !f.ARMED) { } else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
AccInflightCalibrationArmed = 0;
AccInflightCalibrationMeasurementDone = 0; AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1; AccInflightCalibrationSavetoEEProm = 1;
} }
} }
for (i = 0; i < CHECKBOXITEMS; i++) { for(i = 0; i < 4; i++)
rcOptions[i] = (((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) << 1 | (rcData[AUX1] > 1700) << 2 auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | (rcData[AUX1 + i] > 1700) << (3 * i + 2);
| (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5 | (rcData[AUX3] < 1300) << 6 | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 7 | (rcData[AUX3] > 1700) << 8 | (rcData[AUX4] < 1300) << 9 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 10 | (rcData[AUX4] > 1700) << 11) & cfg.activate[i]) > 0; for(i = 0; i < CHECKBOXITEMS; i++)
} rcOptions[i] = (auxState & cfg.activate[i]) > 0;
// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false // note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
if ((rcOptions[BOXACC] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) { if ((rcOptions[BOXACC] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) {
@ -457,38 +448,45 @@ void loop(void)
if (!f.HEADFREE_MODE) { if (!f.HEADFREE_MODE) {
f.HEADFREE_MODE = 1; f.HEADFREE_MODE = 1;
} }
} else } else {
f.HEADFREE_MODE = 0; f.HEADFREE_MODE = 0;
}
if (rcOptions[BOXHEADADJ]) {
headFreeModeHold = heading; // acquire new heading
}
} }
#endif #endif
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
if (rcOptions[BOXGPSHOME]) { if (f.GPS_FIX && GPS_numSat >= 5) {
if (!f.GPS_HOME_MODE) { if (rcOptions[BOXGPSHOME]) {
f.GPS_HOME_MODE = 1; if (!f.GPS_HOME_MODE) {
GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]); f.GPS_HOME_MODE = 1;
nav_mode = NAV_MODE_WP; GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
nav_mode = NAV_MODE_WP;
}
} else {
f.GPS_HOME_MODE = 0;
} }
} else { if (rcOptions[BOXGPSHOLD]) {
f.GPS_HOME_MODE = 0; if (!f.GPS_HOLD_MODE) {
} f.GPS_HOLD_MODE = 1;
if (rcOptions[BOXGPSHOLD]) { GPS_hold[LAT] = GPS_coord[LAT];
if (!f.GPS_HOLD_MODE) { GPS_hold[LON] = GPS_coord[LON];
f.GPS_HOLD_MODE = 1; GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
GPS_hold[LAT] = GPS_coord[LAT]; nav_mode = NAV_MODE_POSHOLD;
GPS_hold[LON] = GPS_coord[LON]; }
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]); } else {
nav_mode = NAV_MODE_POSHOLD; f.GPS_HOLD_MODE = 0;
} }
} else {
f.GPS_HOLD_MODE = 0;
} }
} }
if (rcOptions[BOXPASSTHRU]) { if (rcOptions[BOXPASSTHRU]) {
f.PASSTHRU_MODE = 1; f.PASSTHRU_MODE = 1;
} else } else {
f.PASSTHRU_MODE = 0; f.PASSTHRU_MODE = 0;
}
} else { // not in rc loop } else { // not in rc loop
static int8_t taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes static int8_t taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes
switch (taskOrder++ % 4) { switch (taskOrder++ % 4) {
@ -524,107 +522,105 @@ void loop(void)
} }
} }
computeIMU();
// Measure loop rate just afer reading the sensors
currentTime = micros(); currentTime = micros();
cycleTime = currentTime - previousTime; if (currentTime > loopTime) {
previousTime = currentTime; loopTime = currentTime + cfg.looptime;
#ifdef MPU6050_DMP computeIMU();
mpu6050DmpLoop(); // Measure loop rate just afer reading the sensors
#endif currentTime = micros();
cycleTime = currentTime - previousTime;
#ifdef MAG previousTime = currentTime;
if (sensors(SENSOR_MAG)) {
if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) { #ifdef MPU6050_DMP
int16_t dif = heading - magHold; mpu6050DmpLoop();
if (dif <= -180) #endif
dif += 360;
if (dif >= +180) #ifdef MAG
dif -= 360; if (sensors(SENSOR_MAG)) {
if (f.SMALL_ANGLES_25) if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) {
rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; // 18 deg int16_t dif = heading - magHold;
} else if (dif <= -180)
magHold = heading; dif += 360;
} if (dif >= +180)
#endif dif -= 360;
if (f.SMALL_ANGLES_25)
#ifdef BARO rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; // 18 deg
if (sensors(SENSOR_BARO)) { } else
if (f.BARO_MODE) { magHold = heading;
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > 20) { }
f.BARO_MODE = 0; // so that a new althold reference is defined #endif
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (f.BARO_MODE) {
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > 20) {
f.BARO_MODE = 0; // so that a new althold reference is defined
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
} }
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
} }
} #endif
#endif
if (sensors(SENSOR_GPS)) {
if (sensors(SENSOR_GPS)) { // Check that we really need to navigate ?
// Check that we really need to navigate ? if ((!f.GPS_HOME_MODE && !f.GPS_HOLD_MODE) || (!f.GPS_FIX_HOME)) {
if ((!f.GPS_HOME_MODE && !f.GPS_HOLD_MODE) || (!f.GPS_FIX_HOME)) { // If not. Reset nav loops and all nav related parameters
// If not. Reset nav loops and all nav related parameters GPS_reset_nav();
GPS_reset_nav(); } else {
} else { float sin_yaw_y = sinf(heading * 0.0174532925f);
float sin_yaw_y = sinf(heading * 0.0174532925f); float cos_yaw_x = cosf(heading * 0.0174532925f);
float cos_yaw_x = cosf(heading * 0.0174532925f); if (cfg.nav_slew_rate) {
GPS_angle[ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 10; nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -cfg.nav_slew_rate, cfg.nav_slew_rate); // TODO check this on uint8
GPS_angle[PITCH] = (nav[LON] * sin_yaw_y + nav[LAT] * cos_yaw_x) / 10; nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]),-cfg.nav_slew_rate, cfg.nav_slew_rate);
GPS_angle[ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10;
GPS_angle[PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10;
} else {
GPS_angle[ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 10;
GPS_angle[PITCH] = (nav[LON] * sin_yaw_y + nav[LAT] * cos_yaw_x) / 10;
}
}
} }
} // **** PITCH & ROLL & YAW PID ****
// **** PITCH & ROLL & YAW PID **** for (axis = 0; axis < 3; axis++) {
for (axis = 0; axis < 3; axis++) { if (f.ACC_MODE && axis < 2) { // LEVEL MODE
if (f.ACC_MODE == 1 && axis < 2) { // LEVEL MODE // 50 degrees max inclination
// 50 degrees max inclination errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis];
errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + cfg.accTrim[axis]; //16 bits is ok here #ifdef LEVEL_PDF
#ifdef LEVEL_PDF PTerm = -(int32_t) angle[axis] * cfg.P8[PIDLEVEL] / 100;
PTerm = -(int32_t) angle[axis] * cfg.P8[PIDLEVEL] / 100; #else
#else PTerm = (int32_t) errorAngle *cfg.P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTerm = (int32_t) errorAngle *cfg.P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result #endif
#endif PTerm = constrain(PTerm, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
PTerm = constrain(PTerm, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp // 16 bits is ok here
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp // 16 bits is ok here ITerm = ((int32_t) errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
ITerm = ((int32_t) errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result } else { // ACRO MODE or YAW axis
} else { // ACRO MODE or YAW axis error = (int32_t) rcCommand[axis] * 10 * 8 / cfg.P8[axis]; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
error = (int32_t) rcCommand[axis] * 10 * 8 / cfg.P8[axis]; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2) error -= gyroData[axis];
error -= gyroData[axis];
PTerm = rcCommand[axis];
PTerm = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp // 16 bits is ok here
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp // 16 bits is ok here if (abs(gyroData[axis]) > 640)
if (abs(gyroData[axis]) > 640) errorGyroI[axis] = 0;
errorGyroI[axis] = 0; ITerm = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
ITerm = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 }
PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis]; //16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = ((int32_t) deltaSum * dynD8[axis]) >> 5; //32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm;
} }
PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
mixTable();
delta = gyroData[axis] - lastGyro[axis]; //16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 writeServos();
lastGyro[axis] = gyroData[axis]; writeMotors();
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = ((int32_t) deltaSum * dynD8[axis]) >> 5; //32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm;
} }
mixTable();
writeServos();
writeMotors();
#ifdef TIMINGDEBUG
while (micros() < trollTime + 1750);
// LED0_TOGGLE;
{
if (cycleTime < cn)
cn = cycleTime;
if (cycleTime > cx)
cx = cycleTime;
debug1 = cn;
debug2 = cx;
}
#endif
} }

50
src/mw.h
View file

@ -1,28 +1,9 @@
#pragma once #pragma once
/* Flying Wing: you can change change servo orientation and servo min/max values here */
/* valid for all flight modes, even passThrough mode */
/* need to setup servo directions here; no need to swap servos amongst channels at rx */
#define PITCH_DIRECTION_L 1 // left servo - pitch orientation
#define PITCH_DIRECTION_R -1 // right servo - pitch orientation (opposite sign to PITCH_DIRECTION_L, if servos are mounted in mirrored orientation)
#define ROLL_DIRECTION_L 1 // left servo - roll orientation
#define ROLL_DIRECTION_R 1 // right servo - roll orientation (same sign as ROLL_DIRECTION_L, if servos are mounted in mirrored orientation)
#define WING_LEFT_MIN 1020 // limit servo travel range must be inside [1020;2000]
#define WING_LEFT_MAX 2000 // limit servo travel range must be inside [1020;2000]
#define WING_RIGHT_MIN 1020 // limit servo travel range must be inside [1020;2000]
#define WING_RIGHT_MAX 2000 // limit servo travel range must be inside [1020;2000]
/* experimental
camera trigger function : activated via Rc Options in the GUI, servo output=A2 on promini */
#define CAM_SERVO_HIGH 2000 // the position of HIGH state servo
#define CAM_SERVO_LOW 1020 // the position of LOW state servo
#define CAM_TIME_HIGH 1000 // the duration of HIGH state servo expressed in ms
#define CAM_TIME_LOW 1000 // the duration of LOW state servo expressed in ms
/* for VBAT monitoring frequency */ /* for VBAT monitoring frequency */
#define VBATFREQ 6 // to read battery voltage - nth number of loop iterations #define VBATFREQ 6 // to read battery voltage - nth number of loop iterations
#define VERSION 200 #define VERSION 210
#define LAT 0 #define LAT 0
#define LON 1 #define LON 1
@ -31,7 +12,7 @@
// navigation mode // navigation mode
typedef enum NavigationMode typedef enum NavigationMode
{ {
NAV_MODE_NONE = 1, NAV_MODE_NONE = 0,
NAV_MODE_POSHOLD, NAV_MODE_POSHOLD,
NAV_MODE_WP NAV_MODE_WP
} NavigationMode; } NavigationMode;
@ -46,7 +27,7 @@ typedef enum MultiType
MULTITYPE_GIMBAL = 5, MULTITYPE_GIMBAL = 5,
MULTITYPE_Y6 = 6, MULTITYPE_Y6 = 6,
MULTITYPE_HEX6 = 7, MULTITYPE_HEX6 = 7,
MULTITYPE_FLYING_WING = 8, MULTITYPE_FLYING_WING = 8, // UNSUPPORTED, do not select!
MULTITYPE_Y4 = 9, MULTITYPE_Y4 = 9,
MULTITYPE_HEX6X = 10, MULTITYPE_HEX6X = 10,
MULTITYPE_OCTOX8 = 11, MULTITYPE_OCTOX8 = 11,
@ -63,6 +44,7 @@ typedef enum GimbalFlags {
GIMBAL_NORMAL = 1 << 0, GIMBAL_NORMAL = 1 << 0,
GIMBAL_TILTONLY = 1 << 1, GIMBAL_TILTONLY = 1 << 1,
GIMBAL_DISABLEAUX34 = 1 << 2, GIMBAL_DISABLEAUX34 = 1 << 2,
GIMBAL_FORWARDAUX = 1 << 3,
} GimbalFlags; } GimbalFlags;
/*********** RC alias *****************/ /*********** RC alias *****************/
@ -94,12 +76,9 @@ typedef enum GimbalFlags {
#define BOXPASSTHRU 8 #define BOXPASSTHRU 8
#define BOXHEADFREE 9 #define BOXHEADFREE 9
#define BOXBEEPERON 10 #define BOXBEEPERON 10
/* we want maximum illumination */ #define BOXLEDMAX 11 // we want maximum illumination
#define BOXLEDMAX 11 #define BOXLLIGHTS 12 // enable landing lights at any altitude
/* enable landing lights at any altitude */ #define BOXHEADADJ 13 // acquire heading for HEADFREE mode
#define BOXLLIGHTS 12
/* acquire heading for HEADFREE mode */
#define BOXHEADADJ 13
#define PIDITEMS 10 #define PIDITEMS 10
#define CHECKBOXITEMS 14 #define CHECKBOXITEMS 14
@ -114,6 +93,8 @@ typedef struct config_t {
uint8_t mixerConfiguration; uint8_t mixerConfiguration;
uint32_t enabledFeatures; uint32_t enabledFeatures;
uint16_t looptime; // imu loop time in us
uint8_t P8[PIDITEMS]; uint8_t P8[PIDITEMS];
uint8_t I8[PIDITEMS]; uint8_t I8[PIDITEMS];
uint8_t D8[PIDITEMS]; uint8_t D8[PIDITEMS];
@ -130,7 +111,7 @@ typedef struct config_t {
int16_t accZero[3]; int16_t accZero[3];
int16_t magZero[3]; int16_t magZero[3];
int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/
int16_t accTrim[2]; int16_t angleTrim[2]; // accelerometer trim
// sensor-related stuff // sensor-related stuff
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
@ -148,6 +129,7 @@ typedef struct config_t {
uint8_t rcmap[8]; // mapping of radio channels to internal RPYTA+ order uint8_t rcmap[8]; // mapping of radio channels to internal RPYTA+ order
uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
uint8_t yawdeadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. uint8_t yawdeadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
uint8_t alt_hold_throttle_neutral; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-20
uint8_t spektrum_hires; // spektrum high-resolution y/n (1024/2048bit) uint8_t spektrum_hires; // spektrum high-resolution y/n (1024/2048bit)
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
uint16_t mincheck; // minimum rc end uint16_t mincheck; // minimum rc end
@ -168,8 +150,6 @@ typedef struct config_t {
// mixer-related configuration // mixer-related configuration
int8_t yaw_direction; int8_t yaw_direction;
uint16_t wing_left_mid; // left servo center pos. - use this for initial trim
uint16_t wing_right_mid; // right servo center pos. - use this for initial trim
uint16_t tri_yaw_middle; // tail servo center pos. - use this for initial trim uint16_t tri_yaw_middle; // tail servo center pos. - use this for initial trim
uint16_t tri_yaw_min; // tail servo min uint16_t tri_yaw_min; // tail servo min
uint16_t tri_yaw_max; // tail servo max uint16_t tri_yaw_max; // tail servo max
@ -189,6 +169,7 @@ typedef struct config_t {
uint32_t gps_baudrate; uint32_t gps_baudrate;
uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm) uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
uint8_t gps_lpf; // Low pass filter cut frequency for derivative calculation (default 20Hz) uint8_t gps_lpf; // Low pass filter cut frequency for derivative calculation (default 20Hz)
uint8_t nav_slew_rate; // Adds a rate control to nav output, will smoothen out nav angle spikes
uint8_t nav_controls_heading; // copter faces toward the navigation point, maghold must be enabled for it uint8_t nav_controls_heading; // copter faces toward the navigation point, maghold must be enabled for it
uint16_t nav_speed_min; // cm/sec uint16_t nav_speed_min; // cm/sec
uint16_t nav_speed_max; // cm/sec uint16_t nav_speed_max; // cm/sec
@ -257,15 +238,17 @@ extern int32_t GPS_coord[2];
extern int32_t GPS_home[2]; extern int32_t GPS_home[2];
extern int32_t GPS_hold[2]; extern int32_t GPS_hold[2];
extern uint8_t GPS_numSat; extern uint8_t GPS_numSat;
extern uint16_t GPS_distanceToHome,GPS_distanceToHold; // distance to home or hold point in meters extern uint16_t GPS_distanceToHome; // distance to home or hold point in meters
extern int16_t GPS_directionToHome,GPS_directionToHold; // direction to home or hol point in degrees extern int16_t GPS_directionToHome; // direction to home or hol point in degrees
extern uint16_t GPS_altitude,GPS_speed; // altitude in 0.1m and speed in 0.1m/s extern uint16_t GPS_altitude,GPS_speed; // altitude in 0.1m and speed in 0.1m/s
extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update
extern int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction extern int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
extern uint16_t GPS_ground_course; // degrees*10 extern uint16_t GPS_ground_course; // degrees*10
extern uint8_t GPS_Present; // Checksum from Gps serial extern uint8_t GPS_Present; // Checksum from Gps serial
extern uint8_t GPS_Enable;
extern int16_t nav[2]; extern int16_t nav[2];
extern int8_t nav_mode; // Navigation mode extern int8_t nav_mode; // Navigation mode
extern int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
extern config_t cfg; extern config_t cfg;
extern flags_t f; extern flags_t f;
@ -335,6 +318,7 @@ void gpsInit(uint32_t baudrate);
void GPS_reset_home_position(void); void GPS_reset_home_position(void);
void GPS_reset_nav(void); void GPS_reset_nav(void);
void GPS_set_next_wp(int32_t* lat, int32_t* lon); void GPS_set_next_wp(int32_t* lat, int32_t* lon);
int32_t wrap_18000(int32_t error);
// telemetry // telemetry
void initTelemetry(bool State); void initTelemetry(bool State);

27
src/sensors.c
View file

@ -138,7 +138,7 @@ void batteryInit(void)
static void ACC_Common(void) static void ACC_Common(void)
{ {
static int32_t a[3]; static int32_t a[3];
uint32_t axis; int axis;
if (calibratingA > 0) { if (calibratingA > 0) {
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
@ -156,8 +156,8 @@ static void ACC_Common(void)
cfg.accZero[ROLL] = a[ROLL] / 400; cfg.accZero[ROLL] = a[ROLL] / 400;
cfg.accZero[PITCH] = a[PITCH] / 400; cfg.accZero[PITCH] = a[PITCH] / 400;
cfg.accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G cfg.accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G
cfg.accTrim[ROLL] = 0; cfg.angleTrim[ROLL] = 0;
cfg.accTrim[PITCH] = 0; cfg.angleTrim[PITCH] = 0;
writeParams(1); // write accZero in EEPROM writeParams(1); // write accZero in EEPROM
} }
calibratingA--; calibratingA--;
@ -166,17 +166,16 @@ static void ACC_Common(void)
if (feature(FEATURE_INFLIGHT_ACC_CAL)) { if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
static int32_t b[3]; static int32_t b[3];
static int16_t accZero_saved[3] = { 0, 0, 0 }; static int16_t accZero_saved[3] = { 0, 0, 0 };
static int16_t accTrim_saved[2] = { 0, 0 }; static int16_t angleTrim_saved[2] = { 0, 0 };
// Saving old zeropoints before measurement // Saving old zeropoints before measurement
if (InflightcalibratingA == 50) { if (InflightcalibratingA == 50) {
accZero_saved[ROLL] = cfg.accZero[ROLL]; accZero_saved[ROLL] = cfg.accZero[ROLL];
accZero_saved[PITCH] = cfg.accZero[PITCH]; accZero_saved[PITCH] = cfg.accZero[PITCH];
accZero_saved[YAW] = cfg.accZero[YAW]; accZero_saved[YAW] = cfg.accZero[YAW];
accTrim_saved[ROLL] = cfg.accTrim[ROLL]; angleTrim_saved[ROLL] = cfg.angleTrim[ROLL];
accTrim_saved[PITCH] = cfg.accTrim[PITCH]; angleTrim_saved[PITCH] = cfg.angleTrim[PITCH];
} }
if (InflightcalibratingA > 0) { if (InflightcalibratingA > 0) {
uint8_t axis;
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration // Reset a[axis] at start of calibration
if (InflightcalibratingA == 50) if (InflightcalibratingA == 50)
@ -191,13 +190,13 @@ static void ACC_Common(void)
if (InflightcalibratingA == 1) { if (InflightcalibratingA == 1) {
AccInflightCalibrationActive = 0; AccInflightCalibrationActive = 0;
AccInflightCalibrationMeasurementDone = 1; AccInflightCalibrationMeasurementDone = 1;
toggleBeep = 2; //buzzer for indicatiing the end of calibration toggleBeep = 2; // buzzer for indicatiing the end of calibration
// recover saved values to maintain current flight behavior until new values are transferred // recover saved values to maintain current flight behavior until new values are transferred
cfg.accZero[ROLL] = accZero_saved[ROLL]; cfg.accZero[ROLL] = accZero_saved[ROLL];
cfg.accZero[PITCH] = accZero_saved[PITCH]; cfg.accZero[PITCH] = accZero_saved[PITCH];
cfg.accZero[YAW] = accZero_saved[YAW]; cfg.accZero[YAW] = accZero_saved[YAW];
cfg.accTrim[ROLL] = accTrim_saved[ROLL]; cfg.angleTrim[ROLL] = angleTrim_saved[ROLL];
cfg.accTrim[PITCH] = accTrim_saved[PITCH]; cfg.angleTrim[PITCH] = angleTrim_saved[PITCH];
} }
InflightcalibratingA--; InflightcalibratingA--;
} }
@ -207,8 +206,8 @@ static void ACC_Common(void)
cfg.accZero[ROLL] = b[ROLL] / 50; cfg.accZero[ROLL] = b[ROLL] / 50;
cfg.accZero[PITCH] = b[PITCH] / 50; cfg.accZero[PITCH] = b[PITCH] / 50;
cfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G cfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G
cfg.accTrim[ROLL] = 0; cfg.angleTrim[ROLL] = 0;
cfg.accTrim[PITCH] = 0; cfg.angleTrim[PITCH] = 0;
writeParams(1); // write accZero in EEPROM writeParams(1); // write accZero in EEPROM
} }
} }
@ -254,7 +253,7 @@ void Baro_update(void)
case 2: case 2:
bmp085_start_up(); bmp085_start_up();
baroState++; baroState++;
baroDeadline += 14000; baroDeadline += 26000;
break; break;
case 3: case 3:
baroUP = bmp085_get_up(); baroUP = bmp085_get_up();
@ -272,7 +271,7 @@ static void GYRO_Common(void)
{ {
static int16_t previousGyroADC[3] = { 0, 0, 0 }; static int16_t previousGyroADC[3] = { 0, 0, 0 };
static int32_t g[3]; static int32_t g[3];
uint32_t axis; int axis;
if (calibratingG > 0) { if (calibratingG > 0) {
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {

55
src/serial.c
View file

@ -23,6 +23,7 @@
#define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI #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_BOXNAMES 116 //out message the aux switch names
#define MSP_PIDNAMES 117 //out message the PID 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_SET_RAW_RC 200 //in message 8 rc chan #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_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
@ -33,6 +34,7 @@
#define MSP_MAG_CALIBRATION 206 //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_SET_MISC 207 //in message powermeter trig + 8 free for future use
#define MSP_RESET_CONF 208 //in message no param #define MSP_RESET_CONF 208 //in message no param
#define MSP_WP_SET 209 //in message sets a given WP (WP#,lat, lon, alt, flags)
#define MSP_EEPROM_WRITE 250 //in message no param #define MSP_EEPROM_WRITE 250 //in message no param
@ -71,6 +73,8 @@ static const char pidnames[] =
static uint8_t checksum, indRX, inBuf[INBUF_SIZE]; static uint8_t checksum, indRX, inBuf[INBUF_SIZE];
static uint8_t cmdMSP; static uint8_t cmdMSP;
static bool guiConnected = false; static bool guiConnected = false;
// signal that we're in cli mode
uint8_t cliMode = 0;
void serialize32(uint32_t a) void serialize32(uint32_t a)
{ {
@ -148,8 +152,6 @@ void headSerialError(uint8_t s)
void tailSerialReply(void) void tailSerialReply(void)
{ {
serialize8(checksum); serialize8(checksum);
// no need to send
// UartSendData();
} }
void serializeNames(const char *s) void serializeNames(const char *s)
@ -159,9 +161,6 @@ void serializeNames(const char *s)
serialize8(*c); serialize8(*c);
} }
// signal that we're in cli mode
uint8_t cliMode = 0;
void serialInit(uint32_t baudrate) void serialInit(uint32_t baudrate)
{ {
uartInit(baudrate); uartInit(baudrate);
@ -170,7 +169,8 @@ void serialInit(uint32_t baudrate)
static void evaluateCommand(void) static void evaluateCommand(void)
{ {
uint32_t i; uint32_t i;
uint8_t wp_no;
switch (cmdMSP) { switch (cmdMSP) {
case MSP_SET_RAW_RC: case MSP_SET_RAW_RC:
for (i = 0; i < 8; i++) for (i = 0; i < 8; i++)
@ -225,7 +225,9 @@ static void evaluateCommand(void)
serialize16(cycleTime); serialize16(cycleTime);
serialize16(i2cGetErrorCounter()); serialize16(i2cGetErrorCounter());
serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
serialize32(f.ACC_MODE << BOXACC | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.ARMED << BOXARM | rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG | f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.HEADFREE_MODE << BOXHEADFREE | f.PASSTHRU_MODE << BOXPASSTHRU | rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXHEADADJ] << BOXHEADADJ); serialize32(f.ACC_MODE << BOXACC | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.ARMED << BOXARM | rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.HEADFREE_MODE << BOXHEADFREE | f.PASSTHRU_MODE << BOXPASSTHRU |
rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXHEADADJ] << BOXHEADADJ);
break; break;
case MSP_RAW_IMU: case MSP_RAW_IMU:
headSerialReply(18); headSerialReply(18);
@ -322,6 +324,23 @@ static void evaluateCommand(void)
for (i = 0; i < 8; i++) for (i = 0; i < 8; i++)
serialize8(i + 1); serialize8(i + 1);
break; break;
case MSP_WP:
wp_no = read8(); // get the wp number
headSerialReply(12);
if (wp_no == 0) {
serialize8(0); // wp0
serialize32(GPS_home[LAT]);
serialize32(GPS_home[LON]);
serialize16(0); // altitude will come here
serialize8(0); // nav flag will come here
} else if (wp_no == 16) {
serialize8(16); // wp16
serialize32(GPS_hold[LAT]);
serialize32(GPS_hold[LON]);
serialize16(0); // altitude will come here
serialize8(0); // nav flag will come here
}
break;
case MSP_RESET_CONF: case MSP_RESET_CONF:
checkFirstTime(true); checkFirstTime(true);
headSerialReply(0); headSerialReply(0);
@ -350,6 +369,18 @@ static void evaluateCommand(void)
tailSerialReply(); tailSerialReply();
} }
// evaluate all other incoming serial data
static void evaluateOtherData(uint8_t sr)
{
switch (sr) {
case '#':
cliProcess();
break;
case 'R':
systemReset(true); // reboot to bootloader
break;
}
}
void serialCom(void) void serialCom(void)
{ {
@ -375,13 +406,9 @@ void serialCom(void)
c = uartRead(); c = uartRead();
if (c_state == IDLE) { if (c_state == IDLE) {
// still compliant with older single octet command
// enable CLI
if (c == '#')
cliProcess();
else if (c == 'R')
systemReset(true); // reboot to bootloader
c_state = (c == '$') ? HEADER_START : IDLE; c_state = (c == '$') ? HEADER_START : IDLE;
if (c_state == IDLE)
evaluateOtherData(c); // evaluate all other incoming serial data
} else if (c_state == HEADER_START) { } else if (c_state == HEADER_START) {
c_state = (c == 'M') ? HEADER_M : IDLE; c_state = (c == 'M') ? HEADER_M : IDLE;
} else if (c_state == HEADER_M) { } else if (c_state == HEADER_M) {
@ -412,7 +439,7 @@ void serialCom(void)
c_state = IDLE; c_state = IDLE;
} }
} }
if (!cliMode && !uartAvailable() && feature(FEATURE_TELEMETRY) && f.ARMED) { //The first 2 conditions should never evaluate to true but I'm putting it here anyway - silpstream if (!cliMode && !uartAvailable() && feature(FEATURE_TELEMETRY) && f.ARMED) { // The first 2 conditions should never evaluate to true but I'm putting it here anyway - silpstream
sendTelemetry(); sendTelemetry();
return; return;
} }