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

Bug located in quad position loop, improved hil.

This commit is contained in:
James Goppert 2011-11-19 21:39:14 -05:00
parent 715a7cb748
commit 1c5db5e963
9 changed files with 107 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
apo/ControllerQuad.h
View File

@ -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() {

10
apo/QuadArducopter.h
View File

@ -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

3
cmake/modules/FindArduino.cmake
View File

@ -609,6 +609,3 @@ mark_as_advanced(ARDUINO_CORES_PATH
ARDUINO_OBJCOPY_EEP_FLAGS
ARDUINO_OBJCOPY_HEX_FLAGS)
load_board_settings()
endif()

106
libraries/APO/AP_CommLink.cpp
View File

@ -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:

12
libraries/APO/AP_Controller.cpp
View File

@ -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

4
libraries/APO/AP_Controller.h
View File

@ -64,6 +64,10 @@ public:
AP_Uint8 getMode() {
return _mode;
}
void setMode(MAV_MODE mode) {
_mode = mode;
}
protected:
AP_Navigator * _nav;
AP_Guide * _guide;

8
libraries/APO/AP_Guide.cpp
View File

@ -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

5
libraries/APO/AP_Guide.h
View File

@ -48,6 +48,11 @@ public:
MAV_NAV getMode() const {
return _mode;
}
void setMode(MAV_NAV mode) {
_mode = mode;
}
uint8_t getCurrentIndex() {
return _cmdIndex;
}

6
libraries/APO/AP_Navigator.h
View File

@ -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;
}