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

Added arming mechanism.

This commit is contained in:
James Goppert 2011-10-19 00:21:19 -04:00
parent f89339ea79
commit dcc2c2c0cf
3 changed files with 124 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
apo/ControllerQuad.h
View File

@ -10,6 +10,7 @@
#include "../APO/AP_Controller.h"
#include "../APO/AP_BatteryMonitor.h"
#include "../APO/AP_ArmingMechanism.h"
namespace apo {
@ -78,7 +79,7 @@ public:
PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_DFCUT),
pidPD(new AP_Var_group(k_pidPD, PSTR("DOWN_")), 1, PID_POS_Z_P,
PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM, PID_POS_DFCUT),
_armingClock(0), _thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0),
_armingMechanism(hal,CH_THRUST,CH_YAW,0.1,-0.9,0.9), _thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0),
_cmdRoll(0), _cmdPitch(0), _cmdYawRate(0), _mode(MAV_MODE_LOCKED) {
/*
* allocate radio channels
@ -117,51 +118,7 @@ public:
virtual void update(const float & dt) {
//_hal->debug->printf_P(PSTR("thr: %f, yaw: %f\n"),_hal->rc[CH_THRUST]->getRadioPosition(),_hal->rc[CH_YAW]->getRadioPosition());
// 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
if ( (_hal->getState() != MAV_STATE_ACTIVE) &
(_hal->rc[CH_THRUST]->getRadioPosition() < 0.1) &&
(_hal->rc[CH_YAW]->getRadioPosition() < -0.9) ) {
// 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
//
// to disarm: put stick to bottom left for 100 controller cycles
// (min yaw, min throttle)
else if ( (_hal->getState() == MAV_STATE_ACTIVE) &
(_hal->rc[CH_THRUST]->getRadioPosition() < 0.1) &&
(_hal->rc[CH_YAW]->getRadioPosition() > 0.9) ) {
// 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"));
}
_armingMechanism.update(dt);
// determine flight mode
//
@ -232,7 +189,7 @@ private:
BlockPIDDfb pidRoll, pidPitch, pidYaw;
BlockPID pidYawRate;
BlockPIDDfb pidPN, pidPE, pidPD;
int32_t _armingClock;
AP_ArmingMechanism _armingMechanism;
float _thrustMix, _pitchMix, _rollMix, _yawMix;
float _cmdRoll, _cmdPitch, _cmdYawRate;

55
libraries/APO/AP_ArmingMechanism.cpp Normal file
View File

@ -0,0 +1,55 @@
/*
* AP_ArmingMechanism.cpp
*
*/
#include "AP_ArmingMechanism.h"
#include "AP_RcChannel.h"
#include "../FastSerial/FastSerial.h"
#include "AP_HardwareAbstractionLayer.h"
#include "AP_CommLink.h"
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) ) {
// 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) ) {
// 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"));
}
}
} // apo

65
libraries/APO/AP_ArmingMechanism.h Normal file
View File

@ -0,0 +1,65 @@
/*
* AP_ArmingMechanism.h
*
*/
#ifndef AP_ARMINGMECHANISM_H_
#define AP_ARMINGMECHANISM_H_
#include <inttypes.h>
#include <wiring.h>
namespace apo {
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) {
}
/**
* 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
};
} // namespace apo
#endif /* AP_ARMINGMECHANISM */