mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 09:16:01 +03:00
readded support for failsafe (thanks rimshotcopter for bugging me about it for a month)
new config ooptions for failsafe, so current settings are cleared fixed mistake where yaw servo stuff for tri was still hardcoded even though it had configurable values in cli reduced level default D to 20 from 100 (dunno what effect this has, shrug) untested, enable feature FAILSAFE at your own risk. git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@155 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop
parent
6da3320be8
commit
18db641282
13 changed files with 5303 additions and 5182 deletions
|
|
@ -768,10 +768,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>17</ColumnNumber>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>192</TopLine>
|
||||
<CurrentLine>192</CurrentLine>
|
||||
<TopLine>70</TopLine>
|
||||
<CurrentLine>98</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\cli.c</PathWithFileName>
|
||||
<FilenameWithoutPath>cli.c</FilenameWithoutPath>
|
||||
|
|
@ -784,8 +784,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>186</TopLine>
|
||||
<CurrentLine>186</CurrentLine>
|
||||
<TopLine>152</TopLine>
|
||||
<CurrentLine>165</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\config.c</PathWithFileName>
|
||||
<FilenameWithoutPath>config.c</FilenameWithoutPath>
|
||||
|
|
@ -812,8 +812,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>93</TopLine>
|
||||
<CurrentLine>106</CurrentLine>
|
||||
<TopLine>55</TopLine>
|
||||
<CurrentLine>84</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\main.c</PathWithFileName>
|
||||
<FilenameWithoutPath>main.c</FilenameWithoutPath>
|
||||
|
|
@ -824,10 +824,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<ColumnNumber>120</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>190</TopLine>
|
||||
<CurrentLine>211</CurrentLine>
|
||||
<TopLine>110</TopLine>
|
||||
<CurrentLine>123</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\mixer.c</PathWithFileName>
|
||||
<FilenameWithoutPath>mixer.c</FilenameWithoutPath>
|
||||
|
|
@ -838,10 +838,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>20</ColumnNumber>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>68</TopLine>
|
||||
<CurrentLine>89</CurrentLine>
|
||||
<TopLine>193</TopLine>
|
||||
<CurrentLine>223</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\mw.c</PathWithFileName>
|
||||
<FilenameWithoutPath>mw.c</FilenameWithoutPath>
|
||||
|
|
@ -868,8 +868,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>199</TopLine>
|
||||
<CurrentLine>199</CurrentLine>
|
||||
<TopLine>337</TopLine>
|
||||
<CurrentLine>355</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\serial.c</PathWithFileName>
|
||||
<FilenameWithoutPath>serial.c</FilenameWithoutPath>
|
||||
|
|
@ -880,10 +880,10 @@
|
|||
<FileType>5</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>12</ColumnNumber>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>1</CurrentLine>
|
||||
<TopLine>33</TopLine>
|
||||
<CurrentLine>47</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\board.h</PathWithFileName>
|
||||
<FilenameWithoutPath>board.h</FilenameWithoutPath>
|
||||
|
|
@ -894,10 +894,10 @@
|
|||
<FileType>5</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<ColumnNumber>16</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>101</TopLine>
|
||||
<CurrentLine>122</CurrentLine>
|
||||
<TopLine>240</TopLine>
|
||||
<CurrentLine>240</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\mw.h</PathWithFileName>
|
||||
<FilenameWithoutPath>mw.h</FilenameWithoutPath>
|
||||
|
|
@ -922,10 +922,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<ColumnNumber>8</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<TopLine>35</TopLine>
|
||||
<CurrentLine>46</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\spektrum.c</PathWithFileName>
|
||||
<FilenameWithoutPath>spektrum.c</FilenameWithoutPath>
|
||||
|
|
@ -936,10 +936,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<ColumnNumber>10</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>5</CurrentLine>
|
||||
<CurrentLine>35</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\buzzer.c</PathWithFileName>
|
||||
<FilenameWithoutPath>buzzer.c</FilenameWithoutPath>
|
||||
|
|
@ -1001,8 +1001,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>39</TopLine>
|
||||
<CurrentLine>60</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_hmc5883l.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_hmc5883l.c</FilenameWithoutPath>
|
||||
|
|
@ -1013,10 +1013,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>12</ColumnNumber>
|
||||
<ColumnNumber>1</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>241</TopLine>
|
||||
<CurrentLine>258</CurrentLine>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>15</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_i2c.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_i2c.c</FilenameWithoutPath>
|
||||
|
|
@ -1029,8 +1029,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<TopLine>7</TopLine>
|
||||
<CurrentLine>27</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_mpu3050.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_mpu3050.c</FilenameWithoutPath>
|
||||
|
|
@ -1039,12 +1039,12 @@
|
|||
<GroupNumber>2</GroupNumber>
|
||||
<FileNumber>20</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>351</TopLine>
|
||||
<CurrentLine>361</CurrentLine>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>1</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_pwm.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_pwm.c</FilenameWithoutPath>
|
||||
|
|
@ -1057,8 +1057,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>42</TopLine>
|
||||
<CurrentLine>63</CurrentLine>
|
||||
<TopLine>101</TopLine>
|
||||
<CurrentLine>101</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_system.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_system.c</FilenameWithoutPath>
|
||||
|
|
@ -1097,10 +1097,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>26</ColumnNumber>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>147</TopLine>
|
||||
<CurrentLine>167</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_mpu6050.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_mpu6050.c</FilenameWithoutPath>
|
||||
|
|
@ -1316,8 +1316,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>133</TopLine>
|
||||
<CurrentLine>133</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\baseflight_startups\startup_stm32f10x_md.s</PathWithFileName>
|
||||
<FilenameWithoutPath>startup_stm32f10x_md.s</FilenameWithoutPath>
|
||||
|
|
|
|||
5112
obj/baseflight.hex
5112
obj/baseflight.hex
File diff suppressed because it is too large
Load diff
File diff suppressed because it is too large
Load diff
|
|
@ -36,7 +36,8 @@ typedef enum {
|
|||
FEATURE_CAMTRIG = 1 << 6,
|
||||
FEATURE_GYRO_SMOOTHING = 1 << 7,
|
||||
FEATURE_LED_RING = 1 << 8,
|
||||
FEATURE_GPS = 1 << 9
|
||||
FEATURE_GPS = 1 << 9,
|
||||
FEATURE_FAILSAFE = 1 << 10
|
||||
} AvailableFeatures;
|
||||
|
||||
typedef void (* sensorInitFuncPtr)(void); // sensor init prototype
|
||||
|
|
|
|||
21
src/buzzer.c
21
src/buzzer.c
|
|
@ -24,17 +24,18 @@ void buzzer(uint8_t warn_vbat)
|
|||
beeperOnBox = 0;
|
||||
}
|
||||
//===================== Beeps for failsafe =====================
|
||||
#if defined(FAILSAFE)
|
||||
if (failsafeCnt > (5 * FAILSAVE_DELAY) && armed == 1) {
|
||||
warn_failsafe = 1; //set failsafe warning level to 1 while landing
|
||||
if (failsafeCnt > 5 * (FAILSAVE_DELAY + FAILSAVE_OFF_DELAY))
|
||||
warn_failsafe = 2; //start "find me" signal after landing
|
||||
if (feature(FEATURE_FAILSAFE)) {
|
||||
if (failsafeCnt > (5 * cfg.failsafe_delay) && armed == 1) {
|
||||
warn_failsafe = 1; //set failsafe warning level to 1 while landing
|
||||
if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay))
|
||||
warn_failsafe = 2; //start "find me" signal after landing
|
||||
}
|
||||
if (failsafeCnt > (5 * cfg.failsafe_delay) && armed == 0)
|
||||
warn_failsafe = 2; // tx turned off while motors are off: start "find me" signal
|
||||
if (failsafeCnt == 0)
|
||||
warn_failsafe = 0; // turn off alarm if TX is okay
|
||||
}
|
||||
if (failsafeCnt > (5 * FAILSAVE_DELAY) && armed == 0)
|
||||
warn_failsafe = 2; // tx turned off while motors are off: start "find me" signal
|
||||
if (failsafeCnt == 0)
|
||||
warn_failsafe = 0; // turn off alarm if TX is okay
|
||||
#endif
|
||||
|
||||
//===================== GPS fix notification handling =====================
|
||||
if (sensors(SENSOR_GPS)) {
|
||||
if ((GPSModeHome || GPSModeHold) && !GPS_fix) { //if no fix and gps funtion is activated: do warning beeps.
|
||||
|
|
|
|||
|
|
@ -36,6 +36,7 @@ const char *mixerNames[] = {
|
|||
const char *featureNames[] = {
|
||||
"PPM", "VBAT", "INFLIGHT_ACC_CAL", "SPEKTRUM", "MOTOR_STOP",
|
||||
"SERVO_TILT", "CAMTRIG", "GYRO_SMOOTHING", "LED_RING", "GPS",
|
||||
"FAILSAFE",
|
||||
NULL
|
||||
};
|
||||
|
||||
|
|
@ -90,6 +91,9 @@ const clivalue_t valueTable[] = {
|
|||
{ "mincommand", VAR_UINT16, &cfg.mincommand, 0, 2000 },
|
||||
{ "mincheck", VAR_UINT16, &cfg.mincheck, 0, 2000 },
|
||||
{ "maxcheck", VAR_UINT16, &cfg.maxcheck, 0, 2000 },
|
||||
{ "failsafe_delay", VAR_UINT8, &cfg.failsafe_delay, 0, 200 },
|
||||
{ "failsafe_off_delay", VAR_UINT8, &cfg.failsafe_off_delay, 0, 200 },
|
||||
{ "failsafe_throttle", VAR_UINT16, &cfg.failsafe_throttle, 1000, 2000 },
|
||||
{ "motor_pwm_rate", VAR_UINT16, &cfg.motor_pwm_rate, 50, 498 },
|
||||
{ "servo_pwm_rate", VAR_UINT16, &cfg.servo_pwm_rate, 50, 498 },
|
||||
{ "spektrum_hires", VAR_UINT8, &cfg.spektrum_hires, 0, 1 },
|
||||
|
|
|
|||
11
src/config.c
11
src/config.c
|
|
@ -13,7 +13,7 @@ config_t cfg;
|
|||
const char rcChannelLetters[] = "AERT1234";
|
||||
|
||||
static uint32_t enabledSensors = 0;
|
||||
uint8_t checkNewConf = 15;
|
||||
uint8_t checkNewConf = 16;
|
||||
|
||||
void parseRcChannels(const char *input)
|
||||
{
|
||||
|
|
@ -49,7 +49,7 @@ void readEEPROM(void)
|
|||
|
||||
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, TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
|
||||
cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
|
||||
}
|
||||
|
||||
void writeParams(uint8_t b)
|
||||
|
|
@ -111,7 +111,7 @@ void checkFirstTime(bool reset)
|
|||
cfg.D8[PIDVEL] = 0;
|
||||
cfg.P8[PIDLEVEL] = 70;
|
||||
cfg.I8[PIDLEVEL] = 10;
|
||||
cfg.D8[PIDLEVEL] = 100;
|
||||
cfg.D8[PIDLEVEL] = 20;
|
||||
cfg.P8[PIDMAG] = 40;
|
||||
cfg.rcRate8 = 90;
|
||||
cfg.rcExpo8 = 65;
|
||||
|
|
@ -144,6 +144,11 @@ void checkFirstTime(bool reset)
|
|||
cfg.mincheck = 1100;
|
||||
cfg.maxcheck = 1900;
|
||||
|
||||
// Failsafe Variables
|
||||
cfg.failsafe_delay = 10; // 1sec
|
||||
cfg.failsafe_off_delay = 200; // 20sec
|
||||
cfg.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
|
||||
|
||||
// Motor/ESC/Servo
|
||||
cfg.minthrottle = 1150;
|
||||
cfg.maxthrottle = 1850;
|
||||
|
|
|
|||
|
|
@ -1,14 +1,14 @@
|
|||
#include "board.h"
|
||||
|
||||
#define PULSE_1MS (1000) // 1ms pulse width
|
||||
// #define PULSE_PERIOD (2500) // pulse period (400Hz)
|
||||
// #define PULSE_PERIOD_SERVO_DIGITAL (5000) // pulse period for digital servo (200Hz)
|
||||
// #define PULSE_PERIOD_SERVO_ANALOG (20000) // pulse period for analog servo (50Hz)
|
||||
|
||||
// Forward declaration
|
||||
static void pwmIRQHandler(TIM_TypeDef *tim);
|
||||
static void ppmIRQHandler(TIM_TypeDef *tim);
|
||||
|
||||
// external vars (ugh)
|
||||
extern int16_t failsafeCnt;
|
||||
|
||||
// local vars
|
||||
static struct TIM_Channel {
|
||||
TIM_TypeDef *tim;
|
||||
|
|
@ -92,6 +92,7 @@ static void ppmIRQHandler(TIM_TypeDef *tim)
|
|||
Inputs[chan].capture = diff;
|
||||
}
|
||||
chan++;
|
||||
failsafeCnt = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -145,6 +146,8 @@ static void pwmIRQHandler(TIM_TypeDef *tim)
|
|||
|
||||
// switch state
|
||||
state->state = 0;
|
||||
// reset failsafe
|
||||
failsafeCnt = 0;
|
||||
|
||||
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
|
||||
TIM_ICInitStructure.TIM_Channel = channel.channel;
|
||||
|
|
|
|||
|
|
@ -10,6 +10,9 @@
|
|||
static void pwmIRQHandler(TIM_TypeDef *tim);
|
||||
static void ppmIRQHandler(TIM_TypeDef *tim);
|
||||
|
||||
// external vars (ugh)
|
||||
extern int16_t failsafeCnt;
|
||||
|
||||
// local vars
|
||||
static struct TIM_Channel {
|
||||
TIM_TypeDef *tim;
|
||||
|
|
@ -86,6 +89,7 @@ static void ppmIRQHandler(TIM_TypeDef *tim)
|
|||
Inputs[chan].capture = diff;
|
||||
}
|
||||
chan++;
|
||||
failsafeCnt = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -140,6 +144,9 @@ static void pwmIRQHandler(TIM_TypeDef *tim)
|
|||
// switch state
|
||||
state->state = 0;
|
||||
|
||||
// ping failsafe
|
||||
failsafeCnt = 0;
|
||||
|
||||
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
|
||||
TIM_ICInitStructure.TIM_Channel = channel.channel;
|
||||
TIM_ICInit(channel.tim, &TIM_ICInitStructure);
|
||||
|
|
|
|||
|
|
@ -120,7 +120,7 @@ void mixTable(void)
|
|||
motor[0] = PIDMIX(0, +4 / 3, 0); //REAR
|
||||
motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT
|
||||
motor[2] = PIDMIX(+1, -2 / 3, 0); //LEFT
|
||||
servo[4] = constrain(cfg.tri_yaw_middle + cfg.yaw_direction * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
|
||||
servo[4] = constrain(cfg.tri_yaw_middle + cfg.yaw_direction * axisPID[YAW], cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
|
||||
break;
|
||||
|
||||
case MULTITYPE_QUADP:
|
||||
|
|
|
|||
46
src/mw.c
46
src/mw.c
|
|
@ -22,7 +22,7 @@ int16_t annex650_overrun_count = 0;
|
|||
uint8_t armed = 0;
|
||||
uint8_t vbat; // battery voltage in 0.1V steps
|
||||
|
||||
volatile int16_t failsafeCnt = 0;
|
||||
int16_t failsafeCnt = 0;
|
||||
int16_t failsafeEvents = 0;
|
||||
int16_t rcData[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; // interval [1000;2000]
|
||||
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
|
||||
|
|
@ -171,20 +171,6 @@ void annexCode(void)
|
|||
BEEP_OFF;
|
||||
} else
|
||||
buzzerFreq = 4; // low battery
|
||||
|
||||
#if 0
|
||||
if (buzzerFreq) {
|
||||
if (buzzerState && (currentTime > buzzerTime + 250000)) {
|
||||
buzzerState = 0;
|
||||
BEEP_OFF;
|
||||
buzzerTime = currentTime;
|
||||
} else if (!buzzerState && (currentTime > (buzzerTime + (2000000 >> buzzerFreq)))) {
|
||||
buzzerState = 1;
|
||||
BEEP_ON;
|
||||
buzzerTime = currentTime;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now
|
||||
|
|
@ -234,7 +220,6 @@ uint16_t pwmReadRawRC(uint8_t chan)
|
|||
{
|
||||
uint16_t data;
|
||||
|
||||
failsafeCnt = 0;
|
||||
data = pwmRead(cfg.rcmap[chan]);
|
||||
if (data < 750 || data > 2250)
|
||||
data = cfg.midrc;
|
||||
|
|
@ -286,21 +271,22 @@ void loop(void)
|
|||
// TODO clean this up. computeRC should handle this check
|
||||
if (!feature(FEATURE_SPEKTRUM))
|
||||
computeRC();
|
||||
// Failsafe routine - added by MIS
|
||||
#if defined(FAILSAFE)
|
||||
if (failsafeCnt > (5 * FAILSAVE_DELAY) && armed == 1) { // Stabilize, and set Throttle to specified level
|
||||
for (i = 0; i < 3; i++)
|
||||
rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec)
|
||||
rcData[THROTTLE] = FAILSAVE_THR0TTLE;
|
||||
if (failsafeCnt > 5 * (FAILSAVE_DELAY + FAILSAVE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec)
|
||||
armed = 0; //This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
|
||||
okToArm = 0; //to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
|
||||
|
||||
// Failsafe routine
|
||||
if (feature(FEATURE_FAILSAFE)) {
|
||||
if (failsafeCnt > (5 * cfg.failsafe_delay) && armed == 1) { // Stabilize, and set Throttle to specified level
|
||||
for (i = 0; i < 3; i++)
|
||||
rcData[i] = cfg.midrc; // after specified guard time after RC signal is lost (in 0.1sec)
|
||||
rcData[THROTTLE] = cfg.failsafe_throttle;
|
||||
if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay)) { // Turn OFF motors after specified Time (in 0.1sec)
|
||||
armed = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
|
||||
okToArm = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
|
||||
}
|
||||
failsafeEvents++;
|
||||
}
|
||||
failsafeEvents++;
|
||||
failsafeCnt++;
|
||||
}
|
||||
failsafeCnt++;
|
||||
#endif
|
||||
// end of failsave routine - next change is made with RcOptions setting
|
||||
|
||||
if (rcData[THROTTLE] < cfg.mincheck) {
|
||||
errorGyroI[ROLL] = 0;
|
||||
errorGyroI[PITCH] = 0;
|
||||
|
|
@ -413,7 +399,7 @@ void loop(void)
|
|||
}
|
||||
|
||||
// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
|
||||
if ((rcOptions[BOXACC] || (failsafeCnt > 5 * FAILSAVE_DELAY)) && (sensors(SENSOR_ACC))) {
|
||||
if ((rcOptions[BOXACC] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) {
|
||||
// bumpless transfer to Level mode
|
||||
if (!accMode) {
|
||||
errorAngleI[ROLL] = 0;
|
||||
|
|
|
|||
25
src/mw.h
25
src/mw.h
|
|
@ -1,22 +1,5 @@
|
|||
#pragma once
|
||||
|
||||
/* Failsave settings - added by MIS
|
||||
Failsafe check pulse on THROTTLE channel. If the pulse is OFF (on only THROTTLE or on all channels) the failsafe procedure is initiated.
|
||||
After FAILSAVE_DELAY time of pulse absence, the level mode is on (if ACC or nunchuk is avaliable), PITCH, ROLL and YAW is centered
|
||||
and THROTTLE is set to FAILSAVE_THR0TTLE value. You must set this value to descending about 1m/s or so for best results.
|
||||
This value is depended from your configuration, AUW and some other params.
|
||||
Next, afrer FAILSAVE_OFF_DELAY the copter is disarmed, and motors is stopped.
|
||||
If RC pulse coming back before reached FAILSAVE_OFF_DELAY time, after the small quard time the RC control is returned to normal.
|
||||
If you use serial sum PPM, the sum converter must completly turn off the PPM SUM pusles for this FailSafe functionality.*/
|
||||
// #define FAILSAFE // Alex: comment this line if you want to deactivate the failsafe function
|
||||
#define FAILSAVE_DELAY 10 // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example
|
||||
#define FAILSAVE_OFF_DELAY 200 // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example
|
||||
#define FAILSAVE_THR0TTLE (MINTHROTTLE + 200) // Throttle level used for landing - may be relative to MINTHROTTLE - as in this case
|
||||
|
||||
/* you can change the tricopter servo travel here */
|
||||
#define TRI_YAW_CONSTRAINT_MIN 1020
|
||||
#define TRI_YAW_CONSTRAINT_MAX 2000
|
||||
|
||||
/* 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 */
|
||||
|
|
@ -37,7 +20,7 @@
|
|||
#define CAM_TIME_LOW 1000 // the duration of LOW state servo expressed in ms
|
||||
|
||||
/* for VBAT monitoring frequency */
|
||||
#define VBATFREQ 6 // to read battery voltage - keep equal to PSENSORFREQ (6) unless you know what you are doing
|
||||
#define VBATFREQ 6 // to read battery voltage - nth number of loop iterations
|
||||
|
||||
#define VERSION 200
|
||||
|
||||
|
|
@ -148,6 +131,11 @@ typedef struct config_t {
|
|||
uint16_t mincheck; // minimum rc end
|
||||
uint16_t maxcheck; // maximum rc end
|
||||
|
||||
// Failsafe related configuration
|
||||
uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
|
||||
uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
|
||||
uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
|
||||
|
||||
// motor/esc/servo related stuff
|
||||
uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
|
||||
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
|
||||
|
|
@ -187,6 +175,7 @@ extern int16_t angle[2];
|
|||
extern int16_t axisPID[3];
|
||||
extern int16_t rcCommand[4];
|
||||
extern uint8_t rcOptions[CHECKBOXITEMS];
|
||||
extern int16_t failsafeCnt;
|
||||
|
||||
extern int16_t debug1, debug2, debug3, debug4;
|
||||
extern uint8_t armed;
|
||||
|
|
|
|||
|
|
@ -10,9 +10,11 @@ static uint8_t spek_chan_mask;
|
|||
static bool rcFrameComplete = false;
|
||||
static bool spekDataIncoming = false;
|
||||
volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
|
||||
|
||||
static void spektrumDataReceive(uint16_t c);
|
||||
|
||||
// external vars (ugh)
|
||||
extern int16_t failsafeCnt;
|
||||
|
||||
void spektrumInit(void)
|
||||
{
|
||||
if (cfg.spektrum_hires) {
|
||||
|
|
@ -44,12 +46,7 @@ static void spektrumDataReceive(uint16_t c)
|
|||
spekFrame[spekFramePosition] = (uint8_t)c;
|
||||
if (spekFramePosition == SPEK_FRAME_SIZE - 1) {
|
||||
rcFrameComplete = true;
|
||||
#if defined(FAILSAFE)
|
||||
if(failsafeCnt > 20)
|
||||
failsafeCnt -= 20;
|
||||
else
|
||||
failsafeCnt = 0; // clear FailSafe counter
|
||||
#endif
|
||||
failsafeCnt = 0; // clear FailSafe counter
|
||||
} else {
|
||||
spekFramePosition++;
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue