From 0283b8c695c56b1a34e132c4d5435cdaf986e50b Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Oct 2011 02:55:34 -0400 Subject: [PATCH 1/3] Added battery monitoring, arming to apo. Arming added for quadrotor. Need to add to rover still. Battery monitoring added with auto shut-off in quadrotor controller. Finally split apo header and source files to allow faster compiling/ fix cyclic header inclusions. --- apo/ControllerQuad.h | 304 +++++---- apo/QuadArducopter.h | 27 +- libraries/APO/APO_DefaultSetup.h | 6 +- libraries/APO/AP_Autopilot.cpp | 80 ++- libraries/APO/AP_Autopilot.h | 53 +- libraries/APO/AP_BatteryMonitor.cpp | 10 + libraries/APO/AP_BatteryMonitor.h | 51 ++ libraries/APO/AP_CommLink.cpp | 706 ++++++++++++++++++++ libraries/APO/AP_CommLink.h | 696 +------------------ libraries/APO/AP_Controller.cpp | 25 + libraries/APO/AP_Controller.h | 48 +- libraries/APO/AP_Guide.cpp | 229 +++++++ libraries/APO/AP_Guide.h | 231 +------ libraries/APO/AP_HardwareAbstractionLayer.h | 19 +- libraries/APO/AP_Navigator.cpp | 186 ++++++ libraries/APO/AP_Navigator.h | 199 +----- libraries/AP_Common/AP_Loop.h | 4 +- 17 files changed, 1554 insertions(+), 1320 deletions(-) create mode 100644 libraries/APO/AP_BatteryMonitor.cpp create mode 100644 libraries/APO/AP_BatteryMonitor.h diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index e8f3675817..847f5617fd 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -9,6 +9,7 @@ #define CONTROLLERQUAD_H_ #include "../APO/AP_Controller.h" +#include "../APO/AP_BatteryMonitor.h" namespace apo { @@ -61,22 +62,24 @@ public: AP_Controller(nav, guide, hal), pidRoll(new AP_Var_group(k_pidRoll, PSTR("ROLL_")), 1, PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU, - PID_ATT_LIM), + PID_ATT_LIM, PID_ATT_DFCUT), pidPitch(new AP_Var_group(k_pidPitch, PSTR("PITCH_")), 1, PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU, - PID_ATT_LIM), + PID_ATT_LIM, PID_ATT_DFCUT), pidYaw(new AP_Var_group(k_pidYaw, PSTR("YAW_")), 1, PID_YAWPOS_P, PID_YAWPOS_I, PID_YAWPOS_D, - PID_YAWPOS_AWU, PID_YAWPOS_LIM), + PID_YAWPOS_AWU, PID_YAWPOS_LIM, PID_ATT_DFCUT), pidYawRate(new AP_Var_group(k_pidYawRate, PSTR("YAWRT_")), 1, PID_YAWSPEED_P, PID_YAWSPEED_I, PID_YAWSPEED_D, PID_YAWSPEED_AWU, PID_YAWSPEED_LIM, PID_YAWSPEED_DFCUT), pidPN(new AP_Var_group(k_pidPN, PSTR("NORTH_")), 1, PID_POS_P, - PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM), + PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_DFCUT), pidPE(new AP_Var_group(k_pidPE, PSTR("EAST_")), 1, PID_POS_P, - PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM), + 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_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), + _cmdRoll(0), _cmdPitch(0), _cmdYawRate(0), _mode(MAV_MODE_LOCKED) { /* * allocate radio channels * the order of the channels has to match the enumeration above @@ -112,138 +115,203 @@ 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()); - // check for heartbeat + // 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")); + } + + // determine flight mode + // + // check for heartbeat from gcs, if not found go to failsafe if (_hal->heartBeatLost()) { _mode = MAV_MODE_FAILSAFE; - setAllRadioChannelsToNeutral(); - _hal->setState(MAV_STATE_EMERGENCY); - _hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n")); - return; - // if throttle less than 5% cut motor power - } else if (_hal->rc[CH_THRUST]->getRadioPosition() < 0.05) { - _mode = MAV_MODE_LOCKED; - setAllRadioChannelsToNeutral(); - _hal->setState(MAV_STATE_STANDBY); - return; - // if in live mode then set state to active - } else if (_hal->getMode() == MODE_LIVE) { - _hal->setState(MAV_STATE_ACTIVE); - // if in hardware in the loop (control) mode, set to hilsim - } else if (_hal->getMode() == MODE_HIL_CNTL) { - _hal->setState(MAV_STATE_HILSIM); - } - - // manual mode - if (_hal->rc[CH_MODE]->getRadioPosition() > 0) { - _mode = MAV_MODE_MANUAL; + _hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat")); + // if battery less than 5%, go to failsafe + } else if (_hal->batteryMonitor->getPercentage() < 5) { + _mode = MAV_MODE_FAILSAFE; + _hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery")); + // manual/auto switch } else { - _mode = MAV_MODE_AUTO; + // if all emergencies cleared, fall back to standby + if (_hal->getState()==MAV_STATE_EMERGENCY) _hal->setState(MAV_STATE_STANDBY); + if (_hal->rc[CH_MODE]->getRadioPosition() > 0) _mode = MAV_MODE_MANUAL; + else _mode = MAV_MODE_AUTO; } - // commands for inner loop - float cmdRoll = 0; - float cmdPitch = 0; - float cmdYawRate = 0; - float thrustMix = 0; - + // handle flight modes switch(_mode) { - case MAV_MODE_MANUAL: { - setAllRadioChannelsManually(); - // "mix manual" - cmdRoll = -0.5 * _hal->rc[CH_ROLL]->getPosition(); - cmdPitch = -0.5 * _hal->rc[CH_PITCH]->getPosition(); - cmdYawRate = -1 * _hal->rc[CH_YAW]->getPosition(); - thrustMix = _hal->rc[CH_THRUST]->getPosition(); - break; + case MAV_MODE_LOCKED: { + _hal->setState(MAV_STATE_STANDBY); + break; + } + + case MAV_MODE_FAILSAFE: { + _hal->setState(MAV_STATE_EMERGENCY); + break; + } + + case MAV_MODE_MANUAL: { + manualPositionLoop(); + autoAttitudeLoop(dt); + break; + } + + case MAV_MODE_AUTO: { + // until position loop is tested just + // go to standby + _hal->setState(MAV_STATE_STANDBY); + + //attitudeLoop(); + // XXX autoPositionLoop NOT TESTED, don't uncomment yet + //autoPositionLoop(dt); + //autoAttitudeLoop(dt); + break; + } + + default: { + _hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode")); + _hal->setState(MAV_STATE_EMERGENCY); + } } - case MAV_MODE_AUTO: { - - // XXX kills all commands, - // auto not currently implemented - setAllRadioChannelsToNeutral(); - - // position loop - /* - float cmdNorthTilt = pidPN.update(_nav->getPN(),_nav->getVN(),dt); - float cmdEastTilt = pidPE.update(_nav->getPE(),_nav->getVE(),dt); - float cmdDown = pidPD.update(_nav->getPD(),_nav->getVD(),dt); - - // "transform-to-body" - { - float trigSin = sin(-yaw); - float trigCos = cos(-yaw); - _cmdPitch = _cmdEastTilt * trigCos - - _cmdNorthTilt * trigSin; - _cmdRoll = -_cmdEastTilt * trigSin - + _cmdNorthTilt * trigCos; - // note that the north tilt is negative of the pitch - } - - //thrustMix += THRUST_HOVER_OFFSET; - - // "thrust-trim-adjust" - if (fabs(_cmdRoll) > 0.5) { - _thrustMix *= 1.13949393; - } else { - _thrustMix /= cos(_cmdRoll); - } - if (fabs(_cmdPitch) > 0.5) { - _thrustMix *= 1.13949393; - } else { - _thrustMix /= cos(_cmdPitch); - } - */ - } - - } - - // attitude loop - float rollMix = pidRoll.update(cmdRoll - _nav->getRoll(), - _nav->getRollRate(), dt); - float pitchMix = pidPitch.update(cmdPitch - _nav->getPitch(), - _nav->getPitchRate(), dt); - float yawMix = pidYawRate.update(cmdYawRate - _nav->getYawRate(), dt); - - _hal->rc[CH_RIGHT]->setPosition(thrustMix - rollMix + yawMix); - _hal->rc[CH_LEFT]->setPosition(thrustMix + rollMix + yawMix); - _hal->rc[CH_FRONT]->setPosition(thrustMix + pitchMix - yawMix); - _hal->rc[CH_BACK]->setPosition(thrustMix - pitchMix - yawMix); - - //_hal->debug->printf("R: %f\t L: %f\t F: %f\t B: %f\n", - //_hal->rc[CH_RIGHT]->getPosition(), - //_hal->rc[CH_LEFT]->getPosition(), - //_hal->rc[CH_FRONT]->getPosition(), - //_hal->rc[CH_BACK]->getPosition()); - - //_hal->debug->printf( - // "rollMix: %f\t pitchMix: %f\t yawMix: %f\t thrustMix: %f\n", - // rollMix, pitchMix, yawMix, thrustMix); - - //_hal->debug->printf("cmdRoll: %f\t roll: %f\t rollMix: %f\n", - // cmdRoll, _nav->getRoll(), rollMix); - //_hal->debug->printf("cmdPitch: %f\t pitch: %f\t pitchMix: %f\n", - // cmdPitch, _nav->getPitch(), pitchMix); - //_hal->debug->printf("cmdYawRate: %f\t yawRate: %f\t yawMix: %f\n", - // cmdYawRate, _nav->getYawRate(), yawMix); - - //_hal->debug->printf("roll pwm: %d\t pitch pwm: %d\t yaw pwm: %d\t thrust pwm: %d\n", - //_hal->rc[CH_ROLL]->getRadioPwm(), - //_hal->rc[CH_PITCH]->getRadioPwm(), - //_hal->rc[CH_YAW]->getRadioPwm(), - //_hal->rc[CH_THRUST]->getRadioPwm()); + // this sends commands to motors + setMotors(); } + virtual MAV_MODE getMode() { return (MAV_MODE) _mode.get(); } + private: + AP_Uint8 _mode; BlockPIDDfb pidRoll, pidPitch, pidYaw; BlockPID pidYawRate; BlockPIDDfb pidPN, pidPE, pidPD; + int32_t _armingClock; + float _thrustMix, _pitchMix, _rollMix, _yawMix; + float _cmdRoll, _cmdPitch, _cmdYawRate; + + void manualPositionLoop() { + setAllRadioChannelsManually(); + _cmdRoll = -0.5 * _hal->rc[CH_ROLL]->getPosition(); + _cmdPitch = -0.5 * _hal->rc[CH_PITCH]->getPosition(); + _cmdYawRate = -1 * _hal->rc[CH_YAW]->getPosition(); + _thrustMix = _hal->rc[CH_THRUST]->getPosition(); + } + + void autoPositionLoop(float dt) { + float cmdNorthTilt = pidPN.update(_nav->getPN(),_nav->getVN(),dt); + float cmdEastTilt = pidPE.update(_nav->getPE(),_nav->getVE(),dt); + float cmdDown = pidPD.update(_nav->getPD(),_nav->getVD(),dt); + + // "transform-to-body" + { + float trigSin = sin(-_nav->getYaw()); + float trigCos = cos(-_nav->getYaw()); + _cmdPitch = cmdEastTilt * trigCos - cmdNorthTilt * trigSin; + _cmdRoll = -cmdEastTilt * trigSin + cmdNorthTilt * trigCos; + // note that the north tilt is negative of the pitch + } + _cmdYawRate = 0; + + _thrustMix = THRUST_HOVER_OFFSET + cmdDown; + + // "thrust-trim-adjust" + if (fabs(_cmdRoll) > 0.5) _thrustMix *= 1.13949393; + else _thrustMix /= cos(_cmdRoll); + + if (fabs(_cmdPitch) > 0.5) _thrustMix *= 1.13949393; + else _thrustMix /= cos(_cmdPitch); + } + + void autoAttitudeLoop(float dt) { + _rollMix = pidRoll.update(_cmdRoll - _nav->getRoll(), + _nav->getRollRate(), dt); + _pitchMix = pidPitch.update(_cmdPitch - _nav->getPitch(), + _nav->getPitchRate(), dt); + _yawMix = pidYawRate.update(_cmdYawRate - _nav->getYawRate(), dt); + } + + void setMotors() { + + switch (_hal->getState()) { + + case MAV_STATE_ACTIVE: { + digitalWrite(_hal->aLedPin, HIGH); + // turn all motors off if below 0.1 throttle + if (_hal->rc[CH_THRUST]->getRadioPosition() < 0.1) { + setAllRadioChannelsToNeutral(); + } else { + _hal->rc[CH_RIGHT]->setPosition(_thrustMix - _rollMix + _yawMix); + _hal->rc[CH_LEFT]->setPosition(_thrustMix + _rollMix + _yawMix); + _hal->rc[CH_FRONT]->setPosition(_thrustMix + _pitchMix - _yawMix); + _hal->rc[CH_BACK]->setPosition(_thrustMix - _pitchMix - _yawMix); + } + break; + } + case MAV_STATE_EMERGENCY: { + digitalWrite(_hal->aLedPin, LOW); + setAllRadioChannelsToNeutral(); + break; + } + case MAV_STATE_STANDBY: { + digitalWrite(_hal->aLedPin,LOW); + setAllRadioChannelsToNeutral(); + break; + } + default: { + digitalWrite(_hal->aLedPin, LOW); + setAllRadioChannelsToNeutral(); + } + + } + } }; } // namespace apo diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index 4c733e5013..32f402035d 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -39,6 +39,13 @@ static const bool compassEnabled = true; static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; // compass orientation: See AP_Compass_HMC5843.h for possible values +// battery monitoring +static const bool batteryMonitorEnabled = true; +static const uint8_t batteryPin = 0; +static const float batteryVoltageDivRatio = 6; +static const float batteryMinVolt = 10.0; +static const float batteryMaxVolt = 12.4; + static const bool rangeFinderFrontEnabled = false; static const bool rangeFinderBackEnabled = false; static const bool rangeFinderLeftEnabled = false; @@ -47,11 +54,11 @@ static const bool rangeFinderUpEnabled = false; static const bool rangeFinderDownEnabled = false; // loop rates -static const float loop0Rate = 200; // attitude nav -static const float loop1Rate = 50; // controller -static const float loop2Rate = 10; // pos nav/ gcs fast -static const float loop3Rate = 1; // gcs slow -static const float loop4Rate = 0.1; +static const float loopRate = 150; // attitude nav +static const float loop0Rate = 50; // controller +static const float loop1Rate = 5; // pos nav/ gcs fast +static const float loop2Rate = 1; // gcs slow +static const float loop3Rate = 0.1; // position control loop static const float PID_POS_P = 0; @@ -64,13 +71,15 @@ static const float PID_POS_Z_I = 0; static const float PID_POS_Z_D = 0; static const float PID_POS_Z_LIM = 0; static const float PID_POS_Z_AWU = 0; +static const float PID_POS_DFCUT = 10; // cut derivative feedback at 10 hz // attitude control loop -static const float PID_ATT_P = 0.3; +static const float PID_ATT_P = 0.17; static const float PID_ATT_I = 0.5; -static const float PID_ATT_D = 0.08; -static const float PID_ATT_LIM = 0.1; // 10 % -static const float PID_ATT_AWU = 0.03; // 3 % +static const float PID_ATT_D = 0.06; +static const float PID_ATT_LIM = 0.05; // 10 % +static const float PID_ATT_AWU = 0.005; // 0.5 % +static const float PID_ATT_DFCUT = 25; // cut derivative feedback at 25 hz static const float PID_YAWPOS_P = 0; static const float PID_YAWPOS_I = 0; static const float PID_YAWPOS_D = 0; diff --git a/libraries/APO/APO_DefaultSetup.h b/libraries/APO/APO_DefaultSetup.h index 36eaefc552..cbdd2c6688 100644 --- a/libraries/APO/APO_DefaultSetup.h +++ b/libraries/APO/APO_DefaultSetup.h @@ -46,6 +46,10 @@ void setup() { hal->adc = new ADC_CLASS; hal->adc->Init(); + if (batteryMonitorEnabled) { + hal->batteryMonitor = new AP_BatteryMonitor(batteryPin,batteryVoltageDivRatio,batteryMinVolt,batteryMaxVolt); + } + if (gpsEnabled) { Serial1.begin(gpsBaud, 128, 16); // gps hal->debug->println_P(PSTR("initializing gps")); @@ -167,7 +171,7 @@ void setup() { hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal, - loop0Rate, loop1Rate, loop2Rate, loop3Rate); + loopRate, loop0Rate, loop1Rate, loop2Rate, loop3Rate); } void loop() { diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp index 411edc0c3c..61a52e0ce7 100644 --- a/libraries/APO/AP_Autopilot.cpp +++ b/libraries/APO/AP_Autopilot.cpp @@ -6,6 +6,7 @@ */ #include "AP_Autopilot.h" +#include "AP_BatteryMonitor.h" namespace apo { @@ -13,16 +14,21 @@ class AP_HardwareAbstractionLayer; AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, AP_Controller * controller, AP_HardwareAbstractionLayer * hal, - float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate) : - Loop(loop0Rate, callback0, this), _navigator(navigator), _guide(guide), - _controller(controller), _hal(hal), _loop0Rate(loop0Rate), - _loop1Rate(loop1Rate), _loop2Rate(loop2Rate), _loop3Rate(loop3Rate), - _loop4Rate(loop3Rate), callback0Calls(0), clockInit(millis()) { + float loopRate, float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate) : + Loop(loopRate, callback, this), _navigator(navigator), _guide(guide), + _controller(controller), _hal(hal), + callbackCalls(0) { hal->setState(MAV_STATE_BOOT); hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + /* + * Radio setup + */ + hal->debug->println_P(PSTR("initializing radio")); + APM_RC.Init(); // APM Radio initialization, + /* * Calibration */ @@ -69,7 +75,7 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, } hal->debug->println_P(PSTR("waiting for hil packet")); } - delay(1000); + delay(500); } AP_MavlinkCommand::home.setAlt(_navigator->getAlt()); @@ -91,46 +97,31 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, * Attach loops */ hal->debug->println_P(PSTR("attaching loops")); - subLoops().push_back(new Loop(getLoopRate(1), callback1, this)); - subLoops().push_back(new Loop(getLoopRate(2), callback2, this)); - subLoops().push_back(new Loop(getLoopRate(3), callback3, this)); - subLoops().push_back(new Loop(getLoopRate(4), callback4, this)); + subLoops().push_back(new Loop(loop0Rate, callback0, this)); + subLoops().push_back(new Loop(loop1Rate, callback1, this)); + subLoops().push_back(new Loop(loop2Rate, callback2, this)); + subLoops().push_back(new Loop(loop3Rate, callback3, this)); hal->debug->println_P(PSTR("running")); hal->gcs->sendText(SEVERITY_LOW, PSTR("running")); - - if (hal->getMode() == MODE_LIVE) { - hal->setState(MAV_STATE_ACTIVE); - } else { - hal->setState(MAV_STATE_HILSIM); - } - - /* - * Radio setup - */ - hal->debug->println_P(PSTR("initializing radio")); - APM_RC.Init(); // APM Radio initialization, - // start this after control loop is running - - clockInit = millis(); + hal->setState(MAV_STATE_STANDBY); } -void AP_Autopilot::callback0(void * data) { +void AP_Autopilot::callback(void * data) { AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->hal()->debug->println_P(PSTR("callback 0")); + //apo->hal()->debug->println_P(PSTR("callback")); /* * ahrs update */ - - apo->callback0Calls++; + apo->callbackCalls++; if (apo->getNavigator()) - apo->getNavigator()->updateFast(1.0 / apo->getLoopRate(0)); + apo->getNavigator()->updateFast(apo->dt()); } -void AP_Autopilot::callback1(void * data) { +void AP_Autopilot::callback0(void * data) { AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getHal()->debug->println_P(PSTR("callback 1")); + //apo->getHal()->debug->println_P(PSTR("callback 0")); /* * hardware in the loop @@ -145,7 +136,7 @@ void AP_Autopilot::callback1(void * data) { */ if (apo->getController()) { //apo->getHal()->debug->println_P(PSTR("updating controller")); - apo->getController()->update(1. / apo->getLoopRate(1)); + apo->getController()->update(apo->subLoops()[0]->dt()); } /* char msg[50]; @@ -154,9 +145,9 @@ void AP_Autopilot::callback1(void * data) { */ } -void AP_Autopilot::callback2(void * data) { +void AP_Autopilot::callback1(void * data) { AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getHal()->debug->println_P(PSTR("callback 2")); + //apo->getHal()->debug->println_P(PSTR("callback 1")); /* * update guidance laws @@ -171,7 +162,7 @@ void AP_Autopilot::callback2(void * data) { * slow navigation loop update */ if (apo->getNavigator()) { - apo->getNavigator()->updateSlow(1.0 / apo->getLoopRate(2)); + apo->getNavigator()->updateSlow(apo->subLoops()[1]->dt()); } /* @@ -210,9 +201,9 @@ void AP_Autopilot::callback2(void * data) { */ } -void AP_Autopilot::callback3(void * data) { +void AP_Autopilot::callback2(void * data) { AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getHal()->debug->println_P(PSTR("callback 3")); + //apo->getHal()->debug->println_P(PSTR("callback 2")); /* * send telemetry @@ -226,6 +217,11 @@ void AP_Autopilot::callback3(void * data) { //apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU); } + /* + * update battery monitor + */ + if (apo->getHal()->batteryMonitor) apo->getHal()->batteryMonitor->update(); + /* * send heartbeat */ @@ -235,10 +231,10 @@ void AP_Autopilot::callback3(void * data) { * load/loop rate/ram debug */ apo->getHal()->load = apo->load(); - apo->getHal()->debug->printf_P(PSTR("missed calls: %d\n"),uint16_t(millis()*apo->getLoopRate(0)/1000-apo->callback0Calls)); + apo->getHal()->debug->printf_P(PSTR("callback calls: %d\n"),apo->callbackCalls); + apo->callbackCalls = 0; apo->getHal()->debug->printf_P(PSTR("load: %d%%\trate: %f Hz\tfree ram: %d bytes\n"), apo->load(),1.0/apo->dt(),freeMemory()); - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); /* @@ -250,9 +246,9 @@ void AP_Autopilot::callback3(void * data) { //apo->adc()->Ch(6), apo->adc()->Ch(7), apo->adc()->Ch(8)); } -void AP_Autopilot::callback4(void * data) { +void AP_Autopilot::callback3(void * data) { //AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getHal()->debug->println_P(PSTR("callback 4")); + //apo->getHal()->debug->println_P(PSTR("callback 3")); } } // apo diff --git a/libraries/APO/AP_Autopilot.h b/libraries/APO/AP_Autopilot.h index 97c53cd470..e6684fe07f 100644 --- a/libraries/APO/AP_Autopilot.h +++ b/libraries/APO/AP_Autopilot.h @@ -70,7 +70,7 @@ public: */ AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, AP_Controller * controller, AP_HardwareAbstractionLayer * hal, - float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate); + float loopRate, float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate); /** * Accessors @@ -88,68 +88,51 @@ public: return _hal; } - float getLoopRate(uint8_t i) { - switch(i) { - case 0: return _loop0Rate; - case 1: return _loop1Rate; - case 2: return _loop2Rate; - case 3: return _loop3Rate; - case 4: return _loop4Rate; - default: return 0; - } - } - /** * Loop Monitoring */ - uint32_t callback0Calls; - uint32_t clockInit; + uint32_t callbackCalls; private: /** - * Loop 0 Callbacks (fastest) + * Loop Callbacks (fastest) * - inertial navigation * @param data A void pointer used to pass the apo class * so that the apo public interface may be accessed. */ + static void callback(void * data); + + /** + * Loop 0 Callbacks + * - control + * - compass reading + * @see callback + */ static void callback0(void * data); - float _loop0Rate; /** * Loop 1 Callbacks - * - control - * - compass reading - * @see callback0 + * - gps sensor fusion + * - compass sensor fusion + * @see callback */ static void callback1(void * data); - float _loop1Rate; /** * Loop 2 Callbacks - * - gps sensor fusion - * - compass sensor fusion - * @see callback0 + * - slow messages + * @see callback */ static void callback2(void * data); - float _loop2Rate; /** * Loop 3 Callbacks - * - slow messages - * @see callback0 - */ - static void callback3(void * data); - float _loop3Rate; - - /** - * Loop 4 Callbacks * - super slow messages * - log writing - * @see callback0 + * @see callback */ - static void callback4(void * data); - float _loop4Rate; + static void callback3(void * data); /** * Components diff --git a/libraries/APO/AP_BatteryMonitor.cpp b/libraries/APO/AP_BatteryMonitor.cpp new file mode 100644 index 0000000000..e5550d2798 --- /dev/null +++ b/libraries/APO/AP_BatteryMonitor.cpp @@ -0,0 +1,10 @@ +/* + * AP_BatteryMonitor.cpp + * + */ + +#include "AP_BatteryMonitor.h" + +namespace apo { + +} // apo diff --git a/libraries/APO/AP_BatteryMonitor.h b/libraries/APO/AP_BatteryMonitor.h new file mode 100644 index 0000000000..7f82db349d --- /dev/null +++ b/libraries/APO/AP_BatteryMonitor.h @@ -0,0 +1,51 @@ +/* + * AP_BatteryMonitor.h + * + */ + +#ifndef AP_BATTERYMONITOR_H_ +#define AP_BATTERYMONITOR_H_ + +#include +#include + +namespace apo { + +class AP_BatteryMonitor { +public: + AP_BatteryMonitor(uint8_t pin, float voltageDivRatio, float minVolt, float maxVolt) : + _pin(pin), _voltageDivRatio(voltageDivRatio), + _minVolt(minVolt), _maxVolt(maxVolt), _voltage(maxVolt) { + } + + void update() { + // low pass filter on voltage + _voltage = _voltage*.9 + (analogRead(_pin)/255)*_voltageDivRatio*0.1; + } + + /** + * Accessors + */ + float getVoltage() { + return _voltage; + } + + uint8_t getPercentage() { + float norm = (_voltage-_minVolt)/(_maxVolt-_minVolt); + if (norm < 0) norm = 0; + else if (norm > 1) norm = 1; + return 100*norm; + } + +private: + + uint8_t _pin; + float _voltageDivRatio; + float _voltage; + float _minVolt; + float _maxVolt; +}; + +} // namespace apo + +#endif /* AP_BATTERYMONITOR_H_ */ diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp index 92b9d0fdde..cad558ccbd 100644 --- a/libraries/APO/AP_CommLink.cpp +++ b/libraries/APO/AP_CommLink.cpp @@ -5,11 +5,717 @@ * Author: jgoppert */ +#include "../FastSerial/FastSerial.h" #include "AP_CommLink.h" +#include "AP_Navigator.h" +#include "AP_Guide.h" +#include "AP_Controller.h" +#include "AP_HardwareAbstractionLayer.h" +#include "AP_RcChannel.h" +#include "../AP_GPS/AP_GPS.h" +#include "../AP_Math/AP_Math.h" +#include "../AP_IMU/AP_IMU.h" +#include "../AP_Compass/AP_Compass.h" +#include "AP_BatteryMonitor.h" namespace apo { uint8_t MavlinkComm::_nChannels = 0; uint8_t MavlinkComm::_paramNameLengthMax = 13; +AP_CommLink::AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide, + AP_Controller * controller, AP_HardwareAbstractionLayer * hal) : + _link(link), _navigator(navigator), _guide(guide), + _controller(controller), _hal(hal) { +} + +MavlinkComm::MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide, + AP_Controller * controller, AP_HardwareAbstractionLayer * hal) : + AP_CommLink(link, nav, guide, controller, hal), + + // options + _useRelativeAlt(true), + + // commands + _sendingCmds(false), _receivingCmds(false), + _cmdTimeLastSent(millis()), _cmdTimeLastReceived(millis()), + _cmdDestSysId(0), _cmdDestCompId(0), _cmdRequestIndex(0), + _cmdMax(30), _cmdNumberRequested(0), + + // parameters + _parameterCount(0), _queuedParameter(NULL), + _queuedParameterIndex(0) { + + switch (_nChannels) { + case 0: + mavlink_comm_0_port = link; + _channel = MAVLINK_COMM_0; + _nChannels++; + break; + case 1: + mavlink_comm_1_port = link; + _channel = MAVLINK_COMM_1; + _nChannels++; + break; + default: + // signal that number of channels exceeded + _channel = MAVLINK_COMM_3; + break; + } +} + +void MavlinkComm::send() { + // if number of channels exceeded return + if (_channel == MAVLINK_COMM_3) + return; +} + +void MavlinkComm::sendMessage(uint8_t id, uint32_t param) { + //_hal->debug->printf_P(PSTR("send message\n")); + + // if number of channels exceeded return + if (_channel == MAVLINK_COMM_3) + return; + + uint64_t timeStamp = micros(); + + switch (id) { + + case MAVLINK_MSG_ID_HEARTBEAT: { + mavlink_msg_heartbeat_send(_channel, mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); + break; + } + + case MAVLINK_MSG_ID_ATTITUDE: { + mavlink_msg_attitude_send(_channel, timeStamp, + _navigator->getRoll(), _navigator->getPitch(), + _navigator->getYaw(), _navigator->getRollRate(), + _navigator->getPitchRate(), _navigator->getYawRate()); + break; + } + + case MAVLINK_MSG_ID_GLOBAL_POSITION: { + mavlink_msg_global_position_send(_channel, timeStamp, + _navigator->getLat() * rad2Deg, + _navigator->getLon() * rad2Deg, _navigator->getAlt(), + _navigator->getVN(), _navigator->getVE(), + _navigator->getVD()); + break; + } + + case MAVLINK_MSG_ID_GPS_RAW: { + mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(), + _navigator->getLat() * rad2Deg, + _navigator->getLon() * rad2Deg, _navigator->getAlt(), 0, 0, + _navigator->getGroundSpeed(), + _navigator->getYaw() * rad2Deg); + break; + } + + /* + case MAVLINK_MSG_ID_GPS_RAW_INT: { + mavlink_msg_gps_raw_int_send(_channel,timeStamp,_hal->gps->status(), + _navigator->getLat_degInt(), _navigator->getLon_degInt(),_navigator->getAlt_intM(), 0,0, + _navigator->getGroundSpeed(),_navigator->getYaw()*rad2Deg); + break; + } + */ + + case MAVLINK_MSG_ID_SCALED_IMU: { + /* + * accel/gyro debug + */ + /* + Vector3f accel = _hal->imu->get_accel(); + Vector3f gyro = _hal->imu->get_gyro(); + Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"), + accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z); + */ + Vector3f accel = _hal->imu->get_accel(); + Vector3f gyro = _hal->imu->get_gyro(); + mavlink_msg_raw_imu_send(_channel, timeStamp, 1000 * accel.x, + 1000 * accel.y, 1000 * accel.z, 1000 * gyro.x, + 1000 * gyro.y, 1000 * gyro.z, _hal->compass->mag_x, + _hal->compass->mag_y, _hal->compass->mag_z); // XXX THIS IS NOT SCALED FOR MAG + } + + case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: { + int16_t ch[8]; + for (int i = 0; i < 8; i++) + ch[i] = 0; + for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) { + ch[i] = 10000 * _hal->rc[i]->getPosition(); + //_hal->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]); + } + mavlink_msg_rc_channels_scaled_send(_channel, ch[0], ch[1], ch[2], + ch[3], ch[4], ch[5], ch[6], ch[7], 255); + break; + } + + case MAVLINK_MSG_ID_RC_CHANNELS_RAW: { + int16_t ch[8]; + for (int i = 0; i < 8; i++) + ch[i] = 0; + for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) + ch[i] = _hal->rc[i]->getPwm(); + mavlink_msg_rc_channels_raw_send(_channel, ch[0], ch[1], ch[2], + ch[3], ch[4], ch[5], ch[6], ch[7], 255); + break; + } + + case MAVLINK_MSG_ID_SYS_STATUS: { + + uint16_t batteryVoltage = 0; // (milli volts) + uint16_t batteryPercentage = 1000; // times 10 + if (_hal->batteryMonitor) { + batteryPercentage = _hal->batteryMonitor->getPercentage()*10; + batteryVoltage = _hal->batteryMonitor->getVoltage()*1000; + } + mavlink_msg_sys_status_send(_channel, _controller->getMode(), + _guide->getMode(), _hal->getState(), _hal->load * 10, + batteryVoltage, batteryPercentage, _packetDrops); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: { + sendText(SEVERITY_LOW, PSTR("waypoint ack")); + //mavlink_waypoint_ack_t packet; + uint8_t type = 0; // ok (0), error(1) + mavlink_msg_waypoint_ack_send(_channel, _cmdDestSysId, + _cmdDestCompId, type); + + // turn off waypoint send + _receivingCmds = false; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CURRENT: { + mavlink_msg_waypoint_current_send(_channel, + _guide->getCurrentIndex()); + break; + } + + default: { + char msg[50]; + sprintf(msg, "autopilot sending unknown command with id: %d", id); + sendText(SEVERITY_HIGH, msg); + } + + } // switch +} // send message + +void MavlinkComm::receive() { + //_hal->debug->printf_P(PSTR("receive\n")); + // if number of channels exceeded return + // + if (_channel == MAVLINK_COMM_3) + return; + + // receive new packets + mavlink_message_t msg; + mavlink_status_t status; + status.packet_rx_drop_count = 0; + + // process received bytes + while (comm_get_available(_channel)) { + uint8_t c = comm_receive_ch(_channel); + + // Try to get a new message + if (mavlink_parse_char(_channel, c, &msg, &status)) + _handleMessage(&msg); + } + + // Update packet drops counter + _packetDrops += status.packet_rx_drop_count; +} + +void MavlinkComm::sendText(uint8_t severity, const char *str) { + mavlink_msg_statustext_send(_channel, severity, (const int8_t*) str); +} + +void MavlinkComm::sendText(uint8_t severity, const prog_char_t *str) { + mavlink_statustext_t m; + uint8_t i; + for (i = 0; i < sizeof(m.text); i++) { + m.text[i] = pgm_read_byte((const prog_char *) (str++)); + } + if (i < sizeof(m.text)) + m.text[i] = 0; + sendText(severity, (const char *) m.text); +} + +void MavlinkComm::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) { +} + +/** + * sends parameters one at a time + */ +void MavlinkComm::sendParameters() { + //_hal->debug->printf_P(PSTR("send parameters\n")); + // Check to see if we are sending parameters + while (NULL != _queuedParameter) { + AP_Var *vp; + float value; + + // copy the current parameter and prepare to move to the next + vp = _queuedParameter; + _queuedParameter = _queuedParameter->next(); + + // if the parameter can be cast to float, report it here and break out of the loop + value = vp->cast_to_float(); + if (!isnan(value)) { + + char paramName[_paramNameLengthMax]; + vp->copy_name(paramName, sizeof(paramName)); + + mavlink_msg_param_value_send(_channel, (int8_t*) paramName, + value, _countParameters(), _queuedParameterIndex); + + _queuedParameterIndex++; + break; + } + } + +} + +/** + * request commands one at a time + */ +void MavlinkComm::requestCmds() { + //_hal->debug->printf_P(PSTR("requesting commands\n")); + // request cmds one by one + if (_receivingCmds && _cmdRequestIndex <= _cmdNumberRequested) { + mavlink_msg_waypoint_request_send(_channel, _cmdDestSysId, + _cmdDestCompId, _cmdRequestIndex); + } +} + +void MavlinkComm::_handleMessage(mavlink_message_t * msg) { + + uint32_t timeStamp = micros(); + + switch (msg->msgid) { + _hal->debug->printf_P(PSTR("message received: %d"), msg->msgid); + + case MAVLINK_MSG_ID_HEARTBEAT: { + mavlink_heartbeat_t packet; + mavlink_msg_heartbeat_decode(msg, &packet); + _hal->lastHeartBeat = micros(); + break; + } + + case MAVLINK_MSG_ID_GPS_RAW: { + // decode + mavlink_gps_raw_t packet; + mavlink_msg_gps_raw_decode(msg, &packet); + + _navigator->setTimeStamp(timeStamp); + _navigator->setLat(packet.lat * deg2Rad); + _navigator->setLon(packet.lon * deg2Rad); + _navigator->setAlt(packet.alt); + _navigator->setYaw(packet.hdg * deg2Rad); + _navigator->setGroundSpeed(packet.v); + _navigator->setAirSpeed(packet.v); + //_hal->debug->printf_P(PSTR("received hil gps raw packet\n")); + /* + _hal->debug->printf_P(PSTR("received lat: %f deg\tlon: %f deg\talt: %f m\n"), + packet.lat, + packet.lon, + packet.alt); + */ + break; + } + + case MAVLINK_MSG_ID_ATTITUDE: { + // decode + mavlink_attitude_t packet; + mavlink_msg_attitude_decode(msg, &packet); + + // set dcm hil sensor + _navigator->setTimeStamp(timeStamp); + _navigator->setRoll(packet.roll); + _navigator->setPitch(packet.pitch); + _navigator->setYaw(packet.yaw); + _navigator->setRollRate(packet.rollspeed); + _navigator->setPitchRate(packet.pitchspeed); + _navigator->setYawRate(packet.yawspeed); + //_hal->debug->printf_P(PSTR("received hil attitude packet\n")); + break; + } + + case MAVLINK_MSG_ID_ACTION: { + // decode + mavlink_action_t packet; + mavlink_msg_action_decode(msg, &packet); + if (_checkTarget(packet.target, packet.target_component)) + break; + + // do action + sendText(SEVERITY_LOW, PSTR("action received")); + switch (packet.action) { + + case MAV_ACTION_STORAGE_READ: + AP_Var::load_all(); + break; + + case MAV_ACTION_STORAGE_WRITE: + AP_Var::save_all(); + break; + + case MAV_ACTION_CALIBRATE_RC: + case MAV_ACTION_CALIBRATE_GYRO: + case MAV_ACTION_CALIBRATE_MAG: + case MAV_ACTION_CALIBRATE_ACC: + case MAV_ACTION_CALIBRATE_PRESSURE: + case MAV_ACTION_REBOOT: + case MAV_ACTION_REC_START: + case MAV_ACTION_REC_PAUSE: + case MAV_ACTION_REC_STOP: + case MAV_ACTION_TAKEOFF: + case MAV_ACTION_LAND: + case MAV_ACTION_NAVIGATE: + case MAV_ACTION_LOITER: + case MAV_ACTION_MOTORS_START: + case MAV_ACTION_CONFIRM_KILL: + case MAV_ACTION_EMCY_KILL: + case MAV_ACTION_MOTORS_STOP: + case MAV_ACTION_SHUTDOWN: + case MAV_ACTION_CONTINUE: + case MAV_ACTION_SET_MANUAL: + case MAV_ACTION_SET_AUTO: + case MAV_ACTION_LAUNCH: + case MAV_ACTION_RETURN: + case MAV_ACTION_EMCY_LAND: + case MAV_ACTION_HALT: + sendText(SEVERITY_LOW, PSTR("action not implemented")); + break; + default: + sendText(SEVERITY_LOW, PSTR("unknown action")); + break; + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: { + sendText(SEVERITY_LOW, PSTR("waypoint request list")); + + // decode + mavlink_waypoint_request_list_t packet; + mavlink_msg_waypoint_request_list_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // Start sending waypoints + mavlink_msg_waypoint_count_send(_channel, msg->sysid, msg->compid, + _guide->getNumberOfCommands()); + + _cmdTimeLastSent = millis(); + _cmdTimeLastReceived = millis(); + _sendingCmds = true; + _receivingCmds = false; + _cmdDestSysId = msg->sysid; + _cmdDestCompId = msg->compid; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: { + sendText(SEVERITY_LOW, PSTR("waypoint request")); + + // Check if sending waypiont + if (!_sendingCmds) + break; + + // decode + mavlink_waypoint_request_t packet; + mavlink_msg_waypoint_request_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + _hal->debug->printf_P(PSTR("sequence: %d\n"),packet.seq); + AP_MavlinkCommand cmd(packet.seq); + + mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex()); + mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId, + wp.seq, wp.frame, wp.command, wp.current, wp.autocontinue, + wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, + wp.z); + + // update last waypoint comm stamp + _cmdTimeLastSent = millis(); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: { + sendText(SEVERITY_LOW, PSTR("waypoint ack")); + + // decode + mavlink_waypoint_ack_t packet; + mavlink_msg_waypoint_ack_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // check for error + //uint8_t type = packet.type; // ok (0), error(1) + + // turn off waypoint send + _sendingCmds = false; + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + sendText(SEVERITY_LOW, PSTR("param request list")); + + // decode + mavlink_param_request_list_t packet; + mavlink_msg_param_request_list_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // Start sending parameters - next call to ::update will kick the first one out + + _queuedParameter = AP_Var::first(); + _queuedParameterIndex = 0; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: { + sendText(SEVERITY_LOW, PSTR("waypoint clear all")); + + // decode + mavlink_waypoint_clear_all_t packet; + mavlink_msg_waypoint_clear_all_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // clear all waypoints + uint8_t type = 0; // ok (0), error(1) + _guide->setNumberOfCommands(1); + _guide->setCurrentIndex(0); + + // send acknowledgement 3 times to makes sure it is received + for (int i = 0; i < 3; i++) + mavlink_msg_waypoint_ack_send(_channel, msg->sysid, + msg->compid, type); + + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: { + sendText(SEVERITY_LOW, PSTR("waypoint set current")); + + // decode + mavlink_waypoint_set_current_t packet; + mavlink_msg_waypoint_set_current_decode(msg, &packet); + Serial.print("Packet Sequence:"); + Serial.println(packet.seq); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // set current waypoint + Serial.print("Current Index:"); + Serial.println(_guide->getCurrentIndex()); + Serial.flush(); + _guide->setCurrentIndex(packet.seq); + mavlink_msg_waypoint_current_send(_channel, + _guide->getCurrentIndex()); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_COUNT: { + sendText(SEVERITY_LOW, PSTR("waypoint count")); + + // decode + mavlink_waypoint_count_t packet; + mavlink_msg_waypoint_count_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // start waypoint receiving + if (packet.count > _cmdMax) { + packet.count = _cmdMax; + } + _cmdNumberRequested = packet.count; + _cmdTimeLastReceived = millis(); + _receivingCmds = true; + _sendingCmds = false; + _cmdRequestIndex = 0; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT: { + sendText(SEVERITY_LOW, PSTR("waypoint")); + + // Check if receiving waypiont + if (!_receivingCmds) { + //sendText(SEVERITY_HIGH, PSTR("not receiving commands")); + break; + } + + // decode + mavlink_waypoint_t packet; + mavlink_msg_waypoint_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // check if this is the requested waypoint + if (packet.seq != _cmdRequestIndex) { + char warningMsg[50]; + sprintf(warningMsg, + "waypoint request out of sequence: (packet) %d / %d (ap)", + packet.seq, _cmdRequestIndex); + sendText(SEVERITY_HIGH, warningMsg); + break; + } + + _hal->debug->printf_P(PSTR("received waypoint x: %f\ty: %f\tz: %f\n"), + packet.x, + packet.y, + packet.z); + + // store waypoint + AP_MavlinkCommand command(packet); + //sendText(SEVERITY_HIGH, PSTR("waypoint stored")); + _cmdRequestIndex++; + if (_cmdRequestIndex == _cmdNumberRequested) { + sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK); + _receivingCmds = false; + _guide->setNumberOfCommands(_cmdNumberRequested); + //sendText(SEVERITY_LOW, PSTR("waypoint ack sent")); + } else if (_cmdRequestIndex > _cmdNumberRequested) { + _receivingCmds = false; + } + _cmdTimeLastReceived = millis(); + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: { + sendText(SEVERITY_LOW, PSTR("param set")); + AP_Var *vp; + AP_Meta_class::Type_id var_type; + + // decode + mavlink_param_set_t packet; + mavlink_msg_param_set_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // set parameter + + char key[_paramNameLengthMax + 1]; + strncpy(key, (char *) packet.param_id, _paramNameLengthMax); + key[_paramNameLengthMax] = 0; + + // find the requested parameter + vp = AP_Var::find(key); + if ((NULL != vp) && // exists + !isnan(packet.param_value) && // not nan + !isinf(packet.param_value)) { // not inf + + // add a small amount before casting parameter values + // from float to integer to avoid truncating to the + // next lower integer value. + const float rounding_addition = 0.01; + + // fetch the variable type ID + var_type = vp->meta_type_id(); + + // handle variables with standard type IDs + if (var_type == AP_Var::k_typeid_float) { + ((AP_Float *) vp)->set_and_save(packet.param_value); + + } else if (var_type == AP_Var::k_typeid_float16) { + ((AP_Float16 *) vp)->set_and_save(packet.param_value); + + } else if (var_type == AP_Var::k_typeid_int32) { + ((AP_Int32 *) vp)->set_and_save( + packet.param_value + rounding_addition); + + } else if (var_type == AP_Var::k_typeid_int16) { + ((AP_Int16 *) vp)->set_and_save( + packet.param_value + rounding_addition); + + } else if (var_type == AP_Var::k_typeid_int8) { + ((AP_Int8 *) vp)->set_and_save( + packet.param_value + rounding_addition); + } else { + // we don't support mavlink set on this parameter + break; + } + + // Report back the new value if we accepted the change + // we send the value we actually set, which could be + // different from the value sent, in case someone sent + // a fractional value to an integer type + mavlink_msg_param_value_send(_channel, (int8_t *) key, + vp->cast_to_float(), _countParameters(), -1); // XXX we don't actually know what its index is... + } + + break; + } // end case + + + } +} + +uint16_t MavlinkComm::_countParameters() { + // if we haven't cached the parameter count yet... + if (0 == _parameterCount) { + AP_Var *vp; + + vp = AP_Var::first(); + do { + // if a parameter responds to cast_to_float then we are going to be able to report it + if (!isnan(vp->cast_to_float())) { + _parameterCount++; + } + } while (NULL != (vp = vp->next())); + } + return _parameterCount; +} + +AP_Var * _findParameter(uint16_t index) { + AP_Var *vp; + + vp = AP_Var::first(); + while (NULL != vp) { + + // if the parameter is reportable + if (!(isnan(vp->cast_to_float()))) { + // if we have counted down to the index we want + if (0 == index) { + // return the parameter + return vp; + } + // count off this parameter, as it is reportable but not + // the one we want + index--; + } + // and move to the next parameter + vp = vp->next(); + } + return NULL; +} + +// check the target +uint8_t MavlinkComm::_checkTarget(uint8_t sysid, uint8_t compid) { + /* + char msg[50]; + sprintf(msg, "target = %d / %d\tcomp = %d / %d", sysid, + mavlink_system.sysid, compid, mavlink_system.compid); + sendText(SEVERITY_LOW, msg); + */ + if (sysid != mavlink_system.sysid) { + //sendText(SEVERITY_LOW, PSTR("system id mismatch")); + return 1; + + } else if (compid != mavlink_system.compid) { + //sendText(SEVERITY_LOW, PSTR("component id mismatch")); + return 0; // XXX currently not receiving correct compid from gcs + + } else { + return 0; // no error + } +} + } // apo diff --git a/libraries/APO/AP_CommLink.h b/libraries/APO/AP_CommLink.h index b40641b143..be473cde9a 100644 --- a/libraries/APO/AP_CommLink.h +++ b/libraries/APO/AP_CommLink.h @@ -19,9 +19,12 @@ #ifndef AP_CommLink_H #define AP_CommLink_H -#include "AP_HardwareAbstractionLayer.h" +#include +#include "../AP_Common/AP_Common.h" +#include "../AP_Common/AP_Vector.h" #include "AP_MavlinkCommand.h" -#include "AP_Controller.h" + +class FastSerial; namespace apo { @@ -43,10 +46,7 @@ class AP_CommLink { public: AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide, - AP_Controller * controller, AP_HardwareAbstractionLayer * hal) : - _link(link), _navigator(navigator), _guide(guide), - _controller(controller), _hal(hal) { - } + AP_Controller * controller, AP_HardwareAbstractionLayer * hal); virtual void send() = 0; virtual void receive() = 0; virtual void sendMessage(uint8_t id, uint32_t param = 0) = 0; @@ -67,264 +67,24 @@ protected: class MavlinkComm: public AP_CommLink { public: MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide, - AP_Controller * controller, AP_HardwareAbstractionLayer * hal) : - AP_CommLink(link, nav, guide, controller, hal), + AP_Controller * controller, AP_HardwareAbstractionLayer * hal); - // options - _useRelativeAlt(true), - - // commands - _sendingCmds(false), _receivingCmds(false), - _cmdTimeLastSent(millis()), _cmdTimeLastReceived(millis()), - _cmdDestSysId(0), _cmdDestCompId(0), _cmdRequestIndex(0), - _cmdMax(30), _cmdNumberRequested(0), - - // parameters - _parameterCount(0), _queuedParameter(NULL), - _queuedParameterIndex(0) { - - switch (_nChannels) { - case 0: - mavlink_comm_0_port = link; - _channel = MAVLINK_COMM_0; - _nChannels++; - break; - case 1: - mavlink_comm_1_port = link; - _channel = MAVLINK_COMM_1; - _nChannels++; - break; - default: - // signal that number of channels exceeded - _channel = MAVLINK_COMM_3; - break; - } - } - - virtual void send() { - // if number of channels exceeded return - if (_channel == MAVLINK_COMM_3) - return; - } - - void sendMessage(uint8_t id, uint32_t param = 0) { - //_hal->debug->printf_P(PSTR("send message\n")); - - // if number of channels exceeded return - if (_channel == MAVLINK_COMM_3) - return; - - uint64_t timeStamp = micros(); - - switch (id) { - - case MAVLINK_MSG_ID_HEARTBEAT: { - mavlink_msg_heartbeat_send(_channel, mavlink_system.type, - MAV_AUTOPILOT_ARDUPILOTMEGA); - break; - } - - case MAVLINK_MSG_ID_ATTITUDE: { - mavlink_msg_attitude_send(_channel, timeStamp, - _navigator->getRoll(), _navigator->getPitch(), - _navigator->getYaw(), _navigator->getRollRate(), - _navigator->getPitchRate(), _navigator->getYawRate()); - break; - } - - case MAVLINK_MSG_ID_GLOBAL_POSITION: { - mavlink_msg_global_position_send(_channel, timeStamp, - _navigator->getLat() * rad2Deg, - _navigator->getLon() * rad2Deg, _navigator->getAlt(), - _navigator->getVN(), _navigator->getVE(), - _navigator->getVD()); - break; - } - - case MAVLINK_MSG_ID_GPS_RAW: { - mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(), - _navigator->getLat() * rad2Deg, - _navigator->getLon() * rad2Deg, _navigator->getAlt(), 0, 0, - _navigator->getGroundSpeed(), - _navigator->getYaw() * rad2Deg); - break; - } - - /* - case MAVLINK_MSG_ID_GPS_RAW_INT: { - mavlink_msg_gps_raw_int_send(_channel,timeStamp,_hal->gps->status(), - _navigator->getLat_degInt(), _navigator->getLon_degInt(),_navigator->getAlt_intM(), 0,0, - _navigator->getGroundSpeed(),_navigator->getYaw()*rad2Deg); - break; - } - */ - - case MAVLINK_MSG_ID_SCALED_IMU: { - /* - * accel/gyro debug - */ - /* - Vector3f accel = _hal->imu->get_accel(); - Vector3f gyro = _hal->imu->get_gyro(); - Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"), - accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z); - */ - Vector3f accel = _hal->imu->get_accel(); - Vector3f gyro = _hal->imu->get_gyro(); - mavlink_msg_raw_imu_send(_channel, timeStamp, 1000 * accel.x, - 1000 * accel.y, 1000 * accel.z, 1000 * gyro.x, - 1000 * gyro.y, 1000 * gyro.z, _hal->compass->mag_x, - _hal->compass->mag_y, _hal->compass->mag_z); // XXX THIS IS NOT SCALED FOR MAG - } - - case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: { - int16_t ch[8]; - for (int i = 0; i < 8; i++) - ch[i] = 0; - for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) { - ch[i] = 10000 * _hal->rc[i]->getPosition(); - //_hal->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]); - } - mavlink_msg_rc_channels_scaled_send(_channel, ch[0], ch[1], ch[2], - ch[3], ch[4], ch[5], ch[6], ch[7], 255); - break; - } - - case MAVLINK_MSG_ID_RC_CHANNELS_RAW: { - int16_t ch[8]; - for (int i = 0; i < 8; i++) - ch[i] = 0; - for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) - ch[i] = _hal->rc[i]->getPwm(); - mavlink_msg_rc_channels_raw_send(_channel, ch[0], ch[1], ch[2], - ch[3], ch[4], ch[5], ch[6], ch[7], 255); - break; - } - - case MAVLINK_MSG_ID_SYS_STATUS: { - - float batteryVoltage, temp; - temp = analogRead(0); - batteryVoltage = ((temp * 5 / 1023) / 0.28); - - mavlink_msg_sys_status_send(_channel, _controller->getMode(), - _guide->getMode(), _hal->getState(), _hal->load * 10, - batteryVoltage * 1000, - (batteryVoltage - 3.3) / (4.2 - 3.3) * 1000, _packetDrops); - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_ACK: { - sendText(SEVERITY_LOW, PSTR("waypoint ack")); - //mavlink_waypoint_ack_t packet; - uint8_t type = 0; // ok (0), error(1) - mavlink_msg_waypoint_ack_send(_channel, _cmdDestSysId, - _cmdDestCompId, type); - - // turn off waypoint send - _receivingCmds = false; - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_CURRENT: { - mavlink_msg_waypoint_current_send(_channel, - _guide->getCurrentIndex()); - break; - } - - default: { - char msg[50]; - sprintf(msg, "autopilot sending unknown command with id: %d", id); - sendText(SEVERITY_HIGH, msg); - } - - } // switch - } // send message - - virtual void receive() { - //_hal->debug->printf_P(PSTR("receive\n")); - // if number of channels exceeded return - // - if (_channel == MAVLINK_COMM_3) - return; - - // receive new packets - mavlink_message_t msg; - mavlink_status_t status; - - // process received bytes - while (comm_get_available(_channel)) { - uint8_t c = comm_receive_ch(_channel); - - // Try to get a new message - if (mavlink_parse_char(_channel, c, &msg, &status)) - _handleMessage(&msg); - } - - // Update packet drops counter - _packetDrops += status.packet_rx_drop_count; - } - - void sendText(uint8_t severity, const char *str) { - mavlink_msg_statustext_send(_channel, severity, (const int8_t*) str); - } - - void sendText(uint8_t severity, const prog_char_t *str) { - mavlink_statustext_t m; - uint8_t i; - for (i = 0; i < sizeof(m.text); i++) { - m.text[i] = pgm_read_byte((const prog_char *) (str++)); - } - if (i < sizeof(m.text)) - m.text[i] = 0; - sendText(severity, (const char *) m.text); - } - - void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) { - } + virtual void send(); + void sendMessage(uint8_t id, uint32_t param = 0); + virtual void receive(); + void sendText(uint8_t severity, const char *str); + void sendText(uint8_t severity, const prog_char_t *str); + void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2); /** * sends parameters one at a time */ - void sendParameters() { - //_hal->debug->printf_P(PSTR("send parameters\n")); - // Check to see if we are sending parameters - while (NULL != _queuedParameter) { - AP_Var *vp; - float value; - - // copy the current parameter and prepare to move to the next - vp = _queuedParameter; - _queuedParameter = _queuedParameter->next(); - - // if the parameter can be cast to float, report it here and break out of the loop - value = vp->cast_to_float(); - if (!isnan(value)) { - - char paramName[_paramNameLengthMax]; - vp->copy_name(paramName, sizeof(paramName)); - - mavlink_msg_param_value_send(_channel, (int8_t*) paramName, - value, _countParameters(), _queuedParameterIndex); - - _queuedParameterIndex++; - break; - } - } - - } + void sendParameters(); /** * request commands one at a time */ - void requestCmds() { - //_hal->debug->printf_P(PSTR("requesting commands\n")); - // request cmds one by one - if (_receivingCmds && _cmdRequestIndex <= _cmdNumberRequested) { - mavlink_msg_waypoint_request_send(_channel, _cmdDestSysId, - _cmdDestCompId, _cmdRequestIndex); - } - } + void requestCmds(); private: @@ -354,432 +114,14 @@ private: uint16_t _packetDrops; static uint8_t _nChannels; - void _handleMessage(mavlink_message_t * msg) { + void _handleMessage(mavlink_message_t * msg); - uint32_t timeStamp = micros(); + uint16_t _countParameters(); - switch (msg->msgid) { - _hal->debug->printf_P(PSTR("message received: %d"), msg->msgid); - - case MAVLINK_MSG_ID_HEARTBEAT: { - mavlink_heartbeat_t packet; - mavlink_msg_heartbeat_decode(msg, &packet); - _hal->lastHeartBeat = micros(); - break; - } - - case MAVLINK_MSG_ID_GPS_RAW: { - // decode - mavlink_gps_raw_t packet; - mavlink_msg_gps_raw_decode(msg, &packet); - - _navigator->setTimeStamp(timeStamp); - _navigator->setLat(packet.lat * deg2Rad); - _navigator->setLon(packet.lon * deg2Rad); - _navigator->setAlt(packet.alt); - _navigator->setYaw(packet.hdg * deg2Rad); - _navigator->setGroundSpeed(packet.v); - _navigator->setAirSpeed(packet.v); - //_hal->debug->printf_P(PSTR("received hil gps raw packet\n")); - /* - _hal->debug->printf_P(PSTR("received lat: %f deg\tlon: %f deg\talt: %f m\n"), - packet.lat, - packet.lon, - packet.alt); - */ - break; - } - - case MAVLINK_MSG_ID_ATTITUDE: { - // decode - mavlink_attitude_t packet; - mavlink_msg_attitude_decode(msg, &packet); - - // set dcm hil sensor - _navigator->setTimeStamp(timeStamp); - _navigator->setRoll(packet.roll); - _navigator->setPitch(packet.pitch); - _navigator->setYaw(packet.yaw); - _navigator->setRollRate(packet.rollspeed); - _navigator->setPitchRate(packet.pitchspeed); - _navigator->setYawRate(packet.yawspeed); - //_hal->debug->printf_P(PSTR("received hil attitude packet\n")); - break; - } - - case MAVLINK_MSG_ID_ACTION: { - // decode - mavlink_action_t packet; - mavlink_msg_action_decode(msg, &packet); - if (_checkTarget(packet.target, packet.target_component)) - break; - - // do action - sendText(SEVERITY_LOW, PSTR("action received")); - switch (packet.action) { - - case MAV_ACTION_STORAGE_READ: - AP_Var::load_all(); - break; - - case MAV_ACTION_STORAGE_WRITE: - AP_Var::save_all(); - break; - - case MAV_ACTION_CALIBRATE_RC: - case MAV_ACTION_CALIBRATE_GYRO: - case MAV_ACTION_CALIBRATE_MAG: - case MAV_ACTION_CALIBRATE_ACC: - case MAV_ACTION_CALIBRATE_PRESSURE: - case MAV_ACTION_REBOOT: - case MAV_ACTION_REC_START: - case MAV_ACTION_REC_PAUSE: - case MAV_ACTION_REC_STOP: - case MAV_ACTION_TAKEOFF: - case MAV_ACTION_LAND: - case MAV_ACTION_NAVIGATE: - case MAV_ACTION_LOITER: - case MAV_ACTION_MOTORS_START: - case MAV_ACTION_CONFIRM_KILL: - case MAV_ACTION_EMCY_KILL: - case MAV_ACTION_MOTORS_STOP: - case MAV_ACTION_SHUTDOWN: - case MAV_ACTION_CONTINUE: - case MAV_ACTION_SET_MANUAL: - case MAV_ACTION_SET_AUTO: - case MAV_ACTION_LAUNCH: - case MAV_ACTION_RETURN: - case MAV_ACTION_EMCY_LAND: - case MAV_ACTION_HALT: - sendText(SEVERITY_LOW, PSTR("action not implemented")); - break; - default: - sendText(SEVERITY_LOW, PSTR("unknown action")); - break; - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: { - sendText(SEVERITY_LOW, PSTR("waypoint request list")); - - // decode - mavlink_waypoint_request_list_t packet; - mavlink_msg_waypoint_request_list_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // Start sending waypoints - mavlink_msg_waypoint_count_send(_channel, msg->sysid, msg->compid, - _guide->getNumberOfCommands()); - - _cmdTimeLastSent = millis(); - _cmdTimeLastReceived = millis(); - _sendingCmds = true; - _receivingCmds = false; - _cmdDestSysId = msg->sysid; - _cmdDestCompId = msg->compid; - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: { - sendText(SEVERITY_LOW, PSTR("waypoint request")); - - // Check if sending waypiont - if (!_sendingCmds) - break; - - // decode - mavlink_waypoint_request_t packet; - mavlink_msg_waypoint_request_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - _hal->debug->printf_P(PSTR("sequence: %d\n"),packet.seq); - AP_MavlinkCommand cmd(packet.seq); - - mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex()); - mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId, - wp.seq, wp.frame, wp.command, wp.current, wp.autocontinue, - wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, - wp.z); - - // update last waypoint comm stamp - _cmdTimeLastSent = millis(); - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_ACK: { - sendText(SEVERITY_LOW, PSTR("waypoint ack")); - - // decode - mavlink_waypoint_ack_t packet; - mavlink_msg_waypoint_ack_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // check for error - //uint8_t type = packet.type; // ok (0), error(1) - - // turn off waypoint send - _sendingCmds = false; - break; - } - - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - sendText(SEVERITY_LOW, PSTR("param request list")); - - // decode - mavlink_param_request_list_t packet; - mavlink_msg_param_request_list_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // Start sending parameters - next call to ::update will kick the first one out - - _queuedParameter = AP_Var::first(); - _queuedParameterIndex = 0; - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: { - sendText(SEVERITY_LOW, PSTR("waypoint clear all")); - - // decode - mavlink_waypoint_clear_all_t packet; - mavlink_msg_waypoint_clear_all_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // clear all waypoints - uint8_t type = 0; // ok (0), error(1) - _guide->setNumberOfCommands(1); - _guide->setCurrentIndex(0); - - // send acknowledgement 3 times to makes sure it is received - for (int i = 0; i < 3; i++) - mavlink_msg_waypoint_ack_send(_channel, msg->sysid, - msg->compid, type); - - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: { - sendText(SEVERITY_LOW, PSTR("waypoint set current")); - - // decode - mavlink_waypoint_set_current_t packet; - mavlink_msg_waypoint_set_current_decode(msg, &packet); - Serial.print("Packet Sequence:"); - Serial.println(packet.seq); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // set current waypoint - Serial.print("Current Index:"); - Serial.println(_guide->getCurrentIndex()); - Serial.flush(); - _guide->setCurrentIndex(packet.seq); - mavlink_msg_waypoint_current_send(_channel, - _guide->getCurrentIndex()); - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_COUNT: { - sendText(SEVERITY_LOW, PSTR("waypoint count")); - - // decode - mavlink_waypoint_count_t packet; - mavlink_msg_waypoint_count_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // start waypoint receiving - if (packet.count > _cmdMax) { - packet.count = _cmdMax; - } - _cmdNumberRequested = packet.count; - _cmdTimeLastReceived = millis(); - _receivingCmds = true; - _sendingCmds = false; - _cmdRequestIndex = 0; - break; - } - - case MAVLINK_MSG_ID_WAYPOINT: { - sendText(SEVERITY_LOW, PSTR("waypoint")); - - // Check if receiving waypiont - if (!_receivingCmds) { - //sendText(SEVERITY_HIGH, PSTR("not receiving commands")); - break; - } - - // decode - mavlink_waypoint_t packet; - mavlink_msg_waypoint_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // check if this is the requested waypoint - if (packet.seq != _cmdRequestIndex) { - char warningMsg[50]; - sprintf(warningMsg, - "waypoint request out of sequence: (packet) %d / %d (ap)", - packet.seq, _cmdRequestIndex); - sendText(SEVERITY_HIGH, warningMsg); - break; - } - - _hal->debug->printf_P(PSTR("received waypoint x: %f\ty: %f\tz: %f\n"), - packet.x, - packet.y, - packet.z); - - // store waypoint - AP_MavlinkCommand command(packet); - //sendText(SEVERITY_HIGH, PSTR("waypoint stored")); - _cmdRequestIndex++; - if (_cmdRequestIndex == _cmdNumberRequested) { - sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK); - _receivingCmds = false; - _guide->setNumberOfCommands(_cmdNumberRequested); - //sendText(SEVERITY_LOW, PSTR("waypoint ack sent")); - } else if (_cmdRequestIndex > _cmdNumberRequested) { - _receivingCmds = false; - } - _cmdTimeLastReceived = millis(); - break; - } - - case MAVLINK_MSG_ID_PARAM_SET: { - sendText(SEVERITY_LOW, PSTR("param set")); - AP_Var *vp; - AP_Meta_class::Type_id var_type; - - // decode - mavlink_param_set_t packet; - mavlink_msg_param_set_decode(msg, &packet); - if (_checkTarget(packet.target_system, packet.target_component)) - break; - - // set parameter - - char key[_paramNameLengthMax + 1]; - strncpy(key, (char *) packet.param_id, _paramNameLengthMax); - key[_paramNameLengthMax] = 0; - - // find the requested parameter - vp = AP_Var::find(key); - if ((NULL != vp) && // exists - !isnan(packet.param_value) && // not nan - !isinf(packet.param_value)) { // not inf - - // add a small amount before casting parameter values - // from float to integer to avoid truncating to the - // next lower integer value. - const float rounding_addition = 0.01; - - // fetch the variable type ID - var_type = vp->meta_type_id(); - - // handle variables with standard type IDs - if (var_type == AP_Var::k_typeid_float) { - ((AP_Float *) vp)->set_and_save(packet.param_value); - - } else if (var_type == AP_Var::k_typeid_float16) { - ((AP_Float16 *) vp)->set_and_save(packet.param_value); - - } else if (var_type == AP_Var::k_typeid_int32) { - ((AP_Int32 *) vp)->set_and_save( - packet.param_value + rounding_addition); - - } else if (var_type == AP_Var::k_typeid_int16) { - ((AP_Int16 *) vp)->set_and_save( - packet.param_value + rounding_addition); - - } else if (var_type == AP_Var::k_typeid_int8) { - ((AP_Int8 *) vp)->set_and_save( - packet.param_value + rounding_addition); - } else { - // we don't support mavlink set on this parameter - break; - } - - // Report back the new value if we accepted the change - // we send the value we actually set, which could be - // different from the value sent, in case someone sent - // a fractional value to an integer type - mavlink_msg_param_value_send(_channel, (int8_t *) key, - vp->cast_to_float(), _countParameters(), -1); // XXX we don't actually know what its index is... - } - - break; - } // end case - - - } - } - - uint16_t _countParameters() { - // if we haven't cached the parameter count yet... - if (0 == _parameterCount) { - AP_Var *vp; - - vp = AP_Var::first(); - do { - // if a parameter responds to cast_to_float then we are going to be able to report it - if (!isnan(vp->cast_to_float())) { - _parameterCount++; - } - } while (NULL != (vp = vp->next())); - } - return _parameterCount; - } - - AP_Var * _findParameter(uint16_t index) { - AP_Var *vp; - - vp = AP_Var::first(); - while (NULL != vp) { - - // if the parameter is reportable - if (!(isnan(vp->cast_to_float()))) { - // if we have counted down to the index we want - if (0 == index) { - // return the parameter - return vp; - } - // count off this parameter, as it is reportable but not - // the one we want - index--; - } - // and move to the next parameter - vp = vp->next(); - } - return NULL; - } + AP_Var * _findParameter(uint16_t index); // check the target - uint8_t _checkTarget(uint8_t sysid, uint8_t compid) { - /* - char msg[50]; - sprintf(msg, "target = %d / %d\tcomp = %d / %d", sysid, - mavlink_system.sysid, compid, mavlink_system.compid); - sendText(SEVERITY_LOW, msg); - */ - if (sysid != mavlink_system.sysid) { - //sendText(SEVERITY_LOW, PSTR("system id mismatch")); - return 1; - - } else if (compid != mavlink_system.compid) { - //sendText(SEVERITY_LOW, PSTR("component id mismatch")); - return 0; // XXX currently not receiving correct compid from gcs - - } else { - return 0; // no error - } - } + uint8_t _checkTarget(uint8_t sysid, uint8_t compid); }; diff --git a/libraries/APO/AP_Controller.cpp b/libraries/APO/AP_Controller.cpp index ebbd5ed524..443fe2afbb 100644 --- a/libraries/APO/AP_Controller.cpp +++ b/libraries/APO/AP_Controller.cpp @@ -6,3 +6,28 @@ */ #include "AP_Controller.h" +#include "AP_HardwareAbstractionLayer.h" +#include "../AP_Common/include/menu.h" +#include "AP_RcChannel.h" + +namespace apo { + +AP_Controller::AP_Controller(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + _nav(nav), _guide(guide), _hal(hal) { + setAllRadioChannelsToNeutral(); +} + +void AP_Controller::setAllRadioChannelsToNeutral() { + for (uint8_t i = 0; i < _hal->rc.getSize(); i++) { + _hal->rc[i]->setPosition(0.0); + } +} + +void AP_Controller::setAllRadioChannelsManually() { + for (uint8_t i = 0; i < _hal->rc.getSize(); i++) { + _hal->rc[i]->setUsingRadio(); + } +} + +} diff --git a/libraries/APO/AP_Controller.h b/libraries/APO/AP_Controller.h index d7dac0c60c..4cfc57609d 100644 --- a/libraries/APO/AP_Controller.h +++ b/libraries/APO/AP_Controller.h @@ -19,37 +19,28 @@ #ifndef AP_Controller_H #define AP_Controller_H -#include "AP_Navigator.h" -#include "AP_Guide.h" -#include "AP_HardwareAbstractionLayer.h" -#include "../AP_Common/AP_Vector.h" -#include "../AP_Common/AP_Var.h" +#include +#include "../GCS_MAVLink/GCS_MAVLink.h" +#include + +class AP_Var_group; namespace apo { +class AP_HardwareAbstractionLayer; +class AP_Guide; +class AP_Navigator; +class Menu; + /// Controller class class AP_Controller { public: AP_Controller(AP_Navigator * nav, AP_Guide * guide, - AP_HardwareAbstractionLayer * hal) : - _nav(nav), _guide(guide), _hal(hal) { - } - + AP_HardwareAbstractionLayer * hal); virtual void update(const float & dt) = 0; - virtual MAV_MODE getMode() = 0; - - void setAllRadioChannelsToNeutral() { - for (uint8_t i = 0; i < _hal->rc.getSize(); i++) { - _hal->rc[i]->setPosition(0.0); - } - } - - void setAllRadioChannelsManually() { - for (uint8_t i = 0; i < _hal->rc.getSize(); i++) { - _hal->rc[i]->setUsingRadio(); - } - } + void setAllRadioChannelsToNeutral(); + void setAllRadioChannelsManually(); protected: AP_Navigator * _nav; @@ -278,23 +269,30 @@ protected: class BlockPIDDfb: public AP_ControllerBlock { public: BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI, - float kD, float iMax, float yMax, const prog_char_t * dLabel = NULL) : + float kD, float iMax, float yMax, float dFCut, + const prog_char_t * dFCutLabel = NULL, + const prog_char_t * dLabel = NULL) : AP_ControllerBlock(group, groupStart, 5), _blockP(group, groupStart, kP), _blockI(group, _blockP.getGroupEnd(), kI, iMax), _blockSaturation(group, _blockI.getGroupEnd(), yMax), - _kD(group, _blockSaturation.getGroupEnd(), kD, dLabel ? : PSTR("d")) { + _blockLowPass(group, _blockSaturation.getGroupEnd(), dFCut, + dFCutLabel ? : PSTR("dFCut")), + _kD(group, _blockLowPass.getGroupEnd(), kD, dLabel ? : PSTR("d")) + { } float update(const float & input, const float & derivative, const float & dt) { + float y = _blockP.update(input) + _blockI.update(input, dt) - _kD - * derivative; + * _blockLowPass.update(derivative,dt); return _blockSaturation.update(y); } protected: BlockP _blockP; BlockI _blockI; BlockSaturation _blockSaturation; + BlockLowPass _blockLowPass; AP_Float _kD; /// derivative gain }; diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp index 488b746610..def30d834c 100644 --- a/libraries/APO/AP_Guide.cpp +++ b/libraries/APO/AP_Guide.cpp @@ -6,3 +6,232 @@ */ #include "AP_Guide.h" +#include "../FastSerial/FastSerial.h" +#include "AP_Navigator.h" +#include "constants.h" +#include "AP_HardwareAbstractionLayer.h" +#include "AP_CommLink.h" + +namespace apo { + +AP_Guide::AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal) : + _navigator(navigator), _hal(hal), _command(AP_MavlinkCommand::home), + _previousCommand(AP_MavlinkCommand::home), + _headingCommand(0), _airSpeedCommand(0), + _groundSpeedCommand(0), _altitudeCommand(0), _pNCmd(0), + _pECmd(0), _pDCmd(0), _mode(MAV_NAV_LOST), + _numberOfCommands(1), _cmdIndex(0), _nextCommandCalls(0), + _nextCommandTimer(0) { +} + +void AP_Guide::setCurrentIndex(uint8_t val){ + _cmdIndex.set_and_save(val); + _command = AP_MavlinkCommand(getCurrentIndex()); + _previousCommand = AP_MavlinkCommand(getPreviousIndex()); + _hal->gcs->sendMessage(MAVLINK_MSG_ID_WAYPOINT_CURRENT); +} + +MavlinkGuide::MavlinkGuide(AP_Navigator * navigator, + AP_HardwareAbstractionLayer * hal) : + AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(), + _rangeFinderLeft(), _rangeFinderRight(), + _group(k_guide, PSTR("guide_")), + _velocityCommand(&_group, 1, 1, PSTR("velCmd")), + _crossTrackGain(&_group, 2, 1, PSTR("xt")), + _crossTrackLim(&_group, 3, 90, PSTR("xtLim")) { + + for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) { + RangeFinder * rF = _hal->rangeFinders[i]; + if (rF == NULL) + continue; + if (rF->orientation_x == 1 && rF->orientation_y == 0 + && rF->orientation_z == 0) + _rangeFinderFront = rF; + else if (rF->orientation_x == -1 && rF->orientation_y == 0 + && rF->orientation_z == 0) + _rangeFinderBack = rF; + else if (rF->orientation_x == 0 && rF->orientation_y == 1 + && rF->orientation_z == 0) + _rangeFinderRight = rF; + else if (rF->orientation_x == 0 && rF->orientation_y == -1 + && rF->orientation_z == 0) + _rangeFinderLeft = rF; + + } +} + +void MavlinkGuide::update() { + // process mavlink commands + handleCommand(); + + // obstacle avoidance overrides + // stop if your going to drive into something in front of you + for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) + _hal->rangeFinders[i]->read(); + float frontDistance = _rangeFinderFront->distance / 200.0; //convert for other adc + if (_rangeFinderFront && frontDistance < 2) { + _mode = MAV_NAV_VECTOR; + + //airSpeedCommand = 0; + //groundSpeedCommand = 0; +// _headingCommand -= 45 * deg2Rad; +// _hal->debug->print("Obstacle Distance (m): "); +// _hal->debug->println(frontDistance); +// _hal->debug->print("Obstacle avoidance Heading Command: "); +// _hal->debug->println(headingCommand); +// _hal->debug->printf_P( +// PSTR("Front Distance, %f\n"), +// frontDistance); + } + if (_rangeFinderBack && _rangeFinderBack->distance < 5) { + _airSpeedCommand = 0; + _groundSpeedCommand = 0; + + } + + if (_rangeFinderLeft && _rangeFinderLeft->distance < 5) { + _airSpeedCommand = 0; + _groundSpeedCommand = 0; + } + + if (_rangeFinderRight && _rangeFinderRight->distance < 5) { + _airSpeedCommand = 0; + _groundSpeedCommand = 0; + } +} + +void MavlinkGuide::nextCommand() { + // within 1 seconds, check if more than 5 calls to next command occur + // if they do, go to home waypoint + if (millis() - _nextCommandTimer < 1000) { + if (_nextCommandCalls > 5) { + Serial.println("commands loading too fast, returning home"); + setCurrentIndex(0); + setNumberOfCommands(1); + _nextCommandCalls = 0; + _nextCommandTimer = millis(); + return; + } + _nextCommandCalls++; + } else { + _nextCommandTimer = millis(); + _nextCommandCalls = 0; + } + + _cmdIndex = getNextIndex(); + //Serial.print("cmd : "); Serial.println(int(_cmdIndex)); + //Serial.print("cmd prev : "); Serial.println(int(getPreviousIndex())); + //Serial.print("cmd num : "); Serial.println(int(getNumberOfCommands())); + _command = AP_MavlinkCommand(getCurrentIndex()); + _previousCommand = AP_MavlinkCommand(getPreviousIndex()); +} + +void MavlinkGuide::handleCommand() { + + // TODO handle more commands + switch (_command.getCommand()) { + + case MAV_CMD_NAV_WAYPOINT: { + + // if we don't have enough waypoint for cross track calcs + // go home + if (_numberOfCommands == 1) { + _mode = MAV_NAV_RETURNING; + _altitudeCommand = AP_MavlinkCommand::home.getAlt(); + _headingCommand = AP_MavlinkCommand::home.bearingTo( + _navigator->getLat_degInt(), _navigator->getLon_degInt()) + + 180 * deg2Rad; + if (_headingCommand > 360 * deg2Rad) + _headingCommand -= 360 * deg2Rad; + + //_hal->debug->printf_P(PSTR("going home: bearing: %f distance: %f\n"), + //headingCommand,AP_MavlinkCommand::home.distanceTo(_navigator->getLat_degInt(),_navigator->getLon_degInt())); + + // if we have 2 or more waypoints do x track navigation + } else { + _mode = MAV_NAV_WAYPOINT; + float alongTrack = _command.alongTrack(_previousCommand, + _navigator->getLat_degInt(), + _navigator->getLon_degInt()); + float distanceToNext = _command.distanceTo( + _navigator->getLat_degInt(), _navigator->getLon_degInt()); + float segmentLength = _previousCommand.distanceTo(_command); + if (distanceToNext < _command.getRadius() || alongTrack + > segmentLength) + { + Serial.println("waypoint reached"); + nextCommand(); + } + _altitudeCommand = _command.getAlt(); + float dXt = _command.crossTrack(_previousCommand, + _navigator->getLat_degInt(), + _navigator->getLon_degInt()); + float temp = dXt * _crossTrackGain * deg2Rad; // crosstrack gain, rad/m + if (temp > _crossTrackLim * deg2Rad) + temp = _crossTrackLim * deg2Rad; + if (temp < -_crossTrackLim * deg2Rad) + temp = -_crossTrackLim * deg2Rad; + float bearing = _previousCommand.bearingTo(_command); + _headingCommand = bearing - temp; + //_hal->debug->printf_P( + //PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\n"), + //bearing * rad2Deg, dXt, _headingCommand * rad2Deg, distanceToNext, alongTrack); + } + + _groundSpeedCommand = _velocityCommand; + + // calculate pN,pE,pD from home and gps coordinates + _pNCmd = _command.getPN(_navigator->getLat_degInt(), + _navigator->getLon_degInt()); + _pECmd = _command.getPE(_navigator->getLat_degInt(), + _navigator->getLon_degInt()); + _pDCmd = _command.getPD(_navigator->getAlt_intM()); + + // debug + //_hal->debug->printf_P( + //PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"), + //getNumberOfCommands(), + //getCurrentIndex(), + //getPreviousIndex()); + + break; + } +// case MAV_CMD_CONDITION_CHANGE_ALT: +// case MAV_CMD_CONDITION_DELAY: +// case MAV_CMD_CONDITION_DISTANCE: +// case MAV_CMD_CONDITION_LAST: +// case MAV_CMD_CONDITION_YAW: +// case MAV_CMD_DO_CHANGE_SPEED: +// case MAV_CMD_DO_CONTROL_VIDEO: +// case MAV_CMD_DO_JUMP: +// case MAV_CMD_DO_LAST: +// case MAV_CMD_DO_LAST: +// case MAV_CMD_DO_REPEAT_RELAY: +// case MAV_CMD_DO_REPEAT_SERVO: +// case MAV_CMD_DO_SET_HOME: +// case MAV_CMD_DO_SET_MODE: +// case MAV_CMD_DO_SET_PARAMETER: +// case MAV_CMD_DO_SET_RELAY: +// case MAV_CMD_DO_SET_SERVO: +// case MAV_CMD_PREFLIGHT_CALIBRATION: +// case MAV_CMD_PREFLIGHT_STORAGE: +// case MAV_CMD_NAV_LAND: +// case MAV_CMD_NAV_LAST: +// case MAV_CMD_NAV_LOITER_TIME: +// case MAV_CMD_NAV_LOITER_TURNS: +// case MAV_CMD_NAV_LOITER_UNLIM: +// case MAV_CMD_NAV_ORIENTATION_TARGET: +// case MAV_CMD_NAV_PATHPLANNING: +// case MAV_CMD_NAV_RETURN_TO_LAUNCH: +// case MAV_CMD_NAV_TAKEOFF: + default: + // unhandled command, skip + Serial.println("unhandled command"); + nextCommand(); + break; + } +} + +} // namespace apo + +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index 8f5b61929b..0944620247 100644 --- a/libraries/APO/AP_Guide.h +++ b/libraries/APO/AP_Guide.h @@ -19,18 +19,16 @@ #ifndef AP_Guide_H #define AP_Guide_H +#include #include "../GCS_MAVLink/GCS_MAVLink.h" -#include "AP_HardwareAbstractionLayer.h" -#include "AP_Navigator.h" -#include "../AP_Common/AP_Common.h" -#include "../AP_Common/AP_Vector.h" #include "AP_MavlinkCommand.h" -#include "constants.h" - -//#include "AP_CommLink.h" +#include "../AP_RangeFinder/AP_RangeFinder.h" namespace apo { +class AP_Navigator; +class AP_HardwareAbstractionLayer; + /// Guide class class AP_Guide { public: @@ -39,15 +37,7 @@ public: * This is the constructor, which requires a link to the navigator. * @param navigator This is the navigator pointer. */ - AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal) : - _navigator(navigator), _hal(hal), _command(AP_MavlinkCommand::home), - _previousCommand(AP_MavlinkCommand::home), - _headingCommand(0), _airSpeedCommand(0), - _groundSpeedCommand(0), _altitudeCommand(0), _pNCmd(0), - _pECmd(0), _pDCmd(0), _mode(MAV_NAV_LOST), - _numberOfCommands(1), _cmdIndex(0), _nextCommandCalls(0), - _nextCommandTimer(0) { - } + AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal); virtual void update() = 0; @@ -60,12 +50,7 @@ public: return _cmdIndex; } - void setCurrentIndex(uint8_t val) { - _cmdIndex.set_and_save(val); - _command = AP_MavlinkCommand(getCurrentIndex()); - _previousCommand = AP_MavlinkCommand(getPreviousIndex()); - //_hal->gcs->sendMessage(MAVLINK_MSG_ID_WAYPOINT_CURRENT); - } + void setCurrentIndex(uint8_t val); uint8_t getNumberOfCommands() { return _numberOfCommands; @@ -140,205 +125,11 @@ protected: class MavlinkGuide: public AP_Guide { public: MavlinkGuide(AP_Navigator * navigator, - AP_HardwareAbstractionLayer * hal) : - AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(), - _rangeFinderLeft(), _rangeFinderRight(), - _group(k_guide, PSTR("guide_")), - _velocityCommand(&_group, 1, 1, PSTR("velCmd")), - _crossTrackGain(&_group, 2, 1, PSTR("xt")), - _crossTrackLim(&_group, 3, 90, PSTR("xtLim")) { + AP_HardwareAbstractionLayer * hal); + virtual void update(); + void nextCommand(); + void handleCommand(); - for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) { - RangeFinder * rF = _hal->rangeFinders[i]; - if (rF == NULL) - continue; - if (rF->orientation_x == 1 && rF->orientation_y == 0 - && rF->orientation_z == 0) - _rangeFinderFront = rF; - else if (rF->orientation_x == -1 && rF->orientation_y == 0 - && rF->orientation_z == 0) - _rangeFinderBack = rF; - else if (rF->orientation_x == 0 && rF->orientation_y == 1 - && rF->orientation_z == 0) - _rangeFinderRight = rF; - else if (rF->orientation_x == 0 && rF->orientation_y == -1 - && rF->orientation_z == 0) - _rangeFinderLeft = rF; - - } - } - - virtual void update() { - // process mavlink commands - handleCommand(); - - // obstacle avoidance overrides - // stop if your going to drive into something in front of you - for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) - _hal->rangeFinders[i]->read(); - float frontDistance = _rangeFinderFront->distance / 200.0; //convert for other adc - if (_rangeFinderFront && frontDistance < 2) { - _mode = MAV_NAV_VECTOR; - - //airSpeedCommand = 0; - //groundSpeedCommand = 0; -// _headingCommand -= 45 * deg2Rad; -// _hal->debug->print("Obstacle Distance (m): "); -// _hal->debug->println(frontDistance); -// _hal->debug->print("Obstacle avoidance Heading Command: "); -// _hal->debug->println(headingCommand); -// _hal->debug->printf_P( -// PSTR("Front Distance, %f\n"), -// frontDistance); - } - if (_rangeFinderBack && _rangeFinderBack->distance < 5) { - _airSpeedCommand = 0; - _groundSpeedCommand = 0; - - } - - if (_rangeFinderLeft && _rangeFinderLeft->distance < 5) { - _airSpeedCommand = 0; - _groundSpeedCommand = 0; - } - - if (_rangeFinderRight && _rangeFinderRight->distance < 5) { - _airSpeedCommand = 0; - _groundSpeedCommand = 0; - } - } - - void nextCommand() { - // within 1 seconds, check if more than 5 calls to next command occur - // if they do, go to home waypoint - if (millis() - _nextCommandTimer < 1000) { - if (_nextCommandCalls > 5) { - Serial.println("commands loading too fast, returning home"); - setCurrentIndex(0); - setNumberOfCommands(1); - _nextCommandCalls = 0; - _nextCommandTimer = millis(); - return; - } - _nextCommandCalls++; - } else { - _nextCommandTimer = millis(); - _nextCommandCalls = 0; - } - - _cmdIndex = getNextIndex(); - //Serial.print("cmd : "); Serial.println(int(_cmdIndex)); - //Serial.print("cmd prev : "); Serial.println(int(getPreviousIndex())); - //Serial.print("cmd num : "); Serial.println(int(getNumberOfCommands())); - _command = AP_MavlinkCommand(getCurrentIndex()); - _previousCommand = AP_MavlinkCommand(getPreviousIndex()); - } - - void handleCommand() { - - // TODO handle more commands - switch (_command.getCommand()) { - - case MAV_CMD_NAV_WAYPOINT: { - - // if we don't have enough waypoint for cross track calcs - // go home - if (_numberOfCommands == 1) { - _mode = MAV_NAV_RETURNING; - _altitudeCommand = AP_MavlinkCommand::home.getAlt(); - _headingCommand = AP_MavlinkCommand::home.bearingTo( - _navigator->getLat_degInt(), _navigator->getLon_degInt()) - + 180 * deg2Rad; - if (_headingCommand > 360 * deg2Rad) - _headingCommand -= 360 * deg2Rad; - - //_hal->debug->printf_P(PSTR("going home: bearing: %f distance: %f\n"), - //headingCommand,AP_MavlinkCommand::home.distanceTo(_navigator->getLat_degInt(),_navigator->getLon_degInt())); - - // if we have 2 or more waypoints do x track navigation - } else { - _mode = MAV_NAV_WAYPOINT; - float alongTrack = _command.alongTrack(_previousCommand, - _navigator->getLat_degInt(), - _navigator->getLon_degInt()); - float distanceToNext = _command.distanceTo( - _navigator->getLat_degInt(), _navigator->getLon_degInt()); - float segmentLength = _previousCommand.distanceTo(_command); - if (distanceToNext < _command.getRadius() || alongTrack - > segmentLength) - { - Serial.println("waypoint reached"); - nextCommand(); - } - _altitudeCommand = _command.getAlt(); - float dXt = _command.crossTrack(_previousCommand, - _navigator->getLat_degInt(), - _navigator->getLon_degInt()); - float temp = dXt * _crossTrackGain * deg2Rad; // crosstrack gain, rad/m - if (temp > _crossTrackLim * deg2Rad) - temp = _crossTrackLim * deg2Rad; - if (temp < -_crossTrackLim * deg2Rad) - temp = -_crossTrackLim * deg2Rad; - float bearing = _previousCommand.bearingTo(_command); - _headingCommand = bearing - temp; - //_hal->debug->printf_P( - //PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\n"), - //bearing * rad2Deg, dXt, _headingCommand * rad2Deg, distanceToNext, alongTrack); - } - - _groundSpeedCommand = _velocityCommand; - - // calculate pN,pE,pD from home and gps coordinates - _pNCmd = _command.getPN(_navigator->getLat_degInt(), - _navigator->getLon_degInt()); - _pECmd = _command.getPE(_navigator->getLat_degInt(), - _navigator->getLon_degInt()); - _pDCmd = _command.getPD(_navigator->getAlt_intM()); - - // debug - //_hal->debug->printf_P( - //PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"), - //getNumberOfCommands(), - //getCurrentIndex(), - //getPreviousIndex()); - - break; - } -// case MAV_CMD_CONDITION_CHANGE_ALT: -// case MAV_CMD_CONDITION_DELAY: -// case MAV_CMD_CONDITION_DISTANCE: -// case MAV_CMD_CONDITION_LAST: -// case MAV_CMD_CONDITION_YAW: -// case MAV_CMD_DO_CHANGE_SPEED: -// case MAV_CMD_DO_CONTROL_VIDEO: -// case MAV_CMD_DO_JUMP: -// case MAV_CMD_DO_LAST: -// case MAV_CMD_DO_LAST: -// case MAV_CMD_DO_REPEAT_RELAY: -// case MAV_CMD_DO_REPEAT_SERVO: -// case MAV_CMD_DO_SET_HOME: -// case MAV_CMD_DO_SET_MODE: -// case MAV_CMD_DO_SET_PARAMETER: -// case MAV_CMD_DO_SET_RELAY: -// case MAV_CMD_DO_SET_SERVO: -// case MAV_CMD_PREFLIGHT_CALIBRATION: -// case MAV_CMD_PREFLIGHT_STORAGE: -// case MAV_CMD_NAV_LAND: -// case MAV_CMD_NAV_LAST: -// case MAV_CMD_NAV_LOITER_TIME: -// case MAV_CMD_NAV_LOITER_TURNS: -// case MAV_CMD_NAV_LOITER_UNLIM: -// case MAV_CMD_NAV_ORIENTATION_TARGET: -// case MAV_CMD_NAV_PATHPLANNING: -// case MAV_CMD_NAV_RETURN_TO_LAUNCH: -// case MAV_CMD_NAV_TAKEOFF: - default: - // unhandled command, skip - Serial.println("unhandled command"); - nextCommand(); - break; - } - } private: RangeFinder * _rangeFinderFront; RangeFinder * _rangeFinderBack; diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_HardwareAbstractionLayer.h index 45e7daaf75..422bb55d36 100644 --- a/libraries/APO/AP_HardwareAbstractionLayer.h +++ b/libraries/APO/AP_HardwareAbstractionLayer.h @@ -8,20 +8,9 @@ #ifndef AP_HARDWAREABSTRACTIONLAYER_H_ #define AP_HARDWAREABSTRACTIONLAYER_H_ -#include "../AP_Common/AP_Common.h" -#include "../FastSerial/FastSerial.h" #include "../AP_Common/AP_Vector.h" #include "../GCS_MAVLink/GCS_MAVLink.h" -#include "../AP_ADC/AP_ADC.h" -#include "../AP_IMU/AP_IMU.h" -#include "../AP_GPS/GPS.h" -#include "../APM_BMP085/APM_BMP085.h" -#include "../AP_Compass/AP_Compass.h" -#include "AP_RcChannel.h" -#include "../AP_RangeFinder/AP_RangeFinder.h" -#include "../GCS_MAVLink/GCS_MAVLink.h" - class AP_ADC; class IMU; class GPS; @@ -29,11 +18,13 @@ class APM_BMP085_Class; class Compass; class BetterStream; class RangeFinder; +class FastSerial; namespace apo { class AP_RcChannel; class AP_CommLink; +class AP_BatteryMonitor; // enumerations enum halMode_t { @@ -50,9 +41,12 @@ class AP_HardwareAbstractionLayer { public: + // default ctors on pointers called on pointers here, this + // allows NULL to be used as a boolean for if the device was + // initialized AP_HardwareAbstractionLayer(halMode_t mode, board_t board, vehicle_t vehicle, uint8_t heartBeatTimeout) : - adc(), gps(), baro(), compass(), rangeFinders(), imu(), rc(), gcs(), + adc(), gps(), baro(), compass(), rangeFinders(), imu(), batteryMonitor(), rc(), gcs(), hil(), debug(), load(), lastHeartBeat(), _heartBeatTimeout(heartBeatTimeout), _mode(mode), _board(board), _vehicle(vehicle), _state(MAV_STATE_UNINIT) { @@ -98,6 +92,7 @@ public: Compass * compass; Vector rangeFinders; IMU * imu; + AP_BatteryMonitor * batteryMonitor; /** * Radio Channels diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp index ff33e8ed2c..46e74d4d87 100644 --- a/libraries/APO/AP_Navigator.cpp +++ b/libraries/APO/AP_Navigator.cpp @@ -6,3 +6,189 @@ */ #include "AP_Navigator.h" +#include "../FastSerial/FastSerial.h" +#include "AP_HardwareAbstractionLayer.h" +#include "../AP_DCM/AP_DCM.h" +#include "../AP_Math/AP_Math.h" +#include "../AP_Compass/AP_Compass.h" +#include "AP_MavlinkCommand.h" +#include "AP_Var_keys.h" +#include "../AP_RangeFinder/AP_RangeFinder.h" +#include "../AP_IMU/AP_IMU.h" +#include "../APM_BMP085/APM_BMP085.h" + +namespace apo { + +AP_Navigator::AP_Navigator(AP_HardwareAbstractionLayer * hal) : + _hal(hal), _timeStamp(0), _roll(0), _rollRate(0), _pitch(0), + _pitchRate(0), _yaw(0), _yawRate(0), _airSpeed(0), + _groundSpeed(0), _vD(0), _lat_degInt(0), + _lon_degInt(0), _alt_intM(0) { +} +void AP_Navigator::calibrate() { +} +float AP_Navigator::getPD() const { + return AP_MavlinkCommand::home.getPD(getAlt_intM()); +} + +float AP_Navigator::getPE() const { + return AP_MavlinkCommand::home.getPE(getLat_degInt(), getLon_degInt()); +} + +float AP_Navigator::getPN() const { + return AP_MavlinkCommand::home.getPN(getLat_degInt(), getLon_degInt()); +} + +void AP_Navigator::setPD(float _pD) { + setAlt(AP_MavlinkCommand::home.getAlt(_pD)); +} + +void AP_Navigator::setPE(float _pE) { + setLat(AP_MavlinkCommand::home.getLat(_pE)); +} + +void AP_Navigator::setPN(float _pN) { + setLon(AP_MavlinkCommand::home.getLon(_pN)); +} + +DcmNavigator::DcmNavigator(AP_HardwareAbstractionLayer * hal) : + AP_Navigator(hal), _dcm(), _imuOffsetAddress(0) { + + // if orientation equal to front, store as front + /** + * rangeFinder is assigned values based on orientation which + * is specified in ArduPilotOne.pde. + */ + for (uint8_t i = 0; i < _hal-> rangeFinders.getSize(); i++) { + if (_hal->rangeFinders[i] == NULL) + continue; + if (_hal->rangeFinders[i]->orientation_x == 0 + && _hal->rangeFinders[i]->orientation_y == 0 + && _hal->rangeFinders[i]->orientation_z == 1) + _rangeFinderDown = _hal->rangeFinders[i]; + } + + if (_hal->getMode() == MODE_LIVE) { + if (_hal->adc) + _hal->imu = new AP_IMU_Oilpan(_hal->adc, k_sensorCalib); + if (_hal->imu) + _dcm = new AP_DCM(_hal->imu, _hal->gps, _hal->compass); + if (_hal->compass) { + _dcm->set_compass(_hal->compass); + + } + } +} +void DcmNavigator::calibrate() { + + AP_Navigator::calibrate(); + + // TODO: handle cold/warm restart + if (_hal->imu) { + _hal->imu->init(IMU::COLD_START,delay); + } +} +void DcmNavigator::updateFast(float dt) { + + if (_hal->getMode() != MODE_LIVE) + return; + + setTimeStamp(micros()); // if running in live mode, record new time stamp + + + //_hal->debug->println_P(PSTR("nav loop")); + + /** + * The altitued is read off the barometer by implementing the following formula: + * altitude (in m) = 44330*(1-(p/po)^(1/5.255)), + * where, po is pressure in Pa at sea level (101325 Pa). + * See http://www.sparkfun.com/tutorials/253 or type this formula + * in a search engine for more information. + * altInt contains the altitude in meters. + */ + if (_hal->baro) { + + if (_rangeFinderDown != NULL && _rangeFinderDown->distance <= 695) + setAlt(_rangeFinderDown->distance); + + else { + float tmp = (_hal->baro->Press / 101325.0); + tmp = pow(tmp, 0.190295); + //setAlt(44330 * (1.0 - tmp)); //sets the altitude in meters XXX wrong, baro reads 0 press + setAlt(0.0); + } + } + + // dcm class for attitude + if (_dcm) { + _dcm->update_DCM(); + setRoll(_dcm->roll); + setPitch(_dcm->pitch); + setYaw(_dcm->yaw); + setRollRate(_dcm->get_gyro().x); + setPitchRate(_dcm->get_gyro().y); + setYawRate(_dcm->get_gyro().z); + + /* + * accel/gyro debug + */ + /* + Vector3f accel = _hal->imu->get_accel(); + Vector3f gyro = _hal->imu->get_gyro(); + Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"), + accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z); + */ + } +} +void DcmNavigator::updateSlow(float dt) { + if (_hal->getMode() != MODE_LIVE) + return; + + setTimeStamp(micros()); // if running in live mode, record new time stamp + + if (_hal->gps) { + _hal->gps->update(); + updateGpsLight(); + if (_hal->gps->fix && _hal->gps->new_data) { + setLat_degInt(_hal->gps->latitude); + setLon_degInt(_hal->gps->longitude); + setAlt_intM(_hal->gps->altitude * 10); // gps in cm, intM in mm + setGroundSpeed(_hal->gps->ground_speed / 100.0); // gps is in cm/s + } + } + + if (_hal->compass) { + _hal->compass->read(); + _hal->compass->calculate(_dcm->get_dcm_matrix()); + _hal->compass->null_offsets(_dcm->get_dcm_matrix()); + //_hal->debug->printf_P(PSTR("heading: %f"), _hal->compass->heading); + } +} +void DcmNavigator::updateGpsLight(void) { + // GPS LED on if we have a fix or Blink GPS LED if we are receiving data + // --------------------------------------------------------------------- + static bool GPS_light = false; + switch (_hal->gps->status()) { + case (2): + //digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. + break; + + case (1): + if (_hal->gps->valid_read == true) { + GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock + if (GPS_light) { + digitalWrite(_hal->cLedPin, LOW); + } else { + digitalWrite(_hal->cLedPin, HIGH); + } + _hal->gps->valid_read = false; + } + break; + + default: + digitalWrite(_hal->cLedPin, LOW); + break; + } +} + +} // namespace apo diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h index 8330112541..9be22e6caf 100644 --- a/libraries/APO/AP_Navigator.h +++ b/libraries/APO/AP_Navigator.h @@ -19,54 +19,30 @@ #ifndef AP_Navigator_H #define AP_Navigator_H -#include "AP_HardwareAbstractionLayer.h" -#include "../AP_DCM/AP_DCM.h" -#include "../AP_Math/AP_Math.h" -#include "../AP_Compass/AP_Compass.h" -#include "AP_MavlinkCommand.h" #include "constants.h" -#include "AP_Var_keys.h" -#include "../AP_RangeFinder/AP_RangeFinder.h" -#include "../AP_IMU/AP_IMU.h" +#include + +class RangeFinder; +class IMU; +class AP_DCM; namespace apo { +class AP_HardwareAbstractionLayer; + /// Navigator class class AP_Navigator { public: - AP_Navigator(AP_HardwareAbstractionLayer * hal) : - _hal(hal), _timeStamp(0), _roll(0), _rollRate(0), _pitch(0), - _pitchRate(0), _yaw(0), _yawRate(0), _airSpeed(0), - _groundSpeed(0), _vD(0), _lat_degInt(0), - _lon_degInt(0), _alt_intM(0) { - } - virtual void calibrate() { - } + AP_Navigator(AP_HardwareAbstractionLayer * hal); + virtual void calibrate(); virtual void updateFast(float dt) = 0; virtual void updateSlow(float dt) = 0; - float getPD() const { - return AP_MavlinkCommand::home.getPD(getAlt_intM()); - } - - float getPE() const { - return AP_MavlinkCommand::home.getPE(getLat_degInt(), getLon_degInt()); - } - - float getPN() const { - return AP_MavlinkCommand::home.getPN(getLat_degInt(), getLon_degInt()); - } - - void setPD(float _pD) { - setAlt(AP_MavlinkCommand::home.getAlt(_pD)); - } - - void setPE(float _pE) { - setLat(AP_MavlinkCommand::home.getLat(_pE)); - } - - void setPN(float _pN) { - setLon(AP_MavlinkCommand::home.getLon(_pN)); - } + float getPD() const; + float getPE() const; + float getPN() const; + void setPD(float _pD); + void setPE(float _pE); + void setPN(float _pN); float getAirSpeed() const { return _airSpeed; @@ -241,146 +217,11 @@ private: uint16_t _imuOffsetAddress; public: - DcmNavigator(AP_HardwareAbstractionLayer * hal) : - AP_Navigator(hal), _dcm(), _imuOffsetAddress(0) { - - // if orientation equal to front, store as front - /** - * rangeFinder is assigned values based on orientation which - * is specified in ArduPilotOne.pde. - */ - for (uint8_t i = 0; i < _hal-> rangeFinders.getSize(); i++) { - if (_hal->rangeFinders[i] == NULL) - continue; - if (_hal->rangeFinders[i]->orientation_x == 0 - && _hal->rangeFinders[i]->orientation_y == 0 - && _hal->rangeFinders[i]->orientation_z == 1) - _rangeFinderDown = _hal->rangeFinders[i]; - } - - if (_hal->getMode() == MODE_LIVE) { - if (_hal->adc) - _hal->imu = new AP_IMU_Oilpan(_hal->adc, k_sensorCalib); - if (_hal->imu) - _dcm = new AP_DCM(_hal->imu, _hal->gps, _hal->compass); - if (_hal->compass) { - _dcm->set_compass(_hal->compass); - - } - } - } - virtual void calibrate() { - - AP_Navigator::calibrate(); - - // TODO: handle cold/warm restart - if (_hal->imu) { - _hal->imu->init(IMU::COLD_START,delay); - } - } - virtual void updateFast(float dt) { - - if (_hal->getMode() != MODE_LIVE) - return; - - setTimeStamp(micros()); // if running in live mode, record new time stamp - - - //_hal->debug->println_P(PSTR("nav loop")); - - /** - * The altitued is read off the barometer by implementing the following formula: - * altitude (in m) = 44330*(1-(p/po)^(1/5.255)), - * where, po is pressure in Pa at sea level (101325 Pa). - * See http://www.sparkfun.com/tutorials/253 or type this formula - * in a search engine for more information. - * altInt contains the altitude in meters. - */ - if (_hal->baro) { - - if (_rangeFinderDown != NULL && _rangeFinderDown->distance <= 695) - setAlt(_rangeFinderDown->distance); - - else { - float tmp = (_hal->baro->Press / 101325.0); - tmp = pow(tmp, 0.190295); - //setAlt(44330 * (1.0 - tmp)); //sets the altitude in meters XXX wrong, baro reads 0 press - setAlt(0.0); - } - } - - // dcm class for attitude - if (_dcm) { - _dcm->update_DCM(); - setRoll(_dcm->roll); - setPitch(_dcm->pitch); - setYaw(_dcm->yaw); - setRollRate(_dcm->get_gyro().x); - setPitchRate(_dcm->get_gyro().y); - setYawRate(_dcm->get_gyro().z); - - /* - * accel/gyro debug - */ - /* - Vector3f accel = _hal->imu->get_accel(); - Vector3f gyro = _hal->imu->get_gyro(); - Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"), - accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z); - */ - } - } - virtual void updateSlow(float dt) { - if (_hal->getMode() != MODE_LIVE) - return; - - setTimeStamp(micros()); // if running in live mode, record new time stamp - - if (_hal->gps) { - _hal->gps->update(); - updateGpsLight(); - if (_hal->gps->fix && _hal->gps->new_data) { - setLat_degInt(_hal->gps->latitude); - setLon_degInt(_hal->gps->longitude); - setAlt_intM(_hal->gps->altitude * 10); // gps in cm, intM in mm - setGroundSpeed(_hal->gps->ground_speed / 100.0); // gps is in cm/s - } - } - - if (_hal->compass) { - _hal->compass->read(); - _hal->compass->calculate(_dcm->get_dcm_matrix()); - _hal->compass->null_offsets(_dcm->get_dcm_matrix()); - //_hal->debug->printf_P(PSTR("heading: %f"), _hal->compass->heading); - } - } - void updateGpsLight(void) { - // GPS LED on if we have a fix or Blink GPS LED if we are receiving data - // --------------------------------------------------------------------- - static bool GPS_light = false; - switch (_hal->gps->status()) { - case (2): - //digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. - break; - - case (1): - if (_hal->gps->valid_read == true) { - GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock - if (GPS_light) { - digitalWrite(_hal->cLedPin, LOW); - } else { - digitalWrite(_hal->cLedPin, HIGH); - } - _hal->gps->valid_read = false; - } - break; - - default: - digitalWrite(_hal->cLedPin, LOW); - break; - } - } - + DcmNavigator(AP_HardwareAbstractionLayer * hal); + virtual void calibrate(); + virtual void updateFast(float dt); + virtual void updateSlow(float dt); + void updateGpsLight(void); }; } // namespace apo diff --git a/libraries/AP_Common/AP_Loop.h b/libraries/AP_Common/AP_Loop.h index ef2ebe25c6..313ce43e79 100644 --- a/libraries/AP_Common/AP_Loop.h +++ b/libraries/AP_Common/AP_Loop.h @@ -30,7 +30,7 @@ public: Vector & subLoops() { return _subLoops; } - uint32_t frequency() { + float frequency() { return 1.0e6/_period; } void frequency(float _frequency) { @@ -45,7 +45,7 @@ public: uint8_t load() { return _load; } -private: +protected: void (*_fptr)(void *); void * _data; uint32_t _period; From 0a0bbb8afe3c153ac97017a35fc290faaff657d1 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Oct 2011 03:15:22 -0400 Subject: [PATCH 2/3] Updated ArduRover/ ArduBoat for new apo changes. Still need to add arming/ disarming to rover/boat controllers. --- ArduBoat/BoatGeneric.h | 50 +++++++++++-------- ArduBoat/ControllerBoat.h | 2 +- ArduRover/ArduRover.cpp | 2 + ArduRover/ArduRover.pde | 4 +- ArduRover/CarStampede.h | 48 ++++++++++++------- ArduRover/ControllerCar.h | 2 +- ArduRover/ControllerTank.h | 2 +- ArduRover/TankGeneric.h | 48 ++++++++++++------- apo/PlaneEasystar.h | 53 ++++++++++++--------- apo/QuadMikrokopter.h | 98 -------------------------------------- apo/apo.pde | 1 + libraries/APO/APO.h | 1 + 12 files changed, 132 insertions(+), 179 deletions(-) delete mode 100644 apo/QuadMikrokopter.h diff --git a/ArduBoat/BoatGeneric.h b/ArduBoat/BoatGeneric.h index 5e47672847..46c4de4d4f 100644 --- a/ArduBoat/BoatGeneric.h +++ b/ArduBoat/BoatGeneric.h @@ -11,7 +11,7 @@ // vehicle options static const apo::vehicle_t vehicle = apo::VEHICLE_BOAT; static const apo::halMode_t halMode = apo::MODE_LIVE; -static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560; +static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; static const uint8_t heartBeatTimeout = 3; // algorithm selection @@ -25,29 +25,40 @@ static const uint8_t heartBeatTimeout = 3; #define COMPASS_CLASS AP_Compass_HMC5843 #define BARO_CLASS APM_BMP085_Class #define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL -#define DEBUG_BAUD 57600 -#define TELEM_BAUD 57600 -#define GPS_BAUD 38400 -#define HIL_BAUD 57600 + +// baud rates +static uint32_t debugBaud = 57600; +static uint32_t telemBaud = 57600; +static uint32_t gpsBaud = 38400; +static uint32_t hilBaud = 57600; // optional sensors -static bool gpsEnabled = false; -static bool baroEnabled = true; -static bool compassEnabled = true; +static const bool gpsEnabled = false; +static const bool baroEnabled = false; +static const bool compassEnabled = true; +static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; +// compass orientation: See AP_Compass_HMC5843.h for possible values -static bool rangeFinderFrontEnabled = true; -static bool rangeFinderBackEnabled = true; -static bool rangeFinderLeftEnabled = true; -static bool rangeFinderRightEnabled = true; -static bool rangeFinderUpEnabled = true; -static bool rangeFinderDownEnabled = true; +// battery monitoring +static const bool batteryMonitorEnabled = true; +static const uint8_t batteryPin = 0; +static const float batteryVoltageDivRatio = 6; +static const float batteryMinVolt = 10.0; +static const float batteryMaxVolt = 12.4; + +static const bool rangeFinderFrontEnabled = false; +static const bool rangeFinderBackEnabled = false; +static const bool rangeFinderLeftEnabled = false; +static const bool rangeFinderRightEnabled = false; +static const bool rangeFinderUpEnabled = false; +static const bool rangeFinderDownEnabled = false; // loop rates -static const float loop0Rate = 150; -static const float loop1Rate = 100; -static const float loop2Rate = 10; -static const float loop3Rate = 1; -static const float loop4Rate = 0.1; +static const float loopRate = 150; // attitude nav +static const float loop0Rate = 50; // controller +static const float loop1Rate = 5; // pos nav/ gcs fast +static const float loop2Rate = 1; // gcs slow +static const float loop3Rate = 0.1; // gains const float steeringP = 1.0; @@ -55,6 +66,7 @@ const float steeringI = 0.0; const float steeringD = 0.0; const float steeringIMax = 0.0; const float steeringYMax = 3.0; +const float steeringDFCut = 25; const float throttleP = 0.0; const float throttleI = 0.0; diff --git a/ArduBoat/ControllerBoat.h b/ArduBoat/ControllerBoat.h index 4999569acb..c0ac2c7c6f 100644 --- a/ArduBoat/ControllerBoat.h +++ b/ArduBoat/ControllerBoat.h @@ -34,7 +34,7 @@ public: _group(k_cntrl, PSTR("CNTRL_")), _mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")), pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, - steeringI, steeringD, steeringIMax, steeringYMax), + steeringI, steeringD, steeringIMax, steeringYMax, steeringDFCut), pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP, throttleI, throttleD, throttleIMax, throttleYMax, throttleDFCut) { diff --git a/ArduRover/ArduRover.cpp b/ArduRover/ArduRover.cpp index 57f0903250..4d4a29a733 100644 --- a/ArduRover/ArduRover.cpp +++ b/ArduRover/ArduRover.cpp @@ -1,4 +1,5 @@ // Libraries +#include #include #include #include @@ -13,6 +14,7 @@ #include #include #include +#include // Vehicle Configuration #include "CarStampede.h" diff --git a/ArduRover/ArduRover.pde b/ArduRover/ArduRover.pde index cc1a79aa0c..14846ca904 100644 --- a/ArduRover/ArduRover.pde +++ b/ArduRover/ArduRover.pde @@ -1,4 +1,5 @@ // Libraries +#include #include #include #include @@ -13,9 +14,10 @@ #include #include #include +#include // Vehicle Configuration -#include "TankGeneric.h" +#include "CarStampede.h" // ArduPilotOne Default Setup #include "APO_DefaultSetup.h" diff --git a/ArduRover/CarStampede.h b/ArduRover/CarStampede.h index 07a4fe20e4..dc67d6730a 100644 --- a/ArduRover/CarStampede.h +++ b/ArduRover/CarStampede.h @@ -26,29 +26,40 @@ static const uint8_t heartBeatTimeout = 3; #define COMPASS_CLASS AP_Compass_HMC5843 #define BARO_CLASS APM_BMP085_Class #define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL -#define DEBUG_BAUD 57600 -#define TELEM_BAUD 57600 -#define GPS_BAUD 38400 -#define HIL_BAUD 57600 + +// baud rates +static uint32_t debugBaud = 57600; +static uint32_t telemBaud = 57600; +static uint32_t gpsBaud = 38400; +static uint32_t hilBaud = 57600; // optional sensors -static bool gpsEnabled = false; -static bool baroEnabled = true; -static bool compassEnabled = true; +static const bool gpsEnabled = false; +static const bool baroEnabled = false; +static const bool compassEnabled = true; +static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; +// compass orientation: See AP_Compass_HMC5843.h for possible values -static bool rangeFinderFrontEnabled = true; -static bool rangeFinderBackEnabled = true; -static bool rangeFinderLeftEnabled = true; -static bool rangeFinderRightEnabled = true; -static bool rangeFinderUpEnabled = true; -static bool rangeFinderDownEnabled = true; +// battery monitoring +static const bool batteryMonitorEnabled = true; +static const uint8_t batteryPin = 0; +static const float batteryVoltageDivRatio = 6; +static const float batteryMinVolt = 10.0; +static const float batteryMaxVolt = 12.4; + +static const bool rangeFinderFrontEnabled = false; +static const bool rangeFinderBackEnabled = false; +static const bool rangeFinderLeftEnabled = false; +static const bool rangeFinderRightEnabled = false; +static const bool rangeFinderUpEnabled = false; +static const bool rangeFinderDownEnabled = false; // loop rates -static const float loop0Rate = 150; -static const float loop1Rate = 100; -static const float loop2Rate = 10; -static const float loop3Rate = 1; -static const float loop4Rate = 0.1; +static const float loopRate = 150; // attitude nav +static const float loop0Rate = 50; // controller +static const float loop1Rate = 5; // pos nav/ gcs fast +static const float loop2Rate = 1; // gcs slow +static const float loop3Rate = 0.1; // gains static const float steeringP = 1.0; @@ -56,6 +67,7 @@ static const float steeringI = 0.0; static const float steeringD = 0.0; static const float steeringIMax = 0.0; static const float steeringYMax = 3.0; +static const float steeringDFCut = 25; static const float throttleP = 0.0; static const float throttleI = 0.0; diff --git a/ArduRover/ControllerCar.h b/ArduRover/ControllerCar.h index 8bb7e9afb5..686ea64787 100644 --- a/ArduRover/ControllerCar.h +++ b/ArduRover/ControllerCar.h @@ -34,7 +34,7 @@ public: _group(k_cntrl, PSTR("CNTRL_")), _mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")), pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, - steeringI, steeringD, steeringIMax, steeringYMax), + steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut), pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP, throttleI, throttleD, throttleIMax, throttleYMax, throttleDFCut) { diff --git a/ArduRover/ControllerTank.h b/ArduRover/ControllerTank.h index ad8a294a2c..2e0958569c 100644 --- a/ArduRover/ControllerTank.h +++ b/ArduRover/ControllerTank.h @@ -34,7 +34,7 @@ public: _group(k_cntrl, PSTR("CNTRL_")), _mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")), pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, - steeringI, steeringD, steeringIMax, steeringYMax), + steeringI, steeringD, steeringIMax, steeringYMax, steeringDFCut), pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP, throttleI, throttleD, throttleIMax, throttleYMax, throttleDFCut) { diff --git a/ArduRover/TankGeneric.h b/ArduRover/TankGeneric.h index 9af4f9b1d4..10e5af72a3 100644 --- a/ArduRover/TankGeneric.h +++ b/ArduRover/TankGeneric.h @@ -25,29 +25,40 @@ static const uint8_t heartBeatTimeout = 3; #define COMPASS_CLASS AP_Compass_HMC5843 #define BARO_CLASS APM_BMP085_Class #define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL -#define DEBUG_BAUD 57600 -#define TELEM_BAUD 57600 -#define GPS_BAUD 38400 -#define HIL_BAUD 57600 + +// baud rates +static uint32_t debugBaud = 57600; +static uint32_t telemBaud = 57600; +static uint32_t gpsBaud = 38400; +static uint32_t hilBaud = 57600; // optional sensors -static bool gpsEnabled = false; -static bool baroEnabled = false; -static bool compassEnabled = false; +static const bool gpsEnabled = false; +static const bool baroEnabled = false; +static const bool compassEnabled = true; +static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; +// compass orientation: See AP_Compass_HMC5843.h for possible values -static bool rangeFinderFrontEnabled = false; -static bool rangeFinderBackEnabled = false; -static bool rangeFinderLeftEnabled = false; -static bool rangeFinderRightEnabled = false; -static bool rangeFinderUpEnabled = false; -static bool rangeFinderDownEnabled = false; +// battery monitoring +static const bool batteryMonitorEnabled = true; +static const uint8_t batteryPin = 0; +static const float batteryVoltageDivRatio = 6; +static const float batteryMinVolt = 10.0; +static const float batteryMaxVolt = 12.4; + +static const bool rangeFinderFrontEnabled = false; +static const bool rangeFinderBackEnabled = false; +static const bool rangeFinderLeftEnabled = false; +static const bool rangeFinderRightEnabled = false; +static const bool rangeFinderUpEnabled = false; +static const bool rangeFinderDownEnabled = false; // loop rates -static const float loop0Rate = 150; -static const float loop1Rate = 100; -static const float loop2Rate = 10; -static const float loop3Rate = 1; -static const float loop4Rate = 0.1; +static const float loopRate = 150; // attitude nav +static const float loop0Rate = 50; // controller +static const float loop1Rate = 5; // pos nav/ gcs fast +static const float loop2Rate = 1; // gcs slow +static const float loop3Rate = 0.1; // gains const float steeringP = 1.0; @@ -55,6 +66,7 @@ const float steeringI = 0.0; const float steeringD = 0.0; const float steeringIMax = 0.0; const float steeringYMax = 3.0; +const float steeringDFCut = 25; const float throttleP = 0.0; const float throttleI = 0.0; diff --git a/apo/PlaneEasystar.h b/apo/PlaneEasystar.h index 812c4ad449..87649f48a3 100644 --- a/apo/PlaneEasystar.h +++ b/apo/PlaneEasystar.h @@ -8,12 +8,10 @@ #ifndef PLANEEASYSTAR_H_ #define PLANEEASYSTAR_H_ - // vehicle options static const apo::vehicle_t vehicle = apo::VEHICLE_PLANE; -//static const apo::halMode_t halMode = apo::MODE_LIVE; // live mode, actual flight -static const apo::halMode_t halMode = apo::MODE_HIL_CNTL; // hardware in the loop, control level -static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; +static const apo::halMode_t halMode = apo::MODE_LIVE; +static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560; static const uint8_t heartBeatTimeout = 3; // algorithm selection @@ -27,29 +25,40 @@ static const uint8_t heartBeatTimeout = 3; #define COMPASS_CLASS AP_Compass_HMC5843 #define BARO_CLASS APM_BMP085_Class #define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL -#define DEBUG_BAUD 57600 -#define TELEM_BAUD 57600 -#define GPS_BAUD 38400 -#define HIL_BAUD 57600 + +// baud rates +static uint32_t debugBaud = 57600; +static uint32_t telemBaud = 57600; +static uint32_t gpsBaud = 38400; +static uint32_t hilBaud = 57600; // optional sensors -static bool gpsEnabled = false; -static bool baroEnabled = true; -static bool compassEnabled = true; +static const bool gpsEnabled = false; +static const bool baroEnabled = false; +static const bool compassEnabled = true; +static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; +// compass orientation: See AP_Compass_HMC5843.h for possible values -static bool rangeFinderFrontEnabled = true; -static bool rangeFinderBackEnabled = true; -static bool rangeFinderLeftEnabled = true; -static bool rangeFinderRightEnabled = true; -static bool rangeFinderUpEnabled = true; -static bool rangeFinderDownEnabled = true; +// battery monitoring +static const bool batteryMonitorEnabled = true; +static const uint8_t batteryPin = 0; +static const float batteryVoltageDivRatio = 6; +static const float batteryMinVolt = 10.0; +static const float batteryMaxVolt = 12.4; + +static const bool rangeFinderFrontEnabled = false; +static const bool rangeFinderBackEnabled = false; +static const bool rangeFinderLeftEnabled = false; +static const bool rangeFinderRightEnabled = false; +static const bool rangeFinderUpEnabled = false; +static const bool rangeFinderDownEnabled = false; // loop rates -static const float loop0Rate = 150; -static const float loop1Rate = 100; -static const float loop2Rate = 10; -static const float loop3Rate = 1; -static const float loop4Rate = 0.1; +static const float loopRate = 150; // attitude nav +static const float loop0Rate = 50; // controller +static const float loop1Rate = 5; // pos nav/ gcs fast +static const float loop2Rate = 1; // gcs slow +static const float loop3Rate = 0.1; // gains static const float rdrAilMix = 1.0; // since there are no ailerons diff --git a/apo/QuadMikrokopter.h b/apo/QuadMikrokopter.h deleted file mode 100644 index 7aeeacd5e6..0000000000 --- a/apo/QuadMikrokopter.h +++ /dev/null @@ -1,98 +0,0 @@ -/* - * QuadMikrokopter - * - * Created on: May 1, 2011 - * Author: jgoppert - */ - -#ifndef QUADMIKROKOPTER_H_ -#define QUADMIKROKOPTER_H_ - -// vehicle options -static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD; -static const apo::halMode_t halMode = apo::MODE_LIVE; -static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560; -static const uint8_t heartBeatTimeout = 3; - -// algorithm selection -#define CONTROLLER_CLASS ControllerQuad -#define GUIDE_CLASS MavlinkGuide -#define NAVIGATOR_CLASS DcmNavigator -#define COMMLINK_CLASS MavlinkComm - -// hardware selection -#define ADC_CLASS AP_ADC_ADS7844 -#define COMPASS_CLASS AP_Compass_HMC5843 -#define BARO_CLASS APM_BMP085_Class -#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL -#define DEBUG_BAUD 57600 -#define TELEM_BAUD 57600 -#define GPS_BAUD 38400 -#define HIL_BAUD 57600 - -// optional sensors -static bool gpsEnabled = false; -static bool baroEnabled = true; -static bool compassEnabled = true; - -static bool rangeFinderFrontEnabled = true; -static bool rangeFinderBackEnabled = true; -static bool rangeFinderLeftEnabled = true; -static bool rangeFinderRightEnabled = true; -static bool rangeFinderUpEnabled = true; -static bool rangeFinderDownEnabled = true; - -// loop rates -static const float loop0Rate = 150; -static const float loop1Rate = 100; -static const float loop2Rate = 10; -static const float loop3Rate = 1; -static const float loop4Rate = 0.1; - -//motor parameters -static const float MOTOR_MAX = 1; -static const float MOTOR_MIN = 0.1; - -// position control loop -static const float PID_POS_INTERVAL = 1 / 100; // 5 hz -static const float PID_POS_P = 0; -static const float PID_POS_I = 0; -static const float PID_POS_D = 0; -static const float PID_POS_LIM = 0; // about 5 deg -static const float PID_POS_AWU = 0; // about 5 deg -static const float PID_POS_Z_P = 0; -static const float PID_POS_Z_I = 0; -static const float PID_POS_Z_D = 0; -static const float PID_POS_Z_LIM = 0; -static const float PID_POS_Z_AWU = 0; - -// attitude control loop -static const float PID_ATT_INTERVAL = 1 / 100; // 100 hz -static const float PID_ATT_P = 0.1; // 0.1 -static const float PID_ATT_I = 0; // 0.0 -static const float PID_ATT_D = 0.1; // 0.1 -static const float PID_ATT_LIM = 1; // 0.01 // 10 % #define MOTORs -static const float PID_ATT_AWU = 0; // 0.0 -static const float PID_YAWPOS_P = 0; -static const float PID_YAWPOS_I = 0; -static const float PID_YAWPOS_D = 0; -static const float PID_YAWPOS_LIM = 0; // 1 rad/s -static const float PID_YAWPOS_AWU = 0; // 1 rad/s -static const float PID_YAWSPEED_P = .2; -static const float PID_YAWSPEED_I = 0; -static const float PID_YAWSPEED_D = 0; -static const float PID_YAWSPEED_LIM = .3; // 0.01 // 10 % MOTORs -static const float PID_YAWSPEED_AWU = 0.0; -static const float PID_YAWSPEED_DFCUT = 3.0; // 3 Radians, about 1 Hz - -// mixing -static const float MIX_REMOTE_WEIGHT = 1; -static const float MIX_POSITION_WEIGHT = 1; -static const float MIX_POSITION_Z_WEIGHT = 1; -static const float MIX_POSITION_YAW_WEIGHT = 1; - -static const float THRUST_HOVER_OFFSET = 0.475; - -#include "ControllerQuad.h" - -#endif /* QUADMIKROKOPTER_H_ */ diff --git a/apo/apo.pde b/apo/apo.pde index dfca3d80ef..a413f824e3 100644 --- a/apo/apo.pde +++ b/apo/apo.pde @@ -18,6 +18,7 @@ // Vehicle Configuration #include "QuadArducopter.h" +//#include "PlaneEasystar.h" // ArduPilotOne Default Setup #include "APO_DefaultSetup.h" diff --git a/libraries/APO/APO.h b/libraries/APO/APO.h index 0e0efafbd7..357591c7e2 100644 --- a/libraries/APO/APO.h +++ b/libraries/APO/APO.h @@ -15,6 +15,7 @@ #include "AP_MavlinkCommand.h" #include "AP_Navigator.h" #include "AP_RcChannel.h" +#include "AP_BatteryMonitor.h" //#include "AP_Var_keys.h" #endif /* APO_H_ */ From e9c357dbcf0899b5ba1062799efffabbf99d06fa Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sun, 16 Oct 2011 21:21:36 +0800 Subject: [PATCH 3/3] APM Planner - 1.0.83 ap mount prep sensor level hil tweaks misc fixs --- Tools/ArdupilotMegaPlanner/Common.cs | 3 +- Tools/ArdupilotMegaPlanner/CurrentState.cs | 2 +- .../ArdupilotMegaPlanner/GCSViews/Firmware.cs | 6 +- .../GCSViews/FlightData.Designer.cs | 11 +- .../GCSViews/FlightData.cs | 28 +++++ .../GCSViews/FlightData.resx | 16 ++- .../GCSViews/Simulation.cs | 30 ++++- Tools/ArdupilotMegaPlanner/MAVLink.cs | 62 ++++++++++ Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs | 112 +++++++++++++++++- .../ArdupilotMegaPlanner/MAVLinkTypesenum.cs | 4 +- .../Properties/AssemblyInfo.cs | 2 +- .../Release/ArdupilotMegaPlanner.application | 2 +- .../bin/Release/GCSViews/FlightData.resx | 16 ++- Tools/ArdupilotMegaPlanner/mavlinklist.pl | 4 +- 14 files changed, 275 insertions(+), 23 deletions(-) diff --git a/Tools/ArdupilotMegaPlanner/Common.cs b/Tools/ArdupilotMegaPlanner/Common.cs index ccd82ba138..7658c48942 100644 --- a/Tools/ArdupilotMegaPlanner/Common.cs +++ b/Tools/ArdupilotMegaPlanner/Common.cs @@ -245,7 +245,8 @@ namespace ArdupilotMega GUIDED = 4, // AUTO control LOITER = 5, // Hold a single location RTL = 6, // AUTO control - CIRCLE = 7 + CIRCLE = 7, + POSITION = 8 } public static bool translateMode(string modein, ref MAVLink.__mavlink_set_nav_mode_t navmode, ref MAVLink.__mavlink_set_mode_t mode) diff --git a/Tools/ArdupilotMegaPlanner/CurrentState.cs b/Tools/ArdupilotMegaPlanner/CurrentState.cs index 4bba5e8269..8febc24fd3 100644 --- a/Tools/ArdupilotMegaPlanner/CurrentState.cs +++ b/Tools/ArdupilotMegaPlanner/CurrentState.cs @@ -26,7 +26,7 @@ namespace ArdupilotMega float _airspeed; float _groundspeed; float _verticalspeed; - public float verticalspeed { get { return _verticalspeed; } set { _verticalspeed = _verticalspeed * 0.8f + value * 0.2f; } } + public float verticalspeed { get { if (float.IsNaN(_verticalspeed)) _verticalspeed = 0; return _verticalspeed; } set { _verticalspeed = _verticalspeed * 0.8f + value * 0.2f; } } public float wind_dir { get; set; } public float wind_vel { get; set; } /// diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs index d24f7c0f12..e7d970530f 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs @@ -388,7 +388,7 @@ namespace ArdupilotMega.GCSViews { lbl_ACY6.Text = temp.name; } - else if (temp.url.ToLower().Contains("firmware/ac2-heli".ToLower())) + else if (temp.url.ToLower().Contains("firmware/ac2-heli-1".ToLower())) { lbl_Heli.Text = temp.name; } @@ -396,6 +396,10 @@ namespace ArdupilotMega.GCSViews { lbl_ACHil.Text = temp.name; } + else + { + Console.WriteLine("No Home "+ temp.name + " " + temp.url); + } } void findfirmware(string findwhat) diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs index e5577aa145..39e165613e 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs @@ -12,6 +12,7 @@ System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle2 = new System.Windows.Forms.DataGridViewCellStyle(); this.contextMenuStrip1 = new System.Windows.Forms.ContextMenuStrip(this.components); this.goHereToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.pointCameraHereToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.MainH = new System.Windows.Forms.SplitContainer(); this.SubMainLeft = new System.Windows.Forms.SplitContainer(); this.hud1 = new hud.HUD(); @@ -91,7 +92,8 @@ // contextMenuStrip1 // this.contextMenuStrip1.Items.AddRange(new System.Windows.Forms.ToolStripItem[] { - this.goHereToolStripMenuItem}); + this.goHereToolStripMenuItem, + this.pointCameraHereToolStripMenuItem}); this.contextMenuStrip1.Name = "contextMenuStrip1"; resources.ApplyResources(this.contextMenuStrip1, "contextMenuStrip1"); // @@ -101,6 +103,12 @@ resources.ApplyResources(this.goHereToolStripMenuItem, "goHereToolStripMenuItem"); this.goHereToolStripMenuItem.Click += new System.EventHandler(this.goHereToolStripMenuItem_Click); // + // pointCameraHereToolStripMenuItem + // + this.pointCameraHereToolStripMenuItem.Name = "pointCameraHereToolStripMenuItem"; + resources.ApplyResources(this.pointCameraHereToolStripMenuItem, "pointCameraHereToolStripMenuItem"); + this.pointCameraHereToolStripMenuItem.Click += new System.EventHandler(this.pointCameraHereToolStripMenuItem_Click); + // // MainH // this.MainH.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle; @@ -1311,5 +1319,6 @@ private System.Windows.Forms.ToolStripMenuItem recordHudToAVIToolStripMenuItem; private System.Windows.Forms.ToolStripMenuItem stopRecordToolStripMenuItem; private MyLabel lbl_logpercent; + private System.Windows.Forms.ToolStripMenuItem pointCameraHereToolStripMenuItem; } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs index bc7a00d89a..705de52256 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs @@ -1339,5 +1339,33 @@ namespace ArdupilotMega.GCSViews } } } + + private void pointCameraHereToolStripMenuItem_Click(object sender, EventArgs e) + { + if (!MainV2.comPort.BaseStream.IsOpen) + { + MessageBox.Show("Please Connect First"); + return; + } + + string alt = (100 * MainV2.cs.multiplierdist).ToString("0"); + Common.InputBox("Enter Alt", "Enter Target Alt (absolute)", ref alt); + + int intalt = (int)(100 * MainV2.cs.multiplierdist); + if (!int.TryParse(alt, out intalt)) + { + MessageBox.Show("Bad Alt"); + return; + } + + if (gotolocation.Lat == 0 || gotolocation.Lng == 0) + { + MessageBox.Show("Bad Lat/Long"); + return; + } + + MainV2.comPort.setMountConfigure(MAVLink.MAV_MOUNT_MODE.MAV_MOUNT_MODE_GPS_POINT, true, true, true); + MainV2.comPort.setMountControl(gotolocation.Lat, gotolocation.Lng, (int)(intalt / MainV2.cs.multiplierdist), true); + } } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx index a8ce828065..d44ff71539 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx @@ -122,13 +122,19 @@ - 134, 22 + 174, 22 Fly To Here + + 174, 22 + + + Point Camera Here + - 135, 26 + 175, 70 contextMenuStrip1 @@ -1735,6 +1741,12 @@ System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + pointCameraHereToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + recordHudToAVIToolStripMenuItem diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs index b45c37da3a..8a84ccf784 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs @@ -682,7 +682,7 @@ namespace ArdupilotMega.GCSViews return Math.Cos(rad); } - //float oldax =0, olday =0, oldaz = 0; + float oldax =0, olday =0, oldaz = 0; DateTime oldtime = DateTime.Now; ArdupilotMega.MAVLink.__mavlink_attitude_t oldatt = new ArdupilotMega.MAVLink.__mavlink_attitude_t(); @@ -742,7 +742,7 @@ namespace ArdupilotMega.GCSViews float rdiff = (float)((att.roll - oldatt.roll) / timediff.TotalSeconds); float ydiff = (float)((att.yaw - oldatt.yaw) / timediff.TotalSeconds); - // Console.WriteLine("{0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", pdiff, rdiff, ydiff, DATA[17][0], DATA[17][1], DATA[17][2]); + //Console.WriteLine("{0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", pdiff, rdiff, ydiff, DATA[17][0], DATA[17][1], DATA[17][2]); oldatt = att; @@ -758,7 +758,21 @@ namespace ArdupilotMega.GCSViews YLScsDrawing.Drawing3d.Vector3d accel3D = HIL.QuadCopter.RPY_to_XYZ(DATA[18][1], DATA[18][0], 0, -9.8); //DATA[18][2] - //accel3D += new YLScsDrawing.Drawing3d.Vector3d(0, 0, -9.8); + float turnrad = (float)(((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704) * 1.943844) / (float)(11.26 * Math.Tan(att.roll))) * ft2m; + + float centripaccel = (float)((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / turnrad; + + //Console.WriteLine("old {0} {1} {2}",accel3D.X,accel3D.Y,accel3D.Z); + + YLScsDrawing.Drawing3d.Vector3d cent3D = HIL.QuadCopter.RPY_to_XYZ(DATA[18][1] - 90, 0, 0, centripaccel); + + accel3D -= cent3D; + + //Console.WriteLine("new {0} {1} {2}", accel3D.X, accel3D.Y, accel3D.Z); + + oldax = DATA[4][5]; + olday = DATA[4][6]; + oldaz = DATA[4][4]; double head = DATA[18][2] - 90; @@ -861,9 +875,13 @@ namespace ArdupilotMega.GCSViews imu.zgyro = (short)(aeroin.Model_fAngVelZ * 1000); //imu.zmag = 0; - imu.xacc = (Int16)(aeroin.Model_fAccelX * 1000); // pitch - imu.yacc = (Int16)(aeroin.Model_fAccelY * 1000); // roll - imu.zacc = (Int16)(aeroin.Model_fAccelZ * 1000); + YLScsDrawing.Drawing3d.Vector3d accel3D = HIL.QuadCopter.RPY_to_XYZ(att.roll, att.pitch, 0, -9.8); //DATA[18][2] + + imu.xacc = (Int16)((accel3D.X + aeroin.Model_fAccelX) * 1000); // pitch + imu.yacc = (Int16)((accel3D.Y + aeroin.Model_fAccelY) * 1000); // roll + imu.zacc = (Int16)((accel3D.Z + aeroin.Model_fAccelZ) * 1000); + + Console.WriteLine("x {0} y {1} z {2}",imu.xacc,imu.yacc,imu.zacc); gps.alt = ((float)(aeroin.Model_fPosZ)); diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index 480a4376ff..01b198c91c 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -1436,6 +1436,68 @@ namespace ArdupilotMega } } + public void setMountConfigure(MAV_MOUNT_MODE mountmode,bool stabroll,bool stabpitch,bool stabyaw) + { + __mavlink_mount_configure_t req = new __mavlink_mount_configure_t(); + + req.target_system = sysid; + req.target_component = compid; + req.mount_mode = (byte)mountmode; + req.stab_pitch = (stabpitch == true) ? (byte)1 : (byte)0; + req.stab_roll = (stabroll == true) ? (byte)1 : (byte)0; + req.stab_yaw = (stabyaw == true) ? (byte)1 : (byte)0; + + generatePacket(MAVLINK_MSG_ID_MOUNT_CONFIGURE, req); + System.Threading.Thread.Sleep(20); + generatePacket(MAVLINK_MSG_ID_MOUNT_CONFIGURE, req); + } + + public void setMountControl(double pa,double pb,double pc,bool islatlng) + { + __mavlink_mount_control_t req = new __mavlink_mount_control_t(); + + req.target_system = sysid; + req.target_component = compid; + if (!islatlng) + { + req.input_a = (int)pa; + req.input_b = (int)pb; + req.input_c = (int)pc; + } + else + { + req.input_a = (int)(pa * 10000000.0); + req.input_b = (int)(pb * 10000000.0); + req.input_c = (int)(pc * 100.0); + } + + generatePacket(MAVLINK_MSG_ID_MOUNT_CONTROL, req); + System.Threading.Thread.Sleep(20); + generatePacket(MAVLINK_MSG_ID_MOUNT_CONTROL, req); + } + + public void setMode(string modein) + { + try + { + MAVLink.__mavlink_set_nav_mode_t navmode = new MAVLink.__mavlink_set_nav_mode_t(); + + MAVLink.__mavlink_set_mode_t mode = new MAVLink.__mavlink_set_mode_t(); + + if (Common.translateMode(modein, ref navmode, ref mode)) + { + MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_NAV_MODE, navmode); + System.Threading.Thread.Sleep(10); + MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_NAV_MODE, navmode); + System.Threading.Thread.Sleep(10); + MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_MODE, mode); + System.Threading.Thread.Sleep(10); + MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_MODE, mode); + } + } + catch { System.Windows.Forms.MessageBox.Show("Failed to change Modes"); } + } + /// /// used for last bad serial characters /// diff --git a/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs b/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs index d5bae2043a..9d16f0c04d 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs @@ -7,8 +7,69 @@ namespace ArdupilotMega { partial class MAVLink { - public byte[] MAVLINK_MESSAGE_LENGTHS = new byte[] {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5}; - public byte[] MAVLINK_MESSAGE_CRCS = new byte[] {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7}; + public byte[] MAVLINK_MESSAGE_LENGTHS = new byte[] {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5}; + public byte[] MAVLINK_MESSAGE_CRCS = new byte[] {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7}; + public enum MAV_MOUNT_MODE + { + MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization | */ + MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM. | */ + MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */ + MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ + MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ + MAV_MOUNT_MODE_ENUM_END=5, /* | */ + }; + + public const byte MAVLINK_MSG_ID_AP_ADC = 153; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_ap_adc_t + { + public ushort adc1; /// ADC output 1 + public ushort adc2; /// ADC output 2 + public ushort adc3; /// ADC output 3 + public ushort adc4; /// ADC output 4 + public ushort adc5; /// ADC output 5 + public ushort adc6; /// ADC output 6 + }; + + public const byte MAVLINK_MSG_ID_AP_ADC_LEN = 12; + public const byte MAVLINK_MSG_ID_153_LEN = 12; + public const byte MAVLINK_MSG_ID_DIGICAM_CONFIGURE = 154; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_digicam_configure_t + { + public byte target_system; /// System ID + public byte target_component; /// Component ID + public byte mode; /// Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) + public ushort shutter_speed; /// Divisor number //e.g. 1000 means 1/1000 (0 means ignore) + public byte aperture; /// F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) + public byte iso; /// ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) + public byte exposure_type; /// Exposure type enumeration from 1 to N (0 means ignore) + public byte command_id; /// Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + public byte engine_cut_off; /// Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) + public byte extra_param; /// Extra parameters enumeration (0 means ignore) + public float extra_value; /// Correspondent value to given extra_param + }; + + public const byte MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN = 15; + public const byte MAVLINK_MSG_ID_154_LEN = 15; + public const byte MAVLINK_MSG_ID_DIGICAM_CONTROL = 155; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_digicam_control_t + { + public byte target_system; /// System ID + public byte target_component; /// Component ID + public byte session; /// 0: stop, 1: start or keep it up //Session control e.g. show/hide lens + public byte zoom_pos; /// 1 to N //Zoom's absolute position (0 means ignore) + public byte zoom_step; /// -100 to 100 //Zooming step value to offset zoom from the current position + public byte focus_lock; /// 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus + public byte shot; /// 0: ignore, 1: shot or start filming + public byte command_id; /// Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + public byte extra_param; /// Extra parameters enumeration (0 means ignore) + public float extra_value; /// Correspondent value to given extra_param + }; + + public const byte MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN = 13; + public const byte MAVLINK_MSG_ID_155_LEN = 13; public const byte MAVLINK_MSG_ID_MEMINFO = 152; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_meminfo_t @@ -19,6 +80,47 @@ namespace ArdupilotMega public const byte MAVLINK_MSG_ID_MEMINFO_LEN = 4; public const byte MAVLINK_MSG_ID_152_LEN = 4; + public const byte MAVLINK_MSG_ID_MOUNT_CONFIGURE = 156; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_mount_configure_t + { + public byte target_system; /// System ID + public byte target_component; /// Component ID + public byte mount_mode; /// mount operating mode (see MAV_MOUNT_MODE enum) + public byte stab_roll; /// (1 = yes, 0 = no) + public byte stab_pitch; /// (1 = yes, 0 = no) + public byte stab_yaw; /// (1 = yes, 0 = no) + }; + + public const byte MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN = 6; + public const byte MAVLINK_MSG_ID_156_LEN = 6; + public const byte MAVLINK_MSG_ID_MOUNT_CONTROL = 157; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_mount_control_t + { + public byte target_system; /// System ID + public byte target_component; /// Component ID + public int input_a; /// pitch(deg*100) or lat, depending on mount mode + public int input_b; /// roll(deg*100) or lon depending on mount mode + public int input_c; /// yaw(deg*100) or alt (in cm) depending on mount mode + public byte save_position; /// if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) + }; + + public const byte MAVLINK_MSG_ID_MOUNT_CONTROL_LEN = 15; + public const byte MAVLINK_MSG_ID_157_LEN = 15; + public const byte MAVLINK_MSG_ID_MOUNT_STATUS = 158; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_mount_status_t + { + public byte target_system; /// System ID + public byte target_component; /// Component ID + public int pointing_a; /// pitch(deg*100) or lat, depending on mount mode + public int pointing_b; /// roll(deg*100) or lon depending on mount mode + public int pointing_c; /// yaw(deg*100) or alt (in cm) depending on mount mode + }; + + public const byte MAVLINK_MSG_ID_MOUNT_STATUS_LEN = 14; + public const byte MAVLINK_MSG_ID_158_LEN = 14; public const byte MAVLINK_MSG_ID_SENSOR_OFFSETS = 150; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_sensor_offsets_t @@ -87,6 +189,10 @@ namespace ArdupilotMega system to control the vehicle attitude and the attitude of various devices such as cameras. |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ + DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ + DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ + DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty| */ DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Empty| Empty| Empty| */ PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ @@ -1346,7 +1452,7 @@ namespace ArdupilotMega MAV_FRAME_LOCAL_ENU = 4 }; -Type[] mavstructs = new Type[] {typeof( __mavlink_heartbeat_t) ,typeof( __mavlink_boot_t) ,null ,typeof( __mavlink_ping_t) ,null ,typeof( __mavlink_change_operator_control_t) ,typeof( __mavlink_change_operator_control_ack_t) ,typeof( __mavlink_auth_key_t) ,null ,typeof( __mavlink_action_ack_t) ,typeof( __mavlink_action_t) ,typeof( __mavlink_set_mode_t) ,typeof( __mavlink_set_nav_mode_t) ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_param_request_read_t) ,typeof( __mavlink_param_request_list_t) ,typeof( __mavlink_param_value_t) ,typeof( __mavlink_param_set_t) ,null ,typeof( __mavlink_gps_raw_int_t) ,typeof( __mavlink_scaled_imu_t) ,typeof( __mavlink_gps_status_t) ,typeof( __mavlink_raw_imu_t) ,typeof( __mavlink_raw_pressure_t) ,typeof( __mavlink_attitude_new_t ) ,typeof( __mavlink_local_position_t) ,typeof( __mavlink_gps_raw_t) ,typeof( __mavlink_global_position_t) ,typeof( __mavlink_sys_status_t) ,typeof( __mavlink_rc_channels_raw_t) ,typeof( __mavlink_rc_channels_scaled_t) ,typeof( __mavlink_servo_output_raw_t) ,typeof( __mavlink_scaled_pressure_t) ,typeof( __mavlink_waypoint_t) ,typeof( __mavlink_waypoint_request_t) ,typeof( __mavlink_waypoint_set_current_t) ,typeof( __mavlink_waypoint_current_t) ,typeof( __mavlink_waypoint_request_list_t) ,typeof( __mavlink_waypoint_count_t) ,typeof( __mavlink_waypoint_clear_all_t) ,typeof( __mavlink_waypoint_reached_t) ,typeof( __mavlink_waypoint_ack_t) ,typeof( __mavlink_waypoint_set_global_reference_t ) ,typeof( __mavlink_gps_local_origin_set_t) ,typeof( __mavlink_local_position_setpoint_set_t) ,typeof( __mavlink_local_position_setpoint_t) ,typeof( __mavlink_control_status_t) ,typeof( __mavlink_safety_set_allowed_area_t) ,typeof( __mavlink_safety_allowed_area_t) ,typeof( __mavlink_set_roll_pitch_yaw_thrust_t) ,typeof( __mavlink_set_roll_pitch_yaw_speed_thrust_t) ,typeof( __mavlink_roll_pitch_yaw_thrust_setpoint_t) ,typeof( __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t) ,null ,typeof( __mavlink_attitude_controller_output_t ) ,typeof( __mavlink_position_controller_output_t ) ,typeof( __mavlink_nav_controller_output_t) ,typeof( __mavlink_position_target_t) ,typeof( __mavlink_state_correction_t) ,typeof( __mavlink_set_altitude_t) ,typeof( __mavlink_request_data_stream_t) ,typeof( __mavlink_request_dynamic_gyro_calibration_t ) ,typeof( __mavlink_request_static_calibration_t ) ,typeof( __mavlink_manual_control_t) ,typeof( __mavlink_rc_channels_override_t) ,null ,null ,typeof( __mavlink_global_position_int_t) ,typeof( __mavlink_vfr_hud_t) ,typeof( __mavlink_command_t) ,typeof( __mavlink_command_ack_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_optical_flow_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_object_detection_event_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_sensor_offsets_t) ,typeof( __mavlink_set_mag_offsets_t) ,typeof( __mavlink_meminfo_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_debug_vect_t) ,typeof( __mavlink_named_value_float_t) ,typeof( __mavlink_named_value_int_t) ,typeof( __mavlink_statustext_t) ,typeof( __mavlink_debug_t) ,null ,}; +Type[] mavstructs = new Type[] {typeof( __mavlink_heartbeat_t) ,typeof( __mavlink_boot_t) ,null ,typeof( __mavlink_ping_t) ,null ,typeof( __mavlink_change_operator_control_t) ,typeof( __mavlink_change_operator_control_ack_t) ,typeof( __mavlink_auth_key_t) ,null ,typeof( __mavlink_action_ack_t) ,typeof( __mavlink_action_t) ,typeof( __mavlink_set_mode_t) ,typeof( __mavlink_set_nav_mode_t) ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_param_request_read_t) ,typeof( __mavlink_param_request_list_t) ,typeof( __mavlink_param_value_t) ,typeof( __mavlink_param_set_t) ,null ,typeof( __mavlink_gps_raw_int_t) ,typeof( __mavlink_scaled_imu_t) ,typeof( __mavlink_gps_status_t) ,typeof( __mavlink_raw_imu_t) ,typeof( __mavlink_raw_pressure_t) ,typeof( __mavlink_attitude_new_t ) ,typeof( __mavlink_local_position_t) ,typeof( __mavlink_gps_raw_t) ,typeof( __mavlink_global_position_t) ,typeof( __mavlink_sys_status_t) ,typeof( __mavlink_rc_channels_raw_t) ,typeof( __mavlink_rc_channels_scaled_t) ,typeof( __mavlink_servo_output_raw_t) ,typeof( __mavlink_scaled_pressure_t) ,typeof( __mavlink_waypoint_t) ,typeof( __mavlink_waypoint_request_t) ,typeof( __mavlink_waypoint_set_current_t) ,typeof( __mavlink_waypoint_current_t) ,typeof( __mavlink_waypoint_request_list_t) ,typeof( __mavlink_waypoint_count_t) ,typeof( __mavlink_waypoint_clear_all_t) ,typeof( __mavlink_waypoint_reached_t) ,typeof( __mavlink_waypoint_ack_t) ,typeof( __mavlink_waypoint_set_global_reference_t ) ,typeof( __mavlink_gps_local_origin_set_t) ,typeof( __mavlink_local_position_setpoint_set_t) ,typeof( __mavlink_local_position_setpoint_t) ,typeof( __mavlink_control_status_t) ,typeof( __mavlink_safety_set_allowed_area_t) ,typeof( __mavlink_safety_allowed_area_t) ,typeof( __mavlink_set_roll_pitch_yaw_thrust_t) ,typeof( __mavlink_set_roll_pitch_yaw_speed_thrust_t) ,typeof( __mavlink_roll_pitch_yaw_thrust_setpoint_t) ,typeof( __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t) ,null ,typeof( __mavlink_attitude_controller_output_t ) ,typeof( __mavlink_position_controller_output_t ) ,typeof( __mavlink_nav_controller_output_t) ,typeof( __mavlink_position_target_t) ,typeof( __mavlink_state_correction_t) ,typeof( __mavlink_set_altitude_t) ,typeof( __mavlink_request_data_stream_t) ,typeof( __mavlink_request_dynamic_gyro_calibration_t ) ,typeof( __mavlink_request_static_calibration_t ) ,typeof( __mavlink_manual_control_t) ,typeof( __mavlink_rc_channels_override_t) ,null ,null ,typeof( __mavlink_global_position_int_t) ,typeof( __mavlink_vfr_hud_t) ,typeof( __mavlink_command_t) ,typeof( __mavlink_command_ack_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_optical_flow_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_object_detection_event_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_sensor_offsets_t) ,typeof( __mavlink_set_mag_offsets_t) ,typeof( __mavlink_meminfo_t) ,typeof( __mavlink_ap_adc_t) ,typeof( __mavlink_digicam_configure_t) ,typeof( __mavlink_digicam_control_t) ,typeof( __mavlink_mount_configure_t) ,typeof( __mavlink_mount_control_t) ,typeof( __mavlink_mount_status_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_debug_vect_t) ,typeof( __mavlink_named_value_float_t) ,typeof( __mavlink_named_value_int_t) ,typeof( __mavlink_statustext_t) ,typeof( __mavlink_debug_t) ,null ,}; } } diff --git a/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs b/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs index 864b322f5d..6f146c83c8 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs @@ -6,8 +6,8 @@ using System.Runtime.InteropServices; namespace ArdupilotMega { partial class MAVLink - { - + { + } } diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index aa806d298a..04800efbd9 100644 --- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs +++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs @@ -34,5 +34,5 @@ using System.Resources; // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] [assembly: AssemblyVersion("1.0.0.0")] -[assembly: AssemblyFileVersion("1.0.82")] +[assembly: AssemblyFileVersion("1.0.83")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application index bd9f307fa9..408b405505 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application +++ b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application @@ -11,7 +11,7 @@ - DxfINYOSh55dWio5h4coAIWRotU= + xG04b9X6TXBWirLuQOgI+3TR8H0= diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/FlightData.resx b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/FlightData.resx index a8ce828065..d44ff71539 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/FlightData.resx +++ b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/FlightData.resx @@ -122,13 +122,19 @@ - 134, 22 + 174, 22 Fly To Here + + 174, 22 + + + Point Camera Here + - 135, 26 + 175, 70 contextMenuStrip1 @@ -1735,6 +1741,12 @@ System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + pointCameraHereToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + recordHudToAVIToolStripMenuItem diff --git a/Tools/ArdupilotMegaPlanner/mavlinklist.pl b/Tools/ArdupilotMegaPlanner/mavlinklist.pl index 43b0b6219e..ba4f5a59dd 100644 --- a/Tools/ArdupilotMegaPlanner/mavlinklist.pl +++ b/Tools/ArdupilotMegaPlanner/mavlinklist.pl @@ -2,8 +2,8 @@ $dir = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/include/common/"; $dir2 = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/include/ardupilotmega/"; # mavlink 1.0 with old structs -#$dir = "C:/Users/hog/Desktop/DIYDrones&avr/ardupilot-mega/libraries/GCS_MAVLink/include/common/"; -#$dir2 = "C:/Users/hog/Desktop/DIYDrones&avr/ardupilot-mega/libraries/GCS_MAVLink/include/ardupilotmega/"; +$dir = "C:/Users/hog/Desktop/DIYDrones/ardupilot-mega/libraries/GCS_MAVLink/include/common/"; +$dir2 = "C:/Users/hog/Desktop/DIYDrones/ardupilot-mega/libraries/GCS_MAVLink/include/ardupilotmega/"; opendir(DIR,$dir) || die print $!; @files2 = readdir(DIR);