void AmbulanceC::manageAlarm(void){
resetSrn();
resetAl();
if(AlLevel==_NONE) disarm();
else arm();
}
//Alarm led///////////////////////////////////////////////////////////////////////////////
void AmbulanceC::playSrn()
{
if(ItrptLevel!=_NONE) MINIMIZE();
if(prnt) {Serial.print("Play siren");}
if(!AlArmed) return;
nAls++; if(nAls>=maxnAls) {if(prnt) Serial.print("nAls>=maxnAls,disarming"); disarm(); return;}
srnOn() ;
enableblinkScene();
if(srnTimeout1!=NULL) srnTimeout1->deleteTimer(); srnTimeout1=NULL;
if(srnTimeout2!=NULL) srnTimeout2->deleteTimer(); srnTimeout2=NULL;
srnTimeout1=t.setTimeout(2*SrnDuration, SRNON);
srnTimeout2=t.setTimeout(3*SrnDuration, SRNOFF);
//t.setTimeout(3*SrnDuration+1000, RESETSRN);
t.setTimeout(3*SrnDuration+1000, RESTORE);
//enableblinkAlLEDFast();
if(prnt) {Serial.print("Pin");Serial.print(AlSrc);}
phone.alarm();
}
int cmd_disarm(string targ) {
if (TP->query_level() < 25) {
write("You are not yet skilled enough to do that!\n");
return 1;
}
attkr = (object)TP->query_current_attacker();
if (!attkr) {
write("You may only disarm in combat.\n");
return 1;
}
weap = attkr->query("weapon1");
if (!weap) weap = attkr->query("weapon2");
if (!weap) {
write ("You opponent is already unarmed.\n");
return 1;
}
if (TP->query("power_delay")) {
write("You can't yet find another opening...\n");
return 1;
}
if (!targ) { targ = TP->query_current_attacker();
write ("You attempt to disarm your opponent.\n");
say (TPN+" twirls "+POSS+" blade towards "+POSS+" foe.\n");
}
TP->block_attack(1);
disarm();
return 1;
}
void MSToggleButtonBase::key(KeySym keysym_,unsigned int,const char *)
{
if (keysym_==XK_Return) { (armed()==MSFalse)?arm():disarm(); }
else if (keysym_==XK_Up) up();
else if (keysym_==XK_Down) down();
else if (keysym_==XK_Left) left();
else if (keysym_==XK_Right) right();
}
static void button_mouseup(control c, int buttons, point xy)
{
if (!isarmed(c))
return;
disarm(c);
unhighlight(c);
if (ptinr(xy, getrect(c)))
activatecontrol(c);
}
static void radio_mouseup(control c, int buttons, point xy)
{
if (! isarmed(c))
return;
disarm(c);
unhighlight(c);
if (! ptinr(xy, getrect(c)))
return;
radio_hit(c);
}
timeout_t TQEvent::timeout(void)
{
timeout_t timeout = get();
if(is_active() && !timeout) {
disarm();
expired();
timeout = get();
Timer::update();
}
return timeout;
}
static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs)
{
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// PID - note this is function pointer set by setPIDController()
pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs);
DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
#ifdef USE_RUNAWAY_TAKEOFF
// Check to see if runaway takeoff detection is active (anti-taz), the pidSum is over the threshold,
// and gyro rate for any axis is above the limit for at least the activate delay period.
// If so, disarm for safety
if (ARMING_FLAG(ARMED)
&& !STATE(FIXED_WING)
&& pidConfig()->runaway_takeoff_prevention
&& !runawayTakeoffCheckDisabled
&& !flipOverAfterCrashMode
&& !runawayTakeoffTemporarilyDisabled
&& (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) {
if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
|| (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
|| (fabsf(pidData[FD_YAW].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD))
&& ((ABS(gyroAbsRateDps(FD_PITCH)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
|| (ABS(gyroAbsRateDps(FD_ROLL)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
|| (ABS(gyroAbsRateDps(FD_YAW)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) {
if (runawayTakeoffTriggerUs == 0) {
runawayTakeoffTriggerUs = currentTimeUs + RUNAWAY_TAKEOFF_ACTIVATE_DELAY;
} else if (currentTimeUs > runawayTakeoffTriggerUs) {
setArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
disarm();
}
} else {
runawayTakeoffTriggerUs = 0;
}
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_TRUE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, runawayTakeoffTriggerUs == 0 ? DEBUG_RUNAWAY_TAKEOFF_FALSE : DEBUG_RUNAWAY_TAKEOFF_TRUE);
} else {
runawayTakeoffTriggerUs = 0;
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_FALSE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
}
#endif
#ifdef USE_PID_AUDIO
if (isModeActivationConditionPresent(BOXPIDAUDIO)) {
pidAudioUpdate();
}
#endif
}
//看门狗线程
void watchdog_entry(void *parameter)
{
while (1)
{
if (rt_sem_take(&watchdog, RT_TICK_PER_SECOND) != RT_EOK)
{
rt_kprintf("watchdog timeout!!!!!.\n");
while (1)
{
Motor_Set(0, 0, 0, 0);
disarm();
rt_thread_suspend(&control_thread);
}
}
if (armed && (abs(ahrs.degree_pitch) > 45.0f || abs(ahrs.degree_roll) > 45.0f))
{
Motor_Set(0, 0, 0, 0);
disarm();
rt_kprintf("degree out of range.\n");
}
}
}
static void checkbox_mouseup(control c, int buttons, point xy)
{
if (! isenabled(c))
return;
disarm(c);
unhighlight(c);
if (! ptinr(xy, getrect(c)))
return;
if (ischecked(c))
uncheck(c);
else
check(c);
activatecontrol(c);
}
void NetAlarm::checkForRemoteControl()
{
if (rcSwitch_.available()) {
unsigned long code = rcSwitch_.getReceivedValue();
DEBUG_PRINT(String("Received remote 433 MHz code: ") + String(code));
if (code == remoteArmCode_) {
arm();
}
else if (code == remoteDisarmCode_) {
disarm();
}
rcSwitch_.resetAvailable();
}
}
void processRcStickPositions(throttleStatus_e throttleStatus)
{
static timeMs_t lastTickTimeMs = 0;
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint32_t rcSticks; // this hold sticks position for command combos
static timeMs_t rcDisarmTimeMs; // this is an extra guard for disarming through switch to prevent that one frame can disarm it
const timeMs_t currentTimeMs = millis();
updateRcStickPositions();
uint32_t stTmp = getRcStickPositions();
if (stTmp == rcSticks) {
if (rcDelayCommand < 250) {
if ((currentTimeMs - lastTickTimeMs) >= MIN_RC_TICK_INTERVAL_MS) {
lastTickTimeMs = currentTimeMs;
rcDelayCommand++;
}
}
} else
rcDelayCommand = 0;
rcSticks = stTmp;
// perform actions
if (!isUsingSticksForArming()) {
if (IS_RC_MODE_ACTIVE(BOXARM)) {
rcDisarmTimeMs = currentTimeMs;
tryArm();
} else {
// Disarming via ARM BOX
// Don't disarm via switch if failsafe is active or receiver doesn't receive data - we can't trust receiver
// and can't afford to risk disarming in the air
if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) {
const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs;
if (disarmDelay > armingConfig()->switchDisarmDelayMs) {
if (armingConfig()->disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) {
disarm(DISARM_SWITCH);
}
}
}
else {
rcDisarmTimeMs = currentTimeMs;
}
}
}
// KILLSWITCH disarms instantly
if (IS_RC_MODE_ACTIVE(BOXKILLSWITCH)) {
disarm(DISARM_KILLSWITCH);
}
if (rcDelayCommand != 20) {
return;
}
if (isUsingSticksForArming()) {
// Disarm on throttle down + yaw
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
// Dont disarm if fixedwing and motorstop
if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
return;
}
else if (ARMING_FLAG(ARMED)) {
disarm(DISARM_STICKS);
}
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
rcDelayCommand = 0; // reset so disarm tone will repeat
}
}
}
if (ARMING_FLAG(ARMED)) {
// actions during armed
return;
}
// actions during not armed
// GYRO calibration
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
return;
}
#if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
// Save waypoint list
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
const bool success = saveNonVolatileWaypointList();
beeper(success ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL);
}
// Load waypoint list
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
const bool success = loadNonVolatileWaypointList();
beeper(success ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL);
}
#endif
// Multiple configuration profiles
if (feature(FEATURE_TX_PROF_SEL)) {
uint8_t i = 0;
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1
i = 1;
else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2
i = 2;
else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3
i = 3;
if (i) {
setConfigProfileAndWriteEEPROM(i - 1);
return;
}
i = 0;
// Multiple battery configuration profiles
if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1
i = 1;
else if (rcSticks == THR_HI + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2
i = 2;
else if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3
i = 3;
if (i) {
setConfigBatteryProfileAndWriteEEPROM(i - 1);
batteryDisableProfileAutoswitch();
activateBatteryProfile();
return;
}
}
// Save config
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
saveConfigAndNotify();
}
// Arming by sticks
if (isUsingSticksForArming()) {
if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
// Auto arm on throttle when using fixedwing and motorstop
if (throttleStatus != THROTTLE_LOW) {
tryArm();
return;
}
}
else {
if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
// Arm via YAW
tryArm();
return;
}
}
}
// Calibrating Acc
if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
return;
}
// Calibrating Mag
if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
ENABLE_STATE(CALIBRATE_MAG);
return;
}
// Accelerometer Trim
if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
applyAndSaveBoardAlignmentDelta(0, -2);
rcDelayCommand = 10;
return;
} else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
applyAndSaveBoardAlignmentDelta(0, 2);
rcDelayCommand = 10;
return;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
applyAndSaveBoardAlignmentDelta(-2, 0);
rcDelayCommand = 10;
return;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
applyAndSaveBoardAlignmentDelta(2, 0);
rcDelayCommand = 10;
return;
}
}
/* deal with a new player command in dungeon or city mode*/
void p_process(void)
{
static int searchval=0;
if (Player.status[BERSERK])
if (goberserk()) {
setgamestatus(SKIP_PLAYER);
drawvision(Player.x,Player.y);
}
if (! gamestatusp(SKIP_PLAYER)) {
if (searchval > 0) {
searchval--;
if (searchval == 0) resetgamestatus(FAST_MOVE);
}
drawvision(Player.x,Player.y);
if (! gamestatusp(FAST_MOVE)) {
searchval = 0;
Cmd = mgetc();
clear_if_necessary();
}
Command_Duration = 0;
switch (Cmd) {
case ' ':
case 13:
setgamestatus(SKIP_MONSTERS);
break; /*no op on space or return*/
case 6:
abortshadowform();
break; /* ^f */
case 7:
wizard();
break; /* ^g */
case 4:
player_dump();
break; /* ^d */
case 9:
display_pack();
morewait();
xredraw();
break; /* ^i */
case 11:
if (gamestatusp(CHEATED)) frobgamestatus();
break;
case 12:
xredraw();
setgamestatus(SKIP_MONSTERS);
break; /* ^l */
#if !defined(WIN32)
case 16:
bufferprint();
setgamestatus(SKIP_MONSTERS);
break; /* ^p */
#else
case 15:
bufferprint();
setgamestatus(SKIP_MONSTERS);
break; /* ^o */
#endif
case 18:
redraw();
setgamestatus(SKIP_MONSTERS);
break; /* ^r */
case 23:
if (gamestatusp(CHEATED)) drawscreen();
break; /* ^w */
case 24: /* ^x */
if (gamestatusp(CHEATED) ||
Player.rank[ADEPT])
wish(1);
Command_Duration = 5;
break;
case 'a':
zapwand();
Command_Duration = Player.speed*8/5;
break;
case 'c':
closedoor();
Command_Duration = Player.speed*2/5;
break;
case 'd':
drop();
Command_Duration = Player.speed*5/5;
break;
case 'e':
eat();
Command_Duration = 30;
break;
case 'f':
fire();
Command_Duration = Player.speed*5/5;
break;
case 'g':
pickup();
Command_Duration = Player.speed*10/5;
break;
case 'i':
do_inventory_control();
break;
case 'm':
magic();
Command_Duration = 12;
break;
case 'o':
opendoor();
Command_Duration = Player.speed*5/5;
break;
case 'p':
pickpocket();
Command_Duration = Player.speed*20/5;
break;
case 'q':
quaff();
Command_Duration = 10;
break;
case 'r':
peruse();
Command_Duration = 20;
break;
case 's':
search(&searchval);
Command_Duration = 20;
break;
case 't':
talk();
Command_Duration = 10;
break;
case 'v':
vault();
Command_Duration = Player.speed*10/5;
break;
case 'x':
examine();
Command_Duration = 1;
break;
case 'z':
bash_location();
Command_Duration = Player.speed*10/5;
break;
case 'A':
activate();
Command_Duration = 10;
break;
case 'C':
callitem();
break;
case 'D':
disarm();
Command_Duration = 30;
break;
case 'E':
dismount_steed();
Command_Duration = Player.speed*10/5;
break;
case 'F':
tacoptions();
break;
case 'G':
give();
Command_Duration = 10;
break;
case 'I':
if (! optionp(TOPINV)) top_inventory_control();
else {
display_possessions();
inventory_control();
}
break;
case 'M':
city_move();
Command_Duration = 10;
break;
case 'O':
setoptions();
#if defined(WIN32)
show_screen();
xredraw();
#endif
break;
case 'P':
show_license();
break; /* actually show_license is in file.c */
case 'Q':
quit();
break;
case 'R':
rename_player();
break;
case 'S':
save(FALSE);
break;
case 'T':
tunnel();
Command_Duration = Player.speed*30/5;
break;
case 'V':
version();
break;
case 'Z':
bash_item();
Command_Duration = Player.speed*10/5;
break;
case '.':
rest();
Command_Duration = 10;
break;
case ',':
Command_Duration = 10;
nap();
break;
case '>':
downstairs();
break;
case '<':
upstairs();
break;
case '@':
p_movefunction(Level->site[Player.x][Player.y].p_locf);
Command_Duration = 5;
break;
case '#':
if (gamestatusp(CHEATED)) editstats();
break; /* RAC - char editor */
case '/':
charid();
setgamestatus(SKIP_MONSTERS);
break;
case '?':
help();
setgamestatus(SKIP_MONSTERS);
break;
case '4':
case 'h':
moveplayer(-1,0);
Command_Duration = Player.speed*5/5;
break;
case '2':
case 'j':
moveplayer(0,1);
Command_Duration = Player.speed*5/5;
break;
case '8':
case 'k':
moveplayer(0,-1);
Command_Duration = Player.speed*5/5;
break;
case '6':
case 'l':
moveplayer(1,0);
Command_Duration = Player.speed*5/5;
break;
case '1':
case 'b':
moveplayer(-1,1);
Command_Duration = Player.speed*5/5;
break;
case '3':
case 'n':
moveplayer(1,1);
Command_Duration = Player.speed*5/5;
break;
case '7':
case 'y':
moveplayer(-1,-1);
Command_Duration = Player.speed*5/5;
break;
case '9':
case 'u':
moveplayer(1,-1);
Command_Duration = Player.speed*5/5;
break;
case '5':
setgamestatus(SKIP_MONSTERS); /* don't do anything; a dummy turn */
Cmd = mgetc();
while ((Cmd != ESCAPE) &&
((Cmd < '1') || (Cmd > '9') || (Cmd=='5'))) {
print3("Run in keypad direction [ESCAPE to abort]: ");
Cmd = mgetc();
}
if (Cmd != ESCAPE)
setgamestatus(FAST_MOVE);
else
clearmsg3();
break;
case 'H':
setgamestatus(FAST_MOVE);
Cmd = 'h';
moveplayer(-1,0);
Command_Duration = Player.speed*4/5;
break;
case 'J':
setgamestatus(FAST_MOVE);
Cmd = 'j';
moveplayer(0,1);
Command_Duration = Player.speed*4/5;
break;
case 'K':
setgamestatus(FAST_MOVE);
Cmd = 'k';
moveplayer(0,-1);
Command_Duration = Player.speed*4/5;
break;
case 'L':
setgamestatus(FAST_MOVE);
Cmd = 'l';
moveplayer(1,0);
Command_Duration = Player.speed*4/5;
break;
case 'B':
setgamestatus(FAST_MOVE);
Cmd = 'b';
moveplayer(-1,1);
Command_Duration = Player.speed*4/5;
break;
case 'N':
setgamestatus(FAST_MOVE);
Cmd = 'n';
moveplayer(1,1);
Command_Duration = Player.speed*4/5;
break;
case 'Y':
setgamestatus(FAST_MOVE);
Cmd = 'y';
moveplayer(-1,-1);
Command_Duration = Player.speed*4/5;
break;
case 'U':
setgamestatus(FAST_MOVE);
Cmd = 'u';
moveplayer(1,-1);
Command_Duration = Player.speed*4/5;
break;
default:
commanderror();
setgamestatus(SKIP_MONSTERS);
break;
}
}
if (Current_Environment != E_COUNTRYSIDE) roomcheck();
screencheck(Player.x,Player.y);
}
void do_disarm( CHAR_DATA *ch, char *argument )
{
CHAR_DATA *victim;
OBJ_DATA *obj;
int chance,hth,ch_weapon,vict_weapon,ch_vict_weapon;
hth = 0;
if ((chance = get_skill(ch,gsn_disarm)) == 0)
{
send_to_char( "You don't know how to disarm opponents.\n\r", ch );
return;
}
if ( get_eq_char( ch, WEAR_WIELD ) == NULL
&& ((hth = get_skill(ch,gsn_brawl)) == 0
|| (IS_NPC(ch) && !IS_SET(ch->off_flags,OFF_DISARM))))
{
send_to_char( "You must wield a weapon to disarm.\n\r", ch );
return;
}
if ( ( victim = ch->fighting ) == NULL )
{
send_to_char( "You aren't fighting anyone.\n\r", ch );
return;
}
if ( ( obj = get_eq_char( victim, WEAR_WIELD ) ) == NULL )
{
send_to_char( "Your opponent is not wielding a weapon.\n\r", ch );
return;
}
/* find weapon skills */
ch_weapon = get_weapon_skill(ch,get_weapon_sn(ch));
vict_weapon = get_weapon_skill(victim,get_weapon_sn(victim));
ch_vict_weapon = get_weapon_skill(ch,get_weapon_sn(victim));
/* skill */
if ( get_eq_char(ch,WEAR_WIELD) == NULL)
chance = chance * hth/150;
else
chance = chance * ch_weapon/100;
chance += (ch_vict_weapon/2 - vict_weapon) / 2;
/* dex vs. strength */
chance += get_curr_stat(ch,STAT_DEX);
chance -= 2 * get_curr_stat(victim,STAT_STR);
/* and now the attack */
if (number_percent() < chance)
{
WAIT_STATE( ch, skill_table[gsn_disarm].beats );
disarm( ch, victim );
}
else
{
WAIT_STATE(ch,skill_table[gsn_disarm].beats);
act("You fail to disarm $N.",ch,NULL,victim,TO_CHAR,1);
act("$n tries to disarm you, but fails.",ch,NULL,victim,TO_VICT,0);
act("$n tries to disarm $N, but fails.",ch,NULL,victim,TO_NOTVICT,0);
}
return;
}
rt_err_t mayday(u8 var)
{
disarm();
return RT_EOK;
}
void MSToggleButtonBase::button1Press(const XEvent *)
{ (armed()==MSFalse)?arm():disarm(); }
//硬件错误时关闭电机
rt_err_t hardfalt_protect(void *stack)
{
Motor_Set(0, 0, 0, 0);
disarm();
return RT_EOK;
}
//断言失败时关闭电机
void assert_protect(const char *c1, const char *c2, rt_size_t size)
{
Motor_Set(0, 0, 0, 0);
disarm();
}
void check_samuraiattack(CHAR_DATA *ch, CHAR_DATA *victim)
{
if (!IS_CLASS(ch, CLASS_SAMURAI)) return;
if (!victim || victim->hit < 1000) return;
switch(ch->pcdata->powers[SAMURAI_FOCUS])
{
default : break;
case 10 :
if (ch->pcdata->powers[SAMURAI_LAST] == 10)
{
send_to_char("You cannot use the same combo twice.\n\r",ch);
break;
}
ch->pcdata->powers[SAMURAI_LAST] = 10;
do_say(ch, "What a worthless opponent you are. Here, let me show you how a real fighter fights.");
one_hit(ch, victim, gsn_lightningslash, 1);
one_hit(ch, victim, gsn_lightningslash, 1);
one_hit(ch, victim, gsn_lightningslash, 1);
break;
case 15 :
if (ch->pcdata->powers[SAMURAI_LAST] == 15)
{
send_to_char("You cannot use the same combo twice.\n\r",ch);
break;
}
ch->pcdata->powers[SAMURAI_LAST] = 15;
if (number_range(1,3) == 2)
{
send_to_char("You fail your attempt to disarm.\n\r",ch);
break;
}
act("#GYou strike out at $N's weapon.#n",ch,NULL,victim,TO_CHAR);
act("#G$n's attack strikes your weaponarm.#n",ch,NULL,victim,TO_VICT);
disarm(ch, victim);
break;
case 20 :
if (ch->pcdata->powers[SAMURAI_LAST] == 20)
{
send_to_char("You cannot use the same combo twice.\n\r",ch);
break;
}
ch->pcdata->powers[SAMURAI_LAST] = 20;
act("#GYour hit $N on a central nerve, paralysing $M.#n",ch,NULL,victim,TO_CHAR);
act("#G$n's attack paralyses you, you cannot move.#n",ch,NULL,victim,TO_VICT);
if (IS_CLASS(victim, CLASS_FAE))
{
send_to_char("You shrug of the attack.\n\r", victim);
send_to_char("They don't seem that affected.\n\r", ch);
}
else WAIT_STATE(victim, 24);
break;
case 25 :
if (ch->pcdata->powers[SAMURAI_LAST] == 25)
{
send_to_char("You cannot use the same combo twice.\n\r",ch);
break;
}
ch->pcdata->powers[SAMURAI_LAST] = 25;
if (number_range(1,3) == 2)
{
send_to_char("You fail your attempt to hurl.\n\r",ch);
break;
}
act("#GYou grab $N and toss $M over your shoulder.#n",ch,NULL,victim,TO_CHAR);
act("#G$n grabs you, and tosses you over $s shoulder.#n",ch,NULL,victim,TO_VICT);
special_hurl(ch, victim);
break;
case 30 :
if (ch->pcdata->powers[SAMURAI_LAST] == 30)
{
send_to_char("You cannot use the same combo twice.\n\r",ch);
break;
}
ch->pcdata->powers[SAMURAI_LAST] = 30;
ch->hit += number_range(2000, 4000);
if (ch->hit > ch->max_hit) ch->hit = ch->max_hit;
act("#GYou feel adrenalin pump through your body, awakening your senses.#n",ch,NULL,victim,TO_CHAR);
act("#G$n flashes a wicked smile.#n",ch,NULL,victim,TO_VICT);
break;
case 35 :
if (ch->pcdata->powers[SAMURAI_LAST] == 35)
{
send_to_char("You cannot use the same combo twice.\n\r",ch);
break;
}
ch->pcdata->powers[SAMURAI_LAST] = 35;
do_say(ch, "I am a master of arms, you cannot even begin to understand what I am able to do!");
one_hit(ch, victim, gsn_backfist, 1);
one_hit(ch, victim, gsn_thrustkick, 1);
one_hit(ch, victim, gsn_monksweep, 1);
one_hit(ch, victim, gsn_jumpkick, 1);
one_hit(ch, victim, gsn_lightningslash, 1);
break;
}
return;
}
//控制线程循环
void control_thread_entry(void *parameter)
{
LED2(5);
wait_dmp();
rt_kprintf("start control\n");
excute_task("wait");
while (1)
{
LED2(armed * 3);
receive_pwm(&pwm); //接受PWM信号
//若遥控器关闭则锁定
if (pwm.switch1 < 2 && pwm.switch1 != -1 && pwm.throttle < 0.05f && pwm.throttle >= 0.0f)
{
arm(SAFE_PWM);
}
/*
符合解锁条件则解锁
1.遥控器开关处于2档或3档 或者 两根遥控器屏蔽跳线接地
2.任务安全需求满足
*/
if ((((pwm.switch1 == 2 || pwm.switch1 == -1) && (GPIO_ReadInputDataBit(GPIOE, GPIO_Pin_3) == Bit_RESET || GPIO_ReadInputDataBit(GPIOE, GPIO_Pin_4) == Bit_RESET)) || check_safe(current_task->depend)))
{
disarm();
}
//得到dmp数据且解锁则执行任务
if (get_dmp() && armed)
{
//若地面站离线 遥控器在线则手动切换模式
if (check_safe(SAFE_PWM) == RT_EOK && check_safe(SAFE_TFCR) != RT_EOK)
{
if (current_task->id != 2 && pwm.switch1 == 1)
{
excute_task("stable");
}
if (current_task->id != 3 && pwm.switch1 == 0 && pwm.switch2 == 0)
{
excute_task("althold");
}
if (current_task->id != 4 && pwm.switch1 == 0 && pwm.switch2 == 1)
{
pos_X = ahrs.x;
pos_y = ahrs.y;
excute_task("loiter");
}
}
//执行任务循环
if (current_task->func(current_task->var) != RT_EOK)
disarm();
}
//若锁定则电机关闭
if (!armed)
Motor_Set(0, 0, 0, 0);
rt_sem_release(&watchdog);
rt_thread_delay(2);
}
}
void remote_thread_entry(void* parameter)
{
char * ptr;
u8 lost=0;
rt_tick_t last=rt_tick_get();
while(1)
{
ptr=pack.buf;
get(ptr++);
if(pack.buf[0]!=(head&0xFF))
goto wrong;
get(ptr++);
if(pack.head.head!=head)
goto wrong;
while(ptr-pack.buf<sizeof(struct tfcr_common))
get(ptr++);
if(!TFCR_IS_PACK_TYPE(pack.head.type))
goto wrong;
//rt_kprintf("head ok %d\n",sizeof(struct tfcr_common));
switch(pack.head.type)
{
case TFCR_TYPE_PING:
while(ptr-pack.buf<sizeof(struct tfcr_ping))
get(ptr++);
if(checksum(pack.buf,sizeof(struct tfcr_ping)-1)!=pack.ping.checksum)
goto wrong;
send_ack(pack.ping.index);
debug("ping %05d\n",pack.ping.index);
if(!tfrc_con)
rt_kprintf("tfcr connected.\n");
tfrc_con=RT_TRUE;
break;
case TFCR_TYPE_TASK:
while(ptr-pack.buf<sizeof(struct tfcr_task))
get(ptr++);
if(checksum(pack.buf,sizeof(struct tfcr_task)-1)!=pack.task.checksum)
goto wrong;
if(rt_strcasecmp(pack.task.name,"mayday")==0)
{
excute_task("mayday");
disarm();
send_ack(pack.task.index);
break;
}
if(pack.task.state==1)
{
fc_task * task=find_task(pack.task.name);
if(task==RT_NULL)
{
send_error(pack.task.index,0xff);
rt_kprintf("no task %s!\n",pack.task.name);
break;
}
u8 result= check_safe(task->depend);
if(result!=0)
{
send_error(pack.task.index,result);
break;
}
if(!excute_task(pack.task.name))
{
send_error(pack.task.index,0xff);
break;
}
arm(task->depend);
send_ack(pack.task.index);
}
else
{
excute_task("wait");
send_ack(pack.task.index);
rt_kprintf("stop task %s.\n",pack.task.name);
}
break;
case TFCR_TYPE_GET_VALUE:
while(ptr-pack.buf<sizeof(struct tfcr_get_value))
get(ptr++);
if(checksum(pack.buf,sizeof(struct tfcr_get_value)-1)!=pack.get.checksum)
goto wrong;
send_value(pack.get.index,pack.get.id);
break;
}
last=rt_tick_get();
lost=0;
continue;
timeout:
if(tfrc_con)
{
if(lost<3)
{
rt_kprintf("tfcr time out.\n");
lost++;
}
else
{
rt_kprintf("tfcr lost connect.\n");
tfrc_con=RT_FALSE;
}
}
continue;
wrong:
rt_kprintf("wrong\n");
if(rt_tick_get()-RT_TICK_PER_SECOND/2>last)
{
rt_tick_t last=rt_tick_get();
goto timeout;
}
}
}
bool check_mode(uint32_t now)
{
static uint32_t prev_time = 0;
static uint32_t time_sticks_have_been_in_arming_position = 0;
// see it has been at least 20 ms
uint32_t dt = now-prev_time;
if (dt < 20000)
{
return false;
}
// if it has, then do stuff
prev_time = now;
// check for failsafe mode
if(check_failsafe())
{
return true;
}
else
{
// check for arming switch
if (get_param_int(PARAM_ARM_STICKS))
{
if (_armed_state == DISARMED)
{
// if left stick is down and to the right
if (pwmRead(get_param_int(PARAM_RC_F_CHANNEL)) < get_param_int(PARAM_RC_F_BOTTOM) + get_param_int(PARAM_ARM_THRESHOLD)
&& pwmRead(get_param_int(PARAM_RC_Z_CHANNEL)) > (get_param_int(PARAM_RC_Z_CENTER) + get_param_int(PARAM_RC_Z_RANGE)/2)
- get_param_int(PARAM_ARM_THRESHOLD))
{
time_sticks_have_been_in_arming_position += dt;
}
else
{
time_sticks_have_been_in_arming_position = 0;
}
if (time_sticks_have_been_in_arming_position > 500000)
{
arm();
time_sticks_have_been_in_arming_position = 0;
}
}
else // _armed_state is ARMED
{
// if left stick is down and to the left
if (pwmRead(get_param_int(PARAM_RC_F_CHANNEL)) < get_param_int(PARAM_RC_F_BOTTOM) +
get_param_int(PARAM_ARM_THRESHOLD)
&& pwmRead(get_param_int(PARAM_RC_Z_CHANNEL)) < (get_param_int(PARAM_RC_Z_CENTER) - get_param_int(PARAM_RC_Z_RANGE)/2)
+ get_param_int(PARAM_ARM_THRESHOLD))
{
time_sticks_have_been_in_arming_position += dt;
}
else
{
time_sticks_have_been_in_arming_position = 0;
}
if (time_sticks_have_been_in_arming_position > 500000)
{
disarm();
time_sticks_have_been_in_arming_position = 0;
}
}
}
else
{
if (rc_switch(get_param_int(PARAM_ARM_CHANNEL)))
{
arm();
}
else
{
disarm();
}
}
}
return true;
}
/*
* processRx called from taskUpdateRxMain
*/
bool processRx(timeUs_t currentTimeUs)
{
static bool armedBeeperOn = false;
if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) {
return false;
}
// in 3D mode, we need to be able to disarm by switch at any time
if (feature(FEATURE_3D)) {
if (!IS_RC_MODE_ACTIVE(BOXARM))
disarm();
}
updateRSSI(currentTimeUs);
if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
failsafeStartMonitoring();
}
failsafeUpdateState();
const throttleStatus_e throttleStatus = calculateThrottleStatus();
const uint8_t throttlePercent = calculateThrottlePercent();
if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
if (throttlePercent >= rxConfig()->airModeActivateThreshold) {
airmodeIsActivated = true; // Prevent Iterm from being reset
}
} else {
airmodeIsActivated = false;
}
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) {
pidResetITerm();
if (currentPidProfile->pidAtMinThrottle)
pidStabilisationState(PID_STABILISATION_ON);
else
pidStabilisationState(PID_STABILISATION_OFF);
} else {
pidStabilisationState(PID_STABILISATION_ON);
}
#ifdef USE_RUNAWAY_TAKEOFF
// If runaway_takeoff_prevention is enabled, accumulate the amount of time that throttle
// is above runaway_takeoff_deactivate_throttle with the any of the R/P/Y sticks deflected
// to at least runaway_takeoff_stick_percent percent while the pidSum on all axis is kept low.
// Once the amount of accumulated time exceeds runaway_takeoff_deactivate_delay then disable
// prevention for the remainder of the battery.
if (ARMING_FLAG(ARMED)
&& pidConfig()->runaway_takeoff_prevention
&& !runawayTakeoffCheckDisabled
&& !flipOverAfterCrashMode
&& !runawayTakeoffTemporarilyDisabled
&& !STATE(FIXED_WING)) {
// Determine if we're in "flight"
// - motors running
// - throttle over runaway_takeoff_deactivate_throttle_percent
// - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent
// - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit
bool inStableFlight = false;
if (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running?
const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle;
const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT);
if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit))
&& (fabsf(pidData[FD_PITCH].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
&& (fabsf(pidData[FD_ROLL].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
&& (fabsf(pidData[FD_YAW].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) {
inStableFlight = true;
if (runawayTakeoffDeactivateUs == 0) {
runawayTakeoffDeactivateUs = currentTimeUs;
}
}
}
// If we're in flight, then accumulate the time and deactivate once it exceeds runaway_takeoff_deactivate_delay milliseconds
if (inStableFlight) {
if (runawayTakeoffDeactivateUs == 0) {
runawayTakeoffDeactivateUs = currentTimeUs;
}
uint16_t deactivateDelay = pidConfig()->runaway_takeoff_deactivate_delay;
// at high throttle levels reduce deactivation delay by 50%
if (throttlePercent >= RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT) {
deactivateDelay = deactivateDelay / 2;
}
if ((cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) > deactivateDelay * 1000) {
runawayTakeoffCheckDisabled = true;
}
} else {
if (runawayTakeoffDeactivateUs != 0) {
runawayTakeoffAccumulatedUs += cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs);
}
runawayTakeoffDeactivateUs = 0;
}
if (runawayTakeoffDeactivateUs == 0) {
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, runawayTakeoffAccumulatedUs / 1000);
} else {
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_TRUE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, (cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) / 1000);
}
} else {
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, DEBUG_RUNAWAY_TAKEOFF_FALSE);
}
#endif
// When armed and motors aren't spinning, do beeps and then disarm
// board after delay so users without buzzer won't lose fingers.
// mixTable constrains motor commands, so checking throttleStatus is enough
if (ARMING_FLAG(ARMED)
&& feature(FEATURE_MOTOR_STOP)
&& !STATE(FIXED_WING)
&& !feature(FEATURE_3D)
&& !isAirmodeActive()
) {
if (isUsingSticksForArming()) {
if (throttleStatus == THROTTLE_LOW) {
if (armingConfig()->auto_disarm_delay != 0
&& (int32_t)(disarmAt - millis()) < 0
) {
// auto-disarm configured and delay is over
disarm();
armedBeeperOn = false;
} else {
// still armed; do warning beeps while armed
beeper(BEEPER_ARMED);
armedBeeperOn = true;
}
} else {
// throttle is not low
if (armingConfig()->auto_disarm_delay != 0) {
// extend disarm time
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;
}
if (armedBeeperOn) {
beeperSilence();
armedBeeperOn = false;
}
}
} else {
// arming is via AUX switch; beep while throttle low
if (throttleStatus == THROTTLE_LOW) {
beeper(BEEPER_ARMED);
armedBeeperOn = true;
} else if (armedBeeperOn) {
beeperSilence();
armedBeeperOn = false;
}
}
}
processRcStickPositions();
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
updateInflightCalibrationState();
}
updateActivatedModes();
#ifdef USE_DSHOT
/* Enable beep warning when the crash flip mode is active */
if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
beeper(BEEPER_CRASH_FLIP_MODE);
}
#endif
if (!cliMode) {
updateAdjustmentStates();
processRcAdjustments(currentControlRateProfile);
}
bool canUseHorizonMode = true;
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeIsActive()) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
canUseHorizonMode = false;
if (!FLIGHT_MODE(ANGLE_MODE)) {
ENABLE_FLIGHT_MODE(ANGLE_MODE);
}
} else {
DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
}
if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
DISABLE_FLIGHT_MODE(ANGLE_MODE);
if (!FLIGHT_MODE(HORIZON_MODE)) {
ENABLE_FLIGHT_MODE(HORIZON_MODE);
}
} else {
DISABLE_FLIGHT_MODE(HORIZON_MODE);
}
#ifdef USE_GPS_RESCUE
if (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE)) {
if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
}
} else {
DISABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
}
#endif
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
LED1_ON;
// increase frequency of attitude task to reduce drift when in angle or horizon mode
rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(500));
} else {
LED1_OFF;
rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(100));
}
if (!IS_RC_MODE_ACTIVE(BOXPREARM) && ARMING_FLAG(WAS_ARMED_WITH_PREARM)) {
DISABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM);
}
#if defined(USE_ACC) || defined(USE_MAG)
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
#if defined(USE_GPS) || defined(USE_MAG)
if (IS_RC_MODE_ACTIVE(BOXMAG)) {
if (!FLIGHT_MODE(MAG_MODE)) {
ENABLE_FLIGHT_MODE(MAG_MODE);
magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
}
} else {
DISABLE_FLIGHT_MODE(MAG_MODE);
}
#endif
if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
if (!FLIGHT_MODE(HEADFREE_MODE)) {
ENABLE_FLIGHT_MODE(HEADFREE_MODE);
}
} else {
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
if (imuQuaternionHeadfreeOffsetSet()){
beeper(BEEPER_RX_SET);
}
}
}
#endif
if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
} else {
DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
}
if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) {
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
#ifdef USE_TELEMETRY
static bool sharedPortTelemetryEnabled = false;
if (feature(FEATURE_TELEMETRY)) {
bool enableSharedPortTelemetry = (!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) || (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY));
if (enableSharedPortTelemetry && !sharedPortTelemetryEnabled) {
mspSerialReleaseSharedTelemetryPorts();
telemetryCheckState();
sharedPortTelemetryEnabled = true;
} else if (!enableSharedPortTelemetry && sharedPortTelemetryEnabled) {
// the telemetry state must be checked immediately so that shared serial ports are released.
telemetryCheckState();
mspSerialAllocatePorts();
sharedPortTelemetryEnabled = false;
}
}
#endif
#ifdef USE_VTX_CONTROL
vtxUpdateActivatedChannel();
if (canUpdateVTX()) {
handleVTXControlButton();
}
#endif
#ifdef USE_ACRO_TRAINER
pidSetAcroTrainerState(IS_RC_MODE_ACTIVE(BOXACROTRAINER) && sensors(SENSOR_ACC));
#endif // USE_ACRO_TRAINER
#ifdef USE_RC_SMOOTHING_FILTER
if (ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) {
beeper(BEEPER_RC_SMOOTHING_INIT_FAIL);
}
#endif
pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY));
return true;
}
void processRcStickPositions(throttleStatus_e throttleStatus)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t rcSticks; // this hold sticks position for command combos
static uint8_t rcDisarmTicks; // this is an extra guard for disarming through switch to prevent that one frame can disarm it
uint8_t stTmp = 0;
int i;
// ------------------ STICKS COMMAND HANDLER --------------------
// checking sticks positions
for (i = 0; i < 4; i++) {
stTmp >>= 2;
if (rcData[i] > rxConfig()->mincheck)
stTmp |= 0x80; // check for MIN
if (rcData[i] < rxConfig()->maxcheck)
stTmp |= 0x40; // check for MAX
}
if (stTmp == rcSticks) {
if (rcDelayCommand < 250)
rcDelayCommand++;
} else
rcDelayCommand = 0;
rcSticks = stTmp;
// perform actions
if (!isUsingSticksToArm) {
if (IS_RC_MODE_ACTIVE(BOXARM)) {
rcDisarmTicks = 0;
// Arming via ARM BOX
tryArm();
} else {
// Disarming via ARM BOX
resetArmingDisabled();
if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) {
rcDisarmTicks++;
if (rcDisarmTicks > 3) {
if (armingConfig()->disarm_kill_switch) {
disarm();
} else if (throttleStatus == THROTTLE_LOW) {
disarm();
}
}
}
}
}
if (rcDelayCommand != 20) {
return;
}
if (isUsingSticksToArm) {
// Disarm on throttle down + yaw
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
if (ARMING_FLAG(ARMED))
disarm();
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
rcDelayCommand = 0; // reset so disarm tone will repeat
}
}
}
if (ARMING_FLAG(ARMED)) {
// actions during armed
return;
}
// actions during not armed
i = 0;
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
// GYRO calibration
gyroStartCalibration(false);
#ifdef GPS
if (feature(FEATURE_GPS)) {
GPS_reset_home_position();
}
#endif
#ifdef BARO
if (sensors(SENSOR_BARO))
baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
#endif
return;
}
if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
// Inflight ACC Calibration
handleInflightCalibrationStickPosition();
return;
}
// Multiple configuration profiles
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1
i = 1;
else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2
i = 2;
else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3
i = 3;
if (i) {
changePidProfile(i - 1);
return;
}
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
saveConfigAndNotify();
}
if (isUsingSticksToArm) {
if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
// Arm via YAW
tryArm();
return;
} else {
resetArmingDisabled();
}
}
if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
// Calibrating Acc
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
return;
}
if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
// Calibrating Mag
ENABLE_STATE(CALIBRATE_MAG);
return;
}
// Accelerometer Trim
rollAndPitchTrims_t accelerometerTrimsDelta;
memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta));
bool shouldApplyRollAndPitchTrimDelta = false;
if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
accelerometerTrimsDelta.values.pitch = 2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
accelerometerTrimsDelta.values.pitch = -2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
accelerometerTrimsDelta.values.roll = 2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
accelerometerTrimsDelta.values.roll = -2;
shouldApplyRollAndPitchTrimDelta = true;
}
if (shouldApplyRollAndPitchTrimDelta) {
applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta);
rcDelayCommand = 0; // allow autorepetition
return;
}
#ifdef USE_DASHBOARD
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
dashboardDisablePageCycling();
}
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
dashboardEnablePageCycling();
}
#endif
#ifdef VTX_CONTROL
if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_HI) {
vtxIncrementBand();
}
if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_LO) {
vtxDecrementBand();
}
if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_HI) {
vtxIncrementChannel();
}
if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_LO) {
vtxDecrementChannel();
}
#endif
}
void processRcStickPositions(throttleStatus_e throttleStatus)
{
// time the sticks are maintained
static int16_t rcDelayMs;
// hold sticks position for command combos
static uint8_t rcSticks;
// an extra guard for disarming through switch to prevent that one frame can disarm it
static uint8_t rcDisarmTicks;
static bool doNotRepeat;
#ifdef USE_CMS
if (cmsInMenu) {
return;
}
#endif
// checking sticks positions
uint8_t stTmp = 0;
for (int i = 0; i < 4; i++) {
stTmp >>= 2;
if (rcData[i] > rxConfig()->mincheck) {
stTmp |= 0x80; // check for MIN
}
if (rcData[i] < rxConfig()->maxcheck) {
stTmp |= 0x40; // check for MAX
}
}
if (stTmp == rcSticks) {
if (rcDelayMs <= INT16_MAX - (getTaskDeltaTime(TASK_SELF) / 1000)) {
rcDelayMs += getTaskDeltaTime(TASK_SELF) / 1000;
}
} else {
rcDelayMs = 0;
doNotRepeat = false;
}
rcSticks = stTmp;
// perform actions
if (!isUsingSticksToArm) {
if (IS_RC_MODE_ACTIVE(BOXARM)) {
rcDisarmTicks = 0;
// Arming via ARM BOX
tryArm();
} else {
// Disarming via ARM BOX
resetArmingDisabled();
if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) {
rcDisarmTicks++;
if (rcDisarmTicks > 3) {
if (armingConfig()->disarm_kill_switch) {
disarm();
} else if (throttleStatus == THROTTLE_LOW) {
disarm();
}
}
}
}
} else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
doNotRepeat = true;
// Disarm on throttle down + yaw
if (ARMING_FLAG(ARMED))
disarm();
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
repeatAfter(STICK_AUTOREPEAT_MS); // disarm tone will repeat
}
}
return;
} else if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
doNotRepeat = true;
if (!ARMING_FLAG(ARMED)) {
// Arm via YAW
tryArm();
} else {
resetArmingDisabled();
}
}
return;
}
if (ARMING_FLAG(ARMED) || doNotRepeat || rcDelayMs <= STICK_DELAY_MS) {
return;
}
doNotRepeat = true;
// actions during not armed
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
// GYRO calibration
gyroStartCalibration(false);
#ifdef USE_GPS
if (feature(FEATURE_GPS)) {
GPS_reset_home_position();
}
#endif
#ifdef USE_BARO
if (sensors(SENSOR_BARO))
baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
#endif
return;
}
if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
// Inflight ACC Calibration
handleInflightCalibrationStickPosition();
return;
}
// Change PID profile
int newPidProfile = 0;
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) { // ROLL left -> PID profile 1
newPidProfile = 1;
} else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) { // PITCH up -> PID profile 2
newPidProfile = 2;
} else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) { // ROLL right -> PID profile 3
newPidProfile = 3;
}
if (newPidProfile) {
changePidProfile(newPidProfile - 1);
return;
}
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
saveConfigAndNotify();
}
if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
// Calibrating Acc
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
return;
}
if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
// Calibrating Mag
ENABLE_STATE(CALIBRATE_MAG);
return;
}
if (FLIGHT_MODE(ANGLE_MODE|HORIZON_MODE)) {
// in ANGLE or HORIZON mode, so use sticks to apply accelerometer trims
rollAndPitchTrims_t accelerometerTrimsDelta;
memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta));
bool shouldApplyRollAndPitchTrimDelta = false;
if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
accelerometerTrimsDelta.values.pitch = 2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
accelerometerTrimsDelta.values.pitch = -2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
accelerometerTrimsDelta.values.roll = 2;
shouldApplyRollAndPitchTrimDelta = true;
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
accelerometerTrimsDelta.values.roll = -2;
shouldApplyRollAndPitchTrimDelta = true;
}
if (shouldApplyRollAndPitchTrimDelta) {
applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta);
repeatAfter(STICK_AUTOREPEAT_MS);
return;
}
} else {
// in ACRO mode, so use sticks to change RATE profile
switch (rcSticks) {
case THR_HI + YAW_CE + PIT_HI + ROL_CE:
changeControlRateProfile(0);
return;
case THR_HI + YAW_CE + PIT_LO + ROL_CE:
changeControlRateProfile(1);
return;
case THR_HI + YAW_CE + PIT_CE + ROL_HI:
changeControlRateProfile(2);
return;
case THR_HI + YAW_CE + PIT_CE + ROL_LO:
changeControlRateProfile(3);
return;
}
}
#ifdef USE_DASHBOARD
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
dashboardDisablePageCycling();
}
if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
dashboardEnablePageCycling();
}
#endif
#ifdef USE_VTX_CONTROL
if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_HI) {
vtxIncrementBand();
}
if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_LO) {
vtxDecrementBand();
}
if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_HI) {
vtxIncrementChannel();
}
if (rcSticks == THR_HI + YAW_HI + PIT_CE + ROL_LO) {
vtxDecrementChannel();
}
#endif
#ifdef USE_CAMERA_CONTROL
if (rcSticks == THR_CE + YAW_HI + PIT_CE + ROL_CE) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_ENTER, 0);
repeatAfter(3 * STICK_DELAY_MS);
} else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_LO) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_LEFT, 0);
repeatAfter(3 * STICK_DELAY_MS);
} else if (rcSticks == THR_CE + YAW_CE + PIT_HI + ROL_CE) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 0);
repeatAfter(3 * STICK_DELAY_MS);
} else if (rcSticks == THR_CE + YAW_CE + PIT_CE + ROL_HI) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_RIGHT, 0);
repeatAfter(3 * STICK_DELAY_MS);
} else if (rcSticks == THR_CE + YAW_CE + PIT_LO + ROL_CE) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_DOWN, 0);
repeatAfter(3 * STICK_DELAY_MS);
} else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_CE) {
cameraControlKeyPress(CAMERA_CONTROL_KEY_UP, 2000);
}
#endif
}
