static void
MultiActivate(
Widget wid,
XEvent *buttonEvent,
String *params,
Cardinal *num_params)
{
/* When a multi click sequence occurs and the user Button Presses and
* holds for a length of time, the final release should look like a
* new/separate activate.
*/
XmArrowButtonWidget aw = (XmArrowButtonWidget) wid;
if (aw->arrowbutton.multiClick == XmMULTICLICK_KEEP)
{
if ((buttonEvent->xbutton.time - aw->arrowbutton.armTimeStamp) >
XtGetMultiClickTime(XtDisplay(aw)))
aw->arrowbutton.click_count = 1;
else
aw->arrowbutton.click_count++;
ActivateCommon((Widget) aw, buttonEvent);
Disarm ((Widget) aw, buttonEvent, params, num_params);
}
}
Missile::Missile(const ShipType::Id &shipId, Body *owner, int power)//: Ship(shipId)
{
if (power < 0) {
m_power = 0;
if (shipId == ShipType::MISSILE_GUIDED) m_power = 1;
if (shipId == ShipType::MISSILE_SMART) m_power = 2;
if (shipId == ShipType::MISSILE_NAVAL) m_power = 3;
} else
m_power = power;
m_owner = owner;
m_type = &ShipType::types[shipId];
SetMass(m_type->hullMass*1000);
SetModel(m_type->modelName.c_str());
SetMassDistributionFromModel();
SetLabel(Lang::MISSILE);
Disarm();
SetFuel(1.0);
SetFuelReserve(0.0);
m_curAICmd = 0;
m_aiMessage = AIERROR_NONE;
m_decelerating = false;
Propulsion::Init( this, GetModel(), m_type->fuelTankMass, m_type->effectiveExhaustVelocity, m_type->linThrust, m_type->angThrust );
}
Missile::Missile(ShipType::Id shipId, Body *owner, int power): Ship(shipId)
{
if (power < 0) {
m_power = 0;
if (shipId == ShipType::MISSILE_GUIDED) m_power = 1;
if (shipId == ShipType::MISSILE_SMART) m_power = 2;
if (shipId == ShipType::MISSILE_NAVAL) m_power = 3;
} else
m_power = power;
m_owner = owner;
SetLabel(Lang::MISSILE);
Disarm();
}
static void
MultiActivate(Widget w, XEvent *event, String *params, Cardinal *num_params)
{
XButtonEvent *ev = (XButtonEvent *)event;
XmDrawnButtonCallbackStruct cbs;
DEBUGOUT(_LtDebug(__FILE__, w, "DrawnB: MultiClick\n"));
if (DB_MultiClick(w) == XmMULTICLICK_KEEP)
{
Time mctime = XtGetMultiClickTime(XtDisplay(w));
if ((event->xbutton.time - DB_ArmTimeStamp(w)) < mctime)
{
DB_ClickCount(w)++;
}
else
{
DB_ClickCount(w) = 1;
}
DB_Armed(w) = False;
if (XtIsRealized(w))
XtClass(w)->core_class.expose(w, event, NULL);
if (ev->type == KeyPress || ev->type == KeyRelease ||
((ev->x >= 0 && ev->x < XtWidth(w)) &&
(ev->y >= 0 && ev->y < XtHeight(w))))
{
if (!Lab_SkipCallback(w) && DB_ActivateCallback(w))
{
cbs.reason = XmCR_ACTIVATE;
cbs.event = event;
cbs.click_count = DB_ClickCount(w);
XFlush(XtDisplay(w));
XtCallCallbackList(w,
DB_ActivateCallback(w),
(XtPointer)&cbs);
}
}
Disarm(w, event, params, num_params);
}
}
// This function is never called if there is a calibration issue.
// called in true section of if (RX_Snapshot[RXChannel_THR] < STICKThrottle_ARMING)
void HandleSticksForArming (void)
{
if ((UIEnableStickCommands==false) || (ActiveRXIndex!=1) || (!IS_TX2_GOOD)) return ; // you cannot use Primary to Arm and Disarm
SystemErrorType = CLR_SYS_ERR_SIGNAL;
if (TCNT1_X_snapshot1==0) TCNT1_X_snapshot1 = CurrentTCNT1_X; // start counting
// DisArm Check
if (IsArmed == true)
{
if (RX_Latest[ActiveRXIndex][RXChannel_RUD] > STICK_LEFT)
{ // Check DisArming manually.
bResetTCNR1_X = false;
if ( (CurrentTCNT1_X - TCNT1_X_snapshot1) > STICKPOSITION_LONG_TIME )
{
Disarm();
return ;
}
}
if (Config.AutoDisarm!=0)
{ // check auto disArm
if (TCNT_X_snapshotAutoDisarm > CurrentTCNT1_X ) TCNT_X_snapshotAutoDisarm = 0; // the CurrentTCNT1_X was high so the disarm condition never true.
if (TCNT_X_snapshotAutoDisarm==0) TCNT_X_snapshotAutoDisarm = CurrentTCNT1_X;
if ((CurrentTCNT1_X - TCNT_X_snapshotAutoDisarm) > (DISARM_TIME * Config.AutoDisarm))
{
Disarm();
return ;
}
}
}
if (IsArmed == false)
{
//int16_t Stick = (Config.RX_Mid[ActiveRXIndex][RXChannel_RUD] + RX_Latest[ActiveRXIndex][RXChannel_RUD] * RX_Div_Factor);
if (RX_Latest[ActiveRXIndex][RXChannel_RUD] < STICK_RIGHT)
{ // Armin Check
bResetTCNR1_X = false;
if ( (CurrentTCNT1_X- TCNT1_X_snapshot1) > STICKPOSITION_LONG_TIME )
{
Arm();
return ;
}
}
//set modes Quad , X-Quad
if (RX_Latest[ActiveRXIndex][RXChannel_AIL] > STICK_LEFT)
{// X-QUAD MODE
bResetTCNR1_X = false;
if ( (CurrentTCNT1_X- TCNT1_X_snapshot1) > STICKPOSITION_LONG_TIME )
{
Config.QuadFlyingMode=QuadFlyingMode_X;
LED_FlashOrangeLED (LED_LONG_TOGGLE,8);
TCNT1_X_snapshot1 =0; // reset timer
}
}
else if ((RX_Latest[ActiveRXIndex][RXChannel_AIL] < STICK_RIGHT))
{ // QUAD COPTER MODE
bResetTCNR1_X = false;
if ( (CurrentTCNT1_X- TCNT1_X_snapshot1) > STICKPOSITION_LONG_TIME )
{
Config.QuadFlyingMode=QuadFlyingMode_PLUS;
LED_FlashOrangeLED (LED_LONG_TOGGLE,4);
TCNT1_X_snapshot1 =0; // reset timer
}
}
// set mode ACRO , Leveling
else if ((RX_Latest[ActiveRXIndex][RXChannel_ELE] < STICK_RIGHT))
{ //nFlyingModes = FLYINGMODE_LEVEL;
bResetTCNR1_X = false;
if ( (CurrentTCNT1_X- TCNT1_X_snapshot1) > STICKPOSITION_LONG_TIME )
{
nFlyingModes = FLYINGMODE_LEVEL;
LED_FlashOrangeLED (LED_LONG_TOGGLE,8);
TCNT1_X_snapshot1 =0; // reset timer
}
}
else if ((RX_Latest[ActiveRXIndex][RXChannel_ELE] > STICK_LEFT))
{ //nFlyingModes = FLYINGMODE_ACRO;
bResetTCNR1_X = false;
if ( (CurrentTCNT1_X- TCNT1_X_snapshot1) > STICKPOSITION_LONG_TIME )
{
nFlyingModes = FLYINGMODE_ACRO;
LED_FlashOrangeLED (LED_LONG_TOGGLE,4);
TCNT1_X_snapshot1 =0; // reset timer
}
}
}
}
void MainLoop(void)
{
RX_CopyLatestReceiverValues();
//RX_Snapshot_1 [RXChannel_THR]= RX_Snapshot[RXChannel_THR];
RX_Snapshot [RXChannel_THR]= RX_Latest[ActiveRXIndex][RXChannel_THR];
Sensors_ReadAll();
ATOMIC_BLOCK(ATOMIC_FORCEON)
{
CurrentTCNT1_X = TCNT1_X;
}
IMU();
#ifdef DEBUG_ME
// Sending Sensors & Motor Data
if (Config.RX_mode==RX_mode_SingleMode)
{
//LED_Orange=~LED_Orange;
#ifdef UART_SEND_SENSORS
Send_Byte('S');
Send_Data((void *)(&Sensors_Latest[0]),24);
Send_Data((void *)(&MotorOut[0]),8);
Send_Byte('E');
#else
Send_Byte('I');
Send_Data((void *)(&Sensors_Latest[0]),12);
Send_Data((void *)(&AnglePitch),4);
Send_Data((void *)(&AngleRoll),4);
Send_Data((void *)(&CompAccZ),4);
Send_Data((void *)(&MotorOut[0]),8);
Send_Byte('E');
#endif
}
#endif
bResetTCNR1_X = true;
if (Config.RX_mode==RX_mode_BuddyMode)
{ // in Buddy mode AUX channel is used for instance switching.
if (IS_TX2_GOOD)
{
if (RX_Latest[RX_MAIN][RXChannel_AUX] < STICK_RIGHT)
{
ActiveRXIndex = 0; // use Primary RX
}
else
{
ActiveRXIndex = 1; // use Secondary RX
}
}
}
////////// Slow Actions inside
// HINT: you can try to skip this if flying to save time for more useful tasks as user cannot access menu when flying
if (TCNT_X_snapshot2 == 0) TCNT_X_snapshot2 = CurrentTCNT1_X;
else if ( ((CurrentTCNT1_X - TCNT_X_snapshot2) > 4) ) // TCNT1_X ticks in 32.768us
{
Menu_MenuShow();
if (Config.VoltageAlarm > 0)
{
//Sensor_GetBattery();
if (Sensors_Latest[V_BAT_Index] < Config.VoltageAlarm)
{
SystemErrorType = SET_SYS_ERR_VOLTAGE;
}
else
{
SystemErrorType = CLR_SYS_ERR_VOLTAGE;
}
}
if (SystemErrorType != SYS_ERR_NON)
{
Buzzer =~Buzzer ;
}
else
{
Buzzer = OFF;
}
if (Config.RX_mode==RX_mode_SingleMode)
{
if ((FlyingModesToggle != LOW) && ( RX_Latest[RX_MAIN][RXChannel_AUX] < STICK_RIGHT ))
{
nFlyingModes = FLYINGMODE_ALTHOLD;
FlyingModesToggle = LOW;
} else
if ((FlyingModesToggle != HIGH) && (RX_Latest[RX_MAIN][RXChannel_AUX] > STICK_LEFT ))
{
//LED_Orange=ON;
nFlyingModes = FLYINGMODE_ACRO;
FlyingModesToggle = HIGH;
} else
if ((FlyingModesToggle != MID) && (RX_Latest[RX_MAIN][RXChannel_AUX]< STICK_LEFT) && ( RX_Latest[RX_MAIN][RXChannel_AUX] > STICK_RIGHT ))
{
nFlyingModes = FLYINGMODE_LEVEL;
FlyingModesToggle = MID;
}
}
//if ((IsArmed == true) && (RX_Snapshot[RXChannel_THR] < STICKThrottle_ARMING+160))
//{ // calibrate when start flying
//DynamicCalibration();
//}
TCNT_X_snapshot2=0;
}
//////////////// EOF Slow Loop
if (RX_Snapshot[RXChannel_THR] < STICKThrottle_ARMING)
{
// Throttle is LOW
// Here you can add code without caring about delays. As there quad is already off and on land.
// here we test different positions of sticks to enable arm/disarm, Quad/X-Quad
HandleSticksForArming();
// Stop motors if Throttle Stick is less than minimum.
ZEROMotors();
#ifndef DEBUG_ME
// Disable when debug
ZERO_Is();
IMU_Reset(); // reset angles for gyro [STABLE MODE]
#endif
}
else
{ // Throttle stick is NOT Down .... TAKE CARE
if (IsArmed==false)
{ // However we are still DisArmed
ZEROMotors();
// Sticks as Keyboard --- we are already disarmed to reach here.
HandleSticksAsKeys();
#ifdef DEBUG_ME
// ENable when debug
GetExpoPoint(RX_Latest[ActiveRXIndex][RXChannel_AIL]);
RX_Snapshot [RXChannel_AIL] = (RXResult );
GetExpoPoint(RX_Latest[ActiveRXIndex][RXChannel_ELE]);
RX_Snapshot [RXChannel_ELE] = (RXResult );
RX_Snapshot [RXChannel_RUD] = (RX_Latest[ActiveRXIndex][RXChannel_RUD] * 0.5 ); // version 0.9.9
#endif
}
else
{ // MOTORS ARE ON HERE .... DANGEROUS
TCNT_X_snapshotAutoDisarm = 0; // ZERO [user may disarm then fly slowly..in this case the qude will disarm once he turned off the stick...because the counter counts once the quad is armed..e.g. if it takes n sec to disarm automatically..user took n-1 sec keeping the stick low after arming then it will take 1 sec to disarm again after lowing the stick under STICKThrottle_ARMING
// Armed & Throttle Stick > MIN . . . We should Fly now.
GetExpoPoint(RX_Latest[ActiveRXIndex][RXChannel_AIL]);
RX_Snapshot [RXChannel_AIL] = (RXResult);
GetExpoPoint(RX_Latest[ActiveRXIndex][RXChannel_ELE]);
RX_Snapshot [RXChannel_ELE] = (RXResult);
RX_Snapshot [RXChannel_RUD] = (RX_Latest[ActiveRXIndex][RXChannel_RUD] * 0.5 ); // version 0.9.9
// Add Throttle to Motors
MotorOut[0] = RX_Snapshot[RXChannel_THR];
MotorOut[1] = RX_Snapshot[RXChannel_THR];
if (Config.FrameType == FRAMETYPE_QUADCOPTER)
{
MotorOut[2] = RX_Snapshot[RXChannel_THR];
MotorOut[3] = RX_Snapshot[RXChannel_THR];
}
else
{
MotorOut[2] = RX_Snapshot[RXChannel_THR];
}
// Add IMU control.
if (Config.FrameType == FRAMETYPE_QUADCOPTER)
{ // Quadcopter
/*
* The logic below depends on board orientation i.e. sensor orientation compared to motor directions.
* the IMU in ACHRO mode is totally independent from user sticks ... so it does not matter how the user
* flies his quad i.e. in X or PLUS .... it does not matter because it is handled in another code lines not here.
* here we handle board orientation only.
*/
if (Config.BoardOrientationMode==QuadFlyingMode_X)
{ // Board Orientation in X-Mode
// {-1,1,1,-1} QUAD_ROL_X
MotorOut[0] -= gyroRoll ;
MotorOut[3] -= gyroRoll ;
MotorOut[1] += gyroRoll ;
MotorOut[2] += gyroRoll ;
// {-1,-1,1,1} QUAD_AIL_X
MotorOut[0] -= gyroPitch;
MotorOut[1] -= gyroPitch;
MotorOut[2] += gyroPitch;
MotorOut[3] += gyroPitch;
}
else
{ // Board Orientation in Plus-Mode
// {0,1,0,-1} QUAD_ROL_PLUS
MotorOut[1] += gyroRoll ;
MotorOut[3] -= gyroRoll ;
// {-1,0,1,0} QUAD_AIL_PLUS
MotorOut[0] -= gyroPitch ;
MotorOut[2] += gyroPitch ;
}
}
else
{ // Balance Tri-Copter
// NOT VALID if Board Orientation is X
MotorOut[2] += gyroPitch * 1.34; // 4/3
gyroPitch = gyroPitch * 0.67; // 2/3
MotorOut[0] -= gyroPitch ;
MotorOut[1] -= gyroPitch ;
gyroRoll = gyroRoll; // * 0.85;
MotorOut[0] -= gyroRoll ;
MotorOut[1] += gyroRoll ;
}
/*
*
* Pilot Control Logic.
* Handles signals from remote control in ACRO mode.
* in stabilization mode controls are added in IMU logic as angles.
*/
if (nFlyingModes == FLYINGMODE_ACRO)
{
if (Config.FrameType == FRAMETYPE_QUADCOPTER)
{
if (Config.QuadFlyingMode==QuadFlyingMode_X)
{
RX_Snapshot[RXChannel_AIL] = RX_Snapshot[RXChannel_AIL] * 0.63; // because we fly X
RX_Snapshot[RXChannel_ELE] = RX_Snapshot[RXChannel_ELE] * 0.63;
// {0.63,-0.63,-0.63,0.63} QUAD_AIL_X
MotorOut[0] += RX_Snapshot[RXChannel_AIL] ;
MotorOut[3] += RX_Snapshot[RXChannel_AIL] ;
MotorOut[1] -= RX_Snapshot[RXChannel_AIL] ;
MotorOut[2] -= RX_Snapshot[RXChannel_AIL] ;
// {0.63,0.63,-0.63,-0.63} QUAD_ELE_X
MotorOut[0] += RX_Snapshot[RXChannel_ELE];
MotorOut[1] += RX_Snapshot[RXChannel_ELE];
MotorOut[2] -= RX_Snapshot[RXChannel_ELE];
MotorOut[3] -= RX_Snapshot[RXChannel_ELE];
}
else
{
// {0.9,0,-0.9,0} QUAD_ELE_PLUS
MotorOut[0] += RX_Snapshot[RXChannel_ELE] ;
MotorOut[2] -= RX_Snapshot[RXChannel_ELE] ;
// {0,-0.9,0,0.9} QUAD_AIL_PLUS
MotorOut[1] -= RX_Snapshot[RXChannel_AIL] ;
MotorOut[3] += RX_Snapshot[RXChannel_AIL] ;
}
}
else
{ // TRICopter
int8_t inv=1; // Flying in Y mode
if (Config.QuadFlyingMode==QuadFlyingMode_X)
{ // Flying in A mode
inv = -1;
}
// {0.5,0.5,1.1,X} TRI_ELE_FRONT
MotorOut[2] -= inv * (RX_Snapshot[RXChannel_ELE]);
RX_Snapshot[RXChannel_ELE] = inv * RX_Snapshot[RXChannel_ELE] ;
MotorOut[0] += RX_Snapshot[RXChannel_ELE] ;
MotorOut[1] += RX_Snapshot[RXChannel_ELE] ;
// {1,-1,0,X} TRI_AIL_FRONT
RX_Snapshot[RXChannel_AIL] = inv * RX_Snapshot[RXChannel_AIL];
MotorOut[0] += RX_Snapshot[RXChannel_AIL] ;
MotorOut[1] -= RX_Snapshot[RXChannel_AIL] ;
}
}
// ACC-Z & SONAR
if (nFlyingModes != FLYINGMODE_ACRO)
{
// in stabilization mode ... activate Acc-Z & Sonar if enabled.
double Landing;
// Handles Sonar & ACC-Z (Z is always handles even if ALTHLD is false)
Landing = IMU_HeightKeeping();
MotorOut[0] += Landing;
MotorOut[1] += Landing;
if (Config.FrameType == FRAMETYPE_QUADCOPTER)
{
MotorOut[2] += Landing;
MotorOut[3] += Landing;
}
else
{
MotorOut[2] += Landing;
}
}
if (Config.FrameType == FRAMETYPE_QUADCOPTER)
{
MotorOut[0] -= gyroYaw;
MotorOut[2] -= gyroYaw;
MotorOut[1] += gyroYaw;
MotorOut[3] += gyroYaw;
}
else
{
MotorOut[3] = (Config.ReverseYAW * gyroYaw) + SERVO_IN_MIDDLE;
MotorOut[3] = MotorOut[3] - (Config.ReverseYAW * RX_Snapshot[RXChannel_RUD]);
}
// Save motors from turning-off
if (MotorOut[0]<MOTORS_IDLE_VALUE) MotorOut[0]=MOTORS_IDLE_VALUE;
if (MotorOut[1]<MOTORS_IDLE_VALUE) MotorOut[1]=MOTORS_IDLE_VALUE;
if (MotorOut[2]<MOTORS_IDLE_VALUE) MotorOut[2]=MOTORS_IDLE_VALUE;
if (Config.FrameType == FRAMETYPE_QUADCOPTER)
{
if (MotorOut[3]<MOTORS_IDLE_VALUE) MotorOut[3]=MOTORS_IDLE_VALUE;
}
// Sending Sensors & Motor Data
if (Config.RX_mode==RX_mode_SingleMode)
{
#ifdef UART_SEND_SENSORS
Send_Byte('S');
Send_Data((void *)(&Sensors_Latest[0]),24);
Send_Data((void *)(&MotorOut[0]),8);
Send_Byte('E');
#else
Send_Byte('I');
Send_Data((void *)(&Sensors_Latest[0]),12);
Send_Data((void *)(&AnglePitch),4);
Send_Data((void *)(&AngleRoll),4);
Send_Data((void *)(&CompAccZ),4);
Send_Data((void *)(&MotorOut[0]),8);
Send_Byte('E');
#endif
}
} // End of ARMED & Throttle > Minimum
} // End of Throttle stick is NOT Down [Armed Could be True or not]
if ((!IS_TX2_GOOD)) // if no signal and there is no AutoLandingMode.
{
ZEROMotors();
if (IsArmed==true)
{
Motor_GenerateOutputSignal();
Disarm();
SystemErrorType = SET_SYS_ERR_SIGNAL; // only error if signal lost while arming
}
//return ; // Do nothing all below depends on TX.
}
Motor_GenerateOutputSignal();
if (bResetTCNR1_X==true)
{
TCNT1_X_snapshot1= 0; // reset timeout
}
}
