Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

APO quad stabilized flight working well.

Browse Source
This commit is contained in:
James Goppert 2011-10-14 18:28:29 -04:00
parent 85fb55b6b7
commit aacc8a6179
5 changed files with 46 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
apo/ControllerQuad.h
View File

@ -152,8 +152,8 @@ public:
case MAV_MODE_MANUAL: {
setAllRadioChannelsManually();
// "mix manual"
cmdRoll = -1 * _hal->rc[CH_ROLL]->getPosition();
cmdPitch = -1 * _hal->rc[CH_PITCH]->getPosition();
cmdRoll = -0.5 * _hal->rc[CH_ROLL]->getPosition();
cmdPitch = -0.5 * _hal->rc[CH_PITCH]->getPosition();
cmdYawRate = -1 * _hal->rc[CH_YAW]->getPosition();
thrustMix = _hal->rc[CH_THRUST]->getPosition();
break;

36
apo/QuadArducopter.h
View File

@ -11,7 +11,7 @@
// vehicle options
static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD;
static const apo::halMode_t halMode = apo::MODE_LIVE;
static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560;
static const uint8_t heartBeatTimeout = 3;
// algorithm selection
@ -47,18 +47,13 @@ static const bool rangeFinderUpEnabled = false;
static const bool rangeFinderDownEnabled = false;
// loop rates
static const float loop0Rate = 150;
static const float loop1Rate = 100;
static const float loop2Rate = 10;
static const float loop3Rate = 1;
static const float loop0Rate = 200; // attitude nav
static const float loop1Rate = 50; // controller
static const float loop2Rate = 10; // pos nav/ gcs fast
static const float loop3Rate = 1; // gcs slow
static const float loop4Rate = 0.1;
//motor parameters
static const float MOTOR_MAX = 1;
static const float MOTOR_MIN = 0.1;
// position control loop
static const float PID_POS_INTERVAL = 1 / 100; // 5 hz
static const float PID_POS_P = 0;
static const float PID_POS_I = 0;
static const float PID_POS_D = 0;
@ -71,30 +66,23 @@ static const float PID_POS_Z_LIM = 0;
static const float PID_POS_Z_AWU = 0;
// attitude control loop
static const float PID_ATT_INTERVAL = 1 / 100; // 100 hz
static const float PID_ATT_P = 0.1; // 0.1
static const float PID_ATT_I = 0; // 0.0
static const float PID_ATT_D = 0.1; // 0.1
static const float PID_ATT_LIM = 1; // 0.01 // 10 % #define MOTORs
static const float PID_ATT_AWU = 0; // 0.0
static const float PID_ATT_P = 0.3;
static const float PID_ATT_I = 0.5;
static const float PID_ATT_D = 0.08;
static const float PID_ATT_LIM = 0.1; // 10 %
static const float PID_ATT_AWU = 0.03; // 3 %
static const float PID_YAWPOS_P = 0;
static const float PID_YAWPOS_I = 0;
static const float PID_YAWPOS_D = 0;
static const float PID_YAWPOS_LIM = 0; // 1 rad/s
static const float PID_YAWPOS_AWU = 0; // 1 rad/s
static const float PID_YAWSPEED_P = .2;
static const float PID_YAWSPEED_P = 0.5;
static const float PID_YAWSPEED_I = 0;
static const float PID_YAWSPEED_D = 0;
static const float PID_YAWSPEED_LIM = .3; // 0.01 // 10 % MOTORs
static const float PID_YAWSPEED_LIM = .1; // 10 % MOTORs
static const float PID_YAWSPEED_AWU = 0.0;
static const float PID_YAWSPEED_DFCUT = 3.0; // 3 Radians, about 1 Hz
// mixing
static const float MIX_REMOTE_WEIGHT = 1;
static const float MIX_POSITION_WEIGHT = 1;
static const float MIX_POSITION_Z_WEIGHT = 1;
static const float MIX_POSITION_YAW_WEIGHT = 1;
static const float THRUST_HOVER_OFFSET = 0.475;
#include "ControllerQuad.h"

39
libraries/APO/AP_Autopilot.cpp
View File

@ -17,7 +17,7 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
Loop(loop0Rate, callback0, this), _navigator(navigator), _guide(guide),
_controller(controller), _hal(hal), _loop0Rate(loop0Rate),
_loop1Rate(loop1Rate), _loop2Rate(loop2Rate), _loop3Rate(loop3Rate),
_loop4Rate(loop3Rate) {
_loop4Rate(loop3Rate), callback0Calls(0), clockInit(millis()) {
hal->setState(MAV_STATE_BOOT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
@ -111,6 +111,8 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
hal->debug->println_P(PSTR("initializing radio"));
APM_RC.Init(); // APM Radio initialization,
// start this after control loop is running
clockInit = millis();
}
void AP_Autopilot::callback0(void * data) {
@ -120,6 +122,8 @@ void AP_Autopilot::callback0(void * data) {
/*
* ahrs update
*/
apo->callback0Calls++;
if (apo->getNavigator())
apo->getNavigator()->updateFast(1.0 / apo->getLoopRate(0));
}
@ -163,19 +167,6 @@ void AP_Autopilot::callback2(void * data) {
apo->getGuide()->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_ATTITUDE);
//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION);
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW);
//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU);
}
/*
* slow navigation loop update
*/
@ -183,6 +174,13 @@ void AP_Autopilot::callback2(void * data) {
apo->getNavigator()->updateSlow(1.0 / apo->getLoopRate(2));
}
/*
* send telemetry
*/
if (apo->getHal()->gcs) {
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE);
}
/*
* handle ground control station communication
*/
@ -215,6 +213,18 @@ void AP_Autopilot::callback2(void * data) {
void AP_Autopilot::callback3(void * data) {
AP_Autopilot * apo = (AP_Autopilot *) data;
//apo->getHal()->debug->println_P(PSTR("callback 3"));
/*
* 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_GLOBAL_POSITION);
//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW);
//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU);
}
/*
* send heartbeat
@ -225,6 +235,7 @@ void AP_Autopilot::callback3(void * data) {
* load/loop rate/ram debug
*/
apo->getHal()->load = apo->load();
apo->getHal()->debug->printf_P(PSTR("missed calls: %d\n"),uint16_t(millis()*apo->getLoopRate(0)/1000-apo->callback0Calls));
apo->getHal()->debug->printf_P(PSTR("load: %d%%\trate: %f Hz\tfree ram: %d bytes\n"),
apo->load(),1.0/apo->dt(),freeMemory());

6
libraries/APO/AP_Autopilot.h
View File

@ -99,6 +99,12 @@ public:
}
}
/**
* Loop Monitoring
*/
uint32_t callback0Calls;
uint32_t clockInit;
private:
/**

1
libraries/APO/AP_Navigator.h
View File

@ -279,6 +279,7 @@ public:
}
}
virtual void updateFast(float dt) {
if (_hal->getMode() != MODE_LIVE)
return;