2011-09-28 21:51:12 -03:00
|
|
|
/*
|
|
|
|
* AP_Controller.cpp
|
|
|
|
*
|
|
|
|
* Created on: Apr 30, 2011
|
|
|
|
* Author: jgoppert
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "AP_Controller.h"
|
2011-10-25 19:53:39 -03:00
|
|
|
#include "../FastSerial/FastSerial.h"
|
|
|
|
#include "AP_ArmingMechanism.h"
|
|
|
|
#include "AP_BatteryMonitor.h"
|
2011-10-16 03:55:34 -03:00
|
|
|
#include "AP_HardwareAbstractionLayer.h"
|
|
|
|
#include "../AP_Common/include/menu.h"
|
|
|
|
#include "AP_RcChannel.h"
|
2011-10-25 19:53:39 -03:00
|
|
|
#include "../GCS_MAVLink/include/mavlink_types.h"
|
|
|
|
#include "constants.h"
|
|
|
|
#include "AP_CommLink.h"
|
2011-10-16 03:55:34 -03:00
|
|
|
|
|
|
|
namespace apo {
|
|
|
|
|
|
|
|
AP_Controller::AP_Controller(AP_Navigator * nav, AP_Guide * guide,
|
2011-10-26 13:31:11 -03:00
|
|
|
AP_HardwareAbstractionLayer * hal, AP_ArmingMechanism * armingMechanism,
|
|
|
|
const uint8_t chMode, const uint16_t key, const prog_char_t * name) :
|
|
|
|
_nav(nav), _guide(guide), _hal(hal), _armingMechanism(armingMechanism),
|
|
|
|
_group(key, name ? : PSTR("CNTRL_")),
|
|
|
|
_chMode(chMode),
|
|
|
|
_mode(&_group, 1, MAV_MODE_LOCKED, PSTR("MODE")) {
|
|
|
|
setAllRadioChannelsToNeutral();
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AP_Controller::setAllRadioChannelsToNeutral() {
|
2011-10-26 13:31:11 -03:00
|
|
|
for (uint8_t i = 0; i < _hal->rc.getSize(); i++) {
|
|
|
|
_hal->rc[i]->setPosition(0.0);
|
|
|
|
}
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AP_Controller::setAllRadioChannelsManually() {
|
2011-10-27 17:30:15 -03:00
|
|
|
//_hal->debug->printf_P(PSTR("\tsize: %d\n"),_hal->rc.getSize());
|
2011-10-26 13:31:11 -03:00
|
|
|
for (uint8_t i = 0; i < _hal->rc.getSize(); i++) {
|
|
|
|
_hal->rc[i]->setUsingRadio();
|
2011-10-27 17:30:15 -03:00
|
|
|
//_hal->debug->printf_P(PSTR("\trc %d: %f\n"),i,_hal->rc[i]->getPosition());
|
2011-10-26 13:31:11 -03:00
|
|
|
}
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
2011-10-25 19:53:39 -03:00
|
|
|
void AP_Controller::update(const float dt) {
|
2011-10-26 13:31:11 -03:00
|
|
|
if (_armingMechanism) _armingMechanism->update(dt);
|
2011-10-25 19:53:39 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
// determine flight mode
|
|
|
|
//
|
|
|
|
// check for heartbeat from gcs, if not found go to failsafe
|
|
|
|
if (_hal->heartBeatLost()) {
|
2011-11-19 22:39:14 -04:00
|
|
|
setMode(MAV_MODE_FAILSAFE);
|
2011-10-26 13:31:11 -03:00
|
|
|
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat"));
|
|
|
|
// if battery less than 5%, go to failsafe
|
|
|
|
} else if (_hal->batteryMonitor && _hal->batteryMonitor->getPercentage() < 5) {
|
2011-11-19 22:39:14 -04:00
|
|
|
setMode(MAV_MODE_FAILSAFE);
|
2011-10-26 13:31:11 -03:00
|
|
|
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery"));
|
|
|
|
// manual/auto switch
|
|
|
|
} else {
|
|
|
|
// if all emergencies cleared, fall back to standby
|
|
|
|
if (_hal->getState()==MAV_STATE_EMERGENCY) _hal->setState(MAV_STATE_STANDBY);
|
2011-11-19 22:39:14 -04:00
|
|
|
|
|
|
|
// if in auto mode and manual switch set, change to manual
|
|
|
|
if (getMode()==MAV_MODE_AUTO && (_hal->rc[_chMode]->getRadioPosition() > 0)) setMode(MAV_MODE_MANUAL);
|
|
|
|
|
|
|
|
// if in manual mode and auto switch set, switch to auto
|
|
|
|
if (getMode()==MAV_MODE_MANUAL && (_hal->rc[_chMode]->getRadioPosition() < 0)) setMode(MAV_MODE_AUTO);
|
2011-10-26 13:31:11 -03:00
|
|
|
}
|
2011-10-25 19:53:39 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
// handle flight modes
|
|
|
|
switch(_mode) {
|
2011-10-25 19:53:39 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
case MAV_MODE_LOCKED: {
|
|
|
|
_hal->setState(MAV_STATE_STANDBY);
|
|
|
|
break;
|
|
|
|
}
|
2011-10-25 19:53:39 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
case MAV_MODE_FAILSAFE: {
|
|
|
|
_hal->setState(MAV_STATE_EMERGENCY);
|
|
|
|
break;
|
|
|
|
}
|
2011-10-25 19:53:39 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
case MAV_MODE_MANUAL: {
|
|
|
|
manualLoop(dt);
|
|
|
|
break;
|
|
|
|
}
|
2011-10-25 19:53:39 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
case MAV_MODE_AUTO: {
|
|
|
|
autoLoop(dt);
|
|
|
|
break;
|
|
|
|
}
|
2011-10-25 19:53:39 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
default: {
|
|
|
|
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode"));
|
|
|
|
_hal->setState(MAV_STATE_EMERGENCY);
|
|
|
|
}
|
|
|
|
}
|
2011-10-25 19:53:39 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
// this sends commands to motors
|
|
|
|
setMotors();
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
2011-10-25 19:53:39 -03:00
|
|
|
|
2011-10-26 14:25:06 -03:00
|
|
|
void AP_Controller::setMotors() {
|
|
|
|
switch (_hal->getState()) {
|
|
|
|
|
|
|
|
case MAV_STATE_ACTIVE: {
|
|
|
|
digitalWrite(_hal->aLedPin, HIGH);
|
|
|
|
setMotorsActive();
|
2011-10-27 17:30:15 -03:00
|
|
|
break;
|
2011-10-26 14:25:06 -03:00
|
|
|
}
|
|
|
|
case MAV_STATE_EMERGENCY: {
|
|
|
|
digitalWrite(_hal->aLedPin, LOW);
|
|
|
|
setMotorsEmergency();
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case MAV_STATE_STANDBY: {
|
|
|
|
digitalWrite(_hal->aLedPin,LOW);
|
|
|
|
setMotorsStandby();
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
default: {
|
|
|
|
setMotorsStandby();
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2011-10-25 19:53:39 -03:00
|
|
|
} // namespace apo
|
2011-10-26 15:59:40 -03:00
|
|
|
// vim:ts=4:sw=4:expandtab
|