APO quad stabilized flight working well.

apo/ControllerQuad.h

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

apo/QuadArducopter.h

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

libraries/APO/AP_Autopilot.cpp

@@ -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));
 }
@@ -164,23 +168,17 @@ void AP_Autopilot::callback2(void * data) {
 	}
 
 	/*
-	 * send telemetry
+	 * slow navigation loop update
 	 */
-	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);
+	if (apo->getNavigator()) {
+		apo->getNavigator()->updateSlow(1.0 / apo->getLoopRate(2));
 	}
 
 	/*
-	 * slow navigation loop update
+	 * send telemetry
 	 */
-	if (apo->getNavigator()) {
-		apo->getNavigator()->updateSlow(1.0 / apo->getLoopRate(2));
+	if (apo->getHal()->gcs) {
+		apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE);
 	}
 
 	/*
@@ -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());
 

libraries/APO/AP_Autopilot.h

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

libraries/APO/AP_Navigator.h

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