Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Some cleanup.

This commit is contained in:
James Goppert 2011-10-19 00:25:00 -04:00
parent 1aa8586558
commit d20d53ee81
3 changed files with 75 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
libraries/APO/AP_ArmingMechanism.cpp
View File

@ -14,42 +14,44 @@ namespace apo {
void AP_ArmingMechanism::update(const float dt) {
// arming
if ( (_hal->getState() != MAV_STATE_ACTIVE) &
(_hal->rc[_ch1]->getRadioPosition() < _ch1Min) &&
(_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) {
// arming
if ( (_hal->getState() != MAV_STATE_ACTIVE) &
(_hal->rc[_ch1]->getRadioPosition() < _ch1Min) &&
(_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) {
// always start clock at 0
if (_armingClock<0) _armingClock = 0;
// always start clock at 0
if (_armingClock<0) _armingClock = 0;
if (_armingClock++ >= 100) {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
_hal->setState(MAV_STATE_ACTIVE);
} else {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("arming"));
}
}
// disarming
else if ( (_hal->getState() == MAV_STATE_ACTIVE) &
(_hal->rc[_ch1]->getRadioPosition() < _ch1Min) &&
(_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) {
if (_armingClock++ >= 100) {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
_hal->setState(MAV_STATE_ACTIVE);
} else {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("arming"));
}
}
// disarming
else if ( (_hal->getState() == MAV_STATE_ACTIVE) &
(_hal->rc[_ch1]->getRadioPosition() < _ch1Min) &&
(_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) {
// always start clock at 0
if (_armingClock>0) _armingClock = 0;
// always start clock at 0
if (_armingClock>0) _armingClock = 0;
if (_armingClock-- <= -100) {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
_hal->setState(MAV_STATE_STANDBY);
} else {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarming"));
}
}
// reset arming clock and report status
else if (_armingClock != 0) {
_armingClock = 0;
if (_hal->getState()==MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
else if (_hal->getState()!=MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
}
if (_armingClock-- <= -100) {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
_hal->setState(MAV_STATE_STANDBY);
} else {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarming"));
}
}
// reset arming clock and report status
else if (_armingClock != 0) {
_armingClock = 0;
if (_hal->getState()==MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
else if (_hal->getState()!=MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
}
}
} // apo
// vim:ts=4:sw=4:expandtab

78
libraries/APO/AP_ArmingMechanism.h
View File

@ -16,50 +16,52 @@ class AP_HardwareAbstractionLayer;
class AP_ArmingMechanism {
public:
/**
* Constructor
*
* @param ch1: typically throttle channel
* @param ch2: typically yaw channel
* @param ch1Min: disarms/arms belows this
* @param ch2Min: disarms below this
* @param ch2Max: arms above this
*/
AP_ArmingMechanism(AP_HardwareAbstractionLayer * hal,
uint8_t ch1, uint8_t ch2, float ch1Min, float ch2Min,
float ch2Max) : _armingClock(0), _hal(hal), _ch1(ch1), _ch2(ch2),
_ch1Min(ch1Min), _ch2Min(ch2Min) {
}
/**
* Constructor
*
* @param ch1: typically throttle channel
* @param ch2: typically yaw channel
* @param ch1Min: disarms/arms belows this
* @param ch2Min: disarms below this
* @param ch2Max: arms above this
*/
AP_ArmingMechanism(AP_HardwareAbstractionLayer * hal,
uint8_t ch1, uint8_t ch2, float ch1Min, float ch2Min,
float ch2Max) : _armingClock(0), _hal(hal), _ch1(ch1), _ch2(ch2),
_ch1Min(ch1Min), _ch2Min(ch2Min) {
}
/**
* update
*
* arming:
*
* to arm: put stick to bottom right for 100 controller cycles
* (max yaw, min throttle)
*
* didn't use clock here in case of millis() roll over
* for long runs
*
* disarming:
*
* to disarm: put stick to bottom left for 100 controller cycles
* (min yaw, min throttle)
*/
void update(const float dt);
/**
* update
*
* arming:
*
* to arm: put stick to bottom right for 100 controller cycles
* (max yaw, min throttle)
*
* didn't use clock here in case of millis() roll over
* for long runs
*
* disarming:
*
* to disarm: put stick to bottom left for 100 controller cycles
* (min yaw, min throttle)
*/
void update(const float dt);
private:
AP_HardwareAbstractionLayer * _hal;
uint8_t _armingClock;
uint8_t _ch1; /// typically throttle channel
uint8_t _ch2; /// typically yaw channel
float _ch1Min; /// arms/disarms below this on ch1
float _ch2Min; /// disarms below this on ch2
float _ch2Max; /// arms above this on ch2
AP_HardwareAbstractionLayer * _hal;
uint8_t _armingClock;
uint8_t _ch1; /// typically throttle channel
uint8_t _ch2; /// typically yaw channel
float _ch1Min; /// arms/disarms below this on ch1
float _ch2Min; /// disarms below this on ch2
float _ch2Max; /// arms above this on ch2
};
} // namespace apo
#endif /* AP_ARMINGMECHANISM */
// vim:ts=4:sw=4:expandtab

1
libraries/APO/AP_Guide.h
View File

@ -146,3 +146,4 @@ private:
#endif // AP_Guide_H
// vim:ts=4:sw=4:expandtab