Added APM2 support.

CMakeLists.txt

@@ -39,8 +39,8 @@ Please create a separate build directory and run 'cmake /path/to/${PROJECT_NAME}
 
 # options
 if (NOT DEFINED BOARD)
-	message(STATUS "please define the board type (for example: cmake -DBOARD=mega, assuming mega")
-	set(BOARD "mega")
+	message(STATUS "please define the board type (for example: cmake -DBOARD=mega, assuming mega2560")
+	set(BOARD "mega2560")
 endif()
 
 if (NOT DEFINED PORT)

libraries/APO/AP_Autopilot.cpp

@@ -38,6 +38,7 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
     if (board->getMode() != AP_Board::MODE_LIVE) {
         board->hil = new MavlinkComm(board->hilPort, navigator, guide, controller, board, 3);
     }
+    board->gcsPort->printf_P(PSTR("gcs hello\n"));
 
     board->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
     board->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);

libraries/APO/Board_APM1.cpp

@@ -47,15 +47,17 @@ Board_APM1::Board_APM1(mode_e mode, MAV_TYPE vehicle, options_t options) : AP_Bo
     // debug
     Serial.begin(debugBaud, 128, 128);
     debug = &Serial;
-    debug->println_P(PSTR("initializing debug line"));
+    debug->println_P(PSTR("initialized debug port"));
 
     // gcs
     Serial3.begin(telemBaud, 128, 128);
     gcsPort = &Serial3;
+    gcsPort->println_P(PSTR("initialized gcs port"));
 
     // hil
     Serial1.begin(hilBaud, 128, 128);
     hilPort = &Serial1;
+    hilPort->println_P(PSTR("initialized hil port"));
 
     slideSwitchPin = 40;
     pushButtonPin = 41;

libraries/APO/Board_APM2.cpp

@@ -47,15 +47,19 @@ Board_APM2::Board_APM2(mode_e mode, MAV_TYPE vehicle, options_t options) : AP_Bo
     // debug
     Serial.begin(debugBaud, 128, 128);
     debug = &Serial;
-    debug->println_P(PSTR("initializing debug line"));
+    debug->println_P(PSTR("initialized debug port"));
 
     // gcs
     Serial2.begin(telemBaud, 128, 128);
     gcsPort = &Serial2;
+    gcsPort->println_P(PSTR("initialized gcs port"));
+    delay(1000);
 
     // hil
     Serial1.begin(hilBaud, 128, 128);
     hilPort = &Serial1;
+    hilPort->println_P(PSTR("initialized hil port"));
+    delay(1000);
 
     slideSwitchPin = 40;
     pushButtonPin = 41;
@@ -170,9 +174,8 @@ Board_APM2::Board_APM2(mode_e mode, MAV_TYPE vehicle, options_t options) : AP_Bo
      * navigation sensors
      */
     debug->println_P(PSTR("initializing imu"));
-    ins = new AP_InertialSensor_Oilpan(adc);
+    ins = new AP_InertialSensor_MPU6000(53);
     ins->init(scheduler);
-    //ins = new AP_InertialSensor_MPU6000(mpu6000SelectPin)
     debug->println_P(PSTR("initializing ins"));
     imu = new AP_IMU_INS(ins, k_sensorCalib); 
     imu->init(IMU::WARM_START,delay,scheduler);