diff --git a/apo/ControllerPlane.h b/apo/ControllerPlane.h index 307a68de2d..e751d4ef69 100644 --- a/apo/ControllerPlane.h +++ b/apo/ControllerPlane.h @@ -14,180 +14,180 @@ namespace apo { class ControllerPlane: public AP_Controller { public: - enum { - ch_mode = 0, ch_roll, ch_pitch, ch_thrust, ch_yaw - }; - enum { - k_chMode = k_radioChannelsStart, - k_chRoll, - k_chPitch, - k_chYaw, - k_chThr, + enum { + ch_mode = 0, ch_roll, ch_pitch, ch_thrust, ch_yaw + }; + enum { + k_chMode = k_radioChannelsStart, + k_chRoll, + k_chPitch, + k_chYaw, + k_chThr, - k_pidBnkRll = k_controllersStart, - k_pidSpdPit, - k_pidPitPit, - k_pidYwrYaw, - k_pidHdgBnk, - k_pidAltThr, + k_pidBnkRll = k_controllersStart, + k_pidSpdPit, + k_pidPitPit, + k_pidYwrYaw, + k_pidHdgBnk, + k_pidAltThr, - k_trim = k_customStart - }; + k_trim = k_customStart + }; - ControllerPlane(AP_Navigator * nav, AP_Guide * guide, - AP_HardwareAbstractionLayer * hal) : - AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode), - _trimGroup(k_trim, PSTR("trim_")), - _rdrAilMix(&_group, 2, rdrAilMix, PSTR("rdrAilMix")), - _needsTrim(false), - _ailTrim(&_trimGroup, 1, ailTrim, PSTR("ail")), - _elvTrim(&_trimGroup, 2, elvTrim, PSTR("elv")), - _rdrTrim(&_trimGroup, 3, rdrTrim, PSTR("rdr")), - _thrTrim(&_trimGroup, 4, thrTrim, PSTR("thr")), - pidBnkRll(new AP_Var_group(k_pidBnkRll, PSTR("bnkRll_")), 1, - pidBnkRllP, pidBnkRllI, pidBnkRllD, pidBnkRllAwu, - pidBnkRllLim, pidBnkRllDFCut), - pidPitPit(new AP_Var_group(k_pidPitPit, PSTR("pitPit_")), 1, - pidPitPitP, pidPitPitI, pidPitPitD, pidPitPitAwu, - pidPitPitLim, pidPitPitDFCut), - pidSpdPit(new AP_Var_group(k_pidSpdPit, PSTR("spdPit_")), 1, - pidSpdPitP, pidSpdPitI, pidSpdPitD, pidSpdPitAwu, - pidSpdPitLim, pidSpdPitDFCut), - pidYwrYaw(new AP_Var_group(k_pidYwrYaw, PSTR("ywrYaw_")), 1, - pidYwrYawP, pidYwrYawI, pidYwrYawD, pidYwrYawAwu, - pidYwrYawLim, pidYwrYawDFCut), - pidHdgBnk(new AP_Var_group(k_pidHdgBnk, PSTR("hdgBnk_")), 1, - pidHdgBnkP, pidHdgBnkI, pidHdgBnkD, pidHdgBnkAwu, - pidHdgBnkLim, pidHdgBnkDFCut), - pidAltThr(new AP_Var_group(k_pidAltThr, PSTR("altThr_")), 1, - pidAltThrP, pidAltThrI, pidAltThrD, pidAltThrAwu, - pidAltThrLim, pidAltThrDFCut), - requireRadio(false), _aileron(0), _elevator(0), _rudder(0), _throttle(0) { + ControllerPlane(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode), + _trimGroup(k_trim, PSTR("trim_")), + _rdrAilMix(&_group, 2, rdrAilMix, PSTR("rdrAilMix")), + _needsTrim(false), + _ailTrim(&_trimGroup, 1, ailTrim, PSTR("ail")), + _elvTrim(&_trimGroup, 2, elvTrim, PSTR("elv")), + _rdrTrim(&_trimGroup, 3, rdrTrim, PSTR("rdr")), + _thrTrim(&_trimGroup, 4, thrTrim, PSTR("thr")), + pidBnkRll(new AP_Var_group(k_pidBnkRll, PSTR("bnkRll_")), 1, + pidBnkRllP, pidBnkRllI, pidBnkRllD, pidBnkRllAwu, + pidBnkRllLim, pidBnkRllDFCut), + pidPitPit(new AP_Var_group(k_pidPitPit, PSTR("pitPit_")), 1, + pidPitPitP, pidPitPitI, pidPitPitD, pidPitPitAwu, + pidPitPitLim, pidPitPitDFCut), + pidSpdPit(new AP_Var_group(k_pidSpdPit, PSTR("spdPit_")), 1, + pidSpdPitP, pidSpdPitI, pidSpdPitD, pidSpdPitAwu, + pidSpdPitLim, pidSpdPitDFCut), + pidYwrYaw(new AP_Var_group(k_pidYwrYaw, PSTR("ywrYaw_")), 1, + pidYwrYawP, pidYwrYawI, pidYwrYawD, pidYwrYawAwu, + pidYwrYawLim, pidYwrYawDFCut), + pidHdgBnk(new AP_Var_group(k_pidHdgBnk, PSTR("hdgBnk_")), 1, + pidHdgBnkP, pidHdgBnkI, pidHdgBnkD, pidHdgBnkAwu, + pidHdgBnkLim, pidHdgBnkDFCut), + pidAltThr(new AP_Var_group(k_pidAltThr, PSTR("altThr_")), 1, + pidAltThrP, pidAltThrI, pidAltThrD, pidAltThrAwu, + pidAltThrLim, pidAltThrDFCut), + requireRadio(false), _aileron(0), _elevator(0), _rudder(0), _throttle(0) { - _hal->debug->println_P(PSTR("initializing plane controller")); + _hal->debug->println_P(PSTR("initializing plane controller")); - _hal->rc.push_back( - new AP_RcChannel(k_chMode, PSTR("mode_"), APM_RC, 5, 1100, - 1500, 1900, RC_MODE_IN, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chRoll, PSTR("roll_"), APM_RC, 0, 1200, - 1500, 1800, RC_MODE_INOUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chPitch, PSTR("pitch_"), APM_RC, 1, 1200, - 1500, 1800, RC_MODE_INOUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chThr, PSTR("thr_"), APM_RC, 2, 1100, 1100, - 1900, RC_MODE_INOUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chYaw, PSTR("yaw_"), APM_RC, 3, 1200, 1500, - 1800, RC_MODE_INOUT, false)); - } + _hal->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("mode_"), APM_RC, 5, 1100, + 1500, 1900, RC_MODE_IN, false)); + _hal->rc.push_back( + new AP_RcChannel(k_chRoll, PSTR("roll_"), APM_RC, 0, 1200, + 1500, 1800, RC_MODE_INOUT, false)); + _hal->rc.push_back( + new AP_RcChannel(k_chPitch, PSTR("pitch_"), APM_RC, 1, 1200, + 1500, 1800, RC_MODE_INOUT, false)); + _hal->rc.push_back( + new AP_RcChannel(k_chThr, PSTR("thr_"), APM_RC, 2, 1100, 1100, + 1900, RC_MODE_INOUT, false)); + _hal->rc.push_back( + new AP_RcChannel(k_chYaw, PSTR("yaw_"), APM_RC, 3, 1200, 1500, + 1800, RC_MODE_INOUT, false)); + } - void manualLoop(const float dt) { - setAllRadioChannelsManually(); + void manualLoop(const float dt) { + setAllRadioChannelsManually(); - // force auto to read new manual trim - if (_needsTrim == false) - _needsTrim = true; - //_hal->debug->println("manual"); - } + // force auto to read new manual trim + if (_needsTrim == false) + _needsTrim = true; + //_hal->debug->println("manual"); + } - void autoLoop(const float dt) { - float headingError = _guide->getHeadingCommand() - - _nav->getYaw(); - if (headingError > 180 * deg2Rad) - headingError -= 360 * deg2Rad; - if (headingError < -180 * deg2Rad) - headingError += 360 * deg2Rad; + void autoLoop(const float dt) { + float headingError = _guide->getHeadingCommand() + - _nav->getYaw(); + if (headingError > 180 * deg2Rad) + headingError -= 360 * deg2Rad; + if (headingError < -180 * deg2Rad) + headingError += 360 * deg2Rad; - _aileron = pidBnkRll.update( - pidHdgBnk.update(headingError, dt) - _nav->getRoll(), dt); - _elevator = pidPitPit.update( - -pidSpdPit.update( - _guide->getAirSpeedCommand() - _nav->getAirSpeed(), - dt) - _nav->getPitch(), dt); - _rudder = pidYwrYaw.update(-_nav->getYawRate(), dt); + _aileron = pidBnkRll.update( + pidHdgBnk.update(headingError, dt) - _nav->getRoll(), dt); + _elevator = pidPitPit.update( + -pidSpdPit.update( + _guide->getAirSpeedCommand() - _nav->getAirSpeed(), + dt) - _nav->getPitch(), dt); + _rudder = pidYwrYaw.update(-_nav->getYawRate(), dt); - // desired yaw rate is zero, needs washout - _throttle = pidAltThr.update( - _guide->getAltitudeCommand() - _nav->getAlt(), dt); + // desired yaw rate is zero, needs washout + _throttle = pidAltThr.update( + _guide->getAltitudeCommand() - _nav->getAlt(), dt); - // if needs trim - if (_needsTrim) { - // need to subtract current controller deflections so control - // surfaces are actually at the same position as manual flight - _ailTrim = _hal->rc[ch_roll]->getRadioPosition() - _aileron; - _elvTrim = _hal->rc[ch_pitch]->getRadioPosition() - _elevator; - _rdrTrim = _hal->rc[ch_yaw]->getRadioPosition() - _rudder; - _thrTrim = _hal->rc[ch_thrust]->getRadioPosition() - _throttle; - _needsTrim = false; - } + // if needs trim + if (_needsTrim) { + // need to subtract current controller deflections so control + // surfaces are actually at the same position as manual flight + _ailTrim = _hal->rc[ch_roll]->getRadioPosition() - _aileron; + _elvTrim = _hal->rc[ch_pitch]->getRadioPosition() - _elevator; + _rdrTrim = _hal->rc[ch_yaw]->getRadioPosition() - _rudder; + _thrTrim = _hal->rc[ch_thrust]->getRadioPosition() - _throttle; + _needsTrim = false; + } - // actuator mixing/ output - _aileron += _rdrAilMix * _rudder + _ailTrim; - _elevator += _elvTrim; - _rudder += _rdrTrim; - _throttle += _thrTrim; + // actuator mixing/ output + _aileron += _rdrAilMix * _rudder + _ailTrim; + _elevator += _elvTrim; + _rudder += _rdrTrim; + _throttle += _thrTrim; - //_hal->debug->println("automode"); - } + //_hal->debug->println("automode"); + } - void setMotors() { + void setMotors() { - switch (_hal->getState()) { + switch (_hal->getState()) { - case MAV_STATE_ACTIVE: { - digitalWrite(_hal->aLedPin, HIGH); - // turn all motors off if below 0.1 throttle - if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) { - setAllRadioChannelsToNeutral(); - } else { - // actuator mixing/ output - _hal->rc[ch_roll]->setPosition(_aileron); - _hal->rc[ch_yaw]->setPosition(_rudder); - _hal->rc[ch_pitch]->setPosition(_elevator); - _hal->rc[ch_thrust]->setPosition(_throttle); - } - 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(); - } + case MAV_STATE_ACTIVE: { + digitalWrite(_hal->aLedPin, HIGH); + // turn all motors off if below 0.1 throttle + if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) { + setAllRadioChannelsToNeutral(); + } else { + // actuator mixing/ output + _hal->rc[ch_roll]->setPosition(_aileron); + _hal->rc[ch_yaw]->setPosition(_rudder); + _hal->rc[ch_pitch]->setPosition(_elevator); + _hal->rc[ch_thrust]->setPosition(_throttle); + } + 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(); + } - } + } - } + } private: - AP_Var_group _trimGroup; - AP_Uint8 _rdrAilMix; - bool _needsTrim; - AP_Float _ailTrim; - AP_Float _elvTrim; - AP_Float _rdrTrim; - AP_Float _thrTrim; - BlockPID pidBnkRll; // bank error to roll servo deflection - BlockPID pidSpdPit; // speed error to pitch command - BlockPID pidPitPit; // pitch error to pitch servo deflection - BlockPID pidYwrYaw; // yaw rate error to yaw servo deflection - BlockPID pidHdgBnk; // heading error to bank command - BlockPID pidAltThr; // altitude error to throttle deflection - bool requireRadio; - float _aileron; - float _elevator; - float _rudder; - float _throttle; + AP_Var_group _trimGroup; + AP_Uint8 _rdrAilMix; + bool _needsTrim; + AP_Float _ailTrim; + AP_Float _elvTrim; + AP_Float _rdrTrim; + AP_Float _thrTrim; + BlockPID pidBnkRll; // bank error to roll servo deflection + BlockPID pidSpdPit; // speed error to pitch command + BlockPID pidPitPit; // pitch error to pitch servo deflection + BlockPID pidYwrYaw; // yaw rate error to yaw servo deflection + BlockPID pidHdgBnk; // heading error to bank command + BlockPID pidAltThr; // altitude error to throttle deflection + bool requireRadio; + float _aileron; + float _elevator; + float _rudder; + float _throttle; }; } // namespace apo diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 6afd28fc80..39fb5f1eff 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -17,199 +17,199 @@ namespace apo { class ControllerQuad: public AP_Controller { public: - /** - * note that these are not the controller radio channel numbers, they are just - * unique keys so they can be reaccessed from the hal rc vector - */ - enum { - ch_mode = 0, // note scicoslab channels set mode, left, right, front, back order - ch_right, - ch_left, - ch_front, - ch_back, - ch_roll, - ch_pitch, - ch_thrust, - ch_yaw - }; + /** + * note that these are not the controller radio channel numbers, they are just + * unique keys so they can be reaccessed from the hal rc vector + */ + enum { + ch_mode = 0, // note scicoslab channels set mode, left, right, front, back order + ch_right, + ch_left, + ch_front, + ch_back, + ch_roll, + ch_pitch, + ch_thrust, + ch_yaw + }; - // must match channel enum - enum { - k_chMode = k_radioChannelsStart, - k_chRight, - k_chLeft, - k_chFront, - k_chBack, - k_chRoll, - k_chPitch, - k_chThr, - k_chYaw - }; + // must match channel enum + enum { + k_chMode = k_radioChannelsStart, + k_chRight, + k_chLeft, + k_chFront, + k_chBack, + k_chRoll, + k_chPitch, + k_chThr, + k_chYaw + }; - enum { - k_pidGroundSpeed2Throttle = k_controllersStart, - k_pidStr, - k_pidPN, - k_pidPE, - k_pidPD, - k_pidRoll, - k_pidPitch, - k_pidYawRate, - k_pidYaw, - }; + enum { + k_pidGroundSpeed2Throttle = k_controllersStart, + k_pidStr, + k_pidPN, + k_pidPE, + k_pidPD, + k_pidRoll, + k_pidPitch, + k_pidYawRate, + k_pidYaw, + }; - ControllerQuad(AP_Navigator * nav, AP_Guide * guide, - AP_HardwareAbstractionLayer * hal) : - AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode), - 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_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_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_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_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_DFCUT), - pidPD(new AP_Var_group(k_pidPD, PSTR("DOWN_")), 1, PID_POS_Z_P, - PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM, PID_POS_DFCUT), - _thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0), - _cmdRoll(0), _cmdPitch(0), _cmdYawRate(0) { - _hal->debug->println_P(PSTR("initializing quad controller")); + ControllerQuad(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode), + 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_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_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_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_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_DFCUT), + pidPD(new AP_Var_group(k_pidPD, PSTR("DOWN_")), 1, PID_POS_Z_P, + PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM, PID_POS_DFCUT), + _thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0), + _cmdRoll(0), _cmdPitch(0), _cmdYawRate(0) { + _hal->debug->println_P(PSTR("initializing quad controller")); - /* - * allocate radio channels - * the order of the channels has to match the enumeration above - */ - _hal->rc.push_back( - new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100, - 1500, 1900, RC_MODE_IN, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 0, 1100, - 1100, 1900, RC_MODE_OUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 1, 1100, - 1100, 1900, RC_MODE_OUT, false)); + /* + * allocate radio channels + * the order of the channels has to match the enumeration above + */ + _hal->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100, + 1500, 1900, RC_MODE_IN, false)); + _hal->rc.push_back( + new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 0, 1100, + 1100, 1900, RC_MODE_OUT, false)); + _hal->rc.push_back( + new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 1, 1100, + 1100, 1900, RC_MODE_OUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100, - 1100, 1900, RC_MODE_OUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chBack, PSTR("BACK_"), APM_RC, 3, 1100, - 1100, 1900, RC_MODE_OUT, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chRoll, PSTR("ROLL_"), APM_RC, 0, 1100, - 1500, 1900, RC_MODE_IN, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100, - 1500, 1900, RC_MODE_IN, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 2, 1100, - 1100, 1900, RC_MODE_IN, false)); - _hal->rc.push_back( - new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 3, 1100, 1500, - 1900, RC_MODE_IN, false)); - } + _hal->rc.push_back( + new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100, + 1100, 1900, RC_MODE_OUT, false)); + _hal->rc.push_back( + new AP_RcChannel(k_chBack, PSTR("BACK_"), APM_RC, 3, 1100, + 1100, 1900, RC_MODE_OUT, false)); + _hal->rc.push_back( + new AP_RcChannel(k_chRoll, PSTR("ROLL_"), APM_RC, 0, 1100, + 1500, 1900, RC_MODE_IN, false)); + _hal->rc.push_back( + new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100, + 1500, 1900, RC_MODE_IN, false)); + _hal->rc.push_back( + new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 2, 1100, + 1100, 1900, RC_MODE_IN, false)); + _hal->rc.push_back( + new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 3, 1100, 1500, + 1900, RC_MODE_IN, false)); + } private: - BlockPIDDfb pidRoll, pidPitch, pidYaw; - BlockPID pidYawRate; - BlockPIDDfb pidPN, pidPE, pidPD; + BlockPIDDfb pidRoll, pidPitch, pidYaw; + BlockPID pidYawRate; + BlockPIDDfb pidPN, pidPE, pidPD; - float _thrustMix, _pitchMix, _rollMix, _yawMix; - float _cmdRoll, _cmdPitch, _cmdYawRate; + float _thrustMix, _pitchMix, _rollMix, _yawMix; + float _cmdRoll, _cmdPitch, _cmdYawRate; - void manualLoop(const float dt) { - 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(); - autoAttitudeLoop(dt); - } + void manualLoop(const float dt) { + 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(); + autoAttitudeLoop(dt); + } - void autoLoop(const float dt) { - autoPositionLoop(dt); - autoAttitudeLoop(dt); + void autoLoop(const float dt) { + autoPositionLoop(dt); + autoAttitudeLoop(dt); - // XXX currently auto loop not tested, so - // put vehicle in standby - _hal->setState(MAV_STATE_STANDBY); - } + // XXX currently auto loop not tested, so + // put vehicle in standby + _hal->setState(MAV_STATE_STANDBY); + } - 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); + 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; + // "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; + _thrustMix = THRUST_HOVER_OFFSET + cmdDown; - // "thrust-trim-adjust" - if (fabs(_cmdRoll) > 0.5) _thrustMix *= 1.13949393; - else _thrustMix /= cos(_cmdRoll); + // "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); - } + 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 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() { + void setMotors() { - switch (_hal->getState()) { + switch (_hal->getState()) { - case MAV_STATE_ACTIVE: { - digitalWrite(_hal->aLedPin, HIGH); - // turn all motors off if below 0.1 throttle - if (fabs(_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(); - } + case MAV_STATE_ACTIVE: { + digitalWrite(_hal->aLedPin, HIGH); + // turn all motors off if below 0.1 throttle + if (fabs(_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/PlaneEasystar.h b/apo/PlaneEasystar.h index be4e85848b..5fb895a0b2 100644 --- a/apo/PlaneEasystar.h +++ b/apo/PlaneEasystar.h @@ -27,10 +27,10 @@ static const uint8_t heartBeatTimeout = 3; #define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL // baud rates -static uint32_t debugBaud = 57600; -static uint32_t telemBaud = 57600; -static uint32_t gpsBaud = 38400; -static uint32_t hilBaud = 57600; +static uint32_t debugBaud = 57600; +static uint32_t telemBaud = 57600; +static uint32_t gpsBaud = 38400; +static uint32_t hilBaud = 57600; // optional sensors static const bool gpsEnabled = false; diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index c3b914f39d..011ffc8420 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -27,10 +27,10 @@ static const uint8_t heartBeatTimeout = 3; #define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL // baud rates -static uint32_t debugBaud = 57600; -static uint32_t telemBaud = 57600; -static uint32_t gpsBaud = 38400; -static uint32_t hilBaud = 57600; +static uint32_t debugBaud = 57600; +static uint32_t telemBaud = 57600; +static uint32_t gpsBaud = 38400; +static uint32_t hilBaud = 57600; // optional sensors static const bool gpsEnabled = false; diff --git a/libraries/APO/APO_DefaultSetup.h b/libraries/APO/APO_DefaultSetup.h index 2da05dfcf4..14661b2f22 100644 --- a/libraries/APO/APO_DefaultSetup.h +++ b/libraries/APO/APO_DefaultSetup.h @@ -14,170 +14,170 @@ static apo::AP_Autopilot * autoPilot; void setup() { - using namespace apo; - - AP_Var::load_all(); + using namespace apo; - // Declare all parts of the system - AP_Navigator * navigator = NULL; - AP_Guide * guide = NULL; - AP_Controller * controller = NULL; - AP_HardwareAbstractionLayer * hal = NULL; + AP_Var::load_all(); - /* - * Communications - */ - Serial.begin(debugBaud, 128, 128); // debug - - // hardware abstraction layer - hal = new AP_HardwareAbstractionLayer( - halMode, board, vehicle, heartBeatTimeout); - - // debug serial - hal->debug = &Serial; - hal->debug->println_P(PSTR("initializing debug line")); + // Declare all parts of the system + AP_Navigator * navigator = NULL; + AP_Guide * guide = NULL; + AP_Controller * controller = NULL; + AP_HardwareAbstractionLayer * hal = NULL; - /* - * Sensor initialization - */ - if (hal->getMode() == MODE_LIVE) { + /* + * Communications + */ + Serial.begin(debugBaud, 128, 128); // debug - hal->debug->println_P(PSTR("initializing adc")); - hal->adc = new ADC_CLASS; - hal->adc->Init(); + // hardware abstraction layer + hal = new AP_HardwareAbstractionLayer( + halMode, board, vehicle, heartBeatTimeout); - if (batteryMonitorEnabled) { - hal->batteryMonitor = new AP_BatteryMonitor(batteryPin,batteryVoltageDivRatio,batteryMinVolt,batteryMaxVolt); - } + // debug serial + hal->debug = &Serial; + hal->debug->println_P(PSTR("initializing debug line")); - if (gpsEnabled) { - Serial1.begin(gpsBaud, 128, 16); // gps - hal->debug->println_P(PSTR("initializing gps")); - AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps)); - hal->gps = &gpsDriver; - hal->gps->callback = delay; - hal->gps->init(); - } + /* + * Sensor initialization + */ + if (hal->getMode() == MODE_LIVE) { - if (baroEnabled) { - hal->debug->println_P(PSTR("initializing baro")); - hal->baro = new BARO_CLASS; - hal->baro->Init(); - } + hal->debug->println_P(PSTR("initializing adc")); + hal->adc = new ADC_CLASS; + hal->adc->Init(); - if (compassEnabled) { - Wire.begin(); - hal->debug->println_P(PSTR("initializing compass")); - hal->compass = new COMPASS_CLASS; - hal->compass->set_orientation(compassOrientation); - hal->compass->set_offsets(0,0,0); - hal->compass->set_declination(0.0); - hal->compass->init(); - } + if (batteryMonitorEnabled) { + hal->batteryMonitor = new AP_BatteryMonitor(batteryPin,batteryVoltageDivRatio,batteryMinVolt,batteryMaxVolt); + } - /** - * Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not - * initialize them and NULL will be assigned to those corresponding pointers. - * On detecting NULL assigned to any ultrasonic sensor, its corresponding block of code - * will not be executed by the navigator. - * The coordinate system is assigned by the right hand rule with the thumb pointing down. - * In set_orientation, it is defined as (front/back,left/right,down,up) - */ + if (gpsEnabled) { + Serial1.begin(gpsBaud, 128, 16); // gps + hal->debug->println_P(PSTR("initializing gps")); + AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps)); + hal->gps = &gpsDriver; + hal->gps->callback = delay; + hal->gps->init(); + } - if (rangeFinderFrontEnabled) { - hal->debug->println_P(PSTR("initializing front range finder")); - RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(1); - rangeFinder->set_orientation(1, 0, 0); - hal->rangeFinders.push_back(rangeFinder); - } + if (baroEnabled) { + hal->debug->println_P(PSTR("initializing baro")); + hal->baro = new BARO_CLASS; + hal->baro->Init(); + } - if (rangeFinderBackEnabled) { - hal->debug->println_P(PSTR("initializing back range finder")); - RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(2); - rangeFinder->set_orientation(-1, 0, 0); - hal->rangeFinders.push_back(rangeFinder); - } + if (compassEnabled) { + Wire.begin(); + hal->debug->println_P(PSTR("initializing compass")); + hal->compass = new COMPASS_CLASS; + hal->compass->set_orientation(compassOrientation); + hal->compass->set_offsets(0,0,0); + hal->compass->set_declination(0.0); + hal->compass->init(); + } - if (rangeFinderLeftEnabled) { - hal->debug->println_P(PSTR("initializing left range finder")); - RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(3); - rangeFinder->set_orientation(0, -1, 0); - hal->rangeFinders.push_back(rangeFinder); - } + /** + * Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not + * initialize them and NULL will be assigned to those corresponding pointers. + * On detecting NULL assigned to any ultrasonic sensor, its corresponding block of code + * will not be executed by the navigator. + * The coordinate system is assigned by the right hand rule with the thumb pointing down. + * In set_orientation, it is defined as (front/back,left/right,down,up) + */ - if (rangeFinderRightEnabled) { - hal->debug->println_P(PSTR("initializing right range finder")); - RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(4); - rangeFinder->set_orientation(0, 1, 0); - hal->rangeFinders.push_back(rangeFinder); - } + if (rangeFinderFrontEnabled) { + hal->debug->println_P(PSTR("initializing front range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(1); + rangeFinder->set_orientation(1, 0, 0); + hal->rangeFinders.push_back(rangeFinder); + } - if (rangeFinderUpEnabled) { - hal->debug->println_P(PSTR("initializing up range finder")); - RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(5); - rangeFinder->set_orientation(0, 0, -1); - hal->rangeFinders.push_back(rangeFinder); - } + if (rangeFinderBackEnabled) { + hal->debug->println_P(PSTR("initializing back range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(2); + rangeFinder->set_orientation(-1, 0, 0); + hal->rangeFinders.push_back(rangeFinder); + } - if (rangeFinderDownEnabled) { - hal->debug->println_P(PSTR("initializing down range finder")); - RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(6); - rangeFinder->set_orientation(0, 0, 1); - hal->rangeFinders.push_back(rangeFinder); - } + if (rangeFinderLeftEnabled) { + hal->debug->println_P(PSTR("initializing left range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(3); + rangeFinder->set_orientation(0, -1, 0); + hal->rangeFinders.push_back(rangeFinder); + } - } + if (rangeFinderRightEnabled) { + hal->debug->println_P(PSTR("initializing right range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(4); + rangeFinder->set_orientation(0, 1, 0); + hal->rangeFinders.push_back(rangeFinder); + } - /* - * Select guidance, navigation, control algorithms - */ - navigator = new NAVIGATOR_CLASS(hal); - guide = new GUIDE_CLASS(navigator, hal, velCmd, xt, xtLim); - controller = new CONTROLLER_CLASS(navigator,guide,hal); + if (rangeFinderUpEnabled) { + hal->debug->println_P(PSTR("initializing up range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(5); + rangeFinder->set_orientation(0, 0, -1); + hal->rangeFinders.push_back(rangeFinder); + } - /* - * CommLinks - */ - if (board==BOARD_ARDUPILOTMEGA_2) - { - Serial2.begin(telemBaud, 128, 128); // gcs - hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal); - } - else - { - Serial3.begin(telemBaud, 128, 128); // gcs - hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal); - } + if (rangeFinderDownEnabled) { + hal->debug->println_P(PSTR("initializing down range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(6); + rangeFinder->set_orientation(0, 0, 1); + hal->rangeFinders.push_back(rangeFinder); + } - /* - * Hardware in the Loop - */ - if (hal->getMode() == MODE_HIL_CNTL) { - Serial.println("HIL line setting up"); - Serial1.begin(hilBaud, 128, 128); - delay(1000); - Serial1.println("starting hil"); - hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal); - } + } - /* - * Start the autopilot - */ - hal->debug->printf_P(PSTR("initializing autopilot\n")); - hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); + /* + * Select guidance, navigation, control algorithms + */ + navigator = new NAVIGATOR_CLASS(hal); + guide = new GUIDE_CLASS(navigator, hal, velCmd, xt, xtLim); + controller = new CONTROLLER_CLASS(navigator,guide,hal); - autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal, - loopRate, loop0Rate, loop1Rate, loop2Rate, loop3Rate); + /* + * CommLinks + */ + if (board==BOARD_ARDUPILOTMEGA_2) + { + Serial2.begin(telemBaud, 128, 128); // gcs + hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal); + } + else + { + Serial3.begin(telemBaud, 128, 128); // gcs + hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal); + } + + /* + * Hardware in the Loop + */ + if (hal->getMode() == MODE_HIL_CNTL) { + Serial.println("HIL line setting up"); + Serial1.begin(hilBaud, 128, 128); + delay(1000); + Serial1.println("starting hil"); + hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal); + } + + /* + * Start the autopilot + */ + hal->debug->printf_P(PSTR("initializing autopilot\n")); + hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); + + autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal, + loopRate, loop0Rate, loop1Rate, loop2Rate, loop3Rate); } void loop() { - autoPilot->update(); + autoPilot->update(); } #endif //_APO_COMMON_H diff --git a/libraries/APO/AP_ArmingMechanism.cpp b/libraries/APO/AP_ArmingMechanism.cpp index 38e23b8543..ffd7d0a886 100644 --- a/libraries/APO/AP_ArmingMechanism.cpp +++ b/libraries/APO/AP_ArmingMechanism.cpp @@ -16,8 +16,8 @@ void AP_ArmingMechanism::update(const float dt) { // arming if ( (_hal->getState() != MAV_STATE_ACTIVE) && - (fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) && - (_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) { + (fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) && + (_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) { // always start clock at 0 if (_armingClock<0) _armingClock = 0; @@ -31,8 +31,8 @@ void AP_ArmingMechanism::update(const float dt) { } // disarming else if ( (_hal->getState() == MAV_STATE_ACTIVE) && - (fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) && - (_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) { + (fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) && + (_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) { // always start clock at 0 if (_armingClock>0) _armingClock = 0; diff --git a/libraries/APO/AP_ArmingMechanism.h b/libraries/APO/AP_ArmingMechanism.h index e172d822ee..e747a7831e 100644 --- a/libraries/APO/AP_ArmingMechanism.h +++ b/libraries/APO/AP_ArmingMechanism.h @@ -26,16 +26,16 @@ public: * @param ch2Max: arms above this */ AP_ArmingMechanism(AP_HardwareAbstractionLayer * hal, - uint8_t ch1, uint8_t ch2, float ch1Min, float ch2Min, - float ch2Max) : _armingClock(0), _hal(hal), _ch1(ch1), _ch2(ch2), - _ch1Min(ch1Min), _ch2Min(ch2Min), _ch2Max(ch2Max) { + uint8_t ch1, uint8_t ch2, float ch1Min, float ch2Min, + float ch2Max) : _armingClock(0), _hal(hal), _ch1(ch1), _ch2(ch2), + _ch1Min(ch1Min), _ch2Min(ch2Min), _ch2Max(ch2Max) { } /** * update * * arming: - * + * * to arm: put stick to bottom right for 100 controller cycles * (max yaw, min throttle) * diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp index 1c600eef44..e7f943d32a 100644 --- a/libraries/APO/AP_Autopilot.cpp +++ b/libraries/APO/AP_Autopilot.cpp @@ -13,242 +13,242 @@ namespace apo { class AP_HardwareAbstractionLayer; AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, - AP_Controller * controller, AP_HardwareAbstractionLayer * hal, - float loopRate, float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate) : - Loop(loopRate, callback, this), _navigator(navigator), _guide(guide), - _controller(controller), _hal(hal), - callbackCalls(0) { + AP_Controller * controller, AP_HardwareAbstractionLayer * hal, + 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); + 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, + /* + * Radio setup + */ + hal->debug->println_P(PSTR("initializing radio")); + APM_RC.Init(); // APM Radio initialization, - /* - * Calibration - */ - hal->setState(MAV_STATE_CALIBRATING); - hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); - hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + /* + * Calibration + */ + hal->setState(MAV_STATE_CALIBRATING); + hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); - if (navigator) navigator->calibrate(); + if (navigator) navigator->calibrate(); - /* - * Look for valid initial state - */ - while (_navigator) { - // letc gcs known we are alive - hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); - hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); - if (hal->getMode() == MODE_LIVE) { - _navigator->updateSlow(0); - if (hal->gps) { - if (hal->gps->fix) { - break; - } else { - hal->gps->update(); - hal->gcs->sendText(SEVERITY_LOW, - PSTR("waiting for gps lock\n")); - hal->debug->printf_P(PSTR("waiting for gps lock\n")); - } - } else { // no gps, can skip - break; - } - } else if (hal->getMode() == MODE_HIL_CNTL) { // hil - hal->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); - hal->hil->receive(); - Serial.println("HIL Receive Called"); - if (_navigator->getTimeStamp() != 0) { - // give hil a chance to send some packets - for (int i = 0; i < 5; i++) { - hal->debug->println_P(PSTR("reading initial hil packets")); - hal->gcs->sendText(SEVERITY_LOW, - PSTR("reading initial hil packets")); - delay(1000); - } - break; - } - hal->debug->println_P(PSTR("waiting for hil packet")); - } - delay(500); - } - - AP_MavlinkCommand::home.setAlt(_navigator->getAlt()); - AP_MavlinkCommand::home.setLat(_navigator->getLat()); - AP_MavlinkCommand::home.setLon(_navigator->getLon()); - AP_MavlinkCommand::home.setCommand(MAV_CMD_NAV_WAYPOINT); - AP_MavlinkCommand::home.save(); - _hal->debug->printf_P(PSTR("\nhome before load lat: %f deg, lon: %f deg, cmd: %d\n"), - AP_MavlinkCommand::home.getLat()*rad2Deg, - AP_MavlinkCommand::home.getLon()*rad2Deg, - AP_MavlinkCommand::home.getCommand()); - AP_MavlinkCommand::home.load(); - _hal->debug->printf_P(PSTR("\nhome after load lat: %f deg, lon: %f deg, cmd: %d\n"), - AP_MavlinkCommand::home.getLat()*rad2Deg, - AP_MavlinkCommand::home.getLon()*rad2Deg, - AP_MavlinkCommand::home.getCommand()); + /* + * Look for valid initial state + */ + while (_navigator) { + // letc gcs known we are alive + hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + if (hal->getMode() == MODE_LIVE) { + _navigator->updateSlow(0); + if (hal->gps) { + if (hal->gps->fix) { + break; + } else { + hal->gps->update(); + hal->gcs->sendText(SEVERITY_LOW, + PSTR("waiting for gps lock\n")); + hal->debug->printf_P(PSTR("waiting for gps lock\n")); + } + } else { // no gps, can skip + break; + } + } else if (hal->getMode() == MODE_HIL_CNTL) { // hil + hal->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + hal->hil->receive(); + Serial.println("HIL Receive Called"); + if (_navigator->getTimeStamp() != 0) { + // give hil a chance to send some packets + for (int i = 0; i < 5; i++) { + hal->debug->println_P(PSTR("reading initial hil packets")); + hal->gcs->sendText(SEVERITY_LOW, + PSTR("reading initial hil packets")); + delay(1000); + } + break; + } + hal->debug->println_P(PSTR("waiting for hil packet")); + } + delay(500); + } - /* - * Attach loops, stacking for priority - */ - hal->debug->println_P(PSTR("attaching loops")); - 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)); + AP_MavlinkCommand::home.setAlt(_navigator->getAlt()); + AP_MavlinkCommand::home.setLat(_navigator->getLat()); + AP_MavlinkCommand::home.setLon(_navigator->getLon()); + AP_MavlinkCommand::home.setCommand(MAV_CMD_NAV_WAYPOINT); + AP_MavlinkCommand::home.save(); + _hal->debug->printf_P(PSTR("\nhome before load lat: %f deg, lon: %f deg, cmd: %d\n"), + AP_MavlinkCommand::home.getLat()*rad2Deg, + AP_MavlinkCommand::home.getLon()*rad2Deg, + AP_MavlinkCommand::home.getCommand()); + AP_MavlinkCommand::home.load(); + _hal->debug->printf_P(PSTR("\nhome after load lat: %f deg, lon: %f deg, cmd: %d\n"), + AP_MavlinkCommand::home.getLat()*rad2Deg, + AP_MavlinkCommand::home.getLon()*rad2Deg, + AP_MavlinkCommand::home.getCommand()); - hal->debug->println_P(PSTR("running")); - hal->gcs->sendText(SEVERITY_LOW, PSTR("running")); - hal->setState(MAV_STATE_STANDBY); + /* + * Attach loops, stacking for priority + */ + hal->debug->println_P(PSTR("attaching loops")); + 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")); + hal->setState(MAV_STATE_STANDBY); } void AP_Autopilot::callback(void * data) { - AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->hal()->debug->println_P(PSTR("callback")); + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->hal()->debug->println_P(PSTR("callback")); - /* - * ahrs update - */ - apo->callbackCalls++; - if (apo->getNavigator()) - apo->getNavigator()->updateFast(apo->dt()); + /* + * ahrs update + */ + apo->callbackCalls++; + if (apo->getNavigator()) + apo->getNavigator()->updateFast(apo->dt()); } void AP_Autopilot::callback0(void * data) { - AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getHal()->debug->println_P(PSTR("callback 0")); - - /* - * hardware in the loop - */ - if (apo->getHal()->hil && apo->getHal()->getMode() != MODE_LIVE) { - apo->getHal()->hil->receive(); - apo->getHal()->hil->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); - } + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->getHal()->debug->println_P(PSTR("callback 0")); /* - * update control laws - */ - if (apo->getController()) { - //apo->getHal()->debug->println_P(PSTR("updating controller")); - apo->getController()->update(apo->subLoops()[0]->dt()); - } - /* - char msg[50]; - sprintf(msg, "c_hdg: %f, c_thr: %f", apo->guide()->headingCommand, apo->guide()->groundSpeedCommand); - apo->hal()->gcs->sendText(AP_CommLink::SEVERITY_LOW, msg); - */ + * hardware in the loop + */ + if (apo->getHal()->hil && apo->getHal()->getMode() != MODE_LIVE) { + apo->getHal()->hil->receive(); + apo->getHal()->hil->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); + } + + /* + * update control laws + */ + if (apo->getController()) { + //apo->getHal()->debug->println_P(PSTR("updating controller")); + apo->getController()->update(apo->subLoops()[0]->dt()); + } + /* + char msg[50]; + sprintf(msg, "c_hdg: %f, c_thr: %f", apo->guide()->headingCommand, apo->guide()->groundSpeedCommand); + apo->hal()->gcs->sendText(AP_CommLink::SEVERITY_LOW, msg); + */ } void AP_Autopilot::callback1(void * data) { - AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getHal()->debug->println_P(PSTR("callback 1")); - - /* - * update guidance laws - */ - if (apo->getGuide()) - { - //apo->getHal()->debug->println_P(PSTR("updating guide")); - apo->getGuide()->update(); - } + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->getHal()->debug->println_P(PSTR("callback 1")); - /* - * slow navigation loop update - */ - if (apo->getNavigator()) { - apo->getNavigator()->updateSlow(apo->subLoops()[1]->dt()); - } + /* + * update guidance laws + */ + if (apo->getGuide()) + { + //apo->getHal()->debug->println_P(PSTR("updating guide")); + apo->getGuide()->update(); + } - /* - * send telemetry - */ - if (apo->getHal()->gcs) { - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE); - } + /* + * slow navigation loop update + */ + if (apo->getNavigator()) { + apo->getNavigator()->updateSlow(apo->subLoops()[1]->dt()); + } - /* - * handle ground control station communication - */ - if (apo->getHal()->gcs) { - // send messages - apo->getHal()->gcs->requestCmds(); - apo->getHal()->gcs->sendParameters(); + /* + * send telemetry + */ + if (apo->getHal()->gcs) { + apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE); + } - // receive messages - apo->getHal()->gcs->receive(); - } + /* + * handle ground control station communication + */ + if (apo->getHal()->gcs) { + // send messages + apo->getHal()->gcs->requestCmds(); + apo->getHal()->gcs->sendParameters(); - /* - * navigator debug - */ - /* - if (apo->navigator()) { - apo->getHal()->debug->printf_P(PSTR("roll: %f deg\tpitch: %f deg\tyaw: %f deg\n"), - apo->navigator()->getRoll()*rad2Deg, - apo->navigator()->getPitch()*rad2Deg, - apo->navigator()->getYaw()*rad2Deg); - apo->getHal()->debug->printf_P(PSTR("lat: %f deg\tlon: %f deg\talt: %f m\n"), - apo->navigator()->getLat()*rad2Deg, - apo->navigator()->getLon()*rad2Deg, - apo->navigator()->getAlt()); - } - */ + // receive messages + apo->getHal()->gcs->receive(); + } + + /* + * navigator debug + */ + /* + if (apo->navigator()) { + apo->getHal()->debug->printf_P(PSTR("roll: %f deg\tpitch: %f deg\tyaw: %f deg\n"), + apo->navigator()->getRoll()*rad2Deg, + apo->navigator()->getPitch()*rad2Deg, + apo->navigator()->getYaw()*rad2Deg); + apo->getHal()->debug->printf_P(PSTR("lat: %f deg\tlon: %f deg\talt: %f m\n"), + apo->navigator()->getLat()*rad2Deg, + apo->navigator()->getLon()*rad2Deg, + apo->navigator()->getAlt()); + } + */ } void AP_Autopilot::callback2(void * data) { - AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getHal()->debug->println_P(PSTR("callback 2")); - - /* - * send telemetry - */ - if (apo->getHal()->gcs) { - // send messages - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW); - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW); - //apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION); - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU); - } + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->getHal()->debug->println_P(PSTR("callback 2")); - /* - * update battery monitor - */ - if (apo->getHal()->batteryMonitor) apo->getHal()->batteryMonitor->update(); + /* + * send telemetry + */ + if (apo->getHal()->gcs) { + // send messages + apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW); + apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); + apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW); + //apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION); + apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU); + } - /* - * send heartbeat - */ - apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + /* + * update battery monitor + */ + if (apo->getHal()->batteryMonitor) apo->getHal()->batteryMonitor->update(); - /* - * load/loop rate/ram debug - */ - apo->getHal()->load = apo->load(); - 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); + /* + * send heartbeat + */ + apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); - /* - * adc debug - */ - //apo->getDebug().printf_P(PSTR("adc: %d %d %d %d %d %d %d %d\n"), - //apo->adc()->Ch(0), apo->adc()->Ch(1), apo->adc()->Ch(2), - //apo->adc()->Ch(3), apo->adc()->Ch(4), apo->adc()->Ch(5), - //apo->adc()->Ch(6), apo->adc()->Ch(7), apo->adc()->Ch(8)); + /* + * load/loop rate/ram debug + */ + apo->getHal()->load = apo->load(); + 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); + + /* + * adc debug + */ + //apo->getDebug().printf_P(PSTR("adc: %d %d %d %d %d %d %d %d\n"), + //apo->adc()->Ch(0), apo->adc()->Ch(1), apo->adc()->Ch(2), + //apo->adc()->Ch(3), apo->adc()->Ch(4), apo->adc()->Ch(5), + //apo->adc()->Ch(6), apo->adc()->Ch(7), apo->adc()->Ch(8)); } void AP_Autopilot::callback3(void * data) { - //AP_Autopilot * apo = (AP_Autopilot *) data; - //apo->getHal()->debug->println_P(PSTR("callback 3")); + //AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->getHal()->debug->println_P(PSTR("callback 3")); } } // apo diff --git a/libraries/APO/AP_Autopilot.h b/libraries/APO/AP_Autopilot.h index e6684fe07f..5fa9578c92 100644 --- a/libraries/APO/AP_Autopilot.h +++ b/libraries/APO/AP_Autopilot.h @@ -65,88 +65,88 @@ class AP_CommLink; */ class AP_Autopilot: public Loop { public: - /** - * Default constructor - */ - AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, - AP_Controller * controller, AP_HardwareAbstractionLayer * hal, - float loopRate, float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate); + /** + * Default constructor + */ + AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, + AP_Controller * controller, AP_HardwareAbstractionLayer * hal, + float loopRate, float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate); - /** - * Accessors - */ - AP_Navigator * getNavigator() { - return _navigator; - } - AP_Guide * getGuide() { - return _guide; - } - AP_Controller * getController() { - return _controller; - } - AP_HardwareAbstractionLayer * getHal() { - return _hal; - } + /** + * Accessors + */ + AP_Navigator * getNavigator() { + return _navigator; + } + AP_Guide * getGuide() { + return _guide; + } + AP_Controller * getController() { + return _controller; + } + AP_HardwareAbstractionLayer * getHal() { + return _hal; + } - /** - * Loop Monitoring - */ - uint32_t callbackCalls; + /** + * Loop Monitoring + */ + uint32_t callbackCalls; private: - /** - * 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 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); + /** + * Loop 0 Callbacks + * - control + * - compass reading + * @see callback + */ + static void callback0(void * data); - /** - * Loop 1 Callbacks - * - gps sensor fusion - * - compass sensor fusion - * @see callback - */ - static void callback1(void * data); + /** + * Loop 1 Callbacks + * - gps sensor fusion + * - compass sensor fusion + * @see callback + */ + static void callback1(void * data); - /** - * Loop 2 Callbacks - * - slow messages - * @see callback - */ - static void callback2(void * data); + /** + * Loop 2 Callbacks + * - slow messages + * @see callback + */ + static void callback2(void * data); - /** - * Loop 3 Callbacks - * - super slow messages - * - log writing - * @see callback - */ - static void callback3(void * data); + /** + * Loop 3 Callbacks + * - super slow messages + * - log writing + * @see callback + */ + static void callback3(void * data); - /** - * Components - */ - AP_Navigator * _navigator; - AP_Guide * _guide; - AP_Controller * _controller; - AP_HardwareAbstractionLayer * _hal; + /** + * Components + */ + AP_Navigator * _navigator; + AP_Guide * _guide; + AP_Controller * _controller; + AP_HardwareAbstractionLayer * _hal; - /** - * Constants - */ - static const float deg2rad = M_PI / 180; - static const float rad2deg = 180 / M_PI; + /** + * Constants + */ + static const float deg2rad = M_PI / 180; + static const float rad2deg = 180 / M_PI; }; } // namespace apo diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp index cad558ccbd..cdba1208b1 100644 --- a/libraries/APO/AP_CommLink.cpp +++ b/libraries/APO/AP_CommLink.cpp @@ -24,225 +24,225 @@ 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) { + 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), + AP_Controller * controller, AP_HardwareAbstractionLayer * hal) : + AP_CommLink(link, nav, guide, controller, hal), - // options - _useRelativeAlt(true), + // options + _useRelativeAlt(true), - // commands - _sendingCmds(false), _receivingCmds(false), - _cmdTimeLastSent(millis()), _cmdTimeLastReceived(millis()), - _cmdDestSysId(0), _cmdDestCompId(0), _cmdRequestIndex(0), - _cmdMax(30), _cmdNumberRequested(0), + // 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) { + // 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; - } + 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; + // 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")); + //_hal->debug->printf_P(PSTR("send message\n")); - // if number of channels exceeded return - if (_channel == MAVLINK_COMM_3) - return; + // if number of channels exceeded return + if (_channel == MAVLINK_COMM_3) + return; - uint64_t timeStamp = micros(); + uint64_t timeStamp = micros(); - switch (id) { + switch (id) { - case MAVLINK_MSG_ID_HEARTBEAT: { - mavlink_msg_heartbeat_send(_channel, mavlink_system.type, - MAV_AUTOPILOT_ARDUPILOTMEGA); - break; - } + 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_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_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: { + 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_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_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_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_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: { + 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; - } + 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); + 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; - } + // turn off waypoint send + _receivingCmds = false; + break; + } - case MAVLINK_MSG_ID_WAYPOINT_CURRENT: { - mavlink_msg_waypoint_current_send(_channel, - _guide->getCurrentIndex()); - 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); - } + default: { + char msg[50]; + sprintf(msg, "autopilot sending unknown command with id: %d", id); + sendText(SEVERITY_HIGH, msg); + } - } // switch + } // 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; + //_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; + // 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); + // 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); - } + // 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; + // 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); + 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); + 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) { @@ -252,30 +252,30 @@ 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; + //_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(); + // 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)) { + // 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)); + char paramName[_paramNameLengthMax]; + vp->copy_name(paramName, sizeof(paramName)); - mavlink_msg_param_value_send(_channel, (int8_t*) paramName, - value, _countParameters(), _queuedParameterIndex); + mavlink_msg_param_value_send(_channel, (int8_t*) paramName, + value, _countParameters(), _queuedParameterIndex); - _queuedParameterIndex++; - break; - } - } + _queuedParameterIndex++; + break; + } + } } @@ -283,439 +283,439 @@ void MavlinkComm::sendParameters() { * 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); - } + //_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(); + uint32_t timeStamp = micros(); - switch (msg->msgid) { - _hal->debug->printf_P(PSTR("message received: %d"), msg->msgid); + 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_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); + 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; - } + _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); + 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; - } + // 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; + 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) { + // 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_READ: + AP_Var::load_all(); + break; - case MAV_ACTION_STORAGE_WRITE: - AP_Var::save_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 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")); + 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; + // 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()); + // 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; - } + _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")); + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: { + sendText(SEVERITY_LOW, PSTR("waypoint request")); - // Check if sending waypiont - if (!_sendingCmds) - break; + // 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; + // 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); + _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); + 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; - } + // update last waypoint comm stamp + _cmdTimeLastSent = millis(); + break; + } - case MAVLINK_MSG_ID_WAYPOINT_ACK: { - sendText(SEVERITY_LOW, PSTR("waypoint ack")); + 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; + // 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) + // check for error + //uint8_t type = packet.type; // ok (0), error(1) - // turn off waypoint send - _sendingCmds = false; - break; - } + // turn off waypoint send + _sendingCmds = false; + break; + } - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - sendText(SEVERITY_LOW, PSTR("param request list")); + 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; + // 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 + // Start sending parameters - next call to ::update will kick the first one out - _queuedParameter = AP_Var::first(); - _queuedParameterIndex = 0; - break; - } + _queuedParameter = AP_Var::first(); + _queuedParameterIndex = 0; + break; + } - case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: { - sendText(SEVERITY_LOW, PSTR("waypoint clear all")); + 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; + // 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); + // 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); + // 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; - } + break; + } - case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: { - sendText(SEVERITY_LOW, PSTR("waypoint set current")); + 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; + // 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; - } + // 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")); + 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; + // 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; - } + // 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")); + case MAVLINK_MSG_ID_WAYPOINT: { + sendText(SEVERITY_LOW, PSTR("waypoint")); - // Check if receiving waypiont - if (!_receivingCmds) { - //sendText(SEVERITY_HIGH, PSTR("not receiving commands")); - break; - } + // 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; + // 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; - } + // 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); + _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; - } + // 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; + 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; + // decode + mavlink_param_set_t packet; + mavlink_msg_param_set_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; - // set parameter + // set parameter - char key[_paramNameLengthMax + 1]; - strncpy(key, (char *) packet.param_id, _paramNameLengthMax); - key[_paramNameLengthMax] = 0; + 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 + // 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; + // 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(); + // 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); + // 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_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_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_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; - } + } 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... - } + // 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 + break; + } // end case - } + } } uint16_t MavlinkComm::_countParameters() { - // if we haven't cached the parameter count yet... - if (0 == _parameterCount) { - AP_Var *vp; + // 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; + 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; + AP_Var *vp; - vp = AP_Var::first(); - while (NULL != 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; + // 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; + /* + 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 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 - } + } else { + return 0; // no error + } } } // apo diff --git a/libraries/APO/AP_CommLink.h b/libraries/APO/AP_CommLink.h index be473cde9a..a05a00be4f 100644 --- a/libraries/APO/AP_CommLink.h +++ b/libraries/APO/AP_CommLink.h @@ -34,7 +34,7 @@ class AP_Guide; class AP_HardwareAbstractionLayer; enum { - SEVERITY_LOW, SEVERITY_MED, SEVERITY_HIGH + SEVERITY_LOW, SEVERITY_MED, SEVERITY_HIGH }; // forward declarations @@ -45,83 +45,83 @@ enum { class AP_CommLink { public: - AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide, - 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; - virtual void sendText(uint8_t severity, const char *str) = 0; - virtual void sendText(uint8_t severity, const prog_char_t *str) = 0; - virtual void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) = 0; - virtual void sendParameters() = 0; - virtual void requestCmds() = 0; + AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide, + 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; + virtual void sendText(uint8_t severity, const char *str) = 0; + virtual void sendText(uint8_t severity, const prog_char_t *str) = 0; + virtual void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) = 0; + virtual void sendParameters() = 0; + virtual void requestCmds() = 0; protected: - FastSerial * _link; - AP_Navigator * _navigator; - AP_Guide * _guide; - AP_Controller * _controller; - AP_HardwareAbstractionLayer * _hal; + FastSerial * _link; + AP_Navigator * _navigator; + AP_Guide * _guide; + AP_Controller * _controller; + AP_HardwareAbstractionLayer * _hal; }; class MavlinkComm: public AP_CommLink { public: - MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide, - AP_Controller * controller, AP_HardwareAbstractionLayer * hal); + MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide, + AP_Controller * controller, AP_HardwareAbstractionLayer * hal); - 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); + 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(); + /** + * sends parameters one at a time + */ + void sendParameters(); - /** - * request commands one at a time - */ - void requestCmds(); + /** + * request commands one at a time + */ + void requestCmds(); private: - // options - bool _useRelativeAlt; + // options + bool _useRelativeAlt; - // commands - bool _sendingCmds; - bool _receivingCmds; - uint16_t _cmdTimeLastSent; - uint16_t _cmdTimeLastReceived; - uint16_t _cmdDestSysId; - uint16_t _cmdDestCompId; - uint16_t _cmdRequestIndex; - uint16_t _cmdNumberRequested; - uint16_t _cmdMax; - Vector _cmdList; + // commands + bool _sendingCmds; + bool _receivingCmds; + uint16_t _cmdTimeLastSent; + uint16_t _cmdTimeLastReceived; + uint16_t _cmdDestSysId; + uint16_t _cmdDestCompId; + uint16_t _cmdRequestIndex; + uint16_t _cmdNumberRequested; + uint16_t _cmdMax; + Vector _cmdList; - // parameters - static uint8_t _paramNameLengthMax; - uint16_t _parameterCount; - AP_Var * _queuedParameter; - uint16_t _queuedParameterIndex; + // parameters + static uint8_t _paramNameLengthMax; + uint16_t _parameterCount; + AP_Var * _queuedParameter; + uint16_t _queuedParameterIndex; - // channel - mavlink_channel_t _channel; - uint16_t _packetDrops; - static uint8_t _nChannels; + // channel + mavlink_channel_t _channel; + uint16_t _packetDrops; + static uint8_t _nChannels; - void _handleMessage(mavlink_message_t * msg); + void _handleMessage(mavlink_message_t * msg); - uint16_t _countParameters(); + uint16_t _countParameters(); - AP_Var * _findParameter(uint16_t index); + AP_Var * _findParameter(uint16_t index); - // check the target - uint8_t _checkTarget(uint8_t sysid, uint8_t compid); + // check the target + uint8_t _checkTarget(uint8_t sysid, uint8_t compid); }; diff --git a/libraries/APO/AP_Controller.cpp b/libraries/APO/AP_Controller.cpp index c9e704dcbe..c75c9c8220 100644 --- a/libraries/APO/AP_Controller.cpp +++ b/libraries/APO/AP_Controller.cpp @@ -19,79 +19,79 @@ namespace apo { AP_Controller::AP_Controller(AP_Navigator * nav, AP_Guide * guide, - AP_HardwareAbstractionLayer * hal, AP_ArmingMechanism * armingMechanism, - const uint8_t chMode, const uint16_t key, const prog_char_t * name) : - _nav(nav), _guide(guide), _hal(hal), _armingMechanism(armingMechanism), - _group(key, name ? : PSTR("CNTRL_")), - _chMode(chMode), - _mode(&_group, 1, MAV_MODE_LOCKED, PSTR("MODE")) { - setAllRadioChannelsToNeutral(); + AP_HardwareAbstractionLayer * hal, AP_ArmingMechanism * armingMechanism, + const uint8_t chMode, const uint16_t key, const prog_char_t * name) : + _nav(nav), _guide(guide), _hal(hal), _armingMechanism(armingMechanism), + _group(key, name ? : PSTR("CNTRL_")), + _chMode(chMode), + _mode(&_group, 1, MAV_MODE_LOCKED, PSTR("MODE")) { + setAllRadioChannelsToNeutral(); } void AP_Controller::setAllRadioChannelsToNeutral() { - for (uint8_t i = 0; i < _hal->rc.getSize(); i++) { - _hal->rc[i]->setPosition(0.0); - } + 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(); - } + for (uint8_t i = 0; i < _hal->rc.getSize(); i++) { + _hal->rc[i]->setUsingRadio(); + } } void AP_Controller::update(const float dt) { - if (_armingMechanism) _armingMechanism->update(dt); + if (_armingMechanism) _armingMechanism->update(dt); - // determine flight mode - // - // check for heartbeat from gcs, if not found go to failsafe - if (_hal->heartBeatLost()) { - _mode = MAV_MODE_FAILSAFE; - _hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat")); - // if battery less than 5%, go to failsafe - } else if (_hal->batteryMonitor && _hal->batteryMonitor->getPercentage() < 5) { - _mode = MAV_MODE_FAILSAFE; - _hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery")); - // manual/auto switch - } else { - // if all emergencies cleared, fall back to standby - if (_hal->getState()==MAV_STATE_EMERGENCY) _hal->setState(MAV_STATE_STANDBY); - if (_hal->rc[_chMode]->getRadioPosition() > 0) _mode = MAV_MODE_MANUAL; - else _mode = MAV_MODE_AUTO; - } + // determine flight mode + // + // check for heartbeat from gcs, if not found go to failsafe + if (_hal->heartBeatLost()) { + _mode = MAV_MODE_FAILSAFE; + _hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat")); + // if battery less than 5%, go to failsafe + } else if (_hal->batteryMonitor && _hal->batteryMonitor->getPercentage() < 5) { + _mode = MAV_MODE_FAILSAFE; + _hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery")); + // manual/auto switch + } else { + // if all emergencies cleared, fall back to standby + if (_hal->getState()==MAV_STATE_EMERGENCY) _hal->setState(MAV_STATE_STANDBY); + if (_hal->rc[_chMode]->getRadioPosition() > 0) _mode = MAV_MODE_MANUAL; + else _mode = MAV_MODE_AUTO; + } - // handle flight modes - switch(_mode) { + // handle flight modes + switch(_mode) { - case MAV_MODE_LOCKED: { - _hal->setState(MAV_STATE_STANDBY); - break; - } + case MAV_MODE_LOCKED: { + _hal->setState(MAV_STATE_STANDBY); + break; + } - case MAV_MODE_FAILSAFE: { - _hal->setState(MAV_STATE_EMERGENCY); - break; - } + case MAV_MODE_FAILSAFE: { + _hal->setState(MAV_STATE_EMERGENCY); + break; + } - case MAV_MODE_MANUAL: { - manualLoop(dt); - break; - } + case MAV_MODE_MANUAL: { + manualLoop(dt); + break; + } - case MAV_MODE_AUTO: { - autoLoop(dt); - break; - } + case MAV_MODE_AUTO: { + autoLoop(dt); + break; + } - default: { - _hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode")); - _hal->setState(MAV_STATE_EMERGENCY); - } - } + default: { + _hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode")); + _hal->setState(MAV_STATE_EMERGENCY); + } + } - // this sends commands to motors - setMotors(); + // this sends commands to motors + setMotors(); } } // namespace apo diff --git a/libraries/APO/AP_Controller.h b/libraries/APO/AP_Controller.h index bbf5bee718..1a098e434a 100644 --- a/libraries/APO/AP_Controller.h +++ b/libraries/APO/AP_Controller.h @@ -38,281 +38,281 @@ class AP_ArmingMechanism; /// Controller class class AP_Controller { public: - AP_Controller(AP_Navigator * nav, AP_Guide * guide, - AP_HardwareAbstractionLayer * hal, - AP_ArmingMechanism * armingMechanism, - const uint8_t _chMode, - const uint16_t key = k_cntrl, - const prog_char_t * name = NULL); - virtual void update(const float dt); - virtual void setMotors() = 0; + AP_Controller(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal, + AP_ArmingMechanism * armingMechanism, + const uint8_t _chMode, + const uint16_t key = k_cntrl, + const prog_char_t * name = NULL); + virtual void update(const float dt); + virtual void setMotors() = 0; void setAllRadioChannelsToNeutral(); - void setAllRadioChannelsManually(); - virtual void manualLoop(const float dt) { + void setAllRadioChannelsManually(); + virtual void manualLoop(const float dt) { setAllRadioChannelsToNeutral(); }; - virtual void autoLoop(const float dt) { + virtual void autoLoop(const float dt) { setAllRadioChannelsToNeutral(); }; AP_Uint8 getMode() { return _mode; } protected: - AP_Navigator * _nav; - AP_Guide * _guide; - AP_HardwareAbstractionLayer * _hal; - AP_ArmingMechanism * _armingMechanism; + AP_Navigator * _nav; + AP_Guide * _guide; + AP_HardwareAbstractionLayer * _hal; + AP_ArmingMechanism * _armingMechanism; uint8_t _chMode; - AP_Var_group _group; - AP_Uint8 _mode; + AP_Var_group _group; + AP_Uint8 _mode; }; class AP_ControllerBlock { public: - AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart, - uint8_t groupLength) : - _group(group), _groupStart(groupStart), - _groupEnd(groupStart + groupLength) { - } - uint8_t getGroupEnd() { - return _groupEnd; - } + AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart, + uint8_t groupLength) : + _group(group), _groupStart(groupStart), + _groupEnd(groupStart + groupLength) { + } + uint8_t getGroupEnd() { + return _groupEnd; + } protected: - AP_Var_group * _group; /// helps with parameter management - uint8_t _groupStart; - uint8_t _groupEnd; + AP_Var_group * _group; /// helps with parameter management + uint8_t _groupStart; + uint8_t _groupEnd; }; class BlockLowPass: public AP_ControllerBlock { public: - BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut, - const prog_char_t * fCutLabel = NULL) : - AP_ControllerBlock(group, groupStart, 1), - _fCut(group, groupStart, fCut, fCutLabel ? : PSTR("fCut")), - _y(0) { - } - float update(const float & input, const float & dt) { - float RC = 1 / (2 * M_PI * _fCut); // low pass filter - _y = _y + (input - _y) * (dt / (dt + RC)); - return _y; - } + BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut, + const prog_char_t * fCutLabel = NULL) : + AP_ControllerBlock(group, groupStart, 1), + _fCut(group, groupStart, fCut, fCutLabel ? : PSTR("fCut")), + _y(0) { + } + float update(const float & input, const float & dt) { + float RC = 1 / (2 * M_PI * _fCut); // low pass filter + _y = _y + (input - _y) * (dt / (dt + RC)); + return _y; + } protected: - AP_Float _fCut; - float _y; + AP_Float _fCut; + float _y; }; class BlockSaturation: public AP_ControllerBlock { public: - BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax, - const prog_char_t * yMaxLabel = NULL) : - AP_ControllerBlock(group, groupStart, 1), - _yMax(group, groupStart, yMax, yMaxLabel ? : PSTR("yMax")) { - } - float update(const float & input) { + BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax, + const prog_char_t * yMaxLabel = NULL) : + AP_ControllerBlock(group, groupStart, 1), + _yMax(group, groupStart, yMax, yMaxLabel ? : PSTR("yMax")) { + } + float update(const float & input) { - // pid sum - float y = input; + // pid sum + float y = input; - // saturation - if (y > _yMax) - y = _yMax; - if (y < -_yMax) - y = -_yMax; - return y; - } + // saturation + if (y > _yMax) + y = _yMax; + if (y < -_yMax) + y = -_yMax; + return y; + } protected: - AP_Float _yMax; /// output saturation + AP_Float _yMax; /// output saturation }; class BlockDerivative { public: - BlockDerivative() : - _lastInput(0), firstRun(true) { - } - float update(const float & input, const float & dt) { - float derivative = (input - _lastInput) / dt; - _lastInput = input; - if (firstRun) { - firstRun = false; - return 0; - } else - return derivative; - } + BlockDerivative() : + _lastInput(0), firstRun(true) { + } + float update(const float & input, const float & dt) { + float derivative = (input - _lastInput) / dt; + _lastInput = input; + if (firstRun) { + firstRun = false; + return 0; + } else + return derivative; + } protected: - float _lastInput; /// last input - bool firstRun; + float _lastInput; /// last input + bool firstRun; }; class BlockIntegral { public: - BlockIntegral() : - _i(0) { - } - float update(const float & input, const float & dt) { - _i += input * dt; - return _i; - } + BlockIntegral() : + _i(0) { + } + float update(const float & input, const float & dt) { + _i += input * dt; + return _i; + } protected: - float _i; /// integral + float _i; /// integral }; class BlockP: public AP_ControllerBlock { public: - BlockP(AP_Var_group * group, uint8_t groupStart, float kP, - const prog_char_t * kPLabel = NULL) : - AP_ControllerBlock(group, groupStart, 1), - _kP(group, groupStart, kP, kPLabel ? : PSTR("p")) { - } + BlockP(AP_Var_group * group, uint8_t groupStart, float kP, + const prog_char_t * kPLabel = NULL) : + AP_ControllerBlock(group, groupStart, 1), + _kP(group, groupStart, kP, kPLabel ? : PSTR("p")) { + } - float update(const float & input) { - return _kP * input; - } + float update(const float & input) { + return _kP * input; + } protected: - AP_Float _kP; /// proportional gain + AP_Float _kP; /// proportional gain }; class BlockI: public AP_ControllerBlock { public: - BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax, - const prog_char_t * kILabel = NULL, - const prog_char_t * iMaxLabel = NULL) : - AP_ControllerBlock(group, groupStart, 2), - _kI(group, groupStart, kI, kILabel ? : PSTR("i")), - _blockSaturation(group, groupStart + 1, iMax, iMaxLabel ? : PSTR("iMax")), - _eI(0) { - } + BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax, + const prog_char_t * kILabel = NULL, + const prog_char_t * iMaxLabel = NULL) : + AP_ControllerBlock(group, groupStart, 2), + _kI(group, groupStart, kI, kILabel ? : PSTR("i")), + _blockSaturation(group, groupStart + 1, iMax, iMaxLabel ? : PSTR("iMax")), + _eI(0) { + } - float update(const float & input, const float & dt) { - // integral - _eI += input * dt; - _eI = _blockSaturation.update(_eI); - return _kI * _eI; - } + float update(const float & input, const float & dt) { + // integral + _eI += input * dt; + _eI = _blockSaturation.update(_eI); + return _kI * _eI; + } protected: - AP_Float _kI; /// integral gain - BlockSaturation _blockSaturation; /// integrator saturation - float _eI; /// integral of input + AP_Float _kI; /// integral gain + BlockSaturation _blockSaturation; /// integrator saturation + float _eI; /// integral of input }; class BlockD: public AP_ControllerBlock { public: - BlockD(AP_Var_group * group, uint8_t groupStart, float kD, uint8_t dFCut, - const prog_char_t * kDLabel = NULL, - const prog_char_t * dFCutLabel = NULL) : - AP_ControllerBlock(group, groupStart, 2), - _blockLowPass(group, groupStart, dFCut, - dFCutLabel ? : PSTR("dFCut")), - _kD(group, _blockLowPass.getGroupEnd(), kD, - kDLabel ? : PSTR("d")), _eP(0) { - } - float update(const float & input, const float & dt) { - // derivative with low pass - float _eD = _blockLowPass.update((_eP - input) / dt, dt); - // proportional, note must come after derivative - // because derivatve uses _eP as previous value - _eP = input; - return _kD * _eD; - } + BlockD(AP_Var_group * group, uint8_t groupStart, float kD, uint8_t dFCut, + const prog_char_t * kDLabel = NULL, + const prog_char_t * dFCutLabel = NULL) : + AP_ControllerBlock(group, groupStart, 2), + _blockLowPass(group, groupStart, dFCut, + dFCutLabel ? : PSTR("dFCut")), + _kD(group, _blockLowPass.getGroupEnd(), kD, + kDLabel ? : PSTR("d")), _eP(0) { + } + float update(const float & input, const float & dt) { + // derivative with low pass + float _eD = _blockLowPass.update((_eP - input) / dt, dt); + // proportional, note must come after derivative + // because derivatve uses _eP as previous value + _eP = input; + return _kD * _eD; + } protected: - BlockLowPass _blockLowPass; - AP_Float _kD; /// derivative gain - float _eP; /// input + BlockLowPass _blockLowPass; + AP_Float _kD; /// derivative gain + float _eP; /// input }; class BlockPID: public AP_ControllerBlock { public: - BlockPID(AP_Var_group * group, uint8_t groupStart, float kP, float kI, - float kD, float iMax, float yMax, uint8_t dFcut) : - AP_ControllerBlock(group, groupStart, 6), - _blockP(group, groupStart, kP), - _blockI(group, _blockP.getGroupEnd(), kI, iMax), - _blockD(group, _blockI.getGroupEnd(), kD, dFcut), - _blockSaturation(group, _blockD.getGroupEnd(), yMax) { - } + BlockPID(AP_Var_group * group, uint8_t groupStart, float kP, float kI, + float kD, float iMax, float yMax, uint8_t dFcut) : + AP_ControllerBlock(group, groupStart, 6), + _blockP(group, groupStart, kP), + _blockI(group, _blockP.getGroupEnd(), kI, iMax), + _blockD(group, _blockI.getGroupEnd(), kD, dFcut), + _blockSaturation(group, _blockD.getGroupEnd(), yMax) { + } - float update(const float & input, const float & dt) { - return _blockSaturation.update( - _blockP.update(input) + _blockI.update(input, dt) - + _blockD.update(input, dt)); - } + float update(const float & input, const float & dt) { + return _blockSaturation.update( + _blockP.update(input) + _blockI.update(input, dt) + + _blockD.update(input, dt)); + } protected: - BlockP _blockP; - BlockI _blockI; - BlockD _blockD; - BlockSaturation _blockSaturation; + BlockP _blockP; + BlockI _blockI; + BlockD _blockD; + BlockSaturation _blockSaturation; }; class BlockPI: public AP_ControllerBlock { public: - BlockPI(AP_Var_group * group, uint8_t groupStart, float kP, float kI, - float iMax, float yMax) : - AP_ControllerBlock(group, groupStart, 4), - _blockP(group, groupStart, kP), - _blockI(group, _blockP.getGroupEnd(), kI, iMax), - _blockSaturation(group, _blockI.getGroupEnd(), yMax) { - } + BlockPI(AP_Var_group * group, uint8_t groupStart, float kP, float kI, + float iMax, float yMax) : + AP_ControllerBlock(group, groupStart, 4), + _blockP(group, groupStart, kP), + _blockI(group, _blockP.getGroupEnd(), kI, iMax), + _blockSaturation(group, _blockI.getGroupEnd(), yMax) { + } - float update(const float & input, const float & dt) { + float update(const float & input, const float & dt) { - float y = _blockP.update(input) + _blockI.update(input, dt); - return _blockSaturation.update(y); - } + float y = _blockP.update(input) + _blockI.update(input, dt); + return _blockSaturation.update(y); + } protected: - BlockP _blockP; - BlockI _blockI; - BlockSaturation _blockSaturation; + BlockP _blockP; + BlockI _blockI; + BlockSaturation _blockSaturation; }; class BlockPD: public AP_ControllerBlock { public: - BlockPD(AP_Var_group * group, uint8_t groupStart, float kP, float kI, - float kD, float iMax, float yMax, uint8_t dFcut) : - AP_ControllerBlock(group, groupStart, 4), - _blockP(group, groupStart, kP), - _blockD(group, _blockP.getGroupEnd(), kD, dFcut), - _blockSaturation(group, _blockD.getGroupEnd(), yMax) { - } + BlockPD(AP_Var_group * group, uint8_t groupStart, float kP, float kI, + float kD, float iMax, float yMax, uint8_t dFcut) : + AP_ControllerBlock(group, groupStart, 4), + _blockP(group, groupStart, kP), + _blockD(group, _blockP.getGroupEnd(), kD, dFcut), + _blockSaturation(group, _blockD.getGroupEnd(), yMax) { + } - float update(const float & input, const float & dt) { + float update(const float & input, const float & dt) { - float y = _blockP.update(input) + _blockD.update(input, dt); - return _blockSaturation.update(y); - } + float y = _blockP.update(input) + _blockD.update(input, dt); + return _blockSaturation.update(y); + } protected: - BlockP _blockP; - BlockD _blockD; - BlockSaturation _blockSaturation; + BlockP _blockP; + BlockD _blockD; + BlockSaturation _blockSaturation; }; /// PID with derivative feedback (ignores reference derivative) class BlockPIDDfb: public AP_ControllerBlock { public: - BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI, - 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), - _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 - * _blockLowPass.update(derivative,dt); - return _blockSaturation.update(y); - } + BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI, + 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), + _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 + * _blockLowPass.update(derivative,dt); + return _blockSaturation.update(y); + } protected: - BlockP _blockP; - BlockI _blockI; - BlockSaturation _blockSaturation; - BlockLowPass _blockLowPass; - AP_Float _kD; /// derivative gain + BlockP _blockP; + BlockI _blockI; + BlockSaturation _blockSaturation; + BlockLowPass _blockLowPass; + AP_Float _kD; /// derivative gain }; } // apo diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp index 08973db522..b2bec51034 100644 --- a/libraries/APO/AP_Guide.cpp +++ b/libraries/APO/AP_Guide.cpp @@ -15,65 +15,65 @@ 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) { + _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); +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, float velCmd, float xt, float xtLim) : - AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(), - _rangeFinderLeft(), _rangeFinderRight(), - _group(k_guide, PSTR("guide_")), - _velocityCommand(&_group, 1, velCmd, PSTR("velCmd")), - _crossTrackGain(&_group, 2, xt, PSTR("xt")), - _crossTrackLim(&_group, 3, xtLim, PSTR("xtLim")) { + AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) : + AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(), + _rangeFinderLeft(), _rangeFinderRight(), + _group(k_guide, PSTR("guide_")), + _velocityCommand(&_group, 1, velCmd, PSTR("velCmd")), + _crossTrackGain(&_group, 2, xt, PSTR("xt")), + _crossTrackLim(&_group, 3, xtLim, 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; + 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(); + // 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; + // 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; + //airSpeedCommand = 0; + //groundSpeedCommand = 0; // _headingCommand -= 45 * deg2Rad; // _hal->debug->print("Obstacle Distance (m): "); // _hal->debug->println(frontDistance); @@ -82,120 +82,120 @@ void MavlinkGuide::update() { // _hal->debug->printf_P( // PSTR("Front Distance, %f\n"), // frontDistance); - } - if (_rangeFinderBack && _rangeFinderBack->distance < 5) { - _airSpeedCommand = 0; - _groundSpeedCommand = 0; + } + if (_rangeFinderBack && _rangeFinderBack->distance < 5) { + _airSpeedCommand = 0; + _groundSpeedCommand = 0; - } + } - if (_rangeFinderLeft && _rangeFinderLeft->distance < 5) { - _airSpeedCommand = 0; - _groundSpeedCommand = 0; - } + if (_rangeFinderLeft && _rangeFinderLeft->distance < 5) { + _airSpeedCommand = 0; + _groundSpeedCommand = 0; + } - if (_rangeFinderRight && _rangeFinderRight->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; - } + // 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()); + _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()) { + // TODO handle more commands + switch (_command.getCommand()) { - case MAV_CMD_NAV_WAYPOINT: { + 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; + // 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); - } + //_hal->debug->printf_P(PSTR("going home: bearing: %f distance: %f\n"), + //headingCommand,AP_MavlinkCommand::home.distanceTo(_navigator->getLat_degInt(),_navigator->getLon_degInt())); - _groundSpeedCommand = _velocityCommand; + // 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); + } - // 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()); + _groundSpeedCommand = _velocityCommand; - // debug - //_hal->debug->printf_P( - //PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"), - //getNumberOfCommands(), - //getCurrentIndex(), - //getPreviousIndex()); + // 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()); - break; - } + // 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: @@ -224,12 +224,12 @@ void MavlinkGuide::handleCommand() { // 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; - } + default: + // unhandled command, skip + Serial.println("unhandled command"); + nextCommand(); + break; + } } } // namespace apo diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index 4535bbfddd..bf5c8bc663 100644 --- a/libraries/APO/AP_Guide.h +++ b/libraries/APO/AP_Guide.h @@ -33,112 +33,112 @@ class AP_HardwareAbstractionLayer; class AP_Guide { 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); + /** + * 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); - virtual void update() = 0; + virtual void update() = 0; - virtual void nextCommand() = 0; + virtual void nextCommand() = 0; - MAV_NAV getMode() const { - return _mode; - } - uint8_t getCurrentIndex() { - return _cmdIndex; - } + MAV_NAV getMode() const { + return _mode; + } + uint8_t getCurrentIndex() { + return _cmdIndex; + } - void setCurrentIndex(uint8_t val); + void setCurrentIndex(uint8_t val); - uint8_t getNumberOfCommands() { - return _numberOfCommands; - } + uint8_t getNumberOfCommands() { + return _numberOfCommands; + } - void setNumberOfCommands(uint8_t val) { - _numberOfCommands.set_and_save(val); - } + void setNumberOfCommands(uint8_t val) { + _numberOfCommands.set_and_save(val); + } - uint8_t getPreviousIndex() { - // find previous waypoint, TODO, handle non-nav commands - int16_t prevIndex = int16_t(getCurrentIndex()) - 1; - if (prevIndex < 0) - prevIndex = getNumberOfCommands() - 1; - return (uint8_t) prevIndex; - } + uint8_t getPreviousIndex() { + // find previous waypoint, TODO, handle non-nav commands + int16_t prevIndex = int16_t(getCurrentIndex()) - 1; + if (prevIndex < 0) + prevIndex = getNumberOfCommands() - 1; + return (uint8_t) prevIndex; + } - uint8_t getNextIndex() { - // find previous waypoint, TODO, handle non-nav commands - int16_t nextIndex = int16_t(getCurrentIndex()) + 1; - if (nextIndex > (getNumberOfCommands() - 1)) - nextIndex = 0; - return nextIndex; - } + uint8_t getNextIndex() { + // find previous waypoint, TODO, handle non-nav commands + int16_t nextIndex = int16_t(getCurrentIndex()) + 1; + if (nextIndex > (getNumberOfCommands() - 1)) + nextIndex = 0; + return nextIndex; + } - float getHeadingCommand() { - return _headingCommand; - } - float getAirSpeedCommand() { - return _airSpeedCommand; - } - float getGroundSpeedCommand() { - return _groundSpeedCommand; - } - float getAltitudeCommand() { - return _altitudeCommand; - } - float getPNCmd() { - return _pNCmd; - } - float getPECmd() { - return _pECmd; - } - float getPDCmd() { - return _pDCmd; - } - MAV_NAV getMode() { - return _mode; - } - uint8_t getCommandIndex() { - return _cmdIndex; - } + float getHeadingCommand() { + return _headingCommand; + } + float getAirSpeedCommand() { + return _airSpeedCommand; + } + float getGroundSpeedCommand() { + return _groundSpeedCommand; + } + float getAltitudeCommand() { + return _altitudeCommand; + } + float getPNCmd() { + return _pNCmd; + } + float getPECmd() { + return _pECmd; + } + float getPDCmd() { + return _pDCmd; + } + MAV_NAV getMode() { + return _mode; + } + uint8_t getCommandIndex() { + return _cmdIndex; + } protected: - AP_Navigator * _navigator; - AP_HardwareAbstractionLayer * _hal; - AP_MavlinkCommand _command, _previousCommand; - float _headingCommand; - float _airSpeedCommand; - float _groundSpeedCommand; - float _altitudeCommand; - float _pNCmd; - float _pECmd; - float _pDCmd; - MAV_NAV _mode; - AP_Uint8 _numberOfCommands; - AP_Uint8 _cmdIndex; - uint16_t _nextCommandCalls; - uint16_t _nextCommandTimer; + AP_Navigator * _navigator; + AP_HardwareAbstractionLayer * _hal; + AP_MavlinkCommand _command, _previousCommand; + float _headingCommand; + float _airSpeedCommand; + float _groundSpeedCommand; + float _altitudeCommand; + float _pNCmd; + float _pECmd; + float _pDCmd; + MAV_NAV _mode; + AP_Uint8 _numberOfCommands; + AP_Uint8 _cmdIndex; + uint16_t _nextCommandCalls; + uint16_t _nextCommandTimer; }; class MavlinkGuide: public AP_Guide { public: - MavlinkGuide(AP_Navigator * navigator, - AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim); - virtual void update(); - void nextCommand(); - void handleCommand(); + MavlinkGuide(AP_Navigator * navigator, + AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim); + virtual void update(); + void nextCommand(); + void handleCommand(); private: - RangeFinder * _rangeFinderFront; - RangeFinder * _rangeFinderBack; - RangeFinder * _rangeFinderLeft; - RangeFinder * _rangeFinderRight; - AP_Var_group _group; - AP_Float _velocityCommand; - AP_Float _crossTrackGain; - AP_Float _crossTrackLim; + RangeFinder * _rangeFinderFront; + RangeFinder * _rangeFinderBack; + RangeFinder * _rangeFinderLeft; + RangeFinder * _rangeFinderRight; + AP_Var_group _group; + AP_Float _velocityCommand; + AP_Float _crossTrackGain; + AP_Float _crossTrackLim; }; } // namespace apo diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_HardwareAbstractionLayer.h index a1562efd0a..7bbb3e406b 100644 --- a/libraries/APO/AP_HardwareAbstractionLayer.h +++ b/libraries/APO/AP_HardwareAbstractionLayer.h @@ -28,145 +28,146 @@ class AP_BatteryMonitor; // enumerations enum halMode_t { - MODE_LIVE, MODE_HIL_CNTL, -/*MODE_HIL_NAV*/}; + MODE_LIVE, MODE_HIL_CNTL, + /*MODE_HIL_NAV*/ +}; enum board_t { - BOARD_ARDUPILOTMEGA_1280, BOARD_ARDUPILOTMEGA_2560, BOARD_ARDUPILOTMEGA_2 + BOARD_ARDUPILOTMEGA_1280, BOARD_ARDUPILOTMEGA_2560, BOARD_ARDUPILOTMEGA_2 }; enum vehicle_t { - VEHICLE_CAR, VEHICLE_QUAD, VEHICLE_PLANE, VEHICLE_BOAT, VEHICLE_TANK + VEHICLE_CAR, VEHICLE_QUAD, VEHICLE_PLANE, VEHICLE_BOAT, VEHICLE_TANK }; 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(), batteryMonitor(), rc(), gcs(), - hil(), debug(), load(), lastHeartBeat(), - _heartBeatTimeout(heartBeatTimeout), _mode(mode), - _board(board), _vehicle(vehicle), _state(MAV_STATE_UNINIT) { + // 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(), batteryMonitor(), rc(), gcs(), + hil(), debug(), load(), lastHeartBeat(), + _heartBeatTimeout(heartBeatTimeout), _mode(mode), + _board(board), _vehicle(vehicle), _state(MAV_STATE_UNINIT) { - /* - * Board specific hardware initialization - */ - if (board == BOARD_ARDUPILOTMEGA_1280) { - slideSwitchPin = 40; - pushButtonPin = 41; - aLedPin = 37; - bLedPin = 36; - cLedPin = 35; - eepromMaxAddr = 1024; - pinMode(aLedPin, OUTPUT); // extra led - pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; - pinMode(cLedPin, OUTPUT); // gps led - pinMode(slideSwitchPin, INPUT); - pinMode(pushButtonPin, INPUT); - DDRL |= B00000100; // set port L, pint 2 to output for the relay - } else if (board == BOARD_ARDUPILOTMEGA_2560) { - slideSwitchPin = 40; - pushButtonPin = 41; - aLedPin = 37; - bLedPin = 36; - cLedPin = 35; - eepromMaxAddr = 2048; - pinMode(aLedPin, OUTPUT); // extra led - pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; - pinMode(cLedPin, OUTPUT); // gps led - pinMode(slideSwitchPin, INPUT); - pinMode(pushButtonPin, INPUT); - DDRL |= B00000100; // set port L, pint 2 to output for the relay - } else if (board == BOARD_ARDUPILOTMEGA_2) { - slideSwitchPin = 40; - pushButtonPin = 41; - aLedPin = 37; - bLedPin = 36; - cLedPin = 35; - eepromMaxAddr = 2048; - pinMode(aLedPin, OUTPUT); // extra led - pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; - pinMode(cLedPin, OUTPUT); // gps led - pinMode(slideSwitchPin, INPUT); - pinMode(pushButtonPin, INPUT); - DDRL |= B00000100; // set port L, pint 2 to output for the relay - } - } + /* + * Board specific hardware initialization + */ + if (board == BOARD_ARDUPILOTMEGA_1280) { + slideSwitchPin = 40; + pushButtonPin = 41; + aLedPin = 37; + bLedPin = 36; + cLedPin = 35; + eepromMaxAddr = 1024; + pinMode(aLedPin, OUTPUT); // extra led + pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; + pinMode(cLedPin, OUTPUT); // gps led + pinMode(slideSwitchPin, INPUT); + pinMode(pushButtonPin, INPUT); + DDRL |= B00000100; // set port L, pint 2 to output for the relay + } else if (board == BOARD_ARDUPILOTMEGA_2560) { + slideSwitchPin = 40; + pushButtonPin = 41; + aLedPin = 37; + bLedPin = 36; + cLedPin = 35; + eepromMaxAddr = 2048; + pinMode(aLedPin, OUTPUT); // extra led + pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; + pinMode(cLedPin, OUTPUT); // gps led + pinMode(slideSwitchPin, INPUT); + pinMode(pushButtonPin, INPUT); + DDRL |= B00000100; // set port L, pint 2 to output for the relay + } else if (board == BOARD_ARDUPILOTMEGA_2) { + slideSwitchPin = 40; + pushButtonPin = 41; + aLedPin = 37; + bLedPin = 36; + cLedPin = 35; + eepromMaxAddr = 2048; + pinMode(aLedPin, OUTPUT); // extra led + pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; + pinMode(cLedPin, OUTPUT); // gps led + pinMode(slideSwitchPin, INPUT); + pinMode(pushButtonPin, INPUT); + DDRL |= B00000100; // set port L, pint 2 to output for the relay + } + } - /** - * Sensors - */ - AP_ADC * adc; - GPS * gps; - APM_BMP085_Class * baro; - Compass * compass; - Vector rangeFinders; - IMU * imu; - AP_BatteryMonitor * batteryMonitor; + /** + * Sensors + */ + AP_ADC * adc; + GPS * gps; + APM_BMP085_Class * baro; + Compass * compass; + Vector rangeFinders; + IMU * imu; + AP_BatteryMonitor * batteryMonitor; - /** - * Radio Channels - */ - Vector rc; + /** + * Radio Channels + */ + Vector rc; - /** - * Communication Channels - */ - AP_CommLink * gcs; - AP_CommLink * hil; - FastSerial * debug; + /** + * Communication Channels + */ + AP_CommLink * gcs; + AP_CommLink * hil; + FastSerial * debug; - /** - * data - */ - uint8_t load; - uint32_t lastHeartBeat; + /** + * data + */ + uint8_t load; + uint32_t lastHeartBeat; - /** - * settings - */ - uint8_t slideSwitchPin; - uint8_t pushButtonPin; - uint8_t aLedPin; - uint8_t bLedPin; - uint8_t cLedPin; - uint16_t eepromMaxAddr; + /** + * settings + */ + uint8_t slideSwitchPin; + uint8_t pushButtonPin; + uint8_t aLedPin; + uint8_t bLedPin; + uint8_t cLedPin; + uint16_t eepromMaxAddr; - // accessors - halMode_t getMode() { - return _mode; - } - board_t getBoard() { - return _board; - } - vehicle_t getVehicle() { - return _vehicle; - } - MAV_STATE getState() { - return _state; - } - void setState(MAV_STATE state) { - _state = state; - } + // accessors + halMode_t getMode() { + return _mode; + } + board_t getBoard() { + return _board; + } + vehicle_t getVehicle() { + return _vehicle; + } + MAV_STATE getState() { + return _state; + } + void setState(MAV_STATE state) { + _state = state; + } - bool heartBeatLost() { - if (_heartBeatTimeout == 0) - return false; - else - return ((micros() - lastHeartBeat) / 1e6) > _heartBeatTimeout; - } + bool heartBeatLost() { + if (_heartBeatTimeout == 0) + return false; + else + return ((micros() - lastHeartBeat) / 1e6) > _heartBeatTimeout; + } private: - // enumerations - uint8_t _heartBeatTimeout; - halMode_t _mode; - board_t _board; - vehicle_t _vehicle; - MAV_STATE _state; + // enumerations + uint8_t _heartBeatTimeout; + halMode_t _mode; + board_t _board; + vehicle_t _vehicle; + MAV_STATE _state; }; } diff --git a/libraries/APO/AP_MavlinkCommand.cpp b/libraries/APO/AP_MavlinkCommand.cpp index 987e2c0dbe..e95f21572f 100644 --- a/libraries/APO/AP_MavlinkCommand.cpp +++ b/libraries/APO/AP_MavlinkCommand.cpp @@ -10,175 +10,195 @@ namespace apo { AP_MavlinkCommand::AP_MavlinkCommand(const AP_MavlinkCommand & v) : - _data(v._data), _seq(v._seq) { - //if (FastSerial::getInitialized(0)) Serial.println("copy ctor"); + _data(v._data), _seq(v._seq) { + //if (FastSerial::getInitialized(0)) Serial.println("copy ctor"); } AP_MavlinkCommand::AP_MavlinkCommand(uint16_t index, bool doLoad) : - _data(k_commands + index), _seq(index) { + _data(k_commands + index), _seq(index) { - if (FastSerial::getInitialized(0)) { - Serial.println("index ctor"); - Serial.println("++"); - Serial.print("index: "); Serial.println(index); - Serial.print("key: "); Serial.println(k_commands + index); - Serial.println("++"); - } + if (FastSerial::getInitialized(0)) { + Serial.println("index ctor"); + Serial.println("++"); + Serial.print("index: "); + Serial.println(index); + Serial.print("key: "); + Serial.println(k_commands + index); + Serial.println("++"); + } - // default values for structure - _data.get().command = MAV_CMD_NAV_WAYPOINT; - _data.get().autocontinue = true; - _data.get().frame = MAV_FRAME_GLOBAL; - _data.get().param1 = 0; - _data.get().param2 = 10; // radius of 10 meters - _data.get().param3 = 0; - _data.get().param4 = 0; - _data.get().x = 0; - _data.get().y = 0; - _data.get().z = 1000; + // default values for structure + _data.get().command = MAV_CMD_NAV_WAYPOINT; + _data.get().autocontinue = true; + _data.get().frame = MAV_FRAME_GLOBAL; + _data.get().param1 = 0; + _data.get().param2 = 10; // radius of 10 meters + _data.get().param3 = 0; + _data.get().param4 = 0; + _data.get().x = 0; + _data.get().y = 0; + _data.get().z = 1000; - // This is a failsafe measure to stop trying to load a command if it can't load - if (doLoad && !load()) { - Serial.println("load failed, reverting to home waypoint"); - _data = AP_MavlinkCommand::home._data; - _seq = AP_MavlinkCommand::home._seq; - } + // This is a failsafe measure to stop trying to load a command if it can't load + if (doLoad && !load()) { + Serial.println("load failed, reverting to home waypoint"); + _data = AP_MavlinkCommand::home._data; + _seq = AP_MavlinkCommand::home._seq; + } } AP_MavlinkCommand::AP_MavlinkCommand(const mavlink_waypoint_t & cmd) : - _data(k_commands + cmd.seq), _seq(cmd.seq) { - setCommand(MAV_CMD(cmd.command)); - setAutocontinue(cmd.autocontinue); - setFrame(MAV_FRAME(cmd.frame)); - setParam1(cmd.param1); - setParam2(cmd.param2); - setParam3(cmd.param3); - setParam4(cmd.param4); - setX(cmd.x); - setY(cmd.y); - setZ(cmd.z); - save(); + _data(k_commands + cmd.seq), _seq(cmd.seq) { + setCommand(MAV_CMD(cmd.command)); + setAutocontinue(cmd.autocontinue); + setFrame(MAV_FRAME(cmd.frame)); + setParam1(cmd.param1); + setParam2(cmd.param2); + setParam3(cmd.param3); + setParam4(cmd.param4); + setX(cmd.x); + setY(cmd.y); + setZ(cmd.z); + save(); - // reload home if sent - if (cmd.seq == 0) home.load(); + // reload home if sent + if (cmd.seq == 0) home.load(); - Serial.println("============================================================"); - Serial.println("storing new command from mavlink_waypoint_t"); - Serial.print("key: "); Serial.println(_data.key(),DEC); - Serial.print("number: "); Serial.println(cmd.seq,DEC); - Serial.print("command: "); Serial.println(getCommand()); - Serial.print("autocontinue: "); Serial.println(getAutocontinue(),DEC); - Serial.print("frame: "); Serial.println(getFrame(),DEC); - Serial.print("1000*param1: "); Serial.println(int(1000*getParam1()),DEC); - Serial.print("1000*param2: "); Serial.println(int(1000*getParam2()),DEC); - Serial.print("1000*param3: "); Serial.println(int(1000*getParam3()),DEC); - Serial.print("1000*param4: "); Serial.println(int(1000*getParam4()),DEC); - Serial.print("1000*x0: "); Serial.println(int(1000*cmd.x),DEC); - Serial.print("1000*y0: "); Serial.println(int(1000*cmd.y),DEC); - Serial.print("1000*z0: "); Serial.println(int(1000*cmd.z),DEC); - Serial.print("1000*x: "); Serial.println(int(1000*getX()),DEC); - Serial.print("1000*y: "); Serial.println(int(1000*getY()),DEC); - Serial.print("1000*z: "); Serial.println(int(1000*getZ()),DEC); + Serial.println("============================================================"); + Serial.println("storing new command from mavlink_waypoint_t"); + Serial.print("key: "); + Serial.println(_data.key(),DEC); + Serial.print("number: "); + Serial.println(cmd.seq,DEC); + Serial.print("command: "); + Serial.println(getCommand()); + Serial.print("autocontinue: "); + Serial.println(getAutocontinue(),DEC); + Serial.print("frame: "); + Serial.println(getFrame(),DEC); + Serial.print("1000*param1: "); + Serial.println(int(1000*getParam1()),DEC); + Serial.print("1000*param2: "); + Serial.println(int(1000*getParam2()),DEC); + Serial.print("1000*param3: "); + Serial.println(int(1000*getParam3()),DEC); + Serial.print("1000*param4: "); + Serial.println(int(1000*getParam4()),DEC); + Serial.print("1000*x0: "); + Serial.println(int(1000*cmd.x),DEC); + Serial.print("1000*y0: "); + Serial.println(int(1000*cmd.y),DEC); + Serial.print("1000*z0: "); + Serial.println(int(1000*cmd.z),DEC); + Serial.print("1000*x: "); + Serial.println(int(1000*getX()),DEC); + Serial.print("1000*y: "); + Serial.println(int(1000*getY()),DEC); + Serial.print("1000*z: "); + Serial.println(int(1000*getZ()),DEC); - load(); + load(); - Serial.print("1000*x1: "); Serial.println(int(1000*getX()),DEC); - Serial.print("1000*y1: "); Serial.println(int(1000*getY()),DEC); - Serial.print("1000*z1: "); Serial.println(int(1000*getZ()),DEC); - Serial.println("============================================================"); - Serial.flush(); + Serial.print("1000*x1: "); + Serial.println(int(1000*getX()),DEC); + Serial.print("1000*y1: "); + Serial.println(int(1000*getY()),DEC); + Serial.print("1000*z1: "); + Serial.println(int(1000*getZ()),DEC); + Serial.println("============================================================"); + Serial.flush(); } mavlink_waypoint_t AP_MavlinkCommand::convert(uint8_t current) const { - mavlink_waypoint_t mavCmd; - mavCmd.seq = getSeq(); - mavCmd.command = getCommand(); - mavCmd.frame = getFrame(); - mavCmd.param1 = getParam1(); - mavCmd.param2 = getParam2(); - mavCmd.param3 = getParam3(); - mavCmd.param4 = getParam4(); - mavCmd.x = getX(); - mavCmd.y = getY(); - mavCmd.z = getZ(); - mavCmd.autocontinue = getAutocontinue(); - mavCmd.current = (getSeq() == current); - mavCmd.target_component = mavlink_system.compid; - mavCmd.target_system = mavlink_system.sysid; - return mavCmd; + mavlink_waypoint_t mavCmd; + mavCmd.seq = getSeq(); + mavCmd.command = getCommand(); + mavCmd.frame = getFrame(); + mavCmd.param1 = getParam1(); + mavCmd.param2 = getParam2(); + mavCmd.param3 = getParam3(); + mavCmd.param4 = getParam4(); + mavCmd.x = getX(); + mavCmd.y = getY(); + mavCmd.z = getZ(); + mavCmd.autocontinue = getAutocontinue(); + mavCmd.current = (getSeq() == current); + mavCmd.target_component = mavlink_system.compid; + mavCmd.target_system = mavlink_system.sysid; + return mavCmd; } float AP_MavlinkCommand::bearingTo(const AP_MavlinkCommand & next) const { - float deltaLon = next.getLon() - getLon(); - /* - Serial.print("Lon: "); Serial.println(getLon()); - Serial.print("nextLon: "); Serial.println(next.getLon()); - Serial.print("deltaLonDeg * 1e7: "); Serial.println(deltaLon*rad2DegInt); - */ - float bearing = atan2( - sin(deltaLon) * cos(next.getLat()), - cos(getLat()) * sin(next.getLat()) - sin(getLat()) * cos( - next.getLat()) * cos(deltaLon)); - return bearing; + float deltaLon = next.getLon() - getLon(); + /* + Serial.print("Lon: "); Serial.println(getLon()); + Serial.print("nextLon: "); Serial.println(next.getLon()); + Serial.print("deltaLonDeg * 1e7: "); Serial.println(deltaLon*rad2DegInt); + */ + float bearing = atan2( + sin(deltaLon) * cos(next.getLat()), + cos(getLat()) * sin(next.getLat()) - sin(getLat()) * cos( + next.getLat()) * cos(deltaLon)); + return bearing; } float AP_MavlinkCommand::bearingTo(int32_t latDegInt, int32_t lonDegInt) const { - // have to be careful to maintain the precision of the gps coordinate - float deltaLon = (lonDegInt - getLon_degInt()) * degInt2Rad; - float nextLat = latDegInt * degInt2Rad; - float bearing = atan2( - sin(deltaLon) * cos(nextLat), - cos(getLat()) * sin(nextLat) - sin(getLat()) * cos(nextLat) - * cos(deltaLon)); - if (bearing < 0) - bearing += 2 * M_PI; - return bearing; + // have to be careful to maintain the precision of the gps coordinate + float deltaLon = (lonDegInt - getLon_degInt()) * degInt2Rad; + float nextLat = latDegInt * degInt2Rad; + float bearing = atan2( + sin(deltaLon) * cos(nextLat), + cos(getLat()) * sin(nextLat) - sin(getLat()) * cos(nextLat) + * cos(deltaLon)); + if (bearing < 0) + bearing += 2 * M_PI; + return bearing; } float AP_MavlinkCommand::distanceTo(const AP_MavlinkCommand & next) const { - float sinDeltaLat2 = sin((getLat() - next.getLat()) / 2); - float sinDeltaLon2 = sin((getLon() - next.getLon()) / 2); - float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos( - next.getLat()) * sinDeltaLon2 * sinDeltaLon2; - float c = 2 * atan2(sqrt(a), sqrt(1 - a)); - return rEarth * c; + float sinDeltaLat2 = sin((getLat() - next.getLat()) / 2); + float sinDeltaLon2 = sin((getLon() - next.getLon()) / 2); + float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos( + next.getLat()) * sinDeltaLon2 * sinDeltaLon2; + float c = 2 * atan2(sqrt(a), sqrt(1 - a)); + return rEarth * c; } float AP_MavlinkCommand::distanceTo(int32_t lat_degInt, int32_t lon_degInt) const { - float sinDeltaLat2 = sin( - (lat_degInt - getLat_degInt()) * degInt2Rad / 2); - float sinDeltaLon2 = sin( - (lon_degInt - getLon_degInt()) * degInt2Rad / 2); - float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos( - lat_degInt * degInt2Rad) * sinDeltaLon2 * sinDeltaLon2; - float c = 2 * atan2(sqrt(a), sqrt(1 - a)); - /* - Serial.print("wp lat_degInt: "); Serial.println(getLat_degInt()); - Serial.print("wp lon_degInt: "); Serial.println(getLon_degInt()); - Serial.print("lat_degInt: "); Serial.println(lat_degInt); - Serial.print("lon_degInt: "); Serial.println(lon_degInt); - Serial.print("sinDeltaLat2: "); Serial.println(sinDeltaLat2); - Serial.print("sinDeltaLon2: "); Serial.println(sinDeltaLon2); - */ - return rEarth * c; + float sinDeltaLat2 = sin( + (lat_degInt - getLat_degInt()) * degInt2Rad / 2); + float sinDeltaLon2 = sin( + (lon_degInt - getLon_degInt()) * degInt2Rad / 2); + float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos( + lat_degInt * degInt2Rad) * sinDeltaLon2 * sinDeltaLon2; + float c = 2 * atan2(sqrt(a), sqrt(1 - a)); + /* + Serial.print("wp lat_degInt: "); Serial.println(getLat_degInt()); + Serial.print("wp lon_degInt: "); Serial.println(getLon_degInt()); + Serial.print("lat_degInt: "); Serial.println(lat_degInt); + Serial.print("lon_degInt: "); Serial.println(lon_degInt); + Serial.print("sinDeltaLat2: "); Serial.println(sinDeltaLat2); + Serial.print("sinDeltaLon2: "); Serial.println(sinDeltaLon2); + */ + return rEarth * c; } //calculates cross track of a current location float AP_MavlinkCommand::crossTrack(const AP_MavlinkCommand & previous, - int32_t lat_degInt, int32_t lon_degInt) const { - float d = previous.distanceTo(lat_degInt, lon_degInt); - float bCurrent = previous.bearingTo(lat_degInt, lon_degInt); - float bNext = previous.bearingTo(*this); - return asin(sin(d / rEarth) * sin(bCurrent - bNext)) * rEarth; + int32_t lat_degInt, int32_t lon_degInt) const { + float d = previous.distanceTo(lat_degInt, lon_degInt); + float bCurrent = previous.bearingTo(lat_degInt, lon_degInt); + float bNext = previous.bearingTo(*this); + return asin(sin(d / rEarth) * sin(bCurrent - bNext)) * rEarth; } // calculates along track distance of a current location float AP_MavlinkCommand::alongTrack(const AP_MavlinkCommand & previous, - int32_t lat_degInt, int32_t lon_degInt) const { - // ignores lat/lon since single prec. - float dXt = this->crossTrack(previous,lat_degInt, lon_degInt); - float d = previous.distanceTo(lat_degInt, lon_degInt); - return dXt / tan(asin(dXt / d)); + int32_t lat_degInt, int32_t lon_degInt) const { + // ignores lat/lon since single prec. + float dXt = this->crossTrack(previous,lat_degInt, lon_degInt); + float d = previous.distanceTo(lat_degInt, lon_degInt); + return dXt / tan(asin(dXt / d)); } diff --git a/libraries/APO/AP_MavlinkCommand.h b/libraries/APO/AP_MavlinkCommand.h index a50002cb95..067ad0c2d2 100644 --- a/libraries/APO/AP_MavlinkCommand.h +++ b/libraries/APO/AP_MavlinkCommand.h @@ -18,355 +18,355 @@ namespace apo { class AP_MavlinkCommand { private: - struct CommandStorage { - MAV_CMD command; - bool autocontinue; - MAV_FRAME frame; - float param1; - float param2; - float param3; - float param4; - float x; - float y; - float z; - }; - AP_VarS _data; - uint16_t _seq; + struct CommandStorage { + MAV_CMD command; + bool autocontinue; + MAV_FRAME frame; + float param1; + float param2; + float param3; + float param4; + float x; + float y; + float z; + }; + AP_VarS _data; + uint16_t _seq; public: - static AP_MavlinkCommand home; + static AP_MavlinkCommand home; - /** - * Copy Constructor - */ - AP_MavlinkCommand(const AP_MavlinkCommand & v); + /** + * Copy Constructor + */ + AP_MavlinkCommand(const AP_MavlinkCommand & v); - /** - * Basic Constructor - * @param index Start at zero. - */ - AP_MavlinkCommand(uint16_t index, bool doLoad = true); + /** + * Basic Constructor + * @param index Start at zero. + */ + AP_MavlinkCommand(uint16_t index, bool doLoad = true); - /** - * Constructor for copying/ saving from a mavlink waypoint. - * @param cmd The mavlink_waopint_t structure for the command. - */ - AP_MavlinkCommand(const mavlink_waypoint_t & cmd); + /** + * Constructor for copying/ saving from a mavlink waypoint. + * @param cmd The mavlink_waopint_t structure for the command. + */ + AP_MavlinkCommand(const mavlink_waypoint_t & cmd); - bool save() { - return _data.save(); - } - bool load() { - return _data.load(); - } - uint8_t getSeq() const { - return _seq; - } - bool getAutocontinue() const { - return _data.get().autocontinue; - } - void setAutocontinue( bool val) { - _data.get().autocontinue = val; - } - void setSeq(uint8_t val) { - _seq = val; - } - MAV_CMD getCommand() const { - return _data.get().command; - } - void setCommand(MAV_CMD val) { - _data.get().command = val; - } - MAV_FRAME getFrame() const { - return _data.get().frame; - } - void setFrame(MAV_FRAME val) { - _data.get().frame = val; - } - float getParam1() const { - return _data.get().param1; - } - void setParam1(float val) { - _data.get().param1 = val; - } - float getParam2() const { - return _data.get().param2; - } - void setParam2(float val) { - _data.get().param2 = val; - } - float getParam3() const { - return _data.get().param3; - } - void setParam3(float val) { - _data.get().param3 = val; - } - float getParam4() const { - return _data.get().param4; - } - void setParam4(float val) { - _data.get().param4 = val; - } - float getX() const { - return _data.get().x; - } - void setX(float val) { - _data.get().x = val; - } - float getY() const { - return _data.get().y; - } - void setY(float val) { - _data.get().y = val; - } - float getZ() const { - return _data.get().z; - } - void setZ(float val) { - _data.get().z = val; - } - float getLatDeg() const { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - return getX(); - break; - case MAV_FRAME_LOCAL: - case MAV_FRAME_LOCAL_ENU: - case MAV_FRAME_MISSION: - default: - return 0; - break; - } - } - void setLatDeg(float val) { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - setX(val); - break; - case MAV_FRAME_LOCAL: - case MAV_FRAME_LOCAL_ENU: - case MAV_FRAME_MISSION: - default: - break; - } - } - float getLonDeg() const { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - return getY(); - break; - case MAV_FRAME_LOCAL: - case MAV_FRAME_LOCAL_ENU: - case MAV_FRAME_MISSION: - default: - return 0; - break; - } - } - void setLonDeg(float val) { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - setY(val); - break; - case MAV_FRAME_LOCAL: - case MAV_FRAME_LOCAL_ENU: - case MAV_FRAME_MISSION: - default: - break; - } - } - void setLon(float val) { - setLonDeg(val * rad2Deg); - } - void setLon_degInt(int32_t val) { - setLonDeg(val / 1.0e7); - } - void setLat_degInt(int32_t val) { - setLatDeg(val / 1.0e7); - } - int32_t getLon_degInt() const { - return getLonDeg() * 1e7; - } - int32_t getLat_degInt() const { - return getLatDeg() * 1e7; - } - float getLon() const { - return getLonDeg() * deg2Rad; - } - float getLat() const { - return getLatDeg() * deg2Rad; - } - void setLat(float val) { - setLatDeg(val * rad2Deg); - } - float getAlt() const { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - return getZ(); - break; - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - case MAV_FRAME_LOCAL: - return -getZ() + home.getAlt(); - break; - case MAV_FRAME_LOCAL_ENU: - return getZ() + home.getAlt(); - break; - case MAV_FRAME_MISSION: - default: - return 0; - break; - } - } - /** - * set the altitude in meters - */ - void setAlt(float val) { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - setZ(val); - break; - case MAV_FRAME_LOCAL: - setZ(home.getLonDeg() - val); - break; - case MAV_FRAME_LOCAL_ENU: - setZ(val - home.getLonDeg()); - break; - case MAV_FRAME_MISSION: - default: - break; - } - } - /** - * Get the relative altitude to home - * @return relative altitude in meters - */ - float getRelAlt() const { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - return getZ() - home.getAlt(); - break; - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - case MAV_FRAME_LOCAL: - return -getZ(); - break; - case MAV_FRAME_LOCAL_ENU: - return getZ(); - break; - case MAV_FRAME_MISSION: - default: - return 0; - break; - } - } - /** - * set the relative altitude in meters from home (up) - */ - void setRelAlt(float val) { - switch (getFrame()) { - case MAV_FRAME_GLOBAL: - setZ(val + home.getAlt()); - break; - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - case MAV_FRAME_LOCAL: - setZ(-val); - break; - case MAV_FRAME_LOCAL_ENU: - setZ(val); - break; - case MAV_FRAME_MISSION: - break; - } - } + bool save() { + return _data.save(); + } + bool load() { + return _data.load(); + } + uint8_t getSeq() const { + return _seq; + } + bool getAutocontinue() const { + return _data.get().autocontinue; + } + void setAutocontinue( bool val) { + _data.get().autocontinue = val; + } + void setSeq(uint8_t val) { + _seq = val; + } + MAV_CMD getCommand() const { + return _data.get().command; + } + void setCommand(MAV_CMD val) { + _data.get().command = val; + } + MAV_FRAME getFrame() const { + return _data.get().frame; + } + void setFrame(MAV_FRAME val) { + _data.get().frame = val; + } + float getParam1() const { + return _data.get().param1; + } + void setParam1(float val) { + _data.get().param1 = val; + } + float getParam2() const { + return _data.get().param2; + } + void setParam2(float val) { + _data.get().param2 = val; + } + float getParam3() const { + return _data.get().param3; + } + void setParam3(float val) { + _data.get().param3 = val; + } + float getParam4() const { + return _data.get().param4; + } + void setParam4(float val) { + _data.get().param4 = val; + } + float getX() const { + return _data.get().x; + } + void setX(float val) { + _data.get().x = val; + } + float getY() const { + return _data.get().y; + } + void setY(float val) { + _data.get().y = val; + } + float getZ() const { + return _data.get().z; + } + void setZ(float val) { + _data.get().z = val; + } + float getLatDeg() const { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + return getX(); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_LOCAL_ENU: + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + void setLatDeg(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + setX(val); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_LOCAL_ENU: + case MAV_FRAME_MISSION: + default: + break; + } + } + float getLonDeg() const { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + return getY(); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_LOCAL_ENU: + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + void setLonDeg(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + setY(val); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_LOCAL_ENU: + case MAV_FRAME_MISSION: + default: + break; + } + } + void setLon(float val) { + setLonDeg(val * rad2Deg); + } + void setLon_degInt(int32_t val) { + setLonDeg(val / 1.0e7); + } + void setLat_degInt(int32_t val) { + setLatDeg(val / 1.0e7); + } + int32_t getLon_degInt() const { + return getLonDeg() * 1e7; + } + int32_t getLat_degInt() const { + return getLatDeg() * 1e7; + } + float getLon() const { + return getLonDeg() * deg2Rad; + } + float getLat() const { + return getLatDeg() * deg2Rad; + } + void setLat(float val) { + setLatDeg(val * rad2Deg); + } + float getAlt() const { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + return getZ(); + break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_LOCAL: + return -getZ() + home.getAlt(); + break; + case MAV_FRAME_LOCAL_ENU: + return getZ() + home.getAlt(); + break; + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + /** + * set the altitude in meters + */ + void setAlt(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + setZ(val); + break; + case MAV_FRAME_LOCAL: + setZ(home.getLonDeg() - val); + break; + case MAV_FRAME_LOCAL_ENU: + setZ(val - home.getLonDeg()); + break; + case MAV_FRAME_MISSION: + default: + break; + } + } + /** + * Get the relative altitude to home + * @return relative altitude in meters + */ + float getRelAlt() const { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + return getZ() - home.getAlt(); + break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_LOCAL: + return -getZ(); + break; + case MAV_FRAME_LOCAL_ENU: + return getZ(); + break; + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + /** + * set the relative altitude in meters from home (up) + */ + void setRelAlt(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + setZ(val + home.getAlt()); + break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_LOCAL: + setZ(-val); + break; + case MAV_FRAME_LOCAL_ENU: + setZ(val); + break; + case MAV_FRAME_MISSION: + break; + } + } - float getRadius() const { - return getParam2(); - } + float getRadius() const { + return getParam2(); + } - void setRadius(float val) { - setParam2(val); - } + void setRadius(float val) { + setParam2(val); + } - /** - * conversion for outbound packets to ground station - * @return output the mavlink_waypoint_t packet - */ - mavlink_waypoint_t convert(uint8_t current) const; + /** + * conversion for outbound packets to ground station + * @return output the mavlink_waypoint_t packet + */ + mavlink_waypoint_t convert(uint8_t current) const; - /** - * Calculate the bearing from this command to the next command - * @param next The command to calculate the bearing to. - * @return the bearing - */ - float bearingTo(const AP_MavlinkCommand & next) const; + /** + * Calculate the bearing from this command to the next command + * @param next The command to calculate the bearing to. + * @return the bearing + */ + float bearingTo(const AP_MavlinkCommand & next) const; - /** - * Bearing form this command to a gps coordinate in integer units - * @param latDegInt latitude in degrees E-7 - * @param lonDegInt longitude in degrees E-7 - * @return - */ - float bearingTo(int32_t latDegInt, int32_t lonDegInt) const; + /** + * Bearing form this command to a gps coordinate in integer units + * @param latDegInt latitude in degrees E-7 + * @param lonDegInt longitude in degrees E-7 + * @return + */ + float bearingTo(int32_t latDegInt, int32_t lonDegInt) const; - /** - * Distance to another command - * @param next The command to measure to. - * @return The distance in meters. - */ - float distanceTo(const AP_MavlinkCommand & next) const; + /** + * Distance to another command + * @param next The command to measure to. + * @return The distance in meters. + */ + float distanceTo(const AP_MavlinkCommand & next) const; - /** - * Distance to a gps coordinate in integer units - * @param latDegInt latitude in degrees E-7 - * @param lonDegInt longitude in degrees E-7 - * @return The distance in meters. - */ - float distanceTo(int32_t lat_degInt, int32_t lon_degInt) const; + /** + * Distance to a gps coordinate in integer units + * @param latDegInt latitude in degrees E-7 + * @param lonDegInt longitude in degrees E-7 + * @return The distance in meters. + */ + float distanceTo(int32_t lat_degInt, int32_t lon_degInt) const; - float getPN(int32_t lat_degInt, int32_t lon_degInt) const { - // local tangent approximation at this waypoint - float deltaLat = (lat_degInt - getLat_degInt()) * degInt2Rad; - return deltaLat * rEarth; - } + float getPN(int32_t lat_degInt, int32_t lon_degInt) const { + // local tangent approximation at this waypoint + float deltaLat = (lat_degInt - getLat_degInt()) * degInt2Rad; + return deltaLat * rEarth; + } - float getPE(int32_t lat_degInt, int32_t lon_degInt) const { - // local tangent approximation at this waypoint - float deltaLon = (lon_degInt - getLon_degInt()) * degInt2Rad; - return cos(getLat()) * deltaLon * rEarth; - } + float getPE(int32_t lat_degInt, int32_t lon_degInt) const { + // local tangent approximation at this waypoint + float deltaLon = (lon_degInt - getLon_degInt()) * degInt2Rad; + return cos(getLat()) * deltaLon * rEarth; + } - float getPD(int32_t alt_intM) const { - return -(alt_intM / scale_m - getAlt()); - } + float getPD(int32_t alt_intM) const { + return -(alt_intM / scale_m - getAlt()); + } - float getLat(float pN) const { + float getLat(float pN) const { - return pN / rEarth + getLat(); - } + return pN / rEarth + getLat(); + } - float getLon(float pE) const { + float getLon(float pE) const { - return pE / rEarth / cos(getLat()) + getLon(); - } + return pE / rEarth / cos(getLat()) + getLon(); + } - /** - * Gets altitude in meters - * @param pD alt in meters - * @return - */ - float getAlt(float pD) const { + /** + * Gets altitude in meters + * @param pD alt in meters + * @return + */ + float getAlt(float pD) const { - return getAlt() - pD; - } + return getAlt() - pD; + } - //calculates cross track of a current location - float crossTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const; + //calculates cross track of a current location + float crossTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const; - // calculates along track distance of a current location - float alongTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const; + // calculates along track distance of a current location + float alongTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const; }; } // namespace apo diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp index 4260ef1461..08abd70b67 100644 --- a/libraries/APO/AP_Navigator.cpp +++ b/libraries/APO/AP_Navigator.cpp @@ -20,187 +20,187 @@ 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) { + _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()); + return AP_MavlinkCommand::home.getPD(getAlt_intM()); } float AP_Navigator::getPE() const { - return AP_MavlinkCommand::home.getPE(getLat_degInt(), getLon_degInt()); + return AP_MavlinkCommand::home.getPE(getLat_degInt(), getLon_degInt()); } float AP_Navigator::getPN() const { - return AP_MavlinkCommand::home.getPN(getLat_degInt(), getLon_degInt()); + return AP_MavlinkCommand::home.getPN(getLat_degInt(), getLon_degInt()); } void AP_Navigator::setPD(float _pD) { - setAlt(AP_MavlinkCommand::home.getAlt(_pD)); + setAlt(AP_MavlinkCommand::home.getAlt(_pD)); } void AP_Navigator::setPE(float _pE) { - setLat(AP_MavlinkCommand::home.getLat(_pE)); + setLat(AP_MavlinkCommand::home.getLat(_pE)); } void AP_Navigator::setPN(float _pN) { - setLon(AP_MavlinkCommand::home.getLon(_pN)); + setLon(AP_MavlinkCommand::home.getLon(_pN)); } DcmNavigator::DcmNavigator(AP_HardwareAbstractionLayer * hal) : - AP_Navigator(hal), _dcm(), _imuOffsetAddress(0) { + 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 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->getMode() == MODE_LIVE) { - if (_hal->adc) { - _hal->imu = new AP_IMU_Oilpan(_hal->adc, k_sensorCalib); - } + 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->imu) { + _dcm = new AP_DCM(_hal->imu, _hal->gps, _hal->compass); - // tune down dcm - _dcm->kp_roll_pitch(0.030000); - _dcm->ki_roll_pitch(0.00001278), // 50 hz I term + // tune down dcm + _dcm->kp_roll_pitch(0.030000); + _dcm->ki_roll_pitch(0.00001278), // 50 hz I term - // tune down compass in dcm - _dcm->kp_yaw(0.08); - _dcm->ki_yaw(0); - } + // tune down compass in dcm + _dcm->kp_yaw(0.08); + _dcm->ki_yaw(0); + } - if (_hal->compass) { - _dcm->set_compass(_hal->compass); - } - } + if (_hal->compass) { + _dcm->set_compass(_hal->compass); + } + } } void DcmNavigator::calibrate() { - AP_Navigator::calibrate(); + AP_Navigator::calibrate(); - // TODO: handle cold/warm restart - if (_hal->imu) { - _hal->imu->init(IMU::COLD_START,delay); - } + // 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; + if (_hal->getMode() != MODE_LIVE) + return; - setTimeStamp(micros()); // if running in live mode, record new time stamp + setTimeStamp(micros()); // if running in live mode, record new time stamp - //_hal->debug->println_P(PSTR("nav loop")); + //_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) { + /** + * 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); + 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); - } - } + 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_fast(); - setRoll(_dcm->roll); - setPitch(_dcm->pitch); - setYaw(_dcm->yaw); - setRollRate(_dcm->get_gyro().x); - setPitchRate(_dcm->get_gyro().y); - setYawRate(_dcm->get_gyro().z); + // dcm class for attitude + if (_dcm) { + _dcm->update_DCM_fast(); + 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); - */ - } + /* + * 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; + if (_hal->getMode() != MODE_LIVE) + return; - setTimeStamp(micros()); // if running in live mode, record new time stamp + 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->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); - } + 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; + // 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; + 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; - } + default: + digitalWrite(_hal->cLedPin, LOW); + break; + } } } // namespace apo diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h index 9be22e6caf..49db8c5982 100644 --- a/libraries/APO/AP_Navigator.h +++ b/libraries/APO/AP_Navigator.h @@ -33,195 +33,195 @@ class AP_HardwareAbstractionLayer; /// Navigator class class AP_Navigator { public: - AP_Navigator(AP_HardwareAbstractionLayer * hal); - virtual void calibrate(); - virtual void updateFast(float dt) = 0; - virtual void updateSlow(float dt) = 0; - float getPD() const; - float getPE() const; - float getPN() const; - void setPD(float _pD); - void setPE(float _pE); - void setPN(float _pN); + AP_Navigator(AP_HardwareAbstractionLayer * hal); + virtual void calibrate(); + virtual void updateFast(float dt) = 0; + virtual void updateSlow(float dt) = 0; + 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; - } + float getAirSpeed() const { + return _airSpeed; + } - int32_t getAlt_intM() const { - return _alt_intM; - } + int32_t getAlt_intM() const { + return _alt_intM; + } - float getAlt() const { - return _alt_intM / scale_m; - } + float getAlt() const { + return _alt_intM / scale_m; + } - void setAlt(float _alt) { - this->_alt_intM = _alt * scale_m; - } + void setAlt(float _alt) { + this->_alt_intM = _alt * scale_m; + } - float getLat() const { - //Serial.print("getLatfirst"); - //Serial.println(_lat_degInt * degInt2Rad); - return _lat_degInt * degInt2Rad; - } + float getLat() const { + //Serial.print("getLatfirst"); + //Serial.println(_lat_degInt * degInt2Rad); + return _lat_degInt * degInt2Rad; + } - void setLat(float _lat) { - //Serial.print("setLatfirst"); - //Serial.println(_lat * rad2DegInt); - setLat_degInt(_lat*rad2DegInt); - } + void setLat(float _lat) { + //Serial.print("setLatfirst"); + //Serial.println(_lat * rad2DegInt); + setLat_degInt(_lat*rad2DegInt); + } - float getLon() const { - return _lon_degInt * degInt2Rad; - } + float getLon() const { + return _lon_degInt * degInt2Rad; + } - void setLon(float _lon) { - this->_lon_degInt = _lon * rad2DegInt; - } + void setLon(float _lon) { + this->_lon_degInt = _lon * rad2DegInt; + } - float getVD() const { - return _vD; - } + float getVD() const { + return _vD; + } - float getVE() const { - return sin(getYaw()) * getGroundSpeed(); - } + float getVE() const { + return sin(getYaw()) * getGroundSpeed(); + } - float getGroundSpeed() const { - return _groundSpeed; - } + float getGroundSpeed() const { + return _groundSpeed; + } - int32_t getLat_degInt() const { - //Serial.print("getLat_degInt"); - //Serial.println(_lat_degInt); - return _lat_degInt; + int32_t getLat_degInt() const { + //Serial.print("getLat_degInt"); + //Serial.println(_lat_degInt); + return _lat_degInt; - } + } - int32_t getLon_degInt() const { - return _lon_degInt; - } + int32_t getLon_degInt() const { + return _lon_degInt; + } - float getVN() const { - return cos(getYaw()) * getGroundSpeed(); - } + float getVN() const { + return cos(getYaw()) * getGroundSpeed(); + } - float getPitch() const { - return _pitch; - } + float getPitch() const { + return _pitch; + } - float getPitchRate() const { - return _pitchRate; - } + float getPitchRate() const { + return _pitchRate; + } - float getRoll() const { - return _roll; - } + float getRoll() const { + return _roll; + } - float getRollRate() const { - return _rollRate; - } + float getRollRate() const { + return _rollRate; + } - float getYaw() const { - return _yaw; - } + float getYaw() const { + return _yaw; + } - float getYawRate() const { - return _yawRate; - } + float getYawRate() const { + return _yawRate; + } - void setAirSpeed(float airSpeed) { - _airSpeed = airSpeed; - } + void setAirSpeed(float airSpeed) { + _airSpeed = airSpeed; + } - void setAlt_intM(int32_t alt_intM) { - _alt_intM = alt_intM; - } + void setAlt_intM(int32_t alt_intM) { + _alt_intM = alt_intM; + } - void setVD(float vD) { - _vD = vD; - } + void setVD(float vD) { + _vD = vD; + } - void setGroundSpeed(float groundSpeed) { - _groundSpeed = groundSpeed; - } + void setGroundSpeed(float groundSpeed) { + _groundSpeed = groundSpeed; + } - void setLat_degInt(int32_t lat_degInt) { - _lat_degInt = lat_degInt; - //Serial.print("setLat_degInt"); - //Serial.println(_lat_degInt); - } + void setLat_degInt(int32_t lat_degInt) { + _lat_degInt = lat_degInt; + //Serial.print("setLat_degInt"); + //Serial.println(_lat_degInt); + } - void setLon_degInt(int32_t lon_degInt) { - _lon_degInt = lon_degInt; - } + void setLon_degInt(int32_t lon_degInt) { + _lon_degInt = lon_degInt; + } - void setPitch(float pitch) { - _pitch = pitch; - } + void setPitch(float pitch) { + _pitch = pitch; + } - void setPitchRate(float pitchRate) { - _pitchRate = pitchRate; - } + void setPitchRate(float pitchRate) { + _pitchRate = pitchRate; + } - void setRoll(float roll) { - _roll = roll; - } + void setRoll(float roll) { + _roll = roll; + } - void setRollRate(float rollRate) { - _rollRate = rollRate; - } + void setRollRate(float rollRate) { + _rollRate = rollRate; + } - void setYaw(float yaw) { - _yaw = yaw; - } + void setYaw(float yaw) { + _yaw = yaw; + } - void setYawRate(float yawRate) { - _yawRate = yawRate; - } - void setTimeStamp(int32_t timeStamp) { - _timeStamp = timeStamp; - } - int32_t getTimeStamp() const { - return _timeStamp; - } + void setYawRate(float yawRate) { + _yawRate = yawRate; + } + void setTimeStamp(int32_t timeStamp) { + _timeStamp = timeStamp; + } + int32_t getTimeStamp() const { + return _timeStamp; + } protected: - AP_HardwareAbstractionLayer * _hal; + AP_HardwareAbstractionLayer * _hal; private: - int32_t _timeStamp; // micros clock - float _roll; // rad - float _rollRate; //rad/s - float _pitch; // rad - float _pitchRate; // rad/s - float _yaw; // rad - float _yawRate; // rad/s - float _airSpeed; // m/s - float _groundSpeed; // m/s - float _vD; // m/s - int32_t _lat_degInt; // deg / 1e7 - int32_t _lon_degInt; // deg / 1e7 - int32_t _alt_intM; // meters / 1e3 + int32_t _timeStamp; // micros clock + float _roll; // rad + float _rollRate; //rad/s + float _pitch; // rad + float _pitchRate; // rad/s + float _yaw; // rad + float _yawRate; // rad/s + float _airSpeed; // m/s + float _groundSpeed; // m/s + float _vD; // m/s + int32_t _lat_degInt; // deg / 1e7 + int32_t _lon_degInt; // deg / 1e7 + int32_t _alt_intM; // meters / 1e3 }; class DcmNavigator: public AP_Navigator { private: - /** - * Sensors - */ + /** + * Sensors + */ - RangeFinder * _rangeFinderDown; - AP_DCM * _dcm; - IMU * _imu; - uint16_t _imuOffsetAddress; + RangeFinder * _rangeFinderDown; + AP_DCM * _dcm; + IMU * _imu; + uint16_t _imuOffsetAddress; public: - DcmNavigator(AP_HardwareAbstractionLayer * hal); - virtual void calibrate(); - virtual void updateFast(float dt); - virtual void updateSlow(float dt); - void updateGpsLight(void); + 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/APO/AP_RcChannel.cpp b/libraries/APO/AP_RcChannel.cpp index 1c1f823482..122324b563 100644 --- a/libraries/APO/AP_RcChannel.cpp +++ b/libraries/APO/AP_RcChannel.cpp @@ -18,85 +18,85 @@ namespace apo { AP_RcChannel::AP_RcChannel(AP_Var::Key keyValue, const prog_char_t * name, - APM_RC_Class & rc, const uint8_t & ch, const uint16_t & pwmMin, - const uint16_t & pwmNeutral, const uint16_t & pwmMax, - const rcMode_t & rcMode, const bool & reverse, const float & scale) : - AP_Var_group(keyValue, name), _ch(this, 1, ch, PSTR("ch")), - _pwmMin(this, 2, pwmMin, PSTR("pMin")), - _pwmNeutral(this, 3, pwmNeutral, PSTR("pNtrl")), - _pwmMax(this, 4, pwmMax, PSTR("pMax")), - _reverse(this, 5, reverse, PSTR("rev")), - _scale(scale == 0 ? AP_Float(0) : AP_Float(this,6,reverse,PSTR("scale"))), - _rcMode(rcMode), _rc(rc), _pwm(pwmNeutral) { - //Serial.print("pwm after ctor: "); Serial.println(pwmNeutral); - if (rcMode == RC_MODE_IN) - return; - //Serial.print("pwm set for ch: "); Serial.println(int(ch)); - rc.OutputCh(ch, pwmNeutral); + APM_RC_Class & rc, const uint8_t & ch, const uint16_t & pwmMin, + const uint16_t & pwmNeutral, const uint16_t & pwmMax, + const rcMode_t & rcMode, const bool & reverse, const float & scale) : + AP_Var_group(keyValue, name), _ch(this, 1, ch, PSTR("ch")), + _pwmMin(this, 2, pwmMin, PSTR("pMin")), + _pwmNeutral(this, 3, pwmNeutral, PSTR("pNtrl")), + _pwmMax(this, 4, pwmMax, PSTR("pMax")), + _reverse(this, 5, reverse, PSTR("rev")), + _scale(scale == 0 ? AP_Float(0) : AP_Float(this,6,reverse,PSTR("scale"))), + _rcMode(rcMode), _rc(rc), _pwm(pwmNeutral) { + //Serial.print("pwm after ctor: "); Serial.println(pwmNeutral); + if (rcMode == RC_MODE_IN) + return; + //Serial.print("pwm set for ch: "); Serial.println(int(ch)); + rc.OutputCh(ch, pwmNeutral); } uint16_t AP_RcChannel::getRadioPwm() { - if (_rcMode == RC_MODE_OUT) { - Serial.print("tryng to read from output channel: "); - Serial.println(int(_ch)); - return _pwmNeutral; // if this happens give a safe value of neutral - } - return _rc.InputCh(_ch); + if (_rcMode == RC_MODE_OUT) { + Serial.print("tryng to read from output channel: "); + Serial.println(int(_ch)); + return _pwmNeutral; // if this happens give a safe value of neutral + } + return _rc.InputCh(_ch); } void AP_RcChannel::setPwm(uint16_t pwm) { - //Serial.printf("pwm in setPwm: %d\n", pwm); - //Serial.printf("reverse: %s\n", (reverse)?"true":"false"); + //Serial.printf("pwm in setPwm: %d\n", pwm); + //Serial.printf("reverse: %s\n", (reverse)?"true":"false"); - // apply reverse - if (_reverse) - pwm = int16_t(_pwmNeutral - pwm) + _pwmNeutral; - //Serial.printf("pwm after reverse: %d\n", pwm); + // apply reverse + if (_reverse) + pwm = int16_t(_pwmNeutral - pwm) + _pwmNeutral; + //Serial.printf("pwm after reverse: %d\n", pwm); - // apply saturation - if (_pwm > uint8_t(_pwmMax)) - _pwm = _pwmMax; - if (_pwm < uint8_t(_pwmMin)) - _pwm = _pwmMin; - _pwm = pwm; + // apply saturation + if (_pwm > uint8_t(_pwmMax)) + _pwm = _pwmMax; + if (_pwm < uint8_t(_pwmMin)) + _pwm = _pwmMin; + _pwm = pwm; - //Serial.print("ch: "); Serial.print(ch); Serial.print(" pwm: "); Serial.println(pwm); - if (_rcMode == RC_MODE_IN) - return; - _rc.OutputCh(_ch, _pwm); + //Serial.print("ch: "); Serial.print(ch); Serial.print(" pwm: "); Serial.println(pwm); + if (_rcMode == RC_MODE_IN) + return; + _rc.OutputCh(_ch, _pwm); } uint16_t AP_RcChannel::_positionToPwm(const float & position) { - uint16_t pwm; - //Serial.printf("position: %f\n", position); - if (position < 0) - pwm = position * int16_t(_pwmNeutral - _pwmMin) + _pwmNeutral; - else - pwm = position * int16_t(_pwmMax - _pwmNeutral) + _pwmNeutral; + uint16_t pwm; + //Serial.printf("position: %f\n", position); + if (position < 0) + pwm = position * int16_t(_pwmNeutral - _pwmMin) + _pwmNeutral; + else + pwm = position * int16_t(_pwmMax - _pwmNeutral) + _pwmNeutral; - if (pwm > uint16_t(_pwmMax)) - pwm = _pwmMax; - if (pwm < uint16_t(_pwmMin)) - pwm = _pwmMin; - return pwm; + if (pwm > uint16_t(_pwmMax)) + pwm = _pwmMax; + if (pwm < uint16_t(_pwmMin)) + pwm = _pwmMin; + return pwm; } float AP_RcChannel::_pwmToPosition(const uint16_t & pwm) { - float position; - // note a piece-wise linear mapping occurs if the pwm ranges - // are not symmetric about pwmNeutral - if (pwm < uint8_t(_pwmNeutral)) - position = 1.0 * int16_t(pwm - _pwmNeutral) / int16_t( - _pwmNeutral - _pwmMin); - else - position = 1.0 * int16_t(pwm - _pwmNeutral) / int16_t( - _pwmMax - _pwmNeutral); - if (position > 1) - position = 1; - if (position < -1) - position = -1; - return position; + float position; + // note a piece-wise linear mapping occurs if the pwm ranges + // are not symmetric about pwmNeutral + if (pwm < uint8_t(_pwmNeutral)) + position = 1.0 * int16_t(pwm - _pwmNeutral) / int16_t( + _pwmNeutral - _pwmMin); + else + position = 1.0 * int16_t(pwm - _pwmNeutral) / int16_t( + _pwmMax - _pwmNeutral); + if (position > 1) + position = 1; + if (position < -1) + position = -1; + return position; } } // namespace apo diff --git a/libraries/APO/AP_RcChannel.h b/libraries/APO/AP_RcChannel.h index 1a93132863..d2eea1a153 100644 --- a/libraries/APO/AP_RcChannel.h +++ b/libraries/APO/AP_RcChannel.h @@ -14,7 +14,7 @@ namespace apo { enum rcMode_t { - RC_MODE_IN, RC_MODE_OUT, RC_MODE_INOUT + RC_MODE_IN, RC_MODE_OUT, RC_MODE_INOUT }; /// @class AP_RcChannel @@ -23,60 +23,60 @@ class AP_RcChannel: public AP_Var_group { public: - /// Constructor - AP_RcChannel(AP_Var::Key keyValue, const prog_char_t * name, APM_RC_Class & rc, - const uint8_t & ch, const uint16_t & pwmMin, - const uint16_t & pwmNeutral, const uint16_t & pwmMax, - const rcMode_t & rcMode, - const bool & reverse, const float & scale = 0); + /// Constructor + AP_RcChannel(AP_Var::Key keyValue, const prog_char_t * name, APM_RC_Class & rc, + const uint8_t & ch, const uint16_t & pwmMin, + const uint16_t & pwmNeutral, const uint16_t & pwmMax, + const rcMode_t & rcMode, + const bool & reverse, const float & scale = 0); - // configuration - AP_Uint8 _ch; - AP_Uint16 _pwmMin; - AP_Uint16 _pwmNeutral; - AP_Uint16 _pwmMax; - rcMode_t _rcMode; - AP_Bool _reverse; - AP_Float _scale; + // configuration + AP_Uint8 _ch; + AP_Uint16 _pwmMin; + AP_Uint16 _pwmNeutral; + AP_Uint16 _pwmMax; + rcMode_t _rcMode; + AP_Bool _reverse; + AP_Float _scale; - // get - uint16_t getPwm() { - return _pwm; - } - uint16_t getRadioPwm(); - float getPosition() { - return _pwmToPosition(getPwm()); - } - float getRadioPosition() { - return _pwmToPosition(getRadioPwm()); - } - float getScaled() { - return _scale*getPwm(); - } + // get + uint16_t getPwm() { + return _pwm; + } + uint16_t getRadioPwm(); + float getPosition() { + return _pwmToPosition(getPwm()); + } + float getRadioPosition() { + return _pwmToPosition(getRadioPwm()); + } + float getScaled() { + return _scale*getPwm(); + } + + // set + void setUsingRadio() { + if (_rcMode != RC_MODE_OUT) setPwm(getRadioPwm()); + } + void setPwm(uint16_t pwm); + void setPosition(float position) { + setPwm(_positionToPwm(position)); + } + void setScaled(float val) { + setPwm(val/_scale); + } - // set - void setUsingRadio() { - if (_rcMode != RC_MODE_OUT) setPwm(getRadioPwm()); - } - void setPwm(uint16_t pwm); - void setPosition(float position) { - setPwm(_positionToPwm(position)); - } - void setScaled(float val) { - setPwm(val/_scale); - } - protected: - // configuration - APM_RC_Class & _rc; + // configuration + APM_RC_Class & _rc; - // internal states - uint16_t _pwm; // this is the internal state, position is just created when needed + // internal states + uint16_t _pwm; // this is the internal state, position is just created when needed - // private methods - uint16_t _positionToPwm(const float & position); - float _pwmToPosition(const uint16_t & pwm); + // private methods + uint16_t _positionToPwm(const float & position); + float _pwmToPosition(const uint16_t & pwm); }; } // apo diff --git a/libraries/APO/AP_Var_keys.h b/libraries/APO/AP_Var_keys.h index 58a715b591..19f9508f81 100644 --- a/libraries/APO/AP_Var_keys.h +++ b/libraries/APO/AP_Var_keys.h @@ -3,20 +3,20 @@ enum keys { - // general - k_config = 0, - k_cntrl, - k_guide, - k_sensorCalib, + // general + k_config = 0, + k_cntrl, + k_guide, + k_sensorCalib, - k_radioChannelsStart=10, + k_radioChannelsStart=10, - k_controllersStart=30, + k_controllersStart=30, - k_customStart=100, + k_customStart=100, - // 200-256 reserved for commands - k_commands = 200 + // 200-256 reserved for commands + k_commands = 200 }; // max 256 keys diff --git a/libraries/APO/examples/MavlinkTest/MavlinkTest.pde b/libraries/APO/examples/MavlinkTest/MavlinkTest.pde index d64be33822..f45cd2e510 100644 --- a/libraries/APO/examples/MavlinkTest/MavlinkTest.pde +++ b/libraries/APO/examples/MavlinkTest/MavlinkTest.pde @@ -1,7 +1,7 @@ /* AnalogReadSerial - Reads an analog input on pin 0, prints the result to the serial monitor - + Reads an analog input on pin 0, prints the result to the serial monitor + This example code is in the public domain. */ @@ -10,40 +10,40 @@ int packetDrops = 0; void handleMessage(mavlink_message_t * msg) { - Serial.print(", received mavlink message: "); - Serial.print(msg->msgid, DEC); + Serial.print(", received mavlink message: "); + Serial.print(msg->msgid, DEC); } void setup() { - Serial.begin(57600); - Serial3.begin(57600); - mavlink_comm_0_port = &Serial3; - packetDrops = 0; + Serial.begin(57600); + Serial3.begin(57600); + mavlink_comm_0_port = &Serial3; + packetDrops = 0; } void loop() { - mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type, - MAV_AUTOPILOT_ARDUPILOTMEGA); - Serial.print("heartbeat sent"); + mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); + Serial.print("heartbeat sent"); - // receive new packets - mavlink_message_t msg; - mavlink_status_t status; + // receive new packets + mavlink_message_t msg; + mavlink_status_t status; - Serial.print(", bytes available: "); - Serial.print(comm_get_available(MAVLINK_COMM_0)); - while (comm_get_available( MAVLINK_COMM_0)) { - uint8_t c = comm_receive_ch(MAVLINK_COMM_0); + Serial.print(", bytes available: "); + Serial.print(comm_get_available(MAVLINK_COMM_0)); + while (comm_get_available( MAVLINK_COMM_0)) { + uint8_t c = comm_receive_ch(MAVLINK_COMM_0); - // Try to get a new message - if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) - handleMessage(&msg); - } + // Try to get a new message + if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) + handleMessage(&msg); + } - // Update packet drops counter - packetDrops += status.packet_rx_drop_count; + // Update packet drops counter + packetDrops += status.packet_rx_drop_count; - Serial.print(", dropped packets: "); - Serial.println(packetDrops); - delay(1000); + Serial.print(", dropped packets: "); + Serial.println(packetDrops); + delay(1000); } diff --git a/libraries/APO/examples/ServoManual/ServoManual.pde b/libraries/APO/examples/ServoManual/ServoManual.pde index 8be346f18a..56f2917389 100644 --- a/libraries/APO/examples/ServoManual/ServoManual.pde +++ b/libraries/APO/examples/ServoManual/ServoManual.pde @@ -19,91 +19,91 @@ using namespace apo; class RadioTest { private: - float testPosition; - int8_t testSign; - enum { - version, - rollKey, - pitchKey, - thrKey, - yawKey, - ch5Key, - ch6Key, - ch7Key, - ch8Key - }; - Vector ch; + float testPosition; + int8_t testSign; + enum { + version, + rollKey, + pitchKey, + thrKey, + yawKey, + ch5Key, + ch6Key, + ch7Key, + ch8Key + }; + Vector ch; public: - RadioTest() : - testPosition(2), testSign(1) { - ch.push_back( - new AP_RcChannel(rollKey, PSTR("ROLL"), APM_RC, 0, 1100, 1500, - 1900)); - ch.push_back( - new AP_RcChannel(pitchKey, PSTR("PITCH"), APM_RC, 1, 1100, - 1500, 1900)); - ch.push_back( - new AP_RcChannel(thrKey, PSTR("THR"), APM_RC, 2, 1100, 1500, - 1900)); - ch.push_back( - new AP_RcChannel(yawKey, PSTR("YAW"), APM_RC, 3, 1100, 1500, - 1900)); - ch.push_back( - new AP_RcChannel(ch5Key, PSTR("CH5"), APM_RC, 4, 1100, 1500, - 1900)); - ch.push_back( - new AP_RcChannel(ch6Key, PSTR("CH6"), APM_RC, 5, 1100, 1500, - 1900)); - ch.push_back( - new AP_RcChannel(ch7Key, PSTR("CH7"), APM_RC, 6, 1100, 1500, - 1900)); - ch.push_back( - new AP_RcChannel(ch8Key, PSTR("CH8"), APM_RC, 7, 1100, 1500, - 1900)); + RadioTest() : + testPosition(2), testSign(1) { + ch.push_back( + new AP_RcChannel(rollKey, PSTR("ROLL"), APM_RC, 0, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(pitchKey, PSTR("PITCH"), APM_RC, 1, 1100, + 1500, 1900)); + ch.push_back( + new AP_RcChannel(thrKey, PSTR("THR"), APM_RC, 2, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(yawKey, PSTR("YAW"), APM_RC, 3, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch5Key, PSTR("CH5"), APM_RC, 4, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch6Key, PSTR("CH6"), APM_RC, 5, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch7Key, PSTR("CH7"), APM_RC, 6, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch8Key, PSTR("CH8"), APM_RC, 7, 1100, 1500, + 1900)); - Serial.begin(115200); - delay(2000); - Serial.println("ArduPilot RC Channel test"); - APM_RC.Init(); // APM Radio initialization - delay(2000); - } + Serial.begin(115200); + delay(2000); + Serial.println("ArduPilot RC Channel test"); + APM_RC.Init(); // APM Radio initialization + delay(2000); + } - void update() { - // read the radio - for (uint8_t i = 0; i < ch.getSize(); i++) - ch[i]->setUsingRadio(); + void update() { + // read the radio + for (uint8_t i = 0; i < ch.getSize(); i++) + ch[i]->setUsingRadio(); - // print channel names - Serial.print("\t\t"); - static char name[7]; - for (uint8_t i = 0; i < ch.getSize(); i++) { - ch[i]->copy_name(name, 7); - Serial.printf("%7s\t", name); - } - Serial.println(); + // print channel names + Serial.print("\t\t"); + static char name[7]; + for (uint8_t i = 0; i < ch.getSize(); i++) { + ch[i]->copy_name(name, 7); + Serial.printf("%7s\t", name); + } + Serial.println(); - // print pwm - Serial.printf("pwm :\t"); - for (uint8_t i = 0; i < ch.getSize(); i++) - Serial.printf("%7d\t", ch[i]->getPwm()); - Serial.println(); + // print pwm + Serial.printf("pwm :\t"); + for (uint8_t i = 0; i < ch.getSize(); i++) + Serial.printf("%7d\t", ch[i]->getPwm()); + Serial.println(); - // print position - Serial.printf("position :\t"); - for (uint8_t i = 0; i < ch.getSize(); i++) - Serial.printf("%7.2f\t", ch[i]->getPosition()); - Serial.println(); + // print position + Serial.printf("position :\t"); + for (uint8_t i = 0; i < ch.getSize(); i++) + Serial.printf("%7.2f\t", ch[i]->getPosition()); + Serial.println(); - delay(500); - } + delay(500); + } }; RadioTest * test; void setup() { - test = new RadioTest; + test = new RadioTest; } void loop() { - test->update(); + test->update(); } diff --git a/libraries/APO/examples/ServoSweep/ServoSweep.pde b/libraries/APO/examples/ServoSweep/ServoSweep.pde index 75827d4f09..1f8744d793 100644 --- a/libraries/APO/examples/ServoSweep/ServoSweep.pde +++ b/libraries/APO/examples/ServoSweep/ServoSweep.pde @@ -19,107 +19,107 @@ using namespace apo; class RadioTest { private: - float testPosition; - int8_t testSign; - enum { - version, - rollKey, - pitchKey, - thrKey, - yawKey, - ch5Key, - ch6Key, - ch7Key, - ch8Key - }; - Vector ch; + float testPosition; + int8_t testSign; + enum { + version, + rollKey, + pitchKey, + thrKey, + yawKey, + ch5Key, + ch6Key, + ch7Key, + ch8Key + }; + Vector ch; public: - RadioTest() : - testPosition(2), testSign(1) { - ch.push_back( - new AP_RcChannel(rollKey, PSTR("ROLL"), APM_RC, 0, 1100, 1500, - 1900)); - ch.push_back( - new AP_RcChannel(pitchKey, PSTR("PITCH"), APM_RC, 1, 1100, - 1500, 1900)); - ch.push_back( - new AP_RcChannel(thrKey, PSTR("THR"), APM_RC, 2, 1100, 1500, - 1900)); - ch.push_back( - new AP_RcChannel(yawKey, PSTR("YAW"), APM_RC, 3, 1100, 1500, - 1900)); - ch.push_back( - new AP_RcChannel(ch5Key, PSTR("CH5"), APM_RC, 4, 1100, 1500, - 1900)); - ch.push_back( - new AP_RcChannel(ch6Key, PSTR("CH6"), APM_RC, 5, 1100, 1500, - 1900)); - ch.push_back( - new AP_RcChannel(ch7Key, PSTR("CH7"), APM_RC, 6, 1100, 1500, - 1900)); - ch.push_back( - new AP_RcChannel(ch8Key, PSTR("CH8"), APM_RC, 7, 1100, 1500, - 1900)); + RadioTest() : + testPosition(2), testSign(1) { + ch.push_back( + new AP_RcChannel(rollKey, PSTR("ROLL"), APM_RC, 0, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(pitchKey, PSTR("PITCH"), APM_RC, 1, 1100, + 1500, 1900)); + ch.push_back( + new AP_RcChannel(thrKey, PSTR("THR"), APM_RC, 2, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(yawKey, PSTR("YAW"), APM_RC, 3, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch5Key, PSTR("CH5"), APM_RC, 4, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch6Key, PSTR("CH6"), APM_RC, 5, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch7Key, PSTR("CH7"), APM_RC, 6, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch8Key, PSTR("CH8"), APM_RC, 7, 1100, 1500, + 1900)); - Serial.begin(115200); - delay(2000); - Serial.println("ArduPilot RC Channel test"); - APM_RC.Init(); // APM Radio initialization - delay(2000); - } + Serial.begin(115200); + delay(2000); + Serial.println("ArduPilot RC Channel test"); + APM_RC.Init(); // APM Radio initialization + delay(2000); + } - void update() { - // update test value - testPosition += testSign * .1; - if (testPosition > 1) { - //eepromRegistry.print(Serial); // show eeprom map - testPosition = 1; - testSign = -1; - } else if (testPosition < -1) { - testPosition = -1; - testSign = 1; - } + void update() { + // update test value + testPosition += testSign * .1; + if (testPosition > 1) { + //eepromRegistry.print(Serial); // show eeprom map + testPosition = 1; + testSign = -1; + } else if (testPosition < -1) { + testPosition = -1; + testSign = 1; + } - // set channel positions - for (uint8_t i = 0; i < ch.getSize(); i++) - ch[i]->setPosition(testPosition); + // set channel positions + for (uint8_t i = 0; i < ch.getSize(); i++) + ch[i]->setPosition(testPosition); - // print test position - Serial.printf("\nnormalized position (%f)\n", testPosition); + // print test position + Serial.printf("\nnormalized position (%f)\n", testPosition); - // print channel names - Serial.print("\t\t"); - static char name[7]; - for (uint8_t i = 0; i < ch.getSize(); i++) { - ch[i]->copy_name(name, 7); - Serial.printf("%7s\t", name); - } - Serial.println(); + // print channel names + Serial.print("\t\t"); + static char name[7]; + for (uint8_t i = 0; i < ch.getSize(); i++) { + ch[i]->copy_name(name, 7); + Serial.printf("%7s\t", name); + } + Serial.println(); - // print pwm - Serial.printf("pwm :\t"); - for (uint8_t i = 0; i < ch.getSize(); i++) - Serial.printf("%7d\t", ch[i]->getRadioPwm()); - Serial.println(); + // print pwm + Serial.printf("pwm :\t"); + for (uint8_t i = 0; i < ch.getSize(); i++) + Serial.printf("%7d\t", ch[i]->getRadioPwm()); + Serial.println(); - // print position - Serial.printf("position :\t"); - for (uint8_t i = 0; i < ch.getSize(); i++) - Serial.printf("%7.2f\t", ch[i]->getRadioPosition()); - Serial.println(); + // print position + Serial.printf("position :\t"); + for (uint8_t i = 0; i < ch.getSize(); i++) + Serial.printf("%7.2f\t", ch[i]->getRadioPosition()); + Serial.println(); - delay(500); - } + delay(500); + } }; RadioTest * test; void setup() { - test = new RadioTest; + test = new RadioTest; } void loop() { - test->update(); + test->update(); } // vim:ts=4:sw=4:expandtab