From 1c5db5e963d544f9192c52936956159b82400c70 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 19 Nov 2011 21:39:14 -0500 Subject: [PATCH] Bug located in quad position loop, improved hil. --- apo/ControllerQuad.h | 57 ++--------------- apo/QuadArducopter.h | 10 +-- cmake/modules/FindArduino.cmake | 3 - libraries/APO/AP_CommLink.cpp | 106 ++++++++++++++++++++++---------- libraries/APO/AP_Controller.cpp | 12 ++-- libraries/APO/AP_Controller.h | 4 ++ libraries/APO/AP_Guide.cpp | 8 +-- libraries/APO/AP_Guide.h | 5 ++ libraries/APO/AP_Navigator.h | 6 +- 9 files changed, 107 insertions(+), 104 deletions(-) diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 7912efbb98..f1ba1b30c3 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -90,6 +90,7 @@ private: autoAttitudeLoop(dt); } void autoPositionLoop(float dt) { + // XXX need to add waypoint coordinates 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); @@ -101,11 +102,6 @@ private: _cmdPitch = cmdEastTilt * trigCos - cmdNorthTilt * trigSin; _cmdRoll = -cmdEastTilt * trigSin + cmdNorthTilt * trigCos; // note that the north tilt is negative of the pitch - - Serial.print(" trigSin: "); - Serial.print(trigSin); - Serial.print(" trigCos: "); - Serial.print(trigCos); } _cmdYawRate = 0; @@ -117,34 +113,9 @@ private: if (fabs(_cmdPitch) > 0.5) _thrustMix *= 1.13949393; else _thrustMix /= cos(_cmdPitch); - - //debug statements - Serial.print(" getPN: "); - Serial.print(_nav->getPN()); - Serial.print(" getPE: "); - Serial.print(_nav->getPE()); - Serial.print(" getPD: "); - Serial.print(_nav->getPD()); - Serial.print(" getVN: "); - Serial.print(_nav->getVN()); - Serial.print(" getVE: "); - Serial.print(_nav->getVE()); - Serial.print(" getVD: "); - Serial.println(_nav->getVD()); - //Serial.print("Roll: "); - //Serial.print(_cmdRoll); - //Serial.print(" Pitch"); - //Serial.print(_cmdPitch); - //Serial.print(" YawRate"); - //Serial.print(_cmdYawRate); - //Serial.print(" ThrustMix"); - //Serial.print(_thrustMix); - //Serial.print(" North Tilt"); - //Serial.print(cmdNorthTilt); - //Serial.print(" East Tilt"); - //Serial.print(cmdEastTilt); - //Serial.print(" Down"); - //Serial.println(cmdDown); + + // debug for position loop + _hal->debug->printf_P(PSTR("cmd: north tilt(%f), east tilt(%f), down(%f), pitch(%f), roll(%f)\n"),cmdNorthTilt,cmdEastTilt,cmdDown,_cmdPitch,_cmdRoll); } void autoAttitudeLoop(float dt) { _rollMix = pidRoll.update(_cmdRoll - _nav->getRoll(), @@ -152,26 +123,6 @@ private: _pitchMix = pidPitch.update(_cmdPitch - _nav->getPitch(), _nav->getPitchRate(), dt); _yawMix = pidYawRate.update(_cmdYawRate - _nav->getYawRate(), dt); - - //debug statements - //Serial.print("Roll Cmd: "); - //Serial.print(_cmdRoll*1000); - //Serial.print(" Nav_GetRoll: "); - //Serial.print(_nav->getRoll()*1000); - //Serial.print(" Roll Error: "); - //Serial.print((_cmdRoll - _nav->getRoll())*1000); - //Serial.print(" Pitch Cmd: "); - //Serial.print(_cmdPitch*1000); - //Serial.print(" Nav_GetPitch: "); - //Serial.print(_nav->getPitch()*1000); - //Serial.print(" Pitch Error: "); - //Serial.println((_cmdPitch - _nav->getPitch())*1000); - //Serial.print(" YawRate Cmd: "); - //Serial.print(_cmdYawRate); - //Serial.print(" Nav_GetYawRate: "); - //Serial.print( _nav->getYawRate()); - //Serial.print(" YawRate Error: "); - //Serial.println(_cmdYawRate - _nav->getYawRate()); } void setMotorsActive() { diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index 547c27cf31..d49cd5e1b7 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -12,7 +12,7 @@ static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD; static const apo::halMode_t halMode = apo::MODE_HIL_CNTL; static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; -static const uint8_t heartBeatTimeout = 3; +static const uint8_t heartBeatTimeout = 0; // algorithm selection #define CONTROLLER_CLASS ControllerQuad @@ -30,7 +30,7 @@ static const uint8_t heartBeatTimeout = 3; static const uint32_t debugBaud = 57600; static const uint32_t telemBaud = 57600; static const uint32_t gpsBaud = 38400; -static const uint32_t hilBaud = 57600; +static const uint32_t hilBaud = 115200; // optional sensors static const bool gpsEnabled = false; @@ -66,10 +66,10 @@ static const float PID_POS_I = 0; static const float PID_POS_D = 0; static const float PID_POS_LIM = 0; // about 5 deg static const float PID_POS_AWU = 0; // about 5 deg -static const float PID_POS_Z_P = 0; +static const float PID_POS_Z_P = 0.1; static const float PID_POS_Z_I = 0; -static const float PID_POS_Z_D = 0; -static const float PID_POS_Z_LIM = 0; +static const float PID_POS_Z_D = 0.2; +static const float PID_POS_Z_LIM = 0.1; static const float PID_POS_Z_AWU = 0; static const float PID_POS_DFCUT = 10; // cut derivative feedback at 10 hz diff --git a/cmake/modules/FindArduino.cmake b/cmake/modules/FindArduino.cmake index dc20e07cd6..d78194cfb5 100644 --- a/cmake/modules/FindArduino.cmake +++ b/cmake/modules/FindArduino.cmake @@ -609,6 +609,3 @@ mark_as_advanced(ARDUINO_CORES_PATH ARDUINO_OBJCOPY_EEP_FLAGS ARDUINO_OBJCOPY_HEX_FLAGS) load_board_settings() - -endif() - diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp index 1df492bde2..6be0bf1842 100644 --- a/libraries/APO/AP_CommLink.cpp +++ b/libraries/APO/AP_CommLink.cpp @@ -131,21 +131,23 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) { */ 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 + int16_t xmag, ymag, zmag; + xmag = ymag = zmag = 0; + if (_hal->compass) { + // XXX THIS IS NOT SCALED + xmag = _hal->compass->mag_x; + ymag = _hal->compass->mag_y; + zmag = _hal->compass->mag_z; + } + mavlink_msg_scaled_imu_send(_channel, timeStamp, + _navigator->getXAccel()*1e3, + _navigator->getYAccel()*1e3, + _navigator->getZAccel()*1e3, + _navigator->getRollRate()*1e3, + _navigator->getPitchRate()*1e3, + _navigator->getYawRate()*1e3, + xmag, ymag, zmag); + break; } case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: { @@ -352,6 +354,9 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) { _navigator->setLat_degInt(packet.lat); _navigator->setLon_degInt(packet.lon); _navigator->setAlt(packet.alt / 1e3); + _navigator->setXAccel(packet.xacc/ 1e3); + _navigator->setYAccel(packet.xacc/ 1e3); + _navigator->setZAccel(packet.xacc/ 1e3); break; } @@ -391,31 +396,68 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) { AP_Var::save_all(); break; - case MAV_ACTION_CALIBRATE_RC: + case MAV_ACTION_MOTORS_START: + _hal->setState(MAV_STATE_ACTIVE); + break; + case MAV_ACTION_CALIBRATE_GYRO: case MAV_ACTION_CALIBRATE_MAG: case MAV_ACTION_CALIBRATE_ACC: case MAV_ACTION_CALIBRATE_PRESSURE: + _hal->setState(MAV_STATE_STANDBY); + _navigator->calibrate(); + break; + + case MAV_ACTION_EMCY_KILL: + case MAV_ACTION_CONFIRM_KILL: + case MAV_ACTION_MOTORS_STOP: + case MAV_ACTION_SHUTDOWN: + _hal->setState(MAV_STATE_STANDBY); + _controller->setMode(MAV_MODE_LOCKED); + break; + + case MAV_ACTION_LAUNCH: + case MAV_ACTION_TAKEOFF: + _controller->setMode(MAV_MODE_AUTO); + _guide->setMode(MAV_NAV_LIFTOFF); + break; + + case MAV_ACTION_LAND: + _guide->setCurrentIndex(0); + _guide->setMode(MAV_NAV_LANDING); + break; + + case MAV_ACTION_EMCY_LAND: + _guide->setMode(MAV_NAV_LANDING); + break; + + case MAV_ACTION_LOITER: + case MAV_ACTION_HALT: + _guide->setMode(MAV_NAV_LOITER); + break; + + case MAV_ACTION_SET_AUTO: + _controller->setMode(MAV_MODE_AUTO); + break; + + case MAV_ACTION_SET_MANUAL: + _controller->setMode(MAV_MODE_MANUAL); + break; + + case MAV_ACTION_RETURN: + _guide->setMode(MAV_NAV_RETURNING); + break; + + case MAV_ACTION_NAVIGATE: + case MAV_ACTION_CONTINUE: + _guide->setMode(MAV_NAV_WAYPOINT); + break; + + case MAV_ACTION_CALIBRATE_RC: 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: diff --git a/libraries/APO/AP_Controller.cpp b/libraries/APO/AP_Controller.cpp index 48c6f6e475..73acf7c066 100644 --- a/libraries/APO/AP_Controller.cpp +++ b/libraries/APO/AP_Controller.cpp @@ -49,18 +49,22 @@ void AP_Controller::update(const float dt) { // // check for heartbeat from gcs, if not found go to failsafe if (_hal->heartBeatLost()) { - _mode = MAV_MODE_FAILSAFE; + setMode(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; + setMode(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; + + // if in auto mode and manual switch set, change to manual + if (getMode()==MAV_MODE_AUTO && (_hal->rc[_chMode]->getRadioPosition() > 0)) setMode(MAV_MODE_MANUAL); + + // if in manual mode and auto switch set, switch to auto + if (getMode()==MAV_MODE_MANUAL && (_hal->rc[_chMode]->getRadioPosition() < 0)) setMode(MAV_MODE_AUTO); } // handle flight modes diff --git a/libraries/APO/AP_Controller.h b/libraries/APO/AP_Controller.h index 4c5fe8a260..d16fe48052 100644 --- a/libraries/APO/AP_Controller.h +++ b/libraries/APO/AP_Controller.h @@ -64,6 +64,10 @@ public: AP_Uint8 getMode() { return _mode; } + void setMode(MAV_MODE mode) { + _mode = mode; + } + protected: AP_Navigator * _nav; AP_Guide * _guide; diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp index 49eaf263a8..81f3bf0fd7 100644 --- a/libraries/APO/AP_Guide.cpp +++ b/libraries/APO/AP_Guide.cpp @@ -146,7 +146,7 @@ void MavlinkGuide::handleCommand() { // check if we are at waypoint or if command // radius is zero within tolerance - if (distanceToNext < _command.getRadius() | distanceToNext < 1e-5) { + if ( (distanceToNext < _command.getRadius()) | (distanceToNext < 1e-5) ) { _hal->gcs->sendText(SEVERITY_LOW,PSTR("waypoint reached (distance)")); _hal->debug->printf_P(PSTR("waypoint reached (distance)")); nextCommand(); @@ -217,9 +217,9 @@ void MavlinkGuide::handleCommand() { _pDCmd = _command.getPD(_navigator->getAlt_intM()); // debug - _hal->debug->printf_P( - PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"), - getNumberOfCommands(), getCurrentIndex(), getPreviousIndex()); + //_hal->debug->printf_P( + //PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"), + //getNumberOfCommands(), getCurrentIndex(), getPreviousIndex()); } } // namespace apo diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index 033e27ed45..95a43b33f7 100644 --- a/libraries/APO/AP_Guide.h +++ b/libraries/APO/AP_Guide.h @@ -48,6 +48,11 @@ public: MAV_NAV getMode() const { return _mode; } + + void setMode(MAV_NAV mode) { + _mode = mode; + } + uint8_t getCurrentIndex() { return _cmdIndex; } diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h index ea772950b1..437386c54b 100644 --- a/libraries/APO/AP_Navigator.h +++ b/libraries/APO/AP_Navigator.h @@ -187,15 +187,15 @@ public: _vD = vD; } - void setXAcc(float xAccel) { + void setXAccel(float xAccel) { _xAccel = xAccel; } - void setYAcc(float yAccel) { + void setYAccel(float yAccel) { _yAccel = yAccel; } - void setZAcc(float zAccel) { + void setZAccel(float zAccel) { _zAccel = zAccel; }