From e6e94837e93c20f49a367ca6d499dd05ca93db82 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 28 Sep 2011 20:51:12 -0400 Subject: [PATCH 1/7] Added APO branch. --- ArduBoat/ArduBoat.pde | 184 ++++ ArduBoat/BoatGeneric.h | 68 ++ ArduBoat/CMakeLists.txt | 80 ++ ArduBoat/ControllerBoat.h | 125 +++ ArduBoat/Makefile | 1 + ArduRover/ArduRover.pde | 184 ++++ ArduRover/CMakeLists.txt | 80 ++ ArduRover/CarStampede.h | 69 ++ ArduRover/ControllerCar.h | 125 +++ ArduRover/ControllerTank.h | 133 +++ ArduRover/Makefile | 1 + ArduRover/TankGeneric.h | 68 ++ apo/CMakeLists.txt | 80 ++ apo/ControllerPlane.h | 215 +++++ apo/ControllerQuad.h | 250 ++++++ apo/Makefile | 1 + apo/PlaneEasystar.h | 113 +++ apo/QuadArducopter.h | 98 +++ apo/QuadMikrokopter.h | 98 +++ apo/apo.pde | 184 ++++ libraries/APO/APO.cpp | 8 + libraries/APO/APO.h | 20 + libraries/APO/AP_Autopilot.cpp | 246 ++++++ libraries/APO/AP_Autopilot.h | 165 ++++ libraries/APO/AP_CommLink.cpp | 15 + libraries/APO/AP_CommLink.h | 790 ++++++++++++++++++ libraries/APO/AP_Controller.cpp | 8 + libraries/APO/AP_Controller.h | 304 +++++++ libraries/APO/AP_Guide.cpp | 8 + libraries/APO/AP_Guide.h | 359 ++++++++ libraries/APO/AP_HardwareAbstractionLayer.cpp | 8 + libraries/APO/AP_HardwareAbstractionLayer.h | 166 ++++ libraries/APO/AP_MavlinkCommand.cpp | 175 ++++ libraries/APO/AP_MavlinkCommand.h | 359 ++++++++ libraries/APO/AP_Navigator.cpp | 8 + libraries/APO/AP_Navigator.h | 391 +++++++++ libraries/APO/AP_RcChannel.cpp | 110 +++ libraries/APO/AP_RcChannel.h | 69 ++ libraries/APO/AP_Var_keys.h | 24 + libraries/APO/CMakeLists.txt | 39 + libraries/APO/constants.h | 23 + libraries/APO/examples/MavlinkTest/Makefile | 2 + .../APO/examples/MavlinkTest/MavlinkTest.pde | 49 ++ libraries/APO/examples/ServoManual/Makefile | 2 + .../APO/examples/ServoManual/ServoManual.pde | 109 +++ libraries/APO/examples/ServoSweep/Makefile | 2 + .../APO/examples/ServoSweep/ServoSweep.pde | 125 +++ libraries/APO/template.h | 32 + libraries/AP_Common/AP_Var.cpp | 161 ++-- libraries/AP_Common/AP_Var.h | 4 + libraries/AP_Common/CMakeLists.txt | 2 +- libraries/FastSerial/CMakeLists.txt | 2 +- libraries/FastSerial/FastSerial.cpp | 3 + libraries/FastSerial/FastSerial.h | 16 + 54 files changed, 5910 insertions(+), 51 deletions(-) create mode 100644 ArduBoat/ArduBoat.pde create mode 100644 ArduBoat/BoatGeneric.h create mode 100644 ArduBoat/CMakeLists.txt create mode 100644 ArduBoat/ControllerBoat.h create mode 100644 ArduBoat/Makefile create mode 100644 ArduRover/ArduRover.pde create mode 100644 ArduRover/CMakeLists.txt create mode 100644 ArduRover/CarStampede.h create mode 100644 ArduRover/ControllerCar.h create mode 100644 ArduRover/ControllerTank.h create mode 100644 ArduRover/Makefile create mode 100644 ArduRover/TankGeneric.h create mode 100644 apo/CMakeLists.txt create mode 100644 apo/ControllerPlane.h create mode 100644 apo/ControllerQuad.h create mode 100644 apo/Makefile create mode 100644 apo/PlaneEasystar.h create mode 100644 apo/QuadArducopter.h create mode 100644 apo/QuadMikrokopter.h create mode 100644 apo/apo.pde create mode 100644 libraries/APO/APO.cpp create mode 100644 libraries/APO/APO.h create mode 100644 libraries/APO/AP_Autopilot.cpp create mode 100644 libraries/APO/AP_Autopilot.h create mode 100644 libraries/APO/AP_CommLink.cpp create mode 100644 libraries/APO/AP_CommLink.h create mode 100644 libraries/APO/AP_Controller.cpp create mode 100644 libraries/APO/AP_Controller.h create mode 100644 libraries/APO/AP_Guide.cpp create mode 100644 libraries/APO/AP_Guide.h create mode 100644 libraries/APO/AP_HardwareAbstractionLayer.cpp create mode 100644 libraries/APO/AP_HardwareAbstractionLayer.h create mode 100644 libraries/APO/AP_MavlinkCommand.cpp create mode 100644 libraries/APO/AP_MavlinkCommand.h create mode 100644 libraries/APO/AP_Navigator.cpp create mode 100644 libraries/APO/AP_Navigator.h create mode 100644 libraries/APO/AP_RcChannel.cpp create mode 100644 libraries/APO/AP_RcChannel.h create mode 100644 libraries/APO/AP_Var_keys.h create mode 100644 libraries/APO/CMakeLists.txt create mode 100644 libraries/APO/constants.h create mode 100644 libraries/APO/examples/MavlinkTest/Makefile create mode 100644 libraries/APO/examples/MavlinkTest/MavlinkTest.pde create mode 100644 libraries/APO/examples/ServoManual/Makefile create mode 100644 libraries/APO/examples/ServoManual/ServoManual.pde create mode 100644 libraries/APO/examples/ServoSweep/Makefile create mode 100644 libraries/APO/examples/ServoSweep/ServoSweep.pde create mode 100644 libraries/APO/template.h diff --git a/ArduBoat/ArduBoat.pde b/ArduBoat/ArduBoat.pde new file mode 100644 index 0000000000..7732fdc332 --- /dev/null +++ b/ArduBoat/ArduBoat.pde @@ -0,0 +1,184 @@ +/* + * ardupilotone + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#define ENABLE_FASTSERIAL_DEBUG + +// Libraries +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +FastSerialPort0(Serial); +FastSerialPort1(Serial1); +FastSerialPort2(Serial2); +FastSerialPort3(Serial3); + +// Vehicle Configuration +#include "BoatGeneric.h" + +/* + * Required Global Declarations + */ + +static apo::AP_Autopilot * autoPilot; + +void setup() { + + using namespace apo; + + AP_Var::load_all(); + + /* + * Communications + */ + Serial.begin(DEBUG_BAUD, 128, 128); // debug + if (board==BOARD_ARDUPILOTMEGA_2) Serial2.begin(TELEM_BAUD, 128, 128); // gcs + else Serial3.begin(TELEM_BAUD, 128, 128); // gcs + + // hardware abstraction layer + AP_HardwareAbstractionLayer * hal = new AP_HardwareAbstractionLayer( + halMode, board, vehicle, heartBeatTimeout); + + // debug serial + hal->debug = &Serial; + hal->debug->println_P(PSTR("initializing debug line")); + + /* + * Initialize Comm Channels + */ + hal->debug->println_P(PSTR("initializing comm channels")); + if (hal->getMode() == MODE_LIVE) { + Serial1.begin(GPS_BAUD, 128, 16); // gps + } else { // hil + Serial1.begin(HIL_BAUD, 128, 128); + } + + /* + * Sensor initialization + */ + if (hal->getMode() == MODE_LIVE) { + hal->debug->println_P(PSTR("initializing adc")); + hal->adc = new ADC_CLASS; + hal->adc->Init(); + + if (gpsEnabled) { + hal->debug->println_P(PSTR("initializing gps")); + AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps)); + hal->gps = &gpsDriver; + hal->gps->init(); + } + + if (baroEnabled) { + hal->debug->println_P(PSTR("initializing baro")); + hal->baro = new BARO_CLASS; + hal->baro->Init(); + } + + if (compassEnabled) { + hal->debug->println_P(PSTR("initializing compass")); + hal->compass = new COMPASS_CLASS; + hal->compass->set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD); + hal->compass->init(); + } + + /** + * Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not + * initialize them and NULL will be assigned to those corresponding pointers. + * On detecting NULL assigned to any ultrasonic sensor, its corresponding block of code + * will not be executed by the navigator. + * The coordinate system is assigned by the right hand rule with the thumb pointing down. + * In set_orientation, it is defind as (front/back,left/right,down,up) + */ + + if (rangeFinderFrontEnabled) { + hal->debug->println_P(PSTR("initializing front range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(1); + rangeFinder->set_orientation(1, 0, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderBackEnabled) { + hal->debug->println_P(PSTR("initializing back range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(2); + rangeFinder->set_orientation(-1, 0, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderLeftEnabled) { + hal->debug->println_P(PSTR("initializing left range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(3); + rangeFinder->set_orientation(0, -1, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderRightEnabled) { + hal->debug->println_P(PSTR("initializing right range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(4); + rangeFinder->set_orientation(0, 1, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderUpEnabled) { + hal->debug->println_P(PSTR("initializing up range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(5); + rangeFinder->set_orientation(0, 0, -1); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderDownEnabled) { + hal->debug->println_P(PSTR("initializing down range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(6); + rangeFinder->set_orientation(0, 0, 1); + hal->rangeFinders.push_back(rangeFinder); + } + + } + + /* + * Select guidance, navigation, control algorithms + */ + AP_Navigator * navigator = new NAVIGATOR_CLASS(hal); + AP_Guide * guide = new GUIDE_CLASS(navigator, hal); + AP_Controller * controller = new CONTROLLER_CLASS(navigator, guide, hal); + + /* + * CommLinks + */ + if (board==BOARD_ARDUPILOTMEGA_2) hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal); + else hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal); + + hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal); + + /* + * Start the autopilot + */ + hal->debug->printf_P(PSTR("initializing arduplane\n")); + hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); + autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal, + loop0Rate, loop1Rate, loop2Rate, loop3Rate); +} + +void loop() { + autoPilot->update(); +} diff --git a/ArduBoat/BoatGeneric.h b/ArduBoat/BoatGeneric.h new file mode 100644 index 0000000000..5e47672847 --- /dev/null +++ b/ArduBoat/BoatGeneric.h @@ -0,0 +1,68 @@ +/* + * BoatGeneric.h + * + * Created on: May 1, 2011 + * Author: jgoppert + */ + +#ifndef BOATGENERIC_H_ +#define BOATGENERIC_H_ + +// vehicle options +static const apo::vehicle_t vehicle = apo::VEHICLE_BOAT; +static const apo::halMode_t halMode = apo::MODE_LIVE; +static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560; +static const uint8_t heartBeatTimeout = 3; + +// algorithm selection +#define CONTROLLER_CLASS ControllerBoat +#define GUIDE_CLASS MavlinkGuide +#define NAVIGATOR_CLASS DcmNavigator +#define COMMLINK_CLASS MavlinkComm + +// hardware selection +#define ADC_CLASS AP_ADC_ADS7844 +#define COMPASS_CLASS AP_Compass_HMC5843 +#define BARO_CLASS APM_BMP085_Class +#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL +#define DEBUG_BAUD 57600 +#define TELEM_BAUD 57600 +#define GPS_BAUD 38400 +#define HIL_BAUD 57600 + +// optional sensors +static bool gpsEnabled = false; +static bool baroEnabled = true; +static bool compassEnabled = true; + +static bool rangeFinderFrontEnabled = true; +static bool rangeFinderBackEnabled = true; +static bool rangeFinderLeftEnabled = true; +static bool rangeFinderRightEnabled = true; +static bool rangeFinderUpEnabled = true; +static bool rangeFinderDownEnabled = true; + +// 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 loop4Rate = 0.1; + +// gains +const float steeringP = 1.0; +const float steeringI = 0.0; +const float steeringD = 0.0; +const float steeringIMax = 0.0; +const float steeringYMax = 3.0; + +const float throttleP = 0.0; +const float throttleI = 0.0; +const float throttleD = 0.0; +const float throttleIMax = 0.0; +const float throttleYMax = 0.0; +const float throttleDFCut = 3.0; + +#include "ControllerBoat.h" + +#endif /* BOATGENERIC_H_ */ diff --git a/ArduBoat/CMakeLists.txt b/ArduBoat/CMakeLists.txt new file mode 100644 index 0000000000..bc9fab25d0 --- /dev/null +++ b/ArduBoat/CMakeLists.txt @@ -0,0 +1,80 @@ +#=============================================================================# +# Author: Sebastian Rohde # +# Date: 30.08.2011 # +#=============================================================================# + + +#====================================================================# +# Settings # +#====================================================================# +set(FIRMWARE_NAME ArduBoat) + +set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board + +set(${FIRMWARE_NAME}_SKETCHES + ardupilotone.pde + ) # Firmware sketches + +set(${FIRMWARE_NAME}_SRCS + ) # Firmware sources + +set(${FIRMWARE_NAME}_HDRS + BoatGeneric.h + CarStampede.h + #CNIRover.h + #CNIBoat.h + ControllerBoat.h + ControllerCar.h + ControllerPlane.h + ControllerQuad.h + PlaneEasystar.h + QuadArducopter.h + QuadMikrokopter.h + ) # Firmware sources + +set(${FIRMWARE_NAME}_LIBS + m + APO + FastSerial + AP_Common + GCS_MAVLink + AP_GPS + APM_RC + AP_DCM + AP_ADC + AP_Compass + AP_IMU + AP_RangeFinder + APM_BMP085 + ModeFilter +) + + +#${CONSOLE_PORT} +set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port +set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd + +include_directories( +${CMAKE_SOURCE_DIR}/libraries/APO +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +) +#====================================================================# +# Target generation # +#====================================================================# +generate_arduino_firmware(${FIRMWARE_NAME}) + +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/${FIRMWARE_NAME}.hex + DESTINATION bin + ) diff --git a/ArduBoat/ControllerBoat.h b/ArduBoat/ControllerBoat.h new file mode 100644 index 0000000000..e25560085c --- /dev/null +++ b/ArduBoat/ControllerBoat.h @@ -0,0 +1,125 @@ +/* + * ControllerBoat.h + * + * Created on: Jun 30, 2011 + * Author: jgoppert + */ + +#ifndef CONTROLLERBOAT_H_ +#define CONTROLLERBOAT_H_ + +#include "../APO/AP_Controller.h" + +namespace apo { + +class ControllerBoat: public AP_Controller { +private: + AP_Var_group _group; + AP_Uint8 _mode; + enum { + k_chMode = k_radioChannelsStart, k_chStr, k_chThr + }; + enum { + k_pidStr = k_controllersStart, k_pidThr + }; + enum { + CH_MODE = 0, CH_STR, CH_THR + }; + BlockPIDDfb pidStr; + BlockPID pidThr; +public: + ControllerBoat(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + AP_Controller(nav, guide, hal), + _group(k_cntrl, PSTR("CNTRL_")), + _mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")), + pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, + steeringI, steeringD, steeringIMax, steeringYMax), + pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP, + throttleI, throttleD, throttleIMax, throttleYMax, + throttleDFCut) { + _hal->debug->println_P(PSTR("initializing boat controller")); + + _hal->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7, 1100, + 1500, 1900)); + _hal->rc.push_back( + new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1540, + 1900)); + _hal->rc.push_back( + new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500, + 1900)); + } + virtual MAV_MODE getMode() { + return (MAV_MODE) _mode.get(); + } + virtual void update(const float & dt) { + + // check for heartbeat + if (_hal->heartBeatLost()) { + _mode = MAV_MODE_FAILSAFE; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n")); + return; + // if throttle less than 5% cut motor power + } else if (_hal->rc[CH_THR]->getRadioPosition() < 0.05) { + _mode = MAV_MODE_LOCKED; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_STANDBY); + return; + // if in live mode then set state to active + } else if (_hal->getMode() == MODE_LIVE) { + _hal->setState(MAV_STATE_ACTIVE); + // if in hardware in the loop (control) mode, set to hilsim + } else if (_hal->getMode() == MODE_HIL_CNTL) { + _hal->setState(MAV_STATE_HILSIM); + } + + // read switch to set mode + if (_hal->rc[CH_MODE]->getRadioPosition() > 0) { + _mode = MAV_MODE_MANUAL; + } else { + _mode = MAV_MODE_AUTO; + } + + // manual mode + switch (_mode) { + + case MAV_MODE_MANUAL: { + setAllRadioChannelsManually(); + //_hal->debug->println("manual"); + break; + } + case MAV_MODE_AUTO: { + float headingError = _guide->getHeadingCommand() + - _nav->getYaw(); + if (headingError > 180 * deg2Rad) + headingError -= 360 * deg2Rad; + if (headingError < -180 * deg2Rad) + headingError += 360 * deg2Rad; + _hal->rc[CH_STR]->setPosition( + pidStr.update(headingError, _nav->getYawRate(), dt)); + _hal->rc[CH_THR]->setPosition( + pidThr.update( + _guide->getGroundSpeedCommand() + - _nav->getGroundSpeed(), dt)); + //_hal->debug->println("automode"); + break; + } + + default: { + setAllRadioChannelsToNeutral(); + _mode = MAV_MODE_FAILSAFE; + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("unknown controller mode\n")); + break; + } + + } + } +}; + +} // namespace apo + +#endif /* CONTROLLERBOAT_H_ */ diff --git a/ArduBoat/Makefile b/ArduBoat/Makefile new file mode 100644 index 0000000000..1d9b6a022d --- /dev/null +++ b/ArduBoat/Makefile @@ -0,0 +1 @@ +include ../libraries/AP_Common/Arduino.mk diff --git a/ArduRover/ArduRover.pde b/ArduRover/ArduRover.pde new file mode 100644 index 0000000000..2366cd6196 --- /dev/null +++ b/ArduRover/ArduRover.pde @@ -0,0 +1,184 @@ +/* + * ardupilotone + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#define ENABLE_FASTSERIAL_DEBUG + +// Libraries +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +FastSerialPort0(Serial); +FastSerialPort1(Serial1); +FastSerialPort2(Serial2); +FastSerialPort3(Serial3); + +// Vehicle Configuration +#include "CarStampede.h" + +/* + * Required Global Declarations + */ + +static apo::AP_Autopilot * autoPilot; + +void setup() { + + using namespace apo; + + AP_Var::load_all(); + + /* + * Communications + */ + Serial.begin(DEBUG_BAUD, 128, 128); // debug + if (board==BOARD_ARDUPILOTMEGA_2) Serial2.begin(TELEM_BAUD, 128, 128); // gcs + else Serial3.begin(TELEM_BAUD, 128, 128); // gcs + + // hardware abstraction layer + AP_HardwareAbstractionLayer * hal = new AP_HardwareAbstractionLayer( + halMode, board, vehicle, heartBeatTimeout); + + // debug serial + hal->debug = &Serial; + hal->debug->println_P(PSTR("initializing debug line")); + + /* + * Initialize Comm Channels + */ + hal->debug->println_P(PSTR("initializing comm channels")); + if (hal->getMode() == MODE_LIVE) { + Serial1.begin(GPS_BAUD, 128, 16); // gps + } else { // hil + Serial1.begin(HIL_BAUD, 128, 128); + } + + /* + * Sensor initialization + */ + if (hal->getMode() == MODE_LIVE) { + hal->debug->println_P(PSTR("initializing adc")); + hal->adc = new ADC_CLASS; + hal->adc->Init(); + + if (gpsEnabled) { + hal->debug->println_P(PSTR("initializing gps")); + AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps)); + hal->gps = &gpsDriver; + hal->gps->init(); + } + + if (baroEnabled) { + hal->debug->println_P(PSTR("initializing baro")); + hal->baro = new BARO_CLASS; + hal->baro->Init(); + } + + if (compassEnabled) { + hal->debug->println_P(PSTR("initializing compass")); + hal->compass = new COMPASS_CLASS; + hal->compass->set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD); + hal->compass->init(); + } + + /** + * Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not + * initialize them and NULL will be assigned to those corresponding pointers. + * On detecting NULL assigned to any ultrasonic sensor, its corresponding block of code + * will not be executed by the navigator. + * The coordinate system is assigned by the right hand rule with the thumb pointing down. + * In set_orientation, it is defind as (front/back,left/right,down,up) + */ + + if (rangeFinderFrontEnabled) { + hal->debug->println_P(PSTR("initializing front range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(1); + rangeFinder->set_orientation(1, 0, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderBackEnabled) { + hal->debug->println_P(PSTR("initializing back range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(2); + rangeFinder->set_orientation(-1, 0, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderLeftEnabled) { + hal->debug->println_P(PSTR("initializing left range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(3); + rangeFinder->set_orientation(0, -1, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderRightEnabled) { + hal->debug->println_P(PSTR("initializing right range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(4); + rangeFinder->set_orientation(0, 1, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderUpEnabled) { + hal->debug->println_P(PSTR("initializing up range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(5); + rangeFinder->set_orientation(0, 0, -1); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderDownEnabled) { + hal->debug->println_P(PSTR("initializing down range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(6); + rangeFinder->set_orientation(0, 0, 1); + hal->rangeFinders.push_back(rangeFinder); + } + + } + + /* + * Select guidance, navigation, control algorithms + */ + AP_Navigator * navigator = new NAVIGATOR_CLASS(hal); + AP_Guide * guide = new GUIDE_CLASS(navigator, hal); + AP_Controller * controller = new CONTROLLER_CLASS(navigator, guide, hal); + + /* + * CommLinks + */ + if (board==BOARD_ARDUPILOTMEGA_2) hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal); + else hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal); + + hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal); + + /* + * Start the autopilot + */ + hal->debug->printf_P(PSTR("initializing arduplane\n")); + hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); + autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal, + loop0Rate, loop1Rate, loop2Rate, loop3Rate); +} + +void loop() { + autoPilot->update(); +} diff --git a/ArduRover/CMakeLists.txt b/ArduRover/CMakeLists.txt new file mode 100644 index 0000000000..07c2f19f3f --- /dev/null +++ b/ArduRover/CMakeLists.txt @@ -0,0 +1,80 @@ +#=============================================================================# +# Author: Sebastian Rohde # +# Date: 30.08.2011 # +#=============================================================================# + + +#====================================================================# +# Settings # +#====================================================================# +set(FIRMWARE_NAME ArduRover) + +set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board + +set(${FIRMWARE_NAME}_SKETCHES + ardupilotone.pde + ) # Firmware sketches + +set(${FIRMWARE_NAME}_SRCS + ) # Firmware sources + +set(${FIRMWARE_NAME}_HDRS + BoatGeneric.h + CarStampede.h + #CNIRover.h + #CNIBoat.h + ControllerBoat.h + ControllerCar.h + ControllerPlane.h + ControllerQuad.h + PlaneEasystar.h + QuadArducopter.h + QuadMikrokopter.h + ) # Firmware sources + +set(${FIRMWARE_NAME}_LIBS + m + APO + FastSerial + AP_Common + GCS_MAVLink + AP_GPS + APM_RC + AP_DCM + AP_ADC + AP_Compass + AP_IMU + AP_RangeFinder + APM_BMP085 + ModeFilter +) + + +#${CONSOLE_PORT} +set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port +set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd + +include_directories( +${CMAKE_SOURCE_DIR}/libraries/APO +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +) +#====================================================================# +# Target generation # +#====================================================================# +generate_arduino_firmware(${FIRMWARE_NAME}) + +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/${FIRMWARE_NAME}.hex + DESTINATION bin + ) diff --git a/ArduRover/CarStampede.h b/ArduRover/CarStampede.h new file mode 100644 index 0000000000..3ed8a0b2b1 --- /dev/null +++ b/ArduRover/CarStampede.h @@ -0,0 +1,69 @@ +/* + * CarStampede.h + * + * Created on: May 1, 2011 + * Author: jgoppert + * + */ + +#ifndef CARSTAMPEDE_H_ +#define CARSTAMPEDE_H_ + +// vehicle options +static const apo::vehicle_t vehicle = apo::VEHICLE_CAR; +static const apo::halMode_t halMode = apo::MODE_LIVE; +static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560; +static const uint8_t heartBeatTimeout = 3; + +// algorithm selection +#define CONTROLLER_CLASS ControllerCar +#define GUIDE_CLASS MavlinkGuide +#define NAVIGATOR_CLASS DcmNavigator +#define COMMLINK_CLASS MavlinkComm + +// hardware selection +#define ADC_CLASS AP_ADC_ADS7844 +#define COMPASS_CLASS AP_Compass_HMC5843 +#define BARO_CLASS APM_BMP085_Class +#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL +#define DEBUG_BAUD 57600 +#define TELEM_BAUD 57600 +#define GPS_BAUD 38400 +#define HIL_BAUD 57600 + +// optional sensors +static bool gpsEnabled = false; +static bool baroEnabled = true; +static bool compassEnabled = true; + +static bool rangeFinderFrontEnabled = true; +static bool rangeFinderBackEnabled = true; +static bool rangeFinderLeftEnabled = true; +static bool rangeFinderRightEnabled = true; +static bool rangeFinderUpEnabled = true; +static bool rangeFinderDownEnabled = true; + +// 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 loop4Rate = 0.1; + +// gains +static const float steeringP = 1.0; +static const float steeringI = 0.0; +static const float steeringD = 0.0; +static const float steeringIMax = 0.0; +static const float steeringYMax = 3.0; + +static const float throttleP = 0.0; +static const float throttleI = 0.0; +static const float throttleD = 0.0; +static const float throttleIMax = 0.0; +static const float throttleYMax = 0.0; +static const float throttleDFCut = 3.0; + +#include "ControllerCar.h" + +#endif /* CARSTAMPEDE_H_ */ diff --git a/ArduRover/ControllerCar.h b/ArduRover/ControllerCar.h new file mode 100644 index 0000000000..5a846df99b --- /dev/null +++ b/ArduRover/ControllerCar.h @@ -0,0 +1,125 @@ +/* + * ControllerCar.h + * + * Created on: Jun 30, 2011 + * Author: jgoppert + */ + +#ifndef CONTROLLERCAR_H_ +#define CONTROLLERCAR_H_ + +#include "../APO/AP_Controller.h" + +namespace apo { + +class ControllerCar: public AP_Controller { +private: + AP_Var_group _group; + AP_Uint8 _mode; + enum { + k_chMode = k_radioChannelsStart, k_chStr, k_chThr + }; + enum { + k_pidStr = k_controllersStart, k_pidThr + }; + enum { + CH_MODE = 0, CH_STR, CH_THR + }; + BlockPIDDfb pidStr; + BlockPID pidThr; +public: + ControllerCar(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + AP_Controller(nav, guide, hal), + _group(k_cntrl, PSTR("CNTRL_")), + _mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")), + pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, + steeringI, steeringD, steeringIMax, steeringYMax), + pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP, + throttleI, throttleD, throttleIMax, throttleYMax, + throttleDFCut) { + _hal->debug->println_P(PSTR("initializing car controller")); + + _hal->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7, 1100, + 1500, 1900)); + _hal->rc.push_back( + new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1540, + 1900)); + _hal->rc.push_back( + new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500, + 1900)); + } + virtual MAV_MODE getMode() { + return (MAV_MODE) _mode.get(); + } + virtual void update(const float & dt) { + + // check for heartbeat + if (_hal->heartBeatLost()) { + _mode = MAV_MODE_FAILSAFE; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n")); + return; + // if throttle less than 5% cut motor power + } else if (_hal->rc[CH_THR]->getRadioPosition() < 0.05) { + _mode = MAV_MODE_LOCKED; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_STANDBY); + return; + // if in live mode then set state to active + } else if (_hal->getMode() == MODE_LIVE) { + _hal->setState(MAV_STATE_ACTIVE); + // if in hardware in the loop (control) mode, set to hilsim + } else if (_hal->getMode() == MODE_HIL_CNTL) { + _hal->setState(MAV_STATE_HILSIM); + } + + // read switch to set mode + if (_hal->rc[CH_MODE]->getRadioPosition() > 0) { + _mode = MAV_MODE_MANUAL; + } else { + _mode = MAV_MODE_AUTO; + } + + // manual mode + switch (_mode) { + + case MAV_MODE_MANUAL: { + setAllRadioChannelsManually(); + //_hal->debug->println("manual"); + break; + } + case MAV_MODE_AUTO: { + float headingError = _guide->getHeadingCommand() + - _nav->getYaw(); + if (headingError > 180 * deg2Rad) + headingError -= 360 * deg2Rad; + if (headingError < -180 * deg2Rad) + headingError += 360 * deg2Rad; + _hal->rc[CH_STR]->setPosition( + pidStr.update(headingError, _nav->getYawRate(), dt)); + _hal->rc[CH_THR]->setPosition( + pidThr.update( + _guide->getGroundSpeedCommand() + - _nav->getGroundSpeed(), dt)); + //_hal->debug->println("automode"); + break; + } + + default: { + setAllRadioChannelsToNeutral(); + _mode = MAV_MODE_FAILSAFE; + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("unknown controller mode\n")); + break; + } + + } + } +}; + +} // namespace apo + +#endif /* CONTROLLERCAR_H_ */ diff --git a/ArduRover/ControllerTank.h b/ArduRover/ControllerTank.h new file mode 100644 index 0000000000..b0bb3f1ef7 --- /dev/null +++ b/ArduRover/ControllerTank.h @@ -0,0 +1,133 @@ +/* + * ControllerTank.h + * + * Created on: Jun 30, 2011 + * Author: jgoppert + */ + +#ifndef CONTROLLERTANK_H_ +#define CONTROLLERTANK_H_ + +#include "../APO/AP_Controller.h" + +namespace apo { + +class ControllerTank: public AP_Controller { +private: + AP_Var_group _group; + AP_Uint8 _mode; + enum { + k_chMode = k_radioChannelsStart, k_chLeft, k_chRight, k_chStr, k_chThr + }; + enum { + k_pidStr = k_controllersStart, k_pidThr + }; + enum { + CH_MODE = 0, CH_LEFT, CH_RIGHT, CH_STR, CH_THR + }; + BlockPIDDfb pidStr; + BlockPID pidThr; +public: + ControllerTank(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + AP_Controller(nav, guide, hal), + _group(k_cntrl, PSTR("CNTRL_")), + _mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")), + pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, + steeringI, steeringD, steeringIMax, steeringYMax), + pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP, + throttleI, throttleD, throttleIMax, throttleYMax, + throttleDFCut) { + _hal->debug->println_P(PSTR("initializing tank controller")); + + _hal->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100, + 1500, 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 0, 1100, 1500, + 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 1, 1100, 1500, + 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1500, + 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500, + 1900, RC_MODE_IN)); + } + virtual MAV_MODE getMode() { + return (MAV_MODE) _mode.get(); + } + void mix(float headingOutput, float throttleOutput) { + _hal->rc[CH_LEFT]->setPosition(throttleOutput + headingOutput); + _hal->rc[CH_RIGHT]->setPosition(throttleOutput - headingOutput); + } + virtual void update(const float & dt) { + + // check for heartbeat + if (_hal->heartBeatLost()) { + _mode = MAV_MODE_FAILSAFE; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n")); + return; + // if throttle less than 5% cut motor power + } else if (_hal->rc[CH_THR]->getRadioPosition() < 0.05) { + _mode = MAV_MODE_LOCKED; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_STANDBY); + return; + // if in live mode then set state to active + } else if (_hal->getMode() == MODE_LIVE) { + _hal->setState(MAV_STATE_ACTIVE); + // if in hardware in the loop (control) mode, set to hilsim + } else if (_hal->getMode() == MODE_HIL_CNTL) { + _hal->setState(MAV_STATE_HILSIM); + } + + // read switch to set mode + if (_hal->rc[CH_MODE]->getRadioPosition() > 0) { + _mode = MAV_MODE_MANUAL; + } else { + _mode = MAV_MODE_AUTO; + } + + // manual mode + switch (_mode) { + + case MAV_MODE_MANUAL: { + setAllRadioChannelsManually(); + mix(_hal->rc[CH_STR]->getPosition(), + _hal->rc[CH_THR]->getPosition()); + break; + } + case MAV_MODE_AUTO: { + float headingError = _guide->getHeadingCommand() + - _nav->getYaw(); + if (headingError > 180 * deg2Rad) + headingError -= 360 * deg2Rad; + if (headingError < -180 * deg2Rad) + headingError += 360 * deg2Rad; + mix(pidStr.update(headingError, _nav->getYawRate(), dt), + pidThr.update(_guide->getGroundSpeedCommand() + - _nav->getGroundSpeed(), dt)); + //_hal->debug->println("automode"); + break; + } + + default: { + setAllRadioChannelsToNeutral(); + _mode = MAV_MODE_FAILSAFE; + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("unknown controller mode\n")); + break; + } + + } + } +}; + +} // namespace apo + +#endif /* CONTROLLERTANK_H_ */ diff --git a/ArduRover/Makefile b/ArduRover/Makefile new file mode 100644 index 0000000000..1d9b6a022d --- /dev/null +++ b/ArduRover/Makefile @@ -0,0 +1 @@ +include ../libraries/AP_Common/Arduino.mk diff --git a/ArduRover/TankGeneric.h b/ArduRover/TankGeneric.h new file mode 100644 index 0000000000..30d7118582 --- /dev/null +++ b/ArduRover/TankGeneric.h @@ -0,0 +1,68 @@ +/* + * TankGeneric.h + * + * Created on: Sep 26, 2011 + * Author: jgoppert + */ + +#ifndef TANKGENERIC_H_ +#define TANKGENERIC_H_ + +// vehicle options +static const apo::vehicle_t vehicle = apo::VEHICLE_TANK; +static const apo::halMode_t halMode = apo::MODE_LIVE; +static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560; +static const uint8_t heartBeatTimeout = 3; + +// algorithm selection +#define CONTROLLER_CLASS ControllerTank +#define GUIDE_CLASS MavlinkGuide +#define NAVIGATOR_CLASS DcmNavigator +#define COMMLINK_CLASS MavlinkComm + +// hardware selection +#define ADC_CLASS AP_ADC_ADS7844 +#define COMPASS_CLASS AP_Compass_HMC5843 +#define BARO_CLASS APM_BMP085_Class +#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL +#define DEBUG_BAUD 57600 +#define TELEM_BAUD 57600 +#define GPS_BAUD 38400 +#define HIL_BAUD 57600 + +// optional sensors +static bool gpsEnabled = false; +static bool baroEnabled = true; +static bool compassEnabled = true; + +static bool rangeFinderFrontEnabled = true; +static bool rangeFinderBackEnabled = true; +static bool rangeFinderLeftEnabled = true; +static bool rangeFinderRightEnabled = true; +static bool rangeFinderUpEnabled = true; +static bool rangeFinderDownEnabled = true; + +// 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 loop4Rate = 0.1; + +// gains +const float steeringP = 1.0; +const float steeringI = 0.0; +const float steeringD = 0.0; +const float steeringIMax = 0.0; +const float steeringYMax = 3.0; + +const float throttleP = 0.0; +const float throttleI = 0.0; +const float throttleD = 0.0; +const float throttleIMax = 0.0; +const float throttleYMax = 0.0; +const float throttleDFCut = 3.0; + +#include "ControllerTank.h" + +#endif /* TANKGENERIC_H_ */ diff --git a/apo/CMakeLists.txt b/apo/CMakeLists.txt new file mode 100644 index 0000000000..8227430d75 --- /dev/null +++ b/apo/CMakeLists.txt @@ -0,0 +1,80 @@ +#=============================================================================# +# Author: Sebastian Rohde # +# Date: 30.08.2011 # +#=============================================================================# + + +#====================================================================# +# Settings # +#====================================================================# +set(FIRMWARE_NAME APO) + +set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board + +set(${FIRMWARE_NAME}_SKETCHES + ardupilotone.pde + ) # Firmware sketches + +set(${FIRMWARE_NAME}_SRCS + ) # Firmware sources + +set(${FIRMWARE_NAME}_HDRS + BoatGeneric.h + CarStampede.h + #CNIRover.h + #CNIBoat.h + ControllerBoat.h + ControllerCar.h + ControllerPlane.h + ControllerQuad.h + PlaneEasystar.h + QuadArducopter.h + QuadMikrokopter.h + ) # Firmware sources + +set(${FIRMWARE_NAME}_LIBS + m + APO + FastSerial + AP_Common + GCS_MAVLink + AP_GPS + APM_RC + AP_DCM + AP_ADC + AP_Compass + AP_IMU + AP_RangeFinder + APM_BMP085 + ModeFilter +) + + +#${CONSOLE_PORT} +set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port +set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd + +include_directories( +${CMAKE_SOURCE_DIR}/libraries/APO +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +) +#====================================================================# +# Target generation # +#====================================================================# +generate_arduino_firmware(${FIRMWARE_NAME}) + +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/${FIRMWARE_NAME}.hex + DESTINATION bin + ) diff --git a/apo/ControllerPlane.h b/apo/ControllerPlane.h new file mode 100644 index 0000000000..539253f8d9 --- /dev/null +++ b/apo/ControllerPlane.h @@ -0,0 +1,215 @@ +/* + * ControllerPlane.h + * + * Created on: Jun 30, 2011 + * Author: jgoppert + */ + +#ifndef CONTROLLERPLANE_H_ +#define CONTROLLERPLANE_H_ + +#include "../APO/AP_Controller.h" + +namespace apo { + +class ControllerPlane: public AP_Controller { +private: + AP_Var_group _group; + AP_Var_group _trimGroup; + AP_Uint8 _mode; + AP_Uint8 _rdrAilMix; + bool _needsTrim; + AP_Float _ailTrim; + AP_Float _elvTrim; + AP_Float _rdrTrim; + AP_Float _thrTrim; + enum { + ch_mode = 0, ch_roll, ch_pitch, ch_thr, ch_yaw + }; + enum { + k_chMode = k_radioChannelsStart, + k_chRoll, + k_chPitch, + k_chYaw, + k_chThr, + + k_pidBnkRll = k_controllersStart, + k_pidSpdPit, + k_pidPitPit, + k_pidYwrYaw, + k_pidHdgBnk, + k_pidAltThr, + + k_trim = k_customStart + }; + BlockPID pidBnkRll; // bank error to roll servo deflection + BlockPID pidSpdPit; // speed error to pitch command + BlockPID pidPitPit; // pitch error to pitch servo deflection + BlockPID pidYwrYaw; // yaw rate error to yaw servo deflection + BlockPID pidHdgBnk; // heading error to bank command + BlockPID pidAltThr; // altitude error to throttle deflection + bool requireRadio; + +public: + ControllerPlane(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + AP_Controller(nav, guide, hal), + _group(k_cntrl, PSTR("cntrl_")), + _trimGroup(k_trim, PSTR("trim_")), + _mode(&_group, 1, MAV_MODE_UNINIT, PSTR("mode")), + _rdrAilMix(&_group, 2, rdrAilMix, PSTR("rdrAilMix")), + _needsTrim(false), + _ailTrim(&_trimGroup, 1, ailTrim, PSTR("ail")), + _elvTrim(&_trimGroup, 2, elvTrim, PSTR("elv")), + _rdrTrim(&_trimGroup, 3, rdrTrim, PSTR("rdr")), + _thrTrim(&_trimGroup, 4, thrTrim, PSTR("thr")), + pidBnkRll(new AP_Var_group(k_pidBnkRll, PSTR("bnkRll_")), 1, + pidBnkRllP, pidBnkRllI, pidBnkRllD, pidBnkRllAwu, + pidBnkRllLim, pidBnkRllDFCut), + pidPitPit(new AP_Var_group(k_pidPitPit, PSTR("pitPit_")), 1, + pidPitPitP, pidPitPitI, pidPitPitD, pidPitPitAwu, + pidPitPitLim, pidPitPitDFCut), + pidSpdPit(new AP_Var_group(k_pidSpdPit, PSTR("spdPit_")), 1, + pidSpdPitP, pidSpdPitI, pidSpdPitD, pidSpdPitAwu, + pidSpdPitLim, pidSpdPitDFCut), + pidYwrYaw(new AP_Var_group(k_pidYwrYaw, PSTR("ywrYaw_")), 1, + pidYwrYawP, pidYwrYawI, pidYwrYawD, pidYwrYawAwu, + pidYwrYawLim, pidYwrYawDFCut), + pidHdgBnk(new AP_Var_group(k_pidHdgBnk, PSTR("hdgBnk_")), 1, + pidHdgBnkP, pidHdgBnkI, pidHdgBnkD, pidHdgBnkAwu, + pidHdgBnkLim, pidHdgBnkDFCut), + pidAltThr(new AP_Var_group(k_pidAltThr, PSTR("altThr_")), 1, + pidAltThrP, pidAltThrI, pidAltThrD, pidAltThrAwu, + pidAltThrLim, pidAltThrDFCut), + requireRadio(false) { + + _hal->debug->println_P(PSTR("initializing car controller")); + + _hal->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("mode_"), APM_RC, 5, 1100, + 1500, 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chRoll, PSTR("roll_"), APM_RC, 0, 1200, + 1500, 1800, RC_MODE_INOUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chPitch, PSTR("pitch_"), APM_RC, 1, 1200, + 1500, 1800, RC_MODE_INOUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chThr, PSTR("thr_"), APM_RC, 2, 1100, 1100, + 1900, RC_MODE_INOUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chYaw, PSTR("yaw_"), APM_RC, 3, 1200, 1500, + 1800, RC_MODE_INOUT)); + } + virtual MAV_MODE getMode() { + return (MAV_MODE) _mode.get(); + } + virtual void update(const float & dt) { + + // check for heartbeat + if (_hal->heartBeatLost()) { + _mode = MAV_MODE_FAILSAFE; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n")); + return; + // if the value of the throttle is less than 5% cut motor power + } else if (requireRadio && _hal->rc[ch_thr]->getRadioPosition() < 0.05) { + _mode = MAV_MODE_LOCKED; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_STANDBY); + return; + // if in live mode then set state to active + } else if (_hal->getMode() == MODE_LIVE) { + _hal->setState(MAV_STATE_ACTIVE); + // if in hardware in the loop (control) mode, set to hilsim + } else if (_hal->getMode() == MODE_HIL_CNTL) { + _hal->setState(MAV_STATE_HILSIM); + } + + // read switch to set mode + if (_hal->rc[ch_mode]->getRadioPosition() > 0) { + _mode = MAV_MODE_MANUAL; + } else { + _mode = MAV_MODE_AUTO; + } + + // manual mode + switch (_mode) { + + case MAV_MODE_MANUAL: { + setAllRadioChannelsManually(); + + // force auto to read new manual trim + if (_needsTrim == false) + _needsTrim = true; + //_hal->debug->println("manual"); + break; + } + case MAV_MODE_AUTO: { + float headingError = _guide->getHeadingCommand() + - _nav->getYaw(); + if (headingError > 180 * deg2Rad) + headingError -= 360 * deg2Rad; + if (headingError < -180 * deg2Rad) + headingError += 360 * deg2Rad; + + float aileron = pidBnkRll.update( + pidHdgBnk.update(headingError, dt) - _nav->getRoll(), dt); + float elevator = pidPitPit.update( + -pidSpdPit.update( + _guide->getAirSpeedCommand() - _nav->getAirSpeed(), + dt) - _nav->getPitch(), dt); + float rudder = pidYwrYaw.update(-_nav->getYawRate(), dt); + // desired yaw rate is zero, needs washout + float throttle = pidAltThr.update( + _guide->getAltitudeCommand() - _nav->getAlt(), dt); + + // if needs trim + if (_needsTrim) { + // need to subtract current controller deflections so control + // surfaces are actually at the same position as manual flight + _ailTrim = _hal->rc[ch_roll]->getRadioPosition() - aileron; + _elvTrim = _hal->rc[ch_pitch]->getRadioPosition() - elevator; + _rdrTrim = _hal->rc[ch_yaw]->getRadioPosition() - rudder; + _thrTrim = _hal->rc[ch_thr]->getRadioPosition() - throttle; + _needsTrim = false; + } + + // actuator mixing/ output + _hal->rc[ch_roll]->setPosition( + aileron + _rdrAilMix * rudder + _ailTrim); + _hal->rc[ch_yaw]->setPosition(rudder + _rdrTrim); + _hal->rc[ch_pitch]->setPosition(elevator + _elvTrim); + _hal->rc[ch_thr]->setPosition(throttle + _thrTrim); + + //_hal->debug->println("automode"); + + + // heading debug +// Serial.print("heading command: "); Serial.println(_guide->getHeadingCommand()); +// Serial.print("heading: "); Serial.println(_nav->getYaw()); +// Serial.print("heading error: "); Serial.println(headingError); + + // alt debug +// Serial.print("alt command: "); Serial.println(_guide->getAltitudeCommand()); +// Serial.print("alt: "); Serial.println(_nav->getAlt()); +// Serial.print("alt error: "); Serial.println(_guide->getAltitudeCommand() - _nav->getAlt()); + break; + } + + default: { + setAllRadioChannelsToNeutral(); + _mode = MAV_MODE_FAILSAFE; + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("unknown controller mode\n")); + break; + } + + } + } +}; + +} // namespace apo + +#endif /* CONTROLLERPLANE_H_ */ diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h new file mode 100644 index 0000000000..3f74739b00 --- /dev/null +++ b/apo/ControllerQuad.h @@ -0,0 +1,250 @@ +/* + * ControllerQuad.h + * + * Created on: Jun 30, 2011 + * Author: jgoppert + */ + +#ifndef CONTROLLERQUAD_H_ +#define CONTROLLERQUAD_H_ + +#include "../APO/AP_Controller.h" + +namespace apo { + +class ControllerQuad: public AP_Controller { +public: + + /** + * note that these are not the controller radio channel numbers, they are just + * unique keys so they can be reaccessed from the hal rc vector + */ + enum { + CH_MODE = 0, // note scicoslab channels set mode, left, right, front, back order + CH_LEFT, // this enum must match this + CH_RIGHT, + CH_FRONT, + CH_BACK, + CH_ROLL, + CH_PITCH, + CH_YAW, + CH_THRUST + }; + + enum { + k_chMode = k_radioChannelsStart, + k_chLeft, + k_chRight, + k_chFront, + k_chBack, + k_chRoll, + k_chPitch, + k_chYaw, + k_chThr + }; + + enum { + k_pidGroundSpeed2Throttle = k_controllersStart, + k_pidStr, + k_pidPN, + k_pidPE, + k_pidPD, + k_pidRoll, + k_pidPitch, + k_pidYawRate, + k_pidYaw, + }; + + ControllerQuad(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + AP_Controller(nav, guide, hal), + pidRoll(new AP_Var_group(k_pidRoll, PSTR("ROLL_")), 1, + PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU, + PID_ATT_LIM), + pidPitch(new AP_Var_group(k_pidPitch, PSTR("PITCH_")), 1, + PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU, + PID_ATT_LIM), + pidYaw(new AP_Var_group(k_pidYaw, PSTR("YAW_")), 1, + PID_YAWPOS_P, PID_YAWPOS_I, PID_YAWPOS_D, + PID_YAWPOS_AWU, PID_YAWPOS_LIM), + pidYawRate(new AP_Var_group(k_pidYawRate, PSTR("YAWRT_")), 1, + PID_YAWSPEED_P, PID_YAWSPEED_I, PID_YAWSPEED_D, + PID_YAWSPEED_AWU, PID_YAWSPEED_LIM, PID_YAWSPEED_DFCUT), + pidPN(new AP_Var_group(k_pidPN, PSTR("NORTH_")), 1, PID_POS_P, + PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM), + pidPE(new AP_Var_group(k_pidPE, PSTR("EAST_")), 1, PID_POS_P, + PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM), + pidPD(new AP_Var_group(k_pidPD, PSTR("DOWN_")), 1, PID_POS_Z_P, + PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM) { + /* + * allocate radio channels + * the order of the channels has to match the enumeration above + */ + _hal->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100, + 1500, 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 0, 1100, + 1100, 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 1, 1100, + 1100, 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100, + 1100, 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chBack, PSTR("BACK_"), APM_RC, 3, 1100, + 1100, 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chRoll, PSTR("ROLL_"), APM_RC, 0, 1100, + 1500, 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100, + 1500, 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 2, 1100, 1500, + 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 3, 1100, + 1100, 1900, RC_MODE_IN)); + } + + virtual void update(const float & dt) { + + // check for heartbeat + if (_hal->heartBeatLost()) { + _mode = MAV_MODE_FAILSAFE; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n")); + return; + // if throttle less than 5% cut motor power + } else if (_hal->rc[CH_THRUST]->getRadioPosition() < 0.05) { + _mode = MAV_MODE_LOCKED; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_STANDBY); + return; + // if in live mode then set state to active + } else if (_hal->getMode() == MODE_LIVE) { + _hal->setState(MAV_STATE_ACTIVE); + // if in hardware in the loop (control) mode, set to hilsim + } else if (_hal->getMode() == MODE_HIL_CNTL) { + _hal->setState(MAV_STATE_HILSIM); + } + + // manual mode + float mixRemoteWeight = 0; + if (_hal->rc[CH_MODE]->getRadioPosition() > 0) { + mixRemoteWeight = 1; + _mode = MAV_MODE_MANUAL; + } else { + _mode = MAV_MODE_AUTO; + } + + // commands for inner loop + float cmdRoll = 0; + float cmdPitch = 0; + float cmdYawRate = 0; + float thrustMix = 0; + + switch(_mode) { + + case MAV_MODE_MANUAL: { + setAllRadioChannelsManually(); + // "mix manual" + cmdRoll = 0.5 * _hal->rc[CH_ROLL]->getPosition() + * mixRemoteWeight; + cmdPitch = 0.5 * _hal->rc[CH_PITCH]->getPosition() + * mixRemoteWeight; + cmdYawRate = 0.5 * _hal->rc[CH_YAW]->getPosition() + * mixRemoteWeight; + thrustMix = _hal->rc[CH_THRUST]->getPosition() * mixRemoteWeight; + break; + } + + case MAV_MODE_AUTO: { + + // XXX kills all commands, + // auto not currently implemented + + // position loop + /* + 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); + + // "transform-to-body" + { + float trigSin = sin(-yaw); + float trigCos = cos(-yaw); + _cmdPitch = _cmdEastTilt * trigCos + - _cmdNorthTilt * trigSin; + _cmdRoll = -_cmdEastTilt * trigSin + + _cmdNorthTilt * trigCos; + // note that the north tilt is negative of the pitch + } + + //thrustMix += THRUST_HOVER_OFFSET; + + // "thrust-trim-adjust" + if (fabs(_cmdRoll) > 0.5) { + _thrustMix *= 1.13949393; + } else { + _thrustMix /= cos(_cmdRoll); + } + if (fabs(_cmdPitch) > 0.5) { + _thrustMix *= 1.13949393; + } else { + _thrustMix /= cos(_cmdPitch); + } + */ + } + + } + + // attitude loop + // XXX negative sign added to nav roll, not sure why this is necessary + // XXX negative sign added to nav roll rate, not sure why this is necessary + float rollMix = pidRoll.update(cmdRoll + _nav->getRoll(), + -_nav->getRollRate(), dt); + // XXX negative sign added to cmdPitch, not sure why this is necessary + float pitchMix = pidPitch.update(-cmdPitch - _nav->getPitch(), + _nav->getPitchRate(), dt); + // XXX negative sign added to cmdYawRate, not sure why this is necessary + float yawMix = pidYawRate.update(-cmdYawRate - _nav->getYawRate(), dt); + + _hal->rc[CH_LEFT]->setPosition(thrustMix + rollMix + yawMix); + _hal->rc[CH_RIGHT]->setPosition(thrustMix - rollMix + yawMix); + _hal->rc[CH_FRONT]->setPosition(thrustMix + pitchMix - yawMix); + _hal->rc[CH_BACK]->setPosition(thrustMix - pitchMix - yawMix); + + // _hal->debug->printf("L: %f\t R: %f\t F: %f\t B: %f\n", + // _hal->rc[CH_LEFT]->getPosition(), + // _hal->rc[CH_RIGHT]->getPosition(), + // _hal->rc[CH_FRONT]->getPosition(), + // _hal->rc[CH_BACK]->getPosition()); + + _hal->debug->printf( + "rollMix: %f\t pitchMix: %f\t yawMix: %f\t thrustMix: %f\n", + rollMix, pitchMix, yawMix, thrustMix); + + // _hal->debug->printf("roll pwm: %d\t pitch pwm: %d\t yaw pwm: %d\t thrust pwm: %d\n", + // _hal->rc[CH_ROLL]->readRadio(), + // _hal->rc[CH_PITCH]->readRadio(), + // _hal->rc[CH_YAW]->readRadio(), + // _hal->rc[CH_THRUST]->readRadio()); + } + virtual MAV_MODE getMode() { + return (MAV_MODE) _mode.get(); + } +private: + AP_Uint8 _mode; + BlockPIDDfb pidRoll, pidPitch, pidYaw; + BlockPID pidYawRate; + BlockPIDDfb pidPN, pidPE, pidPD; + +}; + +} // namespace apo + +#endif /* CONTROLLERQUAD_H_ */ diff --git a/apo/Makefile b/apo/Makefile new file mode 100644 index 0000000000..1d9b6a022d --- /dev/null +++ b/apo/Makefile @@ -0,0 +1 @@ +include ../libraries/AP_Common/Arduino.mk diff --git a/apo/PlaneEasystar.h b/apo/PlaneEasystar.h new file mode 100644 index 0000000000..6acf27b403 --- /dev/null +++ b/apo/PlaneEasystar.h @@ -0,0 +1,113 @@ +/* + * PlaneEasystar.h + * + * Created on: May 1, 2011 + * Author: jgoppert + */ + +#ifndef PLANEEASYSTAR_H_ +#define PLANEEASYSTAR_H_ + + +// vehicle options +static const apo::vehicle_t vehicle = apo::VEHICLE_PLANE; +//static const apo::halMode_t halMode = apo::MODE_LIVE; // live mode, actual flight +static const apo::halMode_t halMode = apo::MODE_HIL_CNTL; // hardware in the loop, control level +static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; +static const uint8_t heartBeatTimeout = 3; + +// algorithm selection +#define CONTROLLER_CLASS ControllerPlane +#define GUIDE_CLASS MavlinkGuide +#define NAVIGATOR_CLASS DcmNavigator +#define COMMLINK_CLASS MavlinkComm + +// hardware selection +#define ADC_CLASS AP_ADC_ADS7844 +#define COMPASS_CLASS AP_Compass_HMC5843 +#define BARO_CLASS APM_BMP085_Class +#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL +#define DEBUG_BAUD 57600 +#define TELEM_BAUD 57600 +#define GPS_BAUD 38400 +#define HIL_BAUD 57600 + +// optional sensors +static bool gpsEnabled = false; +static bool baroEnabled = true; +static bool compassEnabled = true; + +static bool rangeFinderFrontEnabled = true; +static bool rangeFinderBackEnabled = true; +static bool rangeFinderLeftEnabled = true; +static bool rangeFinderRightEnabled = true; +static bool rangeFinderUpEnabled = true; +static bool rangeFinderDownEnabled = true; + +// 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 loop4Rate = 0.1; + +// gains +static const float rdrAilMix = 1.0; // since there are no ailerons + +// bank error to roll servo +static const float pidBnkRllP = -0.5; +static const float pidBnkRllI = 0.0; +static const float pidBnkRllD = 0.0; +static const float pidBnkRllAwu = 0.0; +static const float pidBnkRllLim = 1.0; +static const float pidBnkRllDFCut = 0.0; + +// pitch error to pitch servo +static const float pidPitPitP = -1; +static const float pidPitPitI = 0.0; +static const float pidPitPitD = 0.0; +static const float pidPitPitAwu = 0.0; +static const float pidPitPitLim = 1.0; +static const float pidPitPitDFCut = 0.0; + +// speed error to pitch command +static const float pidSpdPitP = 0.1; +static const float pidSpdPitI = 0.0; +static const float pidSpdPitD = 0.0; +static const float pidSpdPitAwu = 0.0; +static const float pidSpdPitLim = 1.0; +static const float pidSpdPitDFCut = 0.0; + +// yaw rate error to yaw servo +static const float pidYwrYawP = -0.2; +static const float pidYwrYawI = 0.0; +static const float pidYwrYawD = 0.0; +static const float pidYwrYawAwu = 0.0; +static const float pidYwrYawLim = 1.0; +static const float pidYwrYawDFCut = 0.0; + +// heading error to bank angle command +static const float pidHdgBnkP = 1.0; +static const float pidHdgBnkI = 0.0; +static const float pidHdgBnkD = 0.0; +static const float pidHdgBnkAwu = 0.0; +static const float pidHdgBnkLim = 0.5; +static const float pidHdgBnkDFCut = 0.0; + +// altitude error to throttle command +static const float pidAltThrP = .01; +static const float pidAltThrI = 0.0; +static const float pidAltThrD = 0.0; +static const float pidAltThrAwu = 0.0; +static const float pidAltThrLim = 1; +static const float pidAltThrDFCut = 0.0; + +// trim control positions (-1,1) +static const float ailTrim = 0.0; +static const float elvTrim = 0.0; +static const float rdrTrim = 0.0; +static const float thrTrim = 0.5; + +#include "ControllerPlane.h" + +#endif /* PLANEEASYSTAR_H_ */ diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h new file mode 100644 index 0000000000..5151b224fe --- /dev/null +++ b/apo/QuadArducopter.h @@ -0,0 +1,98 @@ +/* + * QuadArducopter.h + * + * Created on: May 1, 2011 + * Author: jgoppert + */ + +#ifndef QUADARDUCOPTER_H_ +#define QUADARDUCOPTER_H_ + +// 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_2560; +static const uint8_t heartBeatTimeout = 3; + +// algorithm selection +#define CONTROLLER_CLASS ControllerQuad +#define GUIDE_CLASS MavlinkGuide +#define NAVIGATOR_CLASS DcmNavigator +#define COMMLINK_CLASS MavlinkComm + +// hardware selection +#define ADC_CLASS AP_ADC_ADS7844 +#define COMPASS_CLASS AP_Compass_HMC5843 +#define BARO_CLASS APM_BMP085_Class +#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL +#define DEBUG_BAUD 57600 +#define TELEM_BAUD 57600 +#define GPS_BAUD 38400 +#define HIL_BAUD 57600 + +// optional sensors +static bool gpsEnabled = false; +static bool baroEnabled = true; +static bool compassEnabled = true; + +static bool rangeFinderFrontEnabled = true; +static bool rangeFinderBackEnabled = true; +static bool rangeFinderLeftEnabled = true; +static bool rangeFinderRightEnabled = true; +static bool rangeFinderUpEnabled = true; +static bool rangeFinderDownEnabled = true; + +// 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 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; +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_I = 0; +static const float PID_POS_Z_D = 0; +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_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_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_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" + +#endif /* QUADARDUCOPTER_H_ */ diff --git a/apo/QuadMikrokopter.h b/apo/QuadMikrokopter.h new file mode 100644 index 0000000000..7aeeacd5e6 --- /dev/null +++ b/apo/QuadMikrokopter.h @@ -0,0 +1,98 @@ +/* + * QuadMikrokopter + * + * Created on: May 1, 2011 + * Author: jgoppert + */ + +#ifndef QUADMIKROKOPTER_H_ +#define QUADMIKROKOPTER_H_ + +// 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_2560; +static const uint8_t heartBeatTimeout = 3; + +// algorithm selection +#define CONTROLLER_CLASS ControllerQuad +#define GUIDE_CLASS MavlinkGuide +#define NAVIGATOR_CLASS DcmNavigator +#define COMMLINK_CLASS MavlinkComm + +// hardware selection +#define ADC_CLASS AP_ADC_ADS7844 +#define COMPASS_CLASS AP_Compass_HMC5843 +#define BARO_CLASS APM_BMP085_Class +#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL +#define DEBUG_BAUD 57600 +#define TELEM_BAUD 57600 +#define GPS_BAUD 38400 +#define HIL_BAUD 57600 + +// optional sensors +static bool gpsEnabled = false; +static bool baroEnabled = true; +static bool compassEnabled = true; + +static bool rangeFinderFrontEnabled = true; +static bool rangeFinderBackEnabled = true; +static bool rangeFinderLeftEnabled = true; +static bool rangeFinderRightEnabled = true; +static bool rangeFinderUpEnabled = true; +static bool rangeFinderDownEnabled = true; + +// 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 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; +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_I = 0; +static const float PID_POS_Z_D = 0; +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_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_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_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" + +#endif /* QUADMIKROKOPTER_H_ */ diff --git a/apo/apo.pde b/apo/apo.pde new file mode 100644 index 0000000000..7e79ec1a9e --- /dev/null +++ b/apo/apo.pde @@ -0,0 +1,184 @@ +/* + * ardupilotone + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#define ENABLE_FASTSERIAL_DEBUG + +// Libraries +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +FastSerialPort0(Serial); +FastSerialPort1(Serial1); +FastSerialPort2(Serial2); +FastSerialPort3(Serial3); + +// Vehicle Configuration +#include "PlaneEasystar.h" + +/* + * Required Global Declarations + */ + +static apo::AP_Autopilot * autoPilot; + +void setup() { + + using namespace apo; + + AP_Var::load_all(); + + /* + * Communications + */ + Serial.begin(DEBUG_BAUD, 128, 128); // debug + if (board==BOARD_ARDUPILOTMEGA_2) Serial2.begin(TELEM_BAUD, 128, 128); // gcs + else Serial3.begin(TELEM_BAUD, 128, 128); // gcs + + // hardware abstraction layer + AP_HardwareAbstractionLayer * hal = new AP_HardwareAbstractionLayer( + halMode, board, vehicle, heartBeatTimeout); + + // debug serial + hal->debug = &Serial; + hal->debug->println_P(PSTR("initializing debug line")); + + /* + * Initialize Comm Channels + */ + hal->debug->println_P(PSTR("initializing comm channels")); + if (hal->getMode() == MODE_LIVE) { + Serial1.begin(GPS_BAUD, 128, 16); // gps + } else { // hil + Serial1.begin(HIL_BAUD, 128, 128); + } + + /* + * Sensor initialization + */ + if (hal->getMode() == MODE_LIVE) { + hal->debug->println_P(PSTR("initializing adc")); + hal->adc = new ADC_CLASS; + hal->adc->Init(); + + if (gpsEnabled) { + hal->debug->println_P(PSTR("initializing gps")); + AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps)); + hal->gps = &gpsDriver; + hal->gps->init(); + } + + if (baroEnabled) { + hal->debug->println_P(PSTR("initializing baro")); + hal->baro = new BARO_CLASS; + hal->baro->Init(); + } + + if (compassEnabled) { + hal->debug->println_P(PSTR("initializing compass")); + hal->compass = new COMPASS_CLASS; + hal->compass->set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD); + hal->compass->init(); + } + + /** + * Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not + * initialize them and NULL will be assigned to those corresponding pointers. + * On detecting NULL assigned to any ultrasonic sensor, its corresponding block of code + * will not be executed by the navigator. + * The coordinate system is assigned by the right hand rule with the thumb pointing down. + * In set_orientation, it is defind as (front/back,left/right,down,up) + */ + + if (rangeFinderFrontEnabled) { + hal->debug->println_P(PSTR("initializing front range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(1); + rangeFinder->set_orientation(1, 0, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderBackEnabled) { + hal->debug->println_P(PSTR("initializing back range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(2); + rangeFinder->set_orientation(-1, 0, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderLeftEnabled) { + hal->debug->println_P(PSTR("initializing left range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(3); + rangeFinder->set_orientation(0, -1, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderRightEnabled) { + hal->debug->println_P(PSTR("initializing right range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(4); + rangeFinder->set_orientation(0, 1, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderUpEnabled) { + hal->debug->println_P(PSTR("initializing up range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(5); + rangeFinder->set_orientation(0, 0, -1); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderDownEnabled) { + hal->debug->println_P(PSTR("initializing down range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(6); + rangeFinder->set_orientation(0, 0, 1); + hal->rangeFinders.push_back(rangeFinder); + } + + } + + /* + * Select guidance, navigation, control algorithms + */ + AP_Navigator * navigator = new NAVIGATOR_CLASS(hal); + AP_Guide * guide = new GUIDE_CLASS(navigator, hal); + AP_Controller * controller = new CONTROLLER_CLASS(navigator, guide, hal); + + /* + * CommLinks + */ + if (board==BOARD_ARDUPILOTMEGA_2) hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal); + else hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal); + + hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal); + + /* + * Start the autopilot + */ + hal->debug->printf_P(PSTR("initializing arduplane\n")); + hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); + autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal, + loop0Rate, loop1Rate, loop2Rate, loop3Rate); +} + +void loop() { + autoPilot->update(); +} diff --git a/libraries/APO/APO.cpp b/libraries/APO/APO.cpp new file mode 100644 index 0000000000..2592d1140c --- /dev/null +++ b/libraries/APO/APO.cpp @@ -0,0 +1,8 @@ +/* + * APO.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "APO.h" diff --git a/libraries/APO/APO.h b/libraries/APO/APO.h new file mode 100644 index 0000000000..0e0efafbd7 --- /dev/null +++ b/libraries/APO/APO.h @@ -0,0 +1,20 @@ +/* + * APO.h + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#ifndef APO_H_ +#define APO_H_ + +#include "AP_Autopilot.h" +#include "AP_Guide.h" +#include "AP_Controller.h" +#include "AP_HardwareAbstractionLayer.h" +#include "AP_MavlinkCommand.h" +#include "AP_Navigator.h" +#include "AP_RcChannel.h" +//#include "AP_Var_keys.h" + +#endif /* APO_H_ */ diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp new file mode 100644 index 0000000000..96dab80e12 --- /dev/null +++ b/libraries/APO/AP_Autopilot.cpp @@ -0,0 +1,246 @@ +/* + * AP_Autopilot.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_Autopilot.h" + +namespace apo { + +class AP_HardwareAbstractionLayer; + +AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, + AP_Controller * controller, AP_HardwareAbstractionLayer * hal, + float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate) : + Loop(loop0Rate, callback0, this), _navigator(navigator), _guide(guide), + _controller(controller), _hal(hal), _loop0Rate(loop0Rate), + _loop1Rate(loop1Rate), _loop2Rate(loop2Rate), _loop3Rate(loop3Rate), + _loop4Rate(loop3Rate) { + + hal->setState(MAV_STATE_BOOT); + hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + + /* + * Calibration + */ + hal->setState(MAV_STATE_CALIBRATING); + hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + navigator->calibrate(); + + // start clock + //uint32_t timeStart = millis(); + //uint16_t gpsWaitTime = 5000; // 5 second wait for gps + + /* + * Look for valid initial state + */ + while (1) { + // letc gcs known we are alive + hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + hal->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + delay(1000); + if (hal->getMode() == MODE_LIVE) { + _navigator->updateSlow(0); + if (_hal->gps) { + if (hal->gps->fix) { + break; + } else { + hal->gcs->sendText(SEVERITY_LOW, + PSTR("waiting for gps lock\n")); + hal->debug->printf_P(PSTR("waiting for gps lock\n")); + } + } else { // no gps, can skip + break; + } + } else if (hal->getMode() == MODE_HIL_CNTL) { // hil + _hal->hil->receive(); + Serial.println("HIL Recieve Called"); + if (_navigator->getTimeStamp() != 0) { + // give hil a chance to send some packets + for (int i = 0; i < 5; i++) { + hal->debug->println_P(PSTR("reading initial hil packets")); + hal->gcs->sendText(SEVERITY_LOW, + PSTR("reading initial hil packets")); + delay(1000); + } + break; + } + hal->debug->println_P(PSTR("waiting for hil packet")); + } + } + + AP_MavlinkCommand::home.setAlt(_navigator->getAlt()); + AP_MavlinkCommand::home.setLat(_navigator->getLat()); + AP_MavlinkCommand::home.setLon(_navigator->getLon()); + AP_MavlinkCommand::home.save(); + _hal->debug->printf_P(PSTR("\nhome before load lat: %f deg, lon: %f deg\n"), + AP_MavlinkCommand::home.getLat()*rad2Deg, + AP_MavlinkCommand::home.getLon()*rad2Deg); + AP_MavlinkCommand::home.load(); + _hal->debug->printf_P(PSTR("home after load lat: %f deg, lon: %f deg\n"), + AP_MavlinkCommand::home.getLat()*rad2Deg, + AP_MavlinkCommand::home.getLon()*rad2Deg); + + /* + * Attach loops + */ + hal->debug->println_P(PSTR("attaching loops")); + subLoops().push_back(new Loop(getLoopRate(1), callback1, this)); + subLoops().push_back(new Loop(getLoopRate(2), callback2, this)); + subLoops().push_back(new Loop(getLoopRate(3), callback3, this)); + subLoops().push_back(new Loop(getLoopRate(4), callback4, this)); + + hal->debug->println_P(PSTR("running")); + hal->gcs->sendText(SEVERITY_LOW, PSTR("running")); + + if (hal->getMode() == MODE_LIVE) { + hal->setState(MAV_STATE_ACTIVE); + } else { + hal->setState(MAV_STATE_HILSIM); + } + + /* + * Radio setup + */ + hal->debug->println_P(PSTR("initializing radio")); + APM_RC.Init(); // APM Radio initialization, + // start this after control loop is running +} + +void AP_Autopilot::callback0(void * data) { + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->hal()->debug->println_P(PSTR("callback 0")); + + /* + * ahrs update + */ + if (apo->getNavigator()) + apo->getNavigator()->updateFast(1.0 / apo->getLoopRate(0)); +} + +void AP_Autopilot::callback1(void * data) { + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->getHal()->debug->println_P(PSTR("callback 1")); + + /* + * hardware in the loop + */ + if (apo->getHal()->hil && apo->getHal()->getMode() != MODE_LIVE) { + apo->getHal()->hil->receive(); + apo->getHal()->hil->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); + } + + /* + * update guidance laws + */ + if (apo->getGuide()) + { + //apo->getHal()->debug->println_P(PSTR("updating guide")); + apo->getGuide()->update(); + } + + /* + * update control laws + */ + if (apo->getController()) { + //apo->getHal()->debug->println_P(PSTR("updating controller")); + apo->getController()->update(1. / apo->getLoopRate(1)); + } + /* + char msg[50]; + sprintf(msg, "c_hdg: %f, c_thr: %f", apo->guide()->headingCommand, apo->guide()->groundSpeedCommand); + apo->hal()->gcs->sendText(AP_CommLink::SEVERITY_LOW, msg); + */ +} + +void AP_Autopilot::callback2(void * data) { + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->getHal()->debug->println_P(PSTR("callback 2")); + + /* + * 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 + */ + if (apo->getNavigator()) { + apo->getNavigator()->updateSlow(1.0 / apo->getLoopRate(2)); + } + + /* + * handle ground control station communication + */ + if (apo->getHal()->gcs) { + // send messages + apo->getHal()->gcs->requestCmds(); + apo->getHal()->gcs->sendParameters(); + + // receive messages + apo->getHal()->gcs->receive(); + } + + /* + * navigator debug + */ + /* + if (apo->navigator()) { + apo->getHal()->debug->printf_P(PSTR("roll: %f deg\tpitch: %f deg\tyaw: %f deg\n"), + apo->navigator()->getRoll()*rad2Deg, + apo->navigator()->getPitch()*rad2Deg, + apo->navigator()->getYaw()*rad2Deg); + apo->getHal()->debug->printf_P(PSTR("lat: %f deg\tlon: %f deg\talt: %f m\n"), + apo->navigator()->getLat()*rad2Deg, + apo->navigator()->getLon()*rad2Deg, + apo->navigator()->getAlt()); + } + */ +} + +void AP_Autopilot::callback3(void * data) { + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->getHal()->debug->println_P(PSTR("callback 3")); + + /* + * send heartbeat + */ + apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + + /* + * load/loop rate/ram debug + */ + apo->getHal()->load = apo->load(); + apo->getHal()->debug->printf_P(PSTR("load: %d%%\trate: %f Hz\tfree ram: %d bytes\n"), + apo->load(),1.0/apo->dt(),freeMemory()); + + apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + + /* + * adc debug + */ + //apo->getDebug().printf_P(PSTR("adc: %d %d %d %d %d %d %d %d\n"), + //apo->adc()->Ch(0), apo->adc()->Ch(1), apo->adc()->Ch(2), + //apo->adc()->Ch(3), apo->adc()->Ch(4), apo->adc()->Ch(5), + //apo->adc()->Ch(6), apo->adc()->Ch(7), apo->adc()->Ch(8)); +} + +void AP_Autopilot::callback4(void * data) { + //AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->getHal()->debug->println_P(PSTR("callback 4")); +} + +} // apo diff --git a/libraries/APO/AP_Autopilot.h b/libraries/APO/AP_Autopilot.h new file mode 100644 index 0000000000..ceb7be2b76 --- /dev/null +++ b/libraries/APO/AP_Autopilot.h @@ -0,0 +1,165 @@ +/* + * AP_Autopilot.h + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#ifndef AP_AUTOPILOT_H_ +#define AP_AUTOPILOT_H_ + +/* + * AVR runtime + */ +#include +#include +#include +#include +/* + * Libraries + */ +#include "../AP_Common/AP_Common.h" +#include "../FastSerial/FastSerial.h" +#include "../AP_GPS/GPS.h" +#include "../APM_RC/APM_RC.h" +#include "../AP_ADC/AP_ADC.h" +#include "../APM_BMP085/APM_BMP085.h" +#include "../AP_Compass/AP_Compass.h" +#include "../AP_Math/AP_Math.h" +#include "../AP_IMU/AP_IMU.h" +#include "../AP_DCM/AP_DCM.h" +#include "../AP_Common/AP_Loop.h" +#include "../GCS_MAVLink/GCS_MAVLink.h" +#include "../AP_RangeFinder/AP_RangeFinder.h" +/* + * Local Modules + */ +#include "AP_HardwareAbstractionLayer.h" +#include "AP_RcChannel.h" +#include "AP_Controller.h" +#include "AP_Navigator.h" +#include "AP_Guide.h" +#include "AP_CommLink.h" + +/** + * ArduPilotOne namespace to protect varibles + * from overlap with avr and libraries etc. + * ArduPilotOne does not use any global + * variables. + */ +namespace apo { + +// forward declarations +class AP_CommLink; + +/** + * This class encapsulates the entire autopilot system + * The constructor takes guide, navigator, and controller + * as well as the hardware abstraction layer. + * + * It inherits from loop to manage + * the subloops and sets the overall + * frequency for the autopilot. + * + + */ +class AP_Autopilot: public Loop { +public: + /** + * Default constructor + */ + AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, + AP_Controller * controller, AP_HardwareAbstractionLayer * hal, + float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate); + + /** + * Accessors + */ + AP_Navigator * getNavigator() { + return _navigator; + } + AP_Guide * getGuide() { + return _guide; + } + AP_Controller * getController() { + return _controller; + } + AP_HardwareAbstractionLayer * getHal() { + return _hal; + } + + float getLoopRate(uint8_t i) { + switch(i) { + case 0: return _loop0Rate; + case 1: return _loop1Rate; + case 2: return _loop2Rate; + case 3: return _loop3Rate; + case 4: return _loop4Rate; + default: return 0; + } + } + +private: + + /** + * Loop 0 Callbacks (fastest) + * - inertial navigation + * @param data A void pointer used to pass the apo class + * so that the apo public interface may be accessed. + */ + static void callback0(void * data); + float _loop0Rate; + + /** + * Loop 1 Callbacks + * - control + * - compass reading + * @see callback0 + */ + static void callback1(void * data); + float _loop1Rate; + + /** + * Loop 2 Callbacks + * - gps sensor fusion + * - compass sensor fusion + * @see callback0 + */ + static void callback2(void * data); + float _loop2Rate; + + /** + * Loop 3 Callbacks + * - slow messages + * @see callback0 + */ + static void callback3(void * data); + float _loop3Rate; + + /** + * Loop 4 Callbacks + * - super slow mesages + * - log writing + * @see callback0 + */ + static void callback4(void * data); + float _loop4Rate; + + /** + * Components + */ + AP_Navigator * _navigator; + AP_Guide * _guide; + AP_Controller * _controller; + AP_HardwareAbstractionLayer * _hal; + + /** + * Constants + */ + static const float deg2rad = M_PI / 180; + static const float rad2deg = 180 / M_PI; +}; + +} // namespace apo + +#endif /* AP_AUTOPILOT_H_ */ diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp new file mode 100644 index 0000000000..92b9d0fdde --- /dev/null +++ b/libraries/APO/AP_CommLink.cpp @@ -0,0 +1,15 @@ +/* + * AP_CommLink.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_CommLink.h" + +namespace apo { + +uint8_t MavlinkComm::_nChannels = 0; +uint8_t MavlinkComm::_paramNameLengthMax = 13; + +} // apo diff --git a/libraries/APO/AP_CommLink.h b/libraries/APO/AP_CommLink.h new file mode 100644 index 0000000000..08d20be57f --- /dev/null +++ b/libraries/APO/AP_CommLink.h @@ -0,0 +1,790 @@ +/* + * AP_CommLink.h + * Copyright (C) James Goppert 2010 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#ifndef AP_CommLink_H +#define AP_CommLink_H + +#include "AP_HardwareAbstractionLayer.h" +#include "AP_MavlinkCommand.h" +#include "AP_Controller.h" + +namespace apo { + +class AP_Controller; +class AP_Navigator; +class AP_Guide; +class AP_HardwareAbstractionLayer; + +enum { + SEVERITY_LOW, SEVERITY_MED, SEVERITY_HIGH +}; + +// forward declarations +//class ArduPilotOne; +//class AP_Controller; + +/// CommLink class +class AP_CommLink { +public: + + AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide, + AP_Controller * controller, AP_HardwareAbstractionLayer * hal) : + _link(link), _navigator(navigator), _guide(guide), + _controller(controller), _hal(hal) { + } + virtual void send() = 0; + virtual void receive() = 0; + virtual void sendMessage(uint8_t id, uint32_t param = 0) = 0; + virtual void sendText(uint8_t severity, const char *str) = 0; + virtual void sendText(uint8_t severity, const prog_char_t *str) = 0; + virtual void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) = 0; + virtual void sendParameters() = 0; + virtual void requestCmds() = 0; + +protected: + FastSerial * _link; + AP_Navigator * _navigator; + AP_Guide * _guide; + AP_Controller * _controller; + AP_HardwareAbstractionLayer * _hal; +}; + +class MavlinkComm: public AP_CommLink { +public: + MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide, + AP_Controller * controller, AP_HardwareAbstractionLayer * hal) : + AP_CommLink(link, nav, guide, controller, hal), + + // options + _useRelativeAlt(true), + + // commands + _sendingCmds(false), _receivingCmds(false), + _cmdTimeLastSent(millis()), _cmdTimeLastReceived(millis()), + _cmdDestSysId(0), _cmdDestCompId(0), _cmdRequestIndex(0), + _cmdMax(30), _cmdNumberRequested(0), + + // parameters + _parameterCount(0), _queuedParameter(NULL), + _queuedParameterIndex(0) { + + switch (_nChannels) { + case 0: + mavlink_comm_0_port = link; + _channel = MAVLINK_COMM_0; + _nChannels++; + break; + case 1: + mavlink_comm_1_port = link; + _channel = MAVLINK_COMM_1; + _nChannels++; + break; + default: + // signal that number of channels exceeded + _channel = MAVLINK_COMM_3; + break; + } + } + + virtual void send() { + // if number of channels exceeded return + if (_channel == MAVLINK_COMM_3) + return; + } + + void sendMessage(uint8_t id, uint32_t param = 0) { + //_hal->debug->printf_P(PSTR("send message\n")); + + // if number of channels exceeded return + if (_channel == MAVLINK_COMM_3) + return; + + uint64_t timeStamp = micros(); + + switch (id) { + + case MAVLINK_MSG_ID_HEARTBEAT: { + mavlink_msg_heartbeat_send(_channel, mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); + break; + } + + case MAVLINK_MSG_ID_ATTITUDE: { + mavlink_msg_attitude_send(_channel, timeStamp, + _navigator->getRoll(), _navigator->getPitch(), + _navigator->getYaw(), _navigator->getRollRate(), + _navigator->getPitchRate(), _navigator->getYawRate()); + break; + } + + case MAVLINK_MSG_ID_GLOBAL_POSITION: { + mavlink_msg_global_position_send(_channel, timeStamp, + _navigator->getLat() * rad2Deg, + _navigator->getLon() * rad2Deg, _navigator->getAlt(), + _navigator->getVN(), _navigator->getVE(), + _navigator->getVD()); + break; + } + + case MAVLINK_MSG_ID_GPS_RAW: { + mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(), + _navigator->getLat() * rad2Deg, + _navigator->getLon() * rad2Deg, _navigator->getAlt(), 0, 0, + _navigator->getGroundSpeed(), + _navigator->getYaw() * rad2Deg); + break; + } + + /* + case MAVLINK_MSG_ID_GPS_RAW_INT: { + mavlink_msg_gps_raw_int_send(_channel,timeStamp,_hal->gps->status(), + _navigator->getLat_degInt(), _navigator->getLon_degInt(),_navigator->getAlt_intM(), 0,0, + _navigator->getGroundSpeed(),_navigator->getYaw()*rad2Deg); + break; + } + */ + + 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 + } + + case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: { + int16_t ch[8]; + for (int i = 0; i < 8; i++) + ch[i] = 0; + for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) { + ch[i] = 10000 * _hal->rc[i]->getPosition(); + //_hal->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]); + } + mavlink_msg_rc_channels_scaled_send(_channel, ch[0], ch[1], ch[2], + ch[3], ch[4], ch[5], ch[6], ch[7], 255); + break; + } + + case MAVLINK_MSG_ID_RC_CHANNELS_RAW: { + int16_t ch[8]; + for (int i = 0; i < 8; i++) + ch[i] = 0; + for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) + ch[i] = _hal->rc[i]->getPwm(); + mavlink_msg_rc_channels_raw_send(_channel, ch[0], ch[1], ch[2], + ch[3], ch[4], ch[5], ch[6], ch[7], 255); + break; + } + + case MAVLINK_MSG_ID_SYS_STATUS: { + + float batteryVoltage, temp; + temp = analogRead(0); + batteryVoltage = ((temp * 5 / 1023) / 0.28); + + mavlink_msg_sys_status_send(_channel, _controller->getMode(), + _guide->getMode(), _hal->getState(), _hal->load * 10, + batteryVoltage * 1000, + (batteryVoltage - 3.3) / (4.2 - 3.3) * 1000, _packetDrops); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: { + sendText(SEVERITY_LOW, PSTR("waypoint ack")); + //mavlink_waypoint_ack_t packet; + uint8_t type = 0; // ok (0), error(1) + mavlink_msg_waypoint_ack_send(_channel, _cmdDestSysId, + _cmdDestCompId, type); + + // turn off waypoint send + _receivingCmds = false; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CURRENT: { + mavlink_msg_waypoint_current_send(_channel, + _guide->getCurrentIndex()); + break; + } + + default: { + char msg[50]; + sprintf(msg, "autopilot sending unknown command with id: %d", id); + sendText(SEVERITY_HIGH, msg); + } + + } // switch + } // send message + + virtual void receive() { + //_hal->debug->printf_P(PSTR("receive\n")); + // if number of channels exceeded return + // + if (_channel == MAVLINK_COMM_3) + return; + + // receive new packets + mavlink_message_t msg; + mavlink_status_t status; + + // process received bytes + while (comm_get_available(_channel)) { + uint8_t c = comm_receive_ch(_channel); + + // Try to get a new message + if (mavlink_parse_char(_channel, c, &msg, &status)) + _handleMessage(&msg); + } + + // Update packet drops counter + _packetDrops += status.packet_rx_drop_count; + } + + void sendText(uint8_t severity, const char *str) { + mavlink_msg_statustext_send(_channel, severity, (const int8_t*) str); + } + + void sendText(uint8_t severity, const prog_char_t *str) { + mavlink_statustext_t m; + uint8_t i; + for (i = 0; i < sizeof(m.text); i++) { + m.text[i] = pgm_read_byte((const prog_char *) (str++)); + } + if (i < sizeof(m.text)) + m.text[i] = 0; + sendText(severity, (const char *) m.text); + } + + void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) { + } + + /** + * sends parameters one at a time + */ + void sendParameters() { + //_hal->debug->printf_P(PSTR("send parameters\n")); + // Check to see if we are sending parameters + while (NULL != _queuedParameter) { + AP_Var *vp; + float value; + + // copy the current parameter and prepare to move to the next + vp = _queuedParameter; + _queuedParameter = _queuedParameter->next(); + + // if the parameter can be cast to float, report it here and break out of the loop + value = vp->cast_to_float(); + if (!isnan(value)) { + + char paramName[_paramNameLengthMax]; + vp->copy_name(paramName, sizeof(paramName)); + + mavlink_msg_param_value_send(_channel, (int8_t*) paramName, + value, _countParameters(), _queuedParameterIndex); + + _queuedParameterIndex++; + break; + } + } + + } + + /** + * request commands one at a time + */ + void requestCmds() { + //_hal->debug->printf_P(PSTR("requesting commands\n")); + // request cmds one by one + if (_receivingCmds && _cmdRequestIndex <= _cmdNumberRequested) { + mavlink_msg_waypoint_request_send(_channel, _cmdDestSysId, + _cmdDestCompId, _cmdRequestIndex); + } + } + +private: + + // options + bool _useRelativeAlt; + + // commands + bool _sendingCmds; + bool _receivingCmds; + uint16_t _cmdTimeLastSent; + uint16_t _cmdTimeLastReceived; + uint16_t _cmdDestSysId; + uint16_t _cmdDestCompId; + uint16_t _cmdRequestIndex; + uint16_t _cmdNumberRequested; + uint16_t _cmdMax; + Vector _cmdList; + + // parameters + static uint8_t _paramNameLengthMax; + uint16_t _parameterCount; + AP_Var * _queuedParameter; + uint16_t _queuedParameterIndex; + + // channel + mavlink_channel_t _channel; + uint16_t _packetDrops; + static uint8_t _nChannels; + + void _handleMessage(mavlink_message_t * msg) { + + uint32_t timeStamp = micros(); + + switch (msg->msgid) { + //_hal->debug->printf_P(PSTR("message received: %d"), msg->msgid); + + case MAVLINK_MSG_ID_HEARTBEAT: { + mavlink_heartbeat_t packet; + mavlink_msg_heartbeat_decode(msg, &packet); + _hal->lastHeartBeat = micros(); + break; + } + + case MAVLINK_MSG_ID_GPS_RAW: { + // decode + mavlink_gps_raw_t packet; + mavlink_msg_gps_raw_decode(msg, &packet); + + _navigator->setTimeStamp(timeStamp); + _navigator->setLat(packet.lat * deg2Rad); + _navigator->setLon(packet.lon * deg2Rad); + _navigator->setAlt(packet.alt); + _navigator->setYaw(packet.hdg * deg2Rad); + _navigator->setGroundSpeed(packet.v); + _navigator->setAirSpeed(packet.v); + //_hal->debug->printf_P(PSTR("received hil gps raw packet\n")); + /* + _hal->debug->printf_P(PSTR("received lat: %f deg\tlon: %f deg\talt: %f m\n"), + packet.lat, + packet.lon, + packet.alt); + */ + break; + } + + case MAVLINK_MSG_ID_ATTITUDE: { + // decode + mavlink_attitude_t packet; + mavlink_msg_attitude_decode(msg, &packet); + + // set dcm hil sensor + _navigator->setTimeStamp(timeStamp); + _navigator->setRoll(packet.roll); + _navigator->setPitch(packet.pitch); + _navigator->setYaw(packet.yaw); + _navigator->setRollRate(packet.rollspeed); + _navigator->setPitchRate(packet.pitchspeed); + _navigator->setYawRate(packet.yawspeed); + //_hal->debug->printf_P(PSTR("received hil attitude packet\n")); + break; + } + + case MAVLINK_MSG_ID_ACTION: { + // decode + mavlink_action_t packet; + mavlink_msg_action_decode(msg, &packet); + if (_checkTarget(packet.target, packet.target_component)) + break; + + // do action + sendText(SEVERITY_LOW, PSTR("action received")); + switch (packet.action) { + + case MAV_ACTION_STORAGE_READ: + AP_Var::load_all(); + break; + + case MAV_ACTION_STORAGE_WRITE: + AP_Var::save_all(); + break; + + case MAV_ACTION_CALIBRATE_RC: + case MAV_ACTION_CALIBRATE_GYRO: + case MAV_ACTION_CALIBRATE_MAG: + case MAV_ACTION_CALIBRATE_ACC: + case MAV_ACTION_CALIBRATE_PRESSURE: + 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: + sendText(SEVERITY_LOW, PSTR("unknown action")); + break; + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: { + sendText(SEVERITY_LOW, PSTR("waypoint request list")); + + // decode + mavlink_waypoint_request_list_t packet; + mavlink_msg_waypoint_request_list_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // Start sending waypoints + mavlink_msg_waypoint_count_send(_channel, msg->sysid, msg->compid, + _guide->getNumberOfCommands()); + + _cmdTimeLastSent = millis(); + _cmdTimeLastReceived = millis(); + _sendingCmds = true; + _receivingCmds = false; + _cmdDestSysId = msg->sysid; + _cmdDestCompId = msg->compid; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: { + sendText(SEVERITY_LOW, PSTR("waypoint request")); + + // Check if sending waypiont + if (!_sendingCmds) + break; + + // decode + mavlink_waypoint_request_t packet; + mavlink_msg_waypoint_request_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + _hal->debug->printf_P(PSTR("sequence: %d\n"),packet.seq); + AP_MavlinkCommand cmd(packet.seq); + cmd.load(); + + mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex()); + mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId, + wp.seq, wp.frame, wp.command, wp.current, wp.autocontinue, + wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, + wp.z); + + // update last waypoint comm stamp + _cmdTimeLastSent = millis(); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: { + sendText(SEVERITY_LOW, PSTR("waypoint ack")); + + // decode + mavlink_waypoint_ack_t packet; + mavlink_msg_waypoint_ack_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // check for error + //uint8_t type = packet.type; // ok (0), error(1) + + // turn off waypoint send + _sendingCmds = false; + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + sendText(SEVERITY_LOW, PSTR("param request list")); + + // decode + mavlink_param_request_list_t packet; + mavlink_msg_param_request_list_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // Start sending parameters - next call to ::update will kick the first one out + + _queuedParameter = AP_Var::first(); + _queuedParameterIndex = 0; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: { + sendText(SEVERITY_LOW, PSTR("waypoint clear all")); + + // decode + mavlink_waypoint_clear_all_t packet; + mavlink_msg_waypoint_clear_all_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // clear all waypoints + uint8_t type = 0; // ok (0), error(1) + _guide->setNumberOfCommands(1); + _guide->setCurrentIndex(0); + + // send acknowledgement 3 times to makes sure it is received + for (int i = 0; i < 3; i++) + mavlink_msg_waypoint_ack_send(_channel, msg->sysid, + msg->compid, type); + + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: { + sendText(SEVERITY_LOW, PSTR("waypoint set current")); + + // decode + mavlink_waypoint_set_current_t packet; + mavlink_msg_waypoint_set_current_decode(msg, &packet); + Serial.print("Packet Sequence:"); + Serial.println(packet.seq); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // set current waypoint + Serial.print("Current Index:"); + Serial.println(_guide->getCurrentIndex()); + Serial.flush(); + _guide->setCurrentIndex(packet.seq); + mavlink_msg_waypoint_current_send(_channel, + _guide->getCurrentIndex()); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_COUNT: { + sendText(SEVERITY_LOW, PSTR("waypoint count")); + + // decode + mavlink_waypoint_count_t packet; + mavlink_msg_waypoint_count_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // start waypoint receiving + if (packet.count > _cmdMax) { + packet.count = _cmdMax; + } + _cmdNumberRequested = packet.count; + _cmdTimeLastReceived = millis(); + _receivingCmds = true; + _sendingCmds = false; + _cmdRequestIndex = 0; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT: { + sendText(SEVERITY_LOW, PSTR("waypoint")); + + // Check if receiving waypiont + if (!_receivingCmds) { + //sendText(SEVERITY_HIGH, PSTR("not receiving commands")); + break; + } + + // decode + mavlink_waypoint_t packet; + mavlink_msg_waypoint_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // check if this is the requested waypoint + if (packet.seq != _cmdRequestIndex) { + char warningMsg[50]; + sprintf(warningMsg, + "waypoint request out of sequence: (packet) %d / %d (ap)", + packet.seq, _cmdRequestIndex); + sendText(SEVERITY_HIGH, warningMsg); + break; + } + + _hal->debug->printf_P(PSTR("received waypoint x: %f\ty: %f\tz: %f\n"), + packet.x, + packet.y, + packet.z); + + // store waypoint + AP_MavlinkCommand command(packet); + //sendText(SEVERITY_HIGH, PSTR("waypoint stored")); + _cmdRequestIndex++; + if (_cmdRequestIndex == _cmdNumberRequested) { + sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK); + _receivingCmds = false; + _guide->setNumberOfCommands(_cmdNumberRequested); + //sendText(SEVERITY_LOW, PSTR("waypoint ack sent")); + } else if (_cmdRequestIndex > _cmdNumberRequested) { + _receivingCmds = false; + } + _cmdTimeLastReceived = millis(); + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: { + sendText(SEVERITY_LOW, PSTR("param set")); + AP_Var *vp; + AP_Meta_class::Type_id var_type; + + // decode + mavlink_param_set_t packet; + mavlink_msg_param_set_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // set parameter + + char key[_paramNameLengthMax + 1]; + strncpy(key, (char *) packet.param_id, _paramNameLengthMax); + key[_paramNameLengthMax] = 0; + + // find the requested parameter + vp = AP_Var::find(key); + if ((NULL != vp) && // exists + !isnan(packet.param_value) && // not nan + !isinf(packet.param_value)) { // not inf + + // add a small amount before casting parameter values + // from float to integer to avoid truncating to the + // next lower integer value. + const float rounding_addition = 0.01; + + // fetch the variable type ID + var_type = vp->meta_type_id(); + + // handle variables with standard type IDs + if (var_type == AP_Var::k_typeid_float) { + ((AP_Float *) vp)->set_and_save(packet.param_value); + + } else if (var_type == AP_Var::k_typeid_float16) { + ((AP_Float16 *) vp)->set_and_save(packet.param_value); + + } else if (var_type == AP_Var::k_typeid_int32) { + ((AP_Int32 *) vp)->set_and_save( + packet.param_value + rounding_addition); + + } else if (var_type == AP_Var::k_typeid_int16) { + ((AP_Int16 *) vp)->set_and_save( + packet.param_value + rounding_addition); + + } else if (var_type == AP_Var::k_typeid_int8) { + ((AP_Int8 *) vp)->set_and_save( + packet.param_value + rounding_addition); + } else { + // we don't support mavlink set on this parameter + break; + } + + // Report back the new value if we accepted the change + // we send the value we actually set, which could be + // different from the value sent, in case someone sent + // a fractional value to an integer type + mavlink_msg_param_value_send(_channel, (int8_t *) key, + vp->cast_to_float(), _countParameters(), -1); // XXX we don't actually know what its index is... + } + + break; + } // end case + + + } + } + + uint16_t _countParameters() { + // if we haven't cached the parameter count yet... + if (0 == _parameterCount) { + AP_Var *vp; + + vp = AP_Var::first(); + do { + // if a parameter responds to cast_to_float then we are going to be able to report it + if (!isnan(vp->cast_to_float())) { + _parameterCount++; + } + } while (NULL != (vp = vp->next())); + } + return _parameterCount; + } + + AP_Var * _findParameter(uint16_t index) { + AP_Var *vp; + + vp = AP_Var::first(); + while (NULL != vp) { + + // if the parameter is reportable + if (!(isnan(vp->cast_to_float()))) { + // if we have counted down to the index we want + if (0 == index) { + // return the parameter + return vp; + } + // count off this parameter, as it is reportable but not + // the one we want + index--; + } + // and move to the next parameter + vp = vp->next(); + } + return NULL; + } + + // check the target + uint8_t _checkTarget(uint8_t sysid, uint8_t compid) { + /* + char msg[50]; + sprintf(msg, "target = %d / %d\tcomp = %d / %d", sysid, + mavlink_system.sysid, compid, mavlink_system.compid); + sendText(SEVERITY_LOW, msg); + */ + if (sysid != mavlink_system.sysid) { + //sendText(SEVERITY_LOW, PSTR("system id mismatch")); + return 1; + + } else if (compid != mavlink_system.compid) { + //sendText(SEVERITY_LOW, PSTR("component id mismatch")); + return 0; // XXX currently not receiving correct compid from gcs + + } else { + return 0; // no error + } + } + +}; + +} // namespace apo + +#endif // AP_CommLink_H +// vim:ts=4:sw=4:tw=78:expandtab diff --git a/libraries/APO/AP_Controller.cpp b/libraries/APO/AP_Controller.cpp new file mode 100644 index 0000000000..ebbd5ed524 --- /dev/null +++ b/libraries/APO/AP_Controller.cpp @@ -0,0 +1,8 @@ +/* + * AP_Controller.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_Controller.h" diff --git a/libraries/APO/AP_Controller.h b/libraries/APO/AP_Controller.h new file mode 100644 index 0000000000..d7dac0c60c --- /dev/null +++ b/libraries/APO/AP_Controller.h @@ -0,0 +1,304 @@ +/* + * AP_Controller.h + * Copyright (C) James Goppert 2010 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#ifndef AP_Controller_H +#define AP_Controller_H + +#include "AP_Navigator.h" +#include "AP_Guide.h" +#include "AP_HardwareAbstractionLayer.h" +#include "../AP_Common/AP_Vector.h" +#include "../AP_Common/AP_Var.h" + +namespace apo { + +/// Controller class +class AP_Controller { +public: + AP_Controller(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + _nav(nav), _guide(guide), _hal(hal) { + } + + virtual void update(const float & dt) = 0; + + virtual MAV_MODE getMode() = 0; + + void setAllRadioChannelsToNeutral() { + for (uint8_t i = 0; i < _hal->rc.getSize(); i++) { + _hal->rc[i]->setPosition(0.0); + } + } + + void setAllRadioChannelsManually() { + for (uint8_t i = 0; i < _hal->rc.getSize(); i++) { + _hal->rc[i]->setUsingRadio(); + } + } + +protected: + AP_Navigator * _nav; + AP_Guide * _guide; + AP_HardwareAbstractionLayer * _hal; +}; + +class AP_ControllerBlock { +public: + AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart, + uint8_t groupLength) : + _group(group), _groupStart(groupStart), + _groupEnd(groupStart + groupLength) { + } + uint8_t getGroupEnd() { + return _groupEnd; + } +protected: + AP_Var_group * _group; /// helps with parameter management + uint8_t _groupStart; + uint8_t _groupEnd; +}; + +class BlockLowPass: public AP_ControllerBlock { +public: + BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut, + const prog_char_t * fCutLabel = NULL) : + AP_ControllerBlock(group, groupStart, 1), + _fCut(group, groupStart, fCut, fCutLabel ? : PSTR("fCut")), + _y(0) { + } + float update(const float & input, const float & dt) { + float RC = 1 / (2 * M_PI * _fCut); // low pass filter + _y = _y + (input - _y) * (dt / (dt + RC)); + return _y; + } +protected: + AP_Float _fCut; + float _y; +}; + +class BlockSaturation: public AP_ControllerBlock { +public: + BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax, + const prog_char_t * yMaxLabel = NULL) : + AP_ControllerBlock(group, groupStart, 1), + _yMax(group, groupStart, yMax, yMaxLabel ? : PSTR("yMax")) { + } + float update(const float & input) { + + // pid sum + float y = input; + + // saturation + if (y > _yMax) + y = _yMax; + if (y < -_yMax) + y = -_yMax; + return y; + } +protected: + AP_Float _yMax; /// output saturation +}; + +class BlockDerivative { +public: + BlockDerivative() : + _lastInput(0), firstRun(true) { + } + float update(const float & input, const float & dt) { + float derivative = (input - _lastInput) / dt; + _lastInput = input; + if (firstRun) { + firstRun = false; + return 0; + } else + return derivative; + } +protected: + float _lastInput; /// last input + bool firstRun; +}; + +class BlockIntegral { +public: + BlockIntegral() : + _i(0) { + } + float update(const float & input, const float & dt) { + _i += input * dt; + return _i; + } +protected: + float _i; /// integral +}; + +class BlockP: public AP_ControllerBlock { +public: + BlockP(AP_Var_group * group, uint8_t groupStart, float kP, + const prog_char_t * kPLabel = NULL) : + AP_ControllerBlock(group, groupStart, 1), + _kP(group, groupStart, kP, kPLabel ? : PSTR("p")) { + } + + float update(const float & input) { + return _kP * input; + } +protected: + AP_Float _kP; /// proportional gain +}; + +class BlockI: public AP_ControllerBlock { +public: + BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax, + const prog_char_t * kILabel = NULL, + const prog_char_t * iMaxLabel = NULL) : + AP_ControllerBlock(group, groupStart, 2), + _kI(group, groupStart, kI, kILabel ? : PSTR("i")), + _blockSaturation(group, groupStart + 1, iMax, iMaxLabel ? : PSTR("iMax")), + _eI(0) { + } + + float update(const float & input, const float & dt) { + // integral + _eI += input * dt; + _eI = _blockSaturation.update(_eI); + return _kI * _eI; + } +protected: + AP_Float _kI; /// integral gain + BlockSaturation _blockSaturation; /// integrator saturation + float _eI; /// integral of input +}; + +class BlockD: public AP_ControllerBlock { +public: + BlockD(AP_Var_group * group, uint8_t groupStart, float kD, uint8_t dFCut, + const prog_char_t * kDLabel = NULL, + const prog_char_t * dFCutLabel = NULL) : + AP_ControllerBlock(group, groupStart, 2), + _blockLowPass(group, groupStart, dFCut, + dFCutLabel ? : PSTR("dFCut")), + _kD(group, _blockLowPass.getGroupEnd(), kD, + kDLabel ? : PSTR("d")), _eP(0) { + } + float update(const float & input, const float & dt) { + // derivative with low pass + float _eD = _blockLowPass.update((_eP - input) / dt, dt); + // proportional, note must come after derivative + // because derivatve uses _eP as previous value + _eP = input; + return _kD * _eD; + } +protected: + BlockLowPass _blockLowPass; + AP_Float _kD; /// derivative gain + float _eP; /// input +}; + +class BlockPID: public AP_ControllerBlock { +public: + BlockPID(AP_Var_group * group, uint8_t groupStart, float kP, float kI, + float kD, float iMax, float yMax, uint8_t dFcut) : + AP_ControllerBlock(group, groupStart, 6), + _blockP(group, groupStart, kP), + _blockI(group, _blockP.getGroupEnd(), kI, iMax), + _blockD(group, _blockI.getGroupEnd(), kD, dFcut), + _blockSaturation(group, _blockD.getGroupEnd(), yMax) { + } + + float update(const float & input, const float & dt) { + return _blockSaturation.update( + _blockP.update(input) + _blockI.update(input, dt) + + _blockD.update(input, dt)); + } +protected: + BlockP _blockP; + BlockI _blockI; + BlockD _blockD; + BlockSaturation _blockSaturation; +}; + +class BlockPI: public AP_ControllerBlock { +public: + BlockPI(AP_Var_group * group, uint8_t groupStart, float kP, float kI, + float iMax, float yMax) : + AP_ControllerBlock(group, groupStart, 4), + _blockP(group, groupStart, kP), + _blockI(group, _blockP.getGroupEnd(), kI, iMax), + _blockSaturation(group, _blockI.getGroupEnd(), yMax) { + } + + float update(const float & input, const float & dt) { + + float y = _blockP.update(input) + _blockI.update(input, dt); + return _blockSaturation.update(y); + } +protected: + BlockP _blockP; + BlockI _blockI; + BlockSaturation _blockSaturation; +}; + +class BlockPD: public AP_ControllerBlock { +public: + BlockPD(AP_Var_group * group, uint8_t groupStart, float kP, float kI, + float kD, float iMax, float yMax, uint8_t dFcut) : + AP_ControllerBlock(group, groupStart, 4), + _blockP(group, groupStart, kP), + _blockD(group, _blockP.getGroupEnd(), kD, dFcut), + _blockSaturation(group, _blockD.getGroupEnd(), yMax) { + } + + float update(const float & input, const float & dt) { + + float y = _blockP.update(input) + _blockD.update(input, dt); + return _blockSaturation.update(y); + } +protected: + BlockP _blockP; + BlockD _blockD; + BlockSaturation _blockSaturation; +}; + +/// PID with derivative feedback (ignores reference derivative) +class BlockPIDDfb: public AP_ControllerBlock { +public: + BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI, + float kD, float iMax, float yMax, const prog_char_t * dLabel = NULL) : + AP_ControllerBlock(group, groupStart, 5), + _blockP(group, groupStart, kP), + _blockI(group, _blockP.getGroupEnd(), kI, iMax), + _blockSaturation(group, _blockI.getGroupEnd(), yMax), + _kD(group, _blockSaturation.getGroupEnd(), kD, dLabel ? : PSTR("d")) { + } + float update(const float & input, const float & derivative, + const float & dt) { + float y = _blockP.update(input) + _blockI.update(input, dt) - _kD + * derivative; + return _blockSaturation.update(y); + } +protected: + BlockP _blockP; + BlockI _blockI; + BlockSaturation _blockSaturation; + AP_Float _kD; /// derivative gain +}; + +} // apo + +#endif // AP_Controller_H +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp new file mode 100644 index 0000000000..488b746610 --- /dev/null +++ b/libraries/APO/AP_Guide.cpp @@ -0,0 +1,8 @@ +/* + * AP_Guide.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_Guide.h" diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h new file mode 100644 index 0000000000..fb4352f830 --- /dev/null +++ b/libraries/APO/AP_Guide.h @@ -0,0 +1,359 @@ +/* + * AP_Guide.h + * Copyright (C) James Goppert 2010 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#ifndef AP_Guide_H +#define AP_Guide_H + +#include "../GCS_MAVLink/GCS_MAVLink.h" +#include "AP_HardwareAbstractionLayer.h" +#include "AP_Navigator.h" +#include "../AP_Common/AP_Common.h" +#include "../AP_Common/AP_Vector.h" +#include "AP_MavlinkCommand.h" +#include "constants.h" + +//#include "AP_CommLink.h" + +namespace apo { + +/// Guide class +class AP_Guide { +public: + + /** + * This is the constructor, which requires a link to the navigator. + * @param navigator This is the navigator pointer. + */ + AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal) : + _navigator(navigator), _hal(hal), _command(0), _previousCommand(0), + _headingCommand(0), _airSpeedCommand(0), + _groundSpeedCommand(0), _altitudeCommand(0), _pNCmd(0), + _pECmd(0), _pDCmd(0), _mode(MAV_NAV_LOST), + _numberOfCommands(1), _cmdIndex(0), _nextCommandCalls(0), + _nextCommandTimer(0) { + } + + virtual void update() = 0; + + virtual void nextCommand() = 0; + + MAV_NAV getMode() const { + return _mode; + } + uint8_t getCurrentIndex() { + return _cmdIndex; + } + + void setCurrentIndex(uint8_t val) { + _cmdIndex.set_and_save(val); + _command = AP_MavlinkCommand(getCurrentIndex()); + _command.load(); + _previousCommand = AP_MavlinkCommand(getPreviousIndex()); + _previousCommand.load(); + //_hal->gcs->sendMessage(MAVLINK_MSG_ID_WAYPOINT_CURRENT); + } + + uint8_t getNumberOfCommands() { + return _numberOfCommands; + } + + void setNumberOfCommands(uint8_t val) { + _numberOfCommands.set_and_save(val); + } + + uint8_t getPreviousIndex() { + // find previous waypoint, TODO, handle non-nav commands + int16_t prevIndex = int16_t(getCurrentIndex()) - 1; + if (prevIndex < 0) + prevIndex = getNumberOfCommands() - 1; + return (uint8_t) prevIndex; + } + + uint8_t getNextIndex() { + // find previous waypoint, TODO, handle non-nav commands + int16_t nextIndex = int16_t(getCurrentIndex()) + 1; + if (nextIndex > (getNumberOfCommands() - 1)) + nextIndex = 0; + return nextIndex; + } + + float getHeadingCommand() { + return _headingCommand; + } + float getAirSpeedCommand() { + return _airSpeedCommand; + } + float getGroundSpeedCommand() { + return _groundSpeedCommand; + } + float getAltitudeCommand() { + return _altitudeCommand; + } + float getPNCmd() { + return _pNCmd; + } + float getPECmd() { + return _pECmd; + } + float getPDCmd() { + return _pDCmd; + } + MAV_NAV getMode() { + return _mode; + } + uint8_t getCommandIndex() { + return _cmdIndex; + } + +protected: + AP_Navigator * _navigator; + AP_HardwareAbstractionLayer * _hal; + AP_MavlinkCommand _command, _previousCommand; + float _headingCommand; + float _airSpeedCommand; + float _groundSpeedCommand; + float _altitudeCommand; + float _pNCmd; + float _pECmd; + float _pDCmd; + MAV_NAV _mode; + AP_Uint8 _numberOfCommands; + AP_Uint8 _cmdIndex; + uint16_t _nextCommandCalls; + uint16_t _nextCommandTimer; +}; + +class MavlinkGuide: public AP_Guide { +public: + MavlinkGuide(AP_Navigator * navigator, + AP_HardwareAbstractionLayer * hal) : + AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(), + _rangeFinderLeft(), _rangeFinderRight(), + _group(k_guide, PSTR("guide_")), + _velocityCommand(&_group, 1, 1, PSTR("velCmd")), + _crossTrackGain(&_group, 2, 2, PSTR("xt")), + _crossTrackLim(&_group, 3, 10, PSTR("xtLim")) { + + for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) { + RangeFinder * rF = _hal->rangeFinders[i]; + if (rF == NULL) + continue; + if (rF->orientation_x == 1 && rF->orientation_y == 0 + && rF->orientation_z == 0) + _rangeFinderFront = rF; + else if (rF->orientation_x == -1 && rF->orientation_y == 0 + && rF->orientation_z == 0) + _rangeFinderBack = rF; + else if (rF->orientation_x == 0 && rF->orientation_y == 1 + && rF->orientation_z == 0) + _rangeFinderRight = rF; + else if (rF->orientation_x == 0 && rF->orientation_y == -1 + && rF->orientation_z == 0) + _rangeFinderLeft = rF; + + } + } + + virtual void update() { +// _hal->debug->printf_P( +// PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"), +// getNumberOfCommands(), +// getCurrentIndex(), +// getPreviousIndex()); + + // if we don't have enough waypoint for cross track calcs + // go home + if (_numberOfCommands == 1) { + _mode = MAV_NAV_RETURNING; + _altitudeCommand = AP_MavlinkCommand::home.getAlt(); + _headingCommand = AP_MavlinkCommand::home.bearingTo( + _navigator->getLat_degInt(), _navigator->getLon_degInt()) + + 180 * deg2Rad; + if (_headingCommand > 360 * deg2Rad) + _headingCommand -= 360 * deg2Rad; + +// _hal->debug->printf_P(PSTR("going home: bearing: %f distance: %f\n"), +// headingCommand,AP_MavlinkCommand::home.distanceTo(_navigator->getLat_degInt(),_navigator->getLon_degInt())); + } else { + _mode = MAV_NAV_WAYPOINT; + _altitudeCommand = _command.getAlt(); + // TODO wrong behavior if 0 selected as waypoint, says previous 0 + float dXt = _command.crossTrack(_previousCommand, + _navigator->getLat_degInt(), + _navigator->getLon_degInt()); + float temp = dXt * _crossTrackGain * deg2Rad; // crosstrack gain, rad/m + if (temp > _crossTrackLim * deg2Rad) + temp = _crossTrackLim * deg2Rad; + if (temp < -_crossTrackLim * deg2Rad) + temp = -_crossTrackLim * deg2Rad; + float bearing = _previousCommand.bearingTo(_command); + _headingCommand = bearing - temp; + float alongTrack = _command.alongTrack(_previousCommand, + _navigator->getLat_degInt(), + _navigator->getLon_degInt()); + float distanceToNext = _command.distanceTo( + _navigator->getLat_degInt(), _navigator->getLon_degInt()); + float segmentLength = _previousCommand.distanceTo(_command); + if (distanceToNext < _command.getRadius() || alongTrack + > segmentLength) + { + Serial.println("radius reached"); + nextCommand(); + } +// _hal->debug->printf_P( +// PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\n"), +// bearing * rad2Deg, dXt, _headingCommand * rad2Deg, distanceToNext, alongTrack); + } + + _groundSpeedCommand = _velocityCommand; + + // TODO : calculate pN,pE,pD from home and gps coordinates + _pNCmd = _command.getPN(_navigator->getLat_degInt(), + _navigator->getLon_degInt()); + _pECmd = _command.getPE(_navigator->getLat_degInt(), + _navigator->getLon_degInt()); + _pDCmd = _command.getPD(_navigator->getAlt_intM()); + + // process mavlink commands + //handleCommand(); + + // obstacle avoidance overrides + // stop if your going to drive into something in front of you + for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) + _hal->rangeFinders[i]->read(); + float frontDistance = _rangeFinderFront->distance / 200.0; //convert for other adc + if (_rangeFinderFront && frontDistance < 2) { + _mode = MAV_NAV_VECTOR; + //airSpeedCommand = 0; + //groundSpeedCommand = 0; +// _headingCommand -= 45 * deg2Rad; +// _hal->debug->print("Obstacle Distance (m): "); +// _hal->debug->println(frontDistance); +// _hal->debug->print("Obstacle avoidance Heading Command: "); +// _hal->debug->println(headingCommand); +// _hal->debug->printf_P( +// PSTR("Front Distance, %f\n"), +// frontDistance); + } + if (_rangeFinderBack && _rangeFinderBack->distance < 5) { + _airSpeedCommand = 0; + _groundSpeedCommand = 0; + + } + + if (_rangeFinderLeft && _rangeFinderLeft->distance < 5) { + _airSpeedCommand = 0; + _groundSpeedCommand = 0; + } + + if (_rangeFinderRight && _rangeFinderRight->distance < 5) { + _airSpeedCommand = 0; + _groundSpeedCommand = 0; + } + } + + void nextCommand() { + // within 1 seconds, check if more than 5 calls to next command occur + // if they do, go to home waypoint + if (millis() - _nextCommandTimer < 1000) { + if (_nextCommandCalls > 5) { + Serial.println("commands loading too fast, returning home"); + setCurrentIndex(0); + setNumberOfCommands(1); + _nextCommandCalls = 0; + _nextCommandTimer = millis(); + return; + } + _nextCommandCalls++; + } else { + _nextCommandTimer = millis(); + _nextCommandCalls = 0; + } + + _cmdIndex = getNextIndex(); + Serial.print("cmd : "); Serial.println(int(_cmdIndex)); + Serial.print("cmd prev : "); Serial.println(int(getPreviousIndex())); + Serial.print("cmd num : "); Serial.println(int(getNumberOfCommands())); + _command = AP_MavlinkCommand(getCurrentIndex()); + _previousCommand = AP_MavlinkCommand(getPreviousIndex()); + } + + void handleCommand(AP_MavlinkCommand command, + AP_MavlinkCommand previousCommand) { + // TODO handle more commands + switch (command.getCommand()) { + case MAV_CMD_NAV_WAYPOINT: { + // if within radius, increment + float d = previousCommand.distanceTo(_navigator->getLat_degInt(), + _navigator->getLon_degInt()); + if (d < command.getRadius()) { + nextCommand(); + Serial.println("radius reached"); + } + break; + } +// case MAV_CMD_CONDITION_CHANGE_ALT: +// case MAV_CMD_CONDITION_DELAY: +// case MAV_CMD_CONDITION_DISTANCE: +// case MAV_CMD_CONDITION_LAST: +// case MAV_CMD_CONDITION_YAW: +// case MAV_CMD_DO_CHANGE_SPEED: +// case MAV_CMD_DO_CONTROL_VIDEO: +// case MAV_CMD_DO_JUMP: +// case MAV_CMD_DO_LAST: +// case MAV_CMD_DO_LAST: +// case MAV_CMD_DO_REPEAT_RELAY: +// case MAV_CMD_DO_REPEAT_SERVO: +// case MAV_CMD_DO_SET_HOME: +// case MAV_CMD_DO_SET_MODE: +// case MAV_CMD_DO_SET_PARAMETER: +// case MAV_CMD_DO_SET_RELAY: +// case MAV_CMD_DO_SET_SERVO: +// case MAV_CMD_PREFLIGHT_CALIBRATION: +// case MAV_CMD_PREFLIGHT_STORAGE: +// case MAV_CMD_NAV_LAND: +// case MAV_CMD_NAV_LAST: +// case MAV_CMD_NAV_LOITER_TIME: +// case MAV_CMD_NAV_LOITER_TURNS: +// case MAV_CMD_NAV_LOITER_UNLIM: +// case MAV_CMD_NAV_ORIENTATION_TARGET: +// case MAV_CMD_NAV_PATHPLANNING: +// case MAV_CMD_NAV_RETURN_TO_LAUNCH: +// case MAV_CMD_NAV_TAKEOFF: + default: + // unhandled command, skip + Serial.println("unhandled command"); + nextCommand(); + break; + } + } +private: + RangeFinder * _rangeFinderFront; + RangeFinder * _rangeFinderBack; + RangeFinder * _rangeFinderLeft; + RangeFinder * _rangeFinderRight; + AP_Var_group _group; + AP_Float _velocityCommand; + AP_Float _crossTrackGain; + AP_Float _crossTrackLim; +}; + +} // namespace apo + +#endif // AP_Guide_H +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_HardwareAbstractionLayer.cpp b/libraries/APO/AP_HardwareAbstractionLayer.cpp new file mode 100644 index 0000000000..afa06769ab --- /dev/null +++ b/libraries/APO/AP_HardwareAbstractionLayer.cpp @@ -0,0 +1,8 @@ +/* + * AP_HardwareAbstractionLayer.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_HardwareAbstractionLayer.h" diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_HardwareAbstractionLayer.h new file mode 100644 index 0000000000..45e7daaf75 --- /dev/null +++ b/libraries/APO/AP_HardwareAbstractionLayer.h @@ -0,0 +1,166 @@ +/* + * AP_HardwareAbstractionLayer.h + * + * Created on: Apr 4, 2011 + * + */ + +#ifndef AP_HARDWAREABSTRACTIONLAYER_H_ +#define AP_HARDWAREABSTRACTIONLAYER_H_ + +#include "../AP_Common/AP_Common.h" +#include "../FastSerial/FastSerial.h" +#include "../AP_Common/AP_Vector.h" +#include "../GCS_MAVLink/GCS_MAVLink.h" + +#include "../AP_ADC/AP_ADC.h" +#include "../AP_IMU/AP_IMU.h" +#include "../AP_GPS/GPS.h" +#include "../APM_BMP085/APM_BMP085.h" +#include "../AP_Compass/AP_Compass.h" +#include "AP_RcChannel.h" +#include "../AP_RangeFinder/AP_RangeFinder.h" +#include "../GCS_MAVLink/GCS_MAVLink.h" + +class AP_ADC; +class IMU; +class GPS; +class APM_BMP085_Class; +class Compass; +class BetterStream; +class RangeFinder; + +namespace apo { + +class AP_RcChannel; +class AP_CommLink; + +// enumerations +enum halMode_t { + MODE_LIVE, MODE_HIL_CNTL, +/*MODE_HIL_NAV*/}; +enum board_t { + BOARD_ARDUPILOTMEGA_1280, BOARD_ARDUPILOTMEGA_2560, BOARD_ARDUPILOTMEGA_2 +}; +enum vehicle_t { + VEHICLE_CAR, VEHICLE_QUAD, VEHICLE_PLANE, VEHICLE_BOAT, VEHICLE_TANK +}; + +class AP_HardwareAbstractionLayer { + +public: + + AP_HardwareAbstractionLayer(halMode_t mode, board_t board, + vehicle_t vehicle, uint8_t heartBeatTimeout) : + adc(), gps(), baro(), compass(), rangeFinders(), imu(), rc(), gcs(), + hil(), debug(), load(), lastHeartBeat(), + _heartBeatTimeout(heartBeatTimeout), _mode(mode), + _board(board), _vehicle(vehicle), _state(MAV_STATE_UNINIT) { + + /* + * Board specific hardware initialization + */ + if (board == BOARD_ARDUPILOTMEGA_1280) { + slideSwitchPin = 40; + pushButtonPin = 41; + aLedPin = 37; + bLedPin = 36; + cLedPin = 35; + eepromMaxAddr = 1024; + pinMode(aLedPin, OUTPUT); // extra led + pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; + pinMode(cLedPin, OUTPUT); // gps led + pinMode(slideSwitchPin, INPUT); + pinMode(pushButtonPin, INPUT); + DDRL |= B00000100; // set port L, pint 2 to output for the relay + } else if (board == BOARD_ARDUPILOTMEGA_2560) { + slideSwitchPin = 40; + pushButtonPin = 41; + aLedPin = 37; + bLedPin = 36; + cLedPin = 35; + eepromMaxAddr = 2048; + pinMode(aLedPin, OUTPUT); // extra led + pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; + pinMode(cLedPin, OUTPUT); // gps led + pinMode(slideSwitchPin, INPUT); + pinMode(pushButtonPin, INPUT); + DDRL |= B00000100; // set port L, pint 2 to output for the relay + } + } + + /** + * Sensors + */ + AP_ADC * adc; + GPS * gps; + APM_BMP085_Class * baro; + Compass * compass; + Vector rangeFinders; + IMU * imu; + + /** + * Radio Channels + */ + Vector rc; + + /** + * Communication Channels + */ + AP_CommLink * gcs; + AP_CommLink * hil; + FastSerial * debug; + + /** + * data + */ + uint8_t load; + uint32_t lastHeartBeat; + + /** + * settings + */ + uint8_t slideSwitchPin; + uint8_t pushButtonPin; + uint8_t aLedPin; + uint8_t bLedPin; + uint8_t cLedPin; + uint16_t eepromMaxAddr; + + // accessors + halMode_t getMode() { + return _mode; + } + board_t getBoard() { + return _board; + } + vehicle_t getVehicle() { + return _vehicle; + } + MAV_STATE getState() { + return _state; + } + void setState(MAV_STATE state) { + _state = state; + } + + bool heartBeatLost() { + if (_heartBeatTimeout == 0) + return false; + else + return ((micros() - lastHeartBeat) / 1e6) > _heartBeatTimeout; + } + +private: + + // enumerations + uint8_t _heartBeatTimeout; + halMode_t _mode; + board_t _board; + vehicle_t _vehicle; + MAV_STATE _state; +}; + +} + +#endif /* AP_HARDWAREABSTRACTIONLAYER_H_ */ diff --git a/libraries/APO/AP_MavlinkCommand.cpp b/libraries/APO/AP_MavlinkCommand.cpp new file mode 100644 index 0000000000..1109a3641b --- /dev/null +++ b/libraries/APO/AP_MavlinkCommand.cpp @@ -0,0 +1,175 @@ +/* + * AP_MavlinkCommand.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_MavlinkCommand.h" + +namespace apo { + +AP_MavlinkCommand::AP_MavlinkCommand(const AP_MavlinkCommand & v) : + _data(v._data), _seq(v._seq) { + //if (FastSerial::getInitialized(0)) Serial.println("copy ctor"); +} + +AP_MavlinkCommand::AP_MavlinkCommand(uint16_t index, bool doLoad) : + _data(k_commands + index), _seq(index) { + + if (FastSerial::getInitialized(0)) { + Serial.println("index ctor"); + Serial.println("++"); + Serial.print("index: "); Serial.println(index); + Serial.print("key: "); Serial.println(k_commands + index); + Serial.println("++"); + } + // This is a failsafe measure to stop trying to load a command if it can't load + if (doLoad && !load()) { + Serial.println("load failed, reverting to home waypoint"); + _data = AP_MavlinkCommand::home._data; + _seq = AP_MavlinkCommand::home._seq; + } +} + +AP_MavlinkCommand::AP_MavlinkCommand(mavlink_waypoint_t cmd) : + _data(k_commands + cmd.seq), _seq(cmd.seq) { + setCommand(MAV_CMD(cmd.command)); + setAutocontinue(cmd.autocontinue); + setFrame(MAV_FRAME(cmd.frame)); + setParam1(cmd.param1); + setParam2(cmd.param2); + setParam3(cmd.param3); + setParam4(cmd.param4); + setX(cmd.x); + setY(cmd.y); + setZ(cmd.z); + save(); + + // reload home if sent + if (cmd.seq == 0) home.load(); + + Serial.println("============================================================"); + Serial.println("storing new command from mavlink_waypoint_t"); + Serial.print("key: "); Serial.println(_data.key(),DEC); + Serial.print("number: "); Serial.println(cmd.seq,DEC); + Serial.print("command: "); Serial.println(getCommand()); + Serial.print("autocontinue: "); Serial.println(getAutocontinue(),DEC); + Serial.print("frame: "); Serial.println(getFrame(),DEC); + Serial.print("1000*param1: "); Serial.println(int(1000*getParam1()),DEC); + Serial.print("1000*param2: "); Serial.println(int(1000*getParam2()),DEC); + Serial.print("1000*param3: "); Serial.println(int(1000*getParam3()),DEC); + Serial.print("1000*param4: "); Serial.println(int(1000*getParam4()),DEC); + Serial.print("1000*x0: "); Serial.println(int(1000*cmd.x),DEC); + Serial.print("1000*y0: "); Serial.println(int(1000*cmd.y),DEC); + Serial.print("1000*z0: "); Serial.println(int(1000*cmd.z),DEC); + Serial.print("1000*x: "); Serial.println(int(1000*getX()),DEC); + Serial.print("1000*y: "); Serial.println(int(1000*getY()),DEC); + Serial.print("1000*z: "); Serial.println(int(1000*getZ()),DEC); + + load(); + + Serial.print("1000*x1: "); Serial.println(int(1000*getX()),DEC); + Serial.print("1000*y1: "); Serial.println(int(1000*getY()),DEC); + Serial.print("1000*z1: "); Serial.println(int(1000*getZ()),DEC); + Serial.println("============================================================"); + Serial.flush(); +} + +mavlink_waypoint_t AP_MavlinkCommand::convert(uint8_t current) { + mavlink_waypoint_t mavCmd; + mavCmd.seq = getSeq(); + mavCmd.command = getCommand(); + mavCmd.frame = getFrame(); + mavCmd.param1 = getParam1(); + mavCmd.param2 = getParam2(); + mavCmd.param3 = getParam3(); + mavCmd.param4 = getParam4(); + mavCmd.x = getX(); + mavCmd.y = getY(); + mavCmd.z = getZ(); + mavCmd.autocontinue = getAutocontinue(); + mavCmd.current = (getSeq() == current); + mavCmd.target_component = mavlink_system.compid; + mavCmd.target_system = mavlink_system.sysid; + return mavCmd; +} + +float AP_MavlinkCommand::bearingTo(AP_MavlinkCommand next) const { + float deltaLon = next.getLon() - getLon(); + /* + Serial.print("Lon: "); Serial.println(getLon()); + Serial.print("nextLon: "); Serial.println(next.getLon()); + Serial.print("deltaLonDeg * 1e7: "); Serial.println(deltaLon*rad2DegInt); + */ + float bearing = atan2( + sin(deltaLon) * cos(next.getLat()), + cos(getLat()) * sin(next.getLat()) - sin(getLat()) * cos( + next.getLat()) * cos(deltaLon)); + return bearing; +} + +float AP_MavlinkCommand::bearingTo(int32_t latDegInt, int32_t lonDegInt) const { + // have to be careful to maintain the precision of the gps coordinate + float deltaLon = (lonDegInt - getLon_degInt()) * degInt2Rad; + float nextLat = latDegInt * degInt2Rad; + float bearing = atan2( + sin(deltaLon) * cos(nextLat), + cos(getLat()) * sin(nextLat) - sin(getLat()) * cos(nextLat) + * cos(deltaLon)); + if (bearing < 0) + bearing += 2 * M_PI; + return bearing; +} + +float AP_MavlinkCommand::distanceTo(AP_MavlinkCommand next) const { + float sinDeltaLat2 = sin((getLat() - next.getLat()) / 2); + float sinDeltaLon2 = sin((getLon() - next.getLon()) / 2); + float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos( + next.getLat()) * sinDeltaLon2 * sinDeltaLon2; + float c = 2 * atan2(sqrt(a), sqrt(1 - a)); + return rEarth * c; +} + +float AP_MavlinkCommand::distanceTo(int32_t lat_degInt, int32_t lon_degInt) const { + float sinDeltaLat2 = sin( + (lat_degInt - getLat_degInt()) * degInt2Rad / 2); + float sinDeltaLon2 = sin( + (lon_degInt - getLon_degInt()) * degInt2Rad / 2); + float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos( + lat_degInt * degInt2Rad) * sinDeltaLon2 * sinDeltaLon2; + float c = 2 * atan2(sqrt(a), sqrt(1 - a)); + /* + Serial.print("wp lat_degInt: "); Serial.println(getLat_degInt()); + Serial.print("wp lon_degInt: "); Serial.println(getLon_degInt()); + Serial.print("lat_degInt: "); Serial.println(lat_degInt); + Serial.print("lon_degInt: "); Serial.println(lon_degInt); + Serial.print("sinDeltaLat2: "); Serial.println(sinDeltaLat2); + Serial.print("sinDeltaLon2: "); Serial.println(sinDeltaLon2); + */ + return rEarth * c; +} + +//calculates cross track of a current location +float AP_MavlinkCommand::crossTrack(AP_MavlinkCommand previous, + int32_t lat_degInt, int32_t lon_degInt) { + float d = previous.distanceTo(lat_degInt, lon_degInt); + float bCurrent = previous.bearingTo(lat_degInt, lon_degInt); + float bNext = previous.bearingTo(*this); + return asin(sin(d / rEarth) * sin(bCurrent - bNext)) * rEarth; + return 0; +} + +// calculates along track distance of a current location +float AP_MavlinkCommand::alongTrack(AP_MavlinkCommand previous, + int32_t lat_degInt, int32_t lon_degInt) { + // ignores lat/lon since single prec. + float dXt = this->crossTrack(previous,lat_degInt, lon_degInt); + float d = previous.distanceTo(lat_degInt, lon_degInt); + return dXt / tan(asin(dXt / d)); +} + + +AP_MavlinkCommand AP_MavlinkCommand::home = AP_MavlinkCommand(0,false); + +} diff --git a/libraries/APO/AP_MavlinkCommand.h b/libraries/APO/AP_MavlinkCommand.h new file mode 100644 index 0000000000..6cb10557e3 --- /dev/null +++ b/libraries/APO/AP_MavlinkCommand.h @@ -0,0 +1,359 @@ +/* + * AP_MavlinkCommand.h + * + * Created on: Apr 4, 2011 + * Author: jgoppert + */ + +#ifndef AP_MAVLINKCOMMAND_H_ +#define AP_MAVLINKCOMMAND_H_ + +#include "../GCS_MAVLink/GCS_MAVLink.h" +#include "../AP_Common/AP_Common.h" +#include "AP_Var_keys.h" +#include "constants.h" +#include "../FastSerial/FastSerial.h" + +namespace apo { + +class AP_MavlinkCommand { +private: + struct CommandStorage { + MAV_CMD command; + bool autocontinue; + MAV_FRAME frame; + float param1; + float param2; + float param3; + float param4; + float x; + float y; + float z; + }; + AP_VarS _data; + uint16_t _seq; +public: + static AP_MavlinkCommand home; + + /** + * Copy Constructor + */ + AP_MavlinkCommand(const AP_MavlinkCommand & v); + + /** + * Basic Constructor + * @param index Start at zero. + */ + AP_MavlinkCommand(uint16_t index, bool doLoad = true); + + /** + * Constructor for copying/ saving from a mavlink waypoint. + * @param cmd The mavlink_waopint_t structure for the command. + */ + AP_MavlinkCommand(mavlink_waypoint_t cmd); + + bool save() { + return _data.save(); + } + bool load() { + return _data.load(); + } + uint8_t getSeq() const { + return _seq; + } + bool getAutocontinue() const { + return _data.get().autocontinue; + } + void setAutocontinue(bool val) { + _data.get().autocontinue = val; + } + void setSeq(uint8_t val) { + _seq = val; + } + MAV_CMD getCommand() const { + return _data.get().command; + } + void setCommand(MAV_CMD val) { + _data.get().command = val; + } + MAV_FRAME getFrame() const { + return _data.get().frame; + } + void setFrame(MAV_FRAME val) { + _data.get().frame = val; + } + float getParam1() const { + return _data.get().param1; + } + void setParam1(float val) { + _data.get().param1 = val; + } + float getParam2() const { + return _data.get().param2; + } + void setParam2(float val) { + _data.get().param2 = val; + } + float getParam3() const { + return _data.get().param3; + } + void setParam3(float val) { + _data.get().param3 = val; + } + float getParam4() const { + return _data.get().param4; + } + void setParam4(float val) { + _data.get().param4 = val; + } + float getX() const { + return _data.get().x; + } + void setX(float val) { + _data.get().x = val; + } + float getY() const { + return _data.get().y; + } + void setY(float val) { + _data.get().y = val; + } + float getZ() const { + return _data.get().z; + } + void setZ(float val) { + _data.get().z = val; + } + float getLatDeg() const { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + return getX(); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + void setLatDeg(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + setX(val); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_MISSION: + default: + break; + } + } + float getLonDeg() const { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + return getY(); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + void setLonDeg(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + setY(val); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_MISSION: + default: + break; + } + } + void setLon(float val) { + setLonDeg(val * rad2Deg); + } + void setLon_degInt(int32_t val) { + setLonDeg(val / 1.0e7); + } + void setLat_degInt(int32_t val) { + setLatDeg(val / 1.0e7); + } + int32_t getLon_degInt() const { + return getLonDeg() * 1e7; + } + int32_t getLat_degInt() const { + return getLatDeg() * 1e7; + } + float getLon() const { + return getLonDeg() * deg2Rad; + } + float getLat() const { + return getLatDeg() * deg2Rad; + } + void setLat(float val) { + setLatDeg(val * rad2Deg); + } + float getAlt() const { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + return getZ(); + break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_LOCAL: + return getZ() + home.getAlt(); + break; + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + /** + * set the altitude in meters + */ + void setAlt(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + setZ(val); + break; + case MAV_FRAME_LOCAL: + setZ(val - home.getLonDeg()); + break; + case MAV_FRAME_MISSION: + default: + break; + } + } + /** + * Get the relative altitude to home + * @return relative altitude in meters + */ + float getRelAlt() const { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + return getZ() - home.getAlt(); + break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_LOCAL: + return getZ(); + break; + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + /** + * set the relative altitude in meters from home + */ + void setRelAlt(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + setZ(val + home.getAlt()); + break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_LOCAL: + setZ(val); + break; + case MAV_FRAME_MISSION: + break; + } + } + + float getRadius() const { + return getParam2(); + } + + void setRadius(float val) { + setParam2(val); + } + + /** + * conversion for outbound packets to ground station + * @return output the mavlink_waypoint_t packet + */ + mavlink_waypoint_t convert(uint8_t current); + + /** + * Calculate the bearing from this command to the next command + * @param next The command to calculate the bearing to. + * @return the bearing + */ + float bearingTo(AP_MavlinkCommand next) const; + + /** + * Bearing form this command to a gps coordinate in integer units + * @param latDegInt latitude in degrees E-7 + * @param lonDegInt longitude in degrees E-7 + * @return + */ + float bearingTo(int32_t latDegInt, int32_t lonDegInt) const; + + /** + * Distance to another command + * @param next The command to measure to. + * @return The distance in meters. + */ + float distanceTo(AP_MavlinkCommand next) const; + + /** + * Distance to a gps coordinate in integer units + * @param latDegInt latitude in degrees E-7 + * @param lonDegInt longitude in degrees E-7 + * @return The distance in meters. + */ + float distanceTo(int32_t lat_degInt, int32_t lon_degInt) const; + + float getPN(int32_t lat_degInt, int32_t lon_degInt) const { + // local tangent approximation at this waypoint + float deltaLat = (lat_degInt - getLat_degInt()) * degInt2Rad; + return deltaLat * rEarth; + } + + float getPE(int32_t lat_degInt, int32_t lon_degInt) const { + // local tangent approximation at this waypoint + float deltaLon = (lon_degInt - getLon_degInt()) * degInt2Rad; + return cos(getLat()) * deltaLon * rEarth; + } + + float getPD(int32_t alt_intM) const { + return -(alt_intM / scale_m - getAlt()); + } + + float getLat(float pN) const { + + return pN / rEarth + getLat(); + } + + float getLon(float pE) const { + + return pE / rEarth / cos(getLat()) + getLon(); + } + + /** + * Gets altitude in meters + * @param pD alt in meters + * @return + */ + float getAlt(float pD) const { + + return getAlt() - pD; + } + + //calculates cross track of a current location + float crossTrack(AP_MavlinkCommand previous, int32_t lat_degInt, int32_t lon_degInt); + + // calculates along track distance of a current location + float alongTrack(AP_MavlinkCommand previous, int32_t lat_degInt, int32_t lon_degInt); +}; + +} // namespace apo + + +#endif /* AP_MAVLINKCOMMAND_H_ */ diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp new file mode 100644 index 0000000000..ff33e8ed2c --- /dev/null +++ b/libraries/APO/AP_Navigator.cpp @@ -0,0 +1,8 @@ +/* + * AP_Navigator.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_Navigator.h" diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h new file mode 100644 index 0000000000..9b87561946 --- /dev/null +++ b/libraries/APO/AP_Navigator.h @@ -0,0 +1,391 @@ +/* + * AP_Navigator.h + * Copyright (C) James Goppert 2010 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#ifndef AP_Navigator_H +#define AP_Navigator_H + +#include "AP_HardwareAbstractionLayer.h" +#include "../AP_DCM/AP_DCM.h" +#include "../AP_Math/AP_Math.h" +#include "../AP_Compass/AP_Compass.h" +#include "AP_MavlinkCommand.h" +#include "constants.h" +#include "AP_Var_keys.h" +#include "../AP_RangeFinder/AP_RangeFinder.h" +#include "../AP_IMU/AP_IMU.h" + +namespace apo { + +/// Navigator class +class AP_Navigator { +public: + AP_Navigator(AP_HardwareAbstractionLayer * hal) : + _hal(hal), _timeStamp(0), _roll(0), _rollRate(0), _pitch(0), + _pitchRate(0), _yaw(0), _yawRate(0), _airSpeed(0), + _groundSpeed(0), _vD(0), _lat_degInt(0), + _lon_degInt(0), _alt_intM(0) { + } + virtual void calibrate() { + } + virtual void updateFast(float dt) = 0; + virtual void updateSlow(float dt) = 0; + float getPD() const { + return AP_MavlinkCommand::home.getPD(getAlt_intM()); + } + + float getPE() const { + return AP_MavlinkCommand::home.getPE(getLat_degInt(), getLon_degInt()); + } + + float getPN() const { + return AP_MavlinkCommand::home.getPN(getLat_degInt(), getLon_degInt()); + } + + void setPD(float _pD) { + setAlt(AP_MavlinkCommand::home.getAlt(_pD)); + } + + void setPE(float _pE) { + setLat(AP_MavlinkCommand::home.getLat(_pE)); + } + + void setPN(float _pN) { + setLon(AP_MavlinkCommand::home.getLon(_pN)); + } + + float getAirSpeed() const { + return _airSpeed; + } + + int32_t getAlt_intM() const { + return _alt_intM; + } + + float getAlt() const { + return _alt_intM / scale_m; + } + + void setAlt(float _alt) { + this->_alt_intM = _alt * scale_m; + } + + float getLat() const { + //Serial.print("getLatfirst"); + //Serial.println(_lat_degInt * degInt2Rad); + return _lat_degInt * degInt2Rad; + } + + void setLat(float _lat) { + //Serial.print("setLatfirst"); + //Serial.println(_lat * rad2DegInt); + setLat_degInt(_lat*rad2DegInt); + } + + float getLon() const { + return _lon_degInt * degInt2Rad; + } + + void setLon(float _lon) { + this->_lon_degInt = _lon * rad2DegInt; + } + + float getVD() const { + return _vD; + } + + float getVE() const { + return sin(getYaw()) * getGroundSpeed(); + } + + float getGroundSpeed() const { + return _groundSpeed; + } + + int32_t getLat_degInt() const { + //Serial.print("getLat_degInt"); + //Serial.println(_lat_degInt); + return _lat_degInt; + + } + + int32_t getLon_degInt() const { + return _lon_degInt; + } + + float getVN() const { + return cos(getYaw()) * getGroundSpeed(); + } + + float getPitch() const { + return _pitch; + } + + float getPitchRate() const { + return _pitchRate; + } + + float getRoll() const { + return _roll; + } + + float getRollRate() const { + return _rollRate; + } + + float getYaw() const { + return _yaw; + } + + float getYawRate() const { + return _yawRate; + } + + void setAirSpeed(float airSpeed) { + _airSpeed = airSpeed; + } + + void setAlt_intM(int32_t alt_intM) { + _alt_intM = alt_intM; + } + + void setVD(float vD) { + _vD = vD; + } + + void setGroundSpeed(float groundSpeed) { + _groundSpeed = groundSpeed; + } + + void setLat_degInt(int32_t lat_degInt) { + _lat_degInt = lat_degInt; + //Serial.print("setLat_degInt"); + //Serial.println(_lat_degInt); + } + + void setLon_degInt(int32_t lon_degInt) { + _lon_degInt = lon_degInt; + } + + void setPitch(float pitch) { + _pitch = pitch; + } + + void setPitchRate(float pitchRate) { + _pitchRate = pitchRate; + } + + void setRoll(float roll) { + _roll = roll; + } + + void setRollRate(float rollRate) { + _rollRate = rollRate; + } + + void setYaw(float yaw) { + _yaw = yaw; + } + + void setYawRate(float yawRate) { + _yawRate = yawRate; + } + void setTimeStamp(int32_t timeStamp) { + _timeStamp = timeStamp; + } + int32_t getTimeStamp() const { + return _timeStamp; + } + +protected: + AP_HardwareAbstractionLayer * _hal; +private: + int32_t _timeStamp; // micros clock + float _roll; // rad + float _rollRate; //rad/s + float _pitch; // rad + float _pitchRate; // rad/s + float _yaw; // rad + float _yawRate; // rad/s + float _airSpeed; // m/s + float _groundSpeed; // m/s + float _vD; // m/s + int32_t _lat_degInt; // deg / 1e7 + int32_t _lon_degInt; // deg / 1e7 + int32_t _alt_intM; // meters / 1e3 +}; + +class DcmNavigator: public AP_Navigator { +private: + /** + * Sensors + */ + + RangeFinder * _rangeFinderDown; + AP_DCM * _dcm; + IMU * _imu; + uint16_t _imuOffsetAddress; + +public: + DcmNavigator(AP_HardwareAbstractionLayer * hal) : + AP_Navigator(hal), _dcm(), _imuOffsetAddress(0) { + + // if orientation equal to front, store as front + /** + * rangeFinder is assigned values based on orientation which + * is specified in ArduPilotOne.pde. + */ + for (uint8_t i = 0; i < _hal-> rangeFinders.getSize(); i++) { + if (_hal->rangeFinders[i] == NULL) + continue; + if (_hal->rangeFinders[i]->orientation_x == 0 + && _hal->rangeFinders[i]->orientation_y == 0 + && _hal->rangeFinders[i]->orientation_z == 1) + _rangeFinderDown = _hal->rangeFinders[i]; + } + + if (_hal->getMode() == MODE_LIVE) { + if (_hal->adc) + _hal->imu = new AP_IMU_Oilpan(_hal->adc, k_sensorCalib); + if (_hal->imu) + _dcm = new AP_DCM(_hal->imu, _hal->gps, _hal->compass); + if (_hal->compass) { + _dcm->set_compass(_hal->compass); + + } + } + } + virtual void calibrate() { + + AP_Navigator::calibrate(); + + // TODO: handle cold restart + if (_hal->imu) { + /* + * Gyro has built in warm up cycle and should + * run first */ + _hal->imu->init_gyro(NULL); + _hal->imu->init_accel(NULL); + } + } + virtual void updateFast(float dt) { + if (_hal->getMode() != MODE_LIVE) + return; + + setTimeStamp(micros()); // if running in live mode, record new time stamp + + + //_hal->debug->println_P(PSTR("nav loop")); + + /** + * The altitued is read off the barometer by implementing the following formula: + * altitude (in m) = 44330*(1-(p/po)^(1/5.255)), + * where, po is pressure in Pa at sea level (101325 Pa). + * See http://www.sparkfun.com/tutorials/253 or type this formula + * in a search engine for more information. + * altInt contains the altitude in meters. + */ + if (_hal->baro) { + + if (_rangeFinderDown != NULL && _rangeFinderDown->distance <= 695) + setAlt(_rangeFinderDown->distance); + + else { + float tmp = (_hal->baro->Press / 101325.0); + tmp = pow(tmp, 0.190295); + //setAlt(44330 * (1.0 - tmp)); //sets the altitude in meters XXX wrong, baro reads 0 press + setAlt(0.0); + } + } + + // dcm class for attitude + if (_dcm) { + _dcm->update_DCM(); + setRoll(_dcm->roll); + setPitch(_dcm->pitch); + setYaw(_dcm->yaw); + setRollRate(_dcm->get_gyro().x); + setPitchRate(_dcm->get_gyro().y); + setYawRate(_dcm->get_gyro().z); + + /* + * 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); + */ + } + } + virtual void updateSlow(float dt) { + if (_hal->getMode() != MODE_LIVE) + return; + + setTimeStamp(micros()); // if running in live mode, record new time stamp + + if (_hal->gps) { + _hal->gps->update(); + updateGpsLight(); + if (_hal->gps->fix && _hal->gps->new_data) { + setLat_degInt(_hal->gps->latitude); + setLon_degInt(_hal->gps->longitude); + setAlt_intM(_hal->gps->altitude * 10); // gps in cm, intM in mm + setGroundSpeed(_hal->gps->ground_speed / 100.0); // gps is in cm/s + } + } + + if (_hal->compass) { + _hal->compass->read(); + _hal->compass->calculate(getRoll(), getPitch()); + _hal->compass->null_offsets(_dcm->get_dcm_matrix()); + } + } + void updateGpsLight(void) { + // GPS LED on if we have a fix or Blink GPS LED if we are receiving data + // --------------------------------------------------------------------- + static bool GPS_light = false; + switch (_hal->gps->status()) { + case (2): + //digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. + break; + + case (1): + if (_hal->gps->valid_read == true) { + GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock + if (GPS_light) { + digitalWrite(_hal->cLedPin, LOW); + } else { + digitalWrite(_hal->cLedPin, HIGH); + } + _hal->gps->valid_read = false; + } + break; + + default: + digitalWrite(_hal->cLedPin, LOW); + break; + } + } + +}; + +} // namespace apo + +#endif // AP_Navigator_H +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_RcChannel.cpp b/libraries/APO/AP_RcChannel.cpp new file mode 100644 index 0000000000..c2adbc5c52 --- /dev/null +++ b/libraries/APO/AP_RcChannel.cpp @@ -0,0 +1,110 @@ +/* + AP_RcChannel.cpp - Radio library for Arduino + Code by Jason Short, James Goppert. DIYDrones.com + + This library is free software; you can redistribute it and / or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + */ + +#include +#include +#include "AP_RcChannel.h" +#include "../AP_Common/AP_Common.h" +#include + +namespace apo { + +AP_RcChannel::AP_RcChannel(AP_Var::Key keyValue, const prog_char_t * name, + APM_RC_Class & rc, const uint8_t & ch, const uint16_t & pwmMin, + const uint16_t & pwmNeutral, const uint16_t & pwmMax, + const rcMode_t & rcMode, const bool & reverse) : + AP_Var_group(keyValue, name), _ch(this, 1, ch, PSTR("ch")), + _pwmMin(this, 2, pwmMin, PSTR("pMin")), + _pwmNeutral(this, 3, pwmNeutral, PSTR("pNtrl")), + _pwmMax(this, 4, pwmMax, PSTR("pMax")), _rcMode(rcMode), + _reverse(this, 5, reverse, PSTR("rev")), _rc(rc), _pwm(pwmNeutral) { + //Serial.print("pwm after ctor: "); Serial.println(pwmNeutral); + if (rcMode == RC_MODE_IN) + return; + //Serial.print("pwm set for ch: "); Serial.println(int(ch)); + rc.OutputCh(ch, pwmNeutral); + +} + +uint16_t AP_RcChannel::getRadioPwm() { + if (_rcMode == RC_MODE_OUT) { + Serial.print("tryng to read from output channel: "); + Serial.println(int(_ch)); + return _pwmNeutral; // if this happens give a safe value of neutral + } + return _rc.InputCh(_ch); +} + +void AP_RcChannel::setUsingRadio() { + setPwm(getRadioPwm()); +} + +void AP_RcChannel::setPwm(uint16_t pwm) { + //Serial.printf("pwm in setPwm: %d\n", pwm); + //Serial.printf("reverse: %s\n", (reverse)?"true":"false"); + + // apply reverse + if (_reverse) + pwm = int16_t(_pwmNeutral - pwm) + _pwmNeutral; + //Serial.printf("pwm after reverse: %d\n", pwm); + + // apply saturation + if (_pwm > uint8_t(_pwmMax)) + _pwm = _pwmMax; + if (_pwm < uint8_t(_pwmMin)) + _pwm = _pwmMin; + _pwm = pwm; + + //Serial.print("ch: "); Serial.print(ch); Serial.print(" pwm: "); Serial.println(pwm); + if (_rcMode == RC_MODE_IN) + return; + _rc.OutputCh(_ch, _pwm); +} + +void AP_RcChannel::setPosition(float position) { + if (position > 1.0) + position = 1.0; + else if (position < -1.0) + position = -1.0; + setPwm(_positionToPwm(position)); +} + +uint16_t AP_RcChannel::_positionToPwm(const float & position) { + uint16_t pwm; + //Serial.printf("position: %f\n", position); + if (position < 0) + pwm = position * int16_t(_pwmNeutral - _pwmMin) + _pwmNeutral; + else + pwm = position * int16_t(_pwmMax - _pwmNeutral) + _pwmNeutral; + + if (pwm > uint16_t(_pwmMax)) + pwm = _pwmMax; + if (pwm < uint16_t(_pwmMin)) + pwm = _pwmMin; + return pwm; +} + +float AP_RcChannel::_pwmToPosition(const uint16_t & pwm) { + float position; + if (pwm < uint8_t(_pwmNeutral)) + position = 1.0 * int16_t(pwm - _pwmNeutral) / int16_t( + _pwmNeutral - _pwmMin); + else + position = 1.0 * int16_t(pwm - _pwmNeutral) / int16_t( + _pwmMax - _pwmNeutral); + if (position > 1) + position = 1; + if (position < -1) + position = -1; + return position; +} + +} // namespace apo diff --git a/libraries/APO/AP_RcChannel.h b/libraries/APO/AP_RcChannel.h new file mode 100644 index 0000000000..b12489a257 --- /dev/null +++ b/libraries/APO/AP_RcChannel.h @@ -0,0 +1,69 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AP_RcChannel.h +/// @brief AP_RcChannel manager + +#ifndef AP_RCCHANNEL_H +#define AP_RCCHANNEL_H + +#include +#include "../APM_RC/APM_RC.h" +#include "../AP_Common/AP_Common.h" +#include "../AP_Common/AP_Var.h" + +namespace apo { + +enum rcMode_t { + RC_MODE_IN, RC_MODE_OUT, RC_MODE_INOUT +}; + +/// @class AP_RcChannel +/// @brief Object managing one RC channel +class AP_RcChannel: public AP_Var_group { + +public: + + /// Constructor + AP_RcChannel(AP_Var::Key key, const prog_char_t * name, APM_RC_Class & rc, + const uint8_t & ch, const uint16_t & pwmMin, + const uint16_t & pwmNeutral, const uint16_t & pwmMax, + const rcMode_t & rcMode = RC_MODE_INOUT, + const bool & reverse = false); + + // configuration + AP_Uint8 _ch;AP_Uint16 _pwmMin;AP_Uint16 _pwmNeutral;AP_Uint16 _pwmMax; + rcMode_t _rcMode;AP_Bool _reverse; + + // set + void setUsingRadio(); + void setPwm(uint16_t pwm); + void setPosition(float position); + + // get + uint16_t getPwm() { + return _pwm; + } + uint16_t getRadioPwm(); + float getPosition() { + return _pwmToPosition(_pwm); + } + float getRadioPosition() { + return _pwmToPosition(getRadioPwm()); + } + +private: + + // configuration + APM_RC_Class & _rc; + + // internal states + uint16_t _pwm; // this is the internal state, position is just created when needed + + // private methods + uint16_t _positionToPwm(const float & position); + float _pwmToPosition(const uint16_t & pwm); +}; + +} // apo + +#endif // AP_RCCHANNEL_H diff --git a/libraries/APO/AP_Var_keys.h b/libraries/APO/AP_Var_keys.h new file mode 100644 index 0000000000..58a715b591 --- /dev/null +++ b/libraries/APO/AP_Var_keys.h @@ -0,0 +1,24 @@ +#ifndef AP_Var_keys_H +#define AP_Var_keys_H + +enum keys { + + // general + k_config = 0, + k_cntrl, + k_guide, + k_sensorCalib, + + k_radioChannelsStart=10, + + k_controllersStart=30, + + k_customStart=100, + + // 200-256 reserved for commands + k_commands = 200 +}; + +// max 256 keys + +#endif diff --git a/libraries/APO/CMakeLists.txt b/libraries/APO/CMakeLists.txt new file mode 100644 index 0000000000..ee964b337b --- /dev/null +++ b/libraries/APO/CMakeLists.txt @@ -0,0 +1,39 @@ +set(LIB_NAME APO) + +set(${LIB_NAME}_SRCS + AP_Autopilot.cpp + AP_CommLink.cpp + AP_Controller.cpp + AP_Guide.cpp + AP_HardwareAbstractionLayer.cpp + AP_MavlinkCommand.cpp + AP_Navigator.cpp + AP_RcChannel.cpp + APO.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_Autopilot.h + AP_CommLink.h + AP_Controller.h + AP_Guide.h + AP_HardwareAbstractionLayer.h + AP_MavlinkCommand.h + AP_Navigator.h + AP_RcChannel.h + AP_Var_keys.h + APO.h + constants.h + template.h +) + +include_directories( +# ${CMAKE_SOURCE_DIR}/libraries/AP_Common + ${CMAKE_SOURCE_DIR}/libraries/FastSerial + ${CMAKE_SOURCE_DIR}/libraries/ModeFilter +# + ) + +set(${LIB_NAME}_BOARD ${BOARD}) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/APO/constants.h b/libraries/APO/constants.h new file mode 100644 index 0000000000..2a4316bcb3 --- /dev/null +++ b/libraries/APO/constants.h @@ -0,0 +1,23 @@ +/* + * constants.h + * + * Created on: Apr 7, 2011 + * Author: nkgentry + */ + +#ifndef CONSTANTS_H_ +#define CONSTANTS_H_ + +#include "math.h" + +const float scale_deg = 1e7; // scale of integer degrees/ degree +const float scale_m = 1e3; // scale of integer meters/ meter +const float rEarth = 6371000; // radius of earth, meters +const float rad2Deg = 180 / M_PI; // radians to degrees +const float deg2Rad = M_PI / 180; // degrees to radians +const float rad2DegInt = rad2Deg * scale_deg; // radians to degrees x 1e7 +const float degInt2Rad = deg2Rad / scale_deg; // degrees x 1e7 to radians + +#define MAV_MODE_FAILSAFE MAV_MODE_TEST3 + +#endif /* CONSTANTS_H_ */ diff --git a/libraries/APO/examples/MavlinkTest/Makefile b/libraries/APO/examples/MavlinkTest/Makefile new file mode 100644 index 0000000000..85b4d8856b --- /dev/null +++ b/libraries/APO/examples/MavlinkTest/Makefile @@ -0,0 +1,2 @@ +BOARD = mega +include ../../../AP_Common/Arduino.mk diff --git a/libraries/APO/examples/MavlinkTest/MavlinkTest.pde b/libraries/APO/examples/MavlinkTest/MavlinkTest.pde new file mode 100644 index 0000000000..d64be33822 --- /dev/null +++ b/libraries/APO/examples/MavlinkTest/MavlinkTest.pde @@ -0,0 +1,49 @@ +/* + AnalogReadSerial + Reads an analog input on pin 0, prints the result to the serial monitor + + This example code is in the public domain. + */ + +#include + +int packetDrops = 0; + +void handleMessage(mavlink_message_t * msg) { + Serial.print(", received mavlink message: "); + Serial.print(msg->msgid, DEC); +} + +void setup() { + Serial.begin(57600); + Serial3.begin(57600); + mavlink_comm_0_port = &Serial3; + packetDrops = 0; +} + +void loop() { + mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); + Serial.print("heartbeat sent"); + + // receive new packets + mavlink_message_t msg; + mavlink_status_t status; + + Serial.print(", bytes available: "); + Serial.print(comm_get_available(MAVLINK_COMM_0)); + while (comm_get_available( MAVLINK_COMM_0)) { + uint8_t c = comm_receive_ch(MAVLINK_COMM_0); + + // Try to get a new message + if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) + handleMessage(&msg); + } + + // Update packet drops counter + packetDrops += status.packet_rx_drop_count; + + Serial.print(", dropped packets: "); + Serial.println(packetDrops); + delay(1000); +} diff --git a/libraries/APO/examples/ServoManual/Makefile b/libraries/APO/examples/ServoManual/Makefile new file mode 100644 index 0000000000..85b4d8856b --- /dev/null +++ b/libraries/APO/examples/ServoManual/Makefile @@ -0,0 +1,2 @@ +BOARD = mega +include ../../../AP_Common/Arduino.mk diff --git a/libraries/APO/examples/ServoManual/ServoManual.pde b/libraries/APO/examples/ServoManual/ServoManual.pde new file mode 100644 index 0000000000..8be346f18a --- /dev/null +++ b/libraries/APO/examples/ServoManual/ServoManual.pde @@ -0,0 +1,109 @@ +/* + Example of RC_Channel library. + Code by James Goppert/ Jason Short. 2010 + DIYDrones.com + + */ +#include +#include +#include +#include // ArduPilot Mega RC Library +#include +#include +FastSerialPort0(Serial) +; // make sure this proceeds variable declarations + +// test settings + +using namespace apo; + +class RadioTest { +private: + float testPosition; + int8_t testSign; + enum { + version, + rollKey, + pitchKey, + thrKey, + yawKey, + ch5Key, + ch6Key, + ch7Key, + ch8Key + }; + Vector ch; +public: + RadioTest() : + testPosition(2), testSign(1) { + ch.push_back( + new AP_RcChannel(rollKey, PSTR("ROLL"), APM_RC, 0, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(pitchKey, PSTR("PITCH"), APM_RC, 1, 1100, + 1500, 1900)); + ch.push_back( + new AP_RcChannel(thrKey, PSTR("THR"), APM_RC, 2, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(yawKey, PSTR("YAW"), APM_RC, 3, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch5Key, PSTR("CH5"), APM_RC, 4, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch6Key, PSTR("CH6"), APM_RC, 5, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch7Key, PSTR("CH7"), APM_RC, 6, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch8Key, PSTR("CH8"), APM_RC, 7, 1100, 1500, + 1900)); + + Serial.begin(115200); + delay(2000); + Serial.println("ArduPilot RC Channel test"); + APM_RC.Init(); // APM Radio initialization + delay(2000); + } + + void update() { + // read the radio + for (uint8_t i = 0; i < ch.getSize(); i++) + ch[i]->setUsingRadio(); + + // print channel names + Serial.print("\t\t"); + static char name[7]; + for (uint8_t i = 0; i < ch.getSize(); i++) { + ch[i]->copy_name(name, 7); + Serial.printf("%7s\t", name); + } + Serial.println(); + + // print pwm + Serial.printf("pwm :\t"); + for (uint8_t i = 0; i < ch.getSize(); i++) + Serial.printf("%7d\t", ch[i]->getPwm()); + Serial.println(); + + // print position + Serial.printf("position :\t"); + for (uint8_t i = 0; i < ch.getSize(); i++) + Serial.printf("%7.2f\t", ch[i]->getPosition()); + Serial.println(); + + delay(500); + } +}; + +RadioTest * test; + +void setup() { + test = new RadioTest; +} + +void loop() { + test->update(); +} diff --git a/libraries/APO/examples/ServoSweep/Makefile b/libraries/APO/examples/ServoSweep/Makefile new file mode 100644 index 0000000000..85b4d8856b --- /dev/null +++ b/libraries/APO/examples/ServoSweep/Makefile @@ -0,0 +1,2 @@ +BOARD = mega +include ../../../AP_Common/Arduino.mk diff --git a/libraries/APO/examples/ServoSweep/ServoSweep.pde b/libraries/APO/examples/ServoSweep/ServoSweep.pde new file mode 100644 index 0000000000..75827d4f09 --- /dev/null +++ b/libraries/APO/examples/ServoSweep/ServoSweep.pde @@ -0,0 +1,125 @@ +/* + Example of RC_Channel library. + Code by James Goppert/ Jason Short. 2010 + DIYDrones.com + + */ +#include +#include +#include +#include // ArduPilot Mega RC Library +#include +#include +FastSerialPort0(Serial) +; // make sure this proceeds variable declarations + +// test settings + +using namespace apo; + +class RadioTest { +private: + float testPosition; + int8_t testSign; + enum { + version, + rollKey, + pitchKey, + thrKey, + yawKey, + ch5Key, + ch6Key, + ch7Key, + ch8Key + }; + Vector ch; +public: + RadioTest() : + testPosition(2), testSign(1) { + ch.push_back( + new AP_RcChannel(rollKey, PSTR("ROLL"), APM_RC, 0, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(pitchKey, PSTR("PITCH"), APM_RC, 1, 1100, + 1500, 1900)); + ch.push_back( + new AP_RcChannel(thrKey, PSTR("THR"), APM_RC, 2, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(yawKey, PSTR("YAW"), APM_RC, 3, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch5Key, PSTR("CH5"), APM_RC, 4, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch6Key, PSTR("CH6"), APM_RC, 5, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch7Key, PSTR("CH7"), APM_RC, 6, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch8Key, PSTR("CH8"), APM_RC, 7, 1100, 1500, + 1900)); + + Serial.begin(115200); + delay(2000); + Serial.println("ArduPilot RC Channel test"); + APM_RC.Init(); // APM Radio initialization + delay(2000); + } + + void update() { + // update test value + testPosition += testSign * .1; + if (testPosition > 1) { + //eepromRegistry.print(Serial); // show eeprom map + testPosition = 1; + testSign = -1; + } else if (testPosition < -1) { + testPosition = -1; + testSign = 1; + } + + // set channel positions + for (uint8_t i = 0; i < ch.getSize(); i++) + ch[i]->setPosition(testPosition); + + // print test position + Serial.printf("\nnormalized position (%f)\n", testPosition); + + // print channel names + Serial.print("\t\t"); + static char name[7]; + for (uint8_t i = 0; i < ch.getSize(); i++) { + ch[i]->copy_name(name, 7); + Serial.printf("%7s\t", name); + } + Serial.println(); + + // print pwm + Serial.printf("pwm :\t"); + for (uint8_t i = 0; i < ch.getSize(); i++) + Serial.printf("%7d\t", ch[i]->getRadioPwm()); + Serial.println(); + + // print position + Serial.printf("position :\t"); + for (uint8_t i = 0; i < ch.getSize(); i++) + Serial.printf("%7.2f\t", ch[i]->getRadioPosition()); + Serial.println(); + + delay(500); + } +}; + +RadioTest * test; + +void setup() { + test = new RadioTest; +} + +void loop() { + test->update(); +} + +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/template.h b/libraries/APO/template.h new file mode 100644 index 0000000000..4bed52d485 --- /dev/null +++ b/libraries/APO/template.h @@ -0,0 +1,32 @@ +/* + * Class.h + * Copyright (C) Author 2011 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#ifndef Class_H +#define Class_H + +namespace apo { + +/// Class desciprtion +class Class { +public: +} + +} // namespace apo + +#endif // Class_H +// vim:ts=4:sw=4:expandtab diff --git a/libraries/AP_Common/AP_Var.cpp b/libraries/AP_Common/AP_Var.cpp index 956f527a3a..474dcb8541 100644 --- a/libraries/AP_Common/AP_Var.cpp +++ b/libraries/AP_Common/AP_Var.cpp @@ -9,19 +9,21 @@ /// @file AP_Var.cpp /// @brief The AP variable store. -#if 0 -# include -extern "C" { extern void delay(unsigned long); } -# define debug(fmt, args...) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args); delay(100); } while(0) -#else -# define debug(fmt, args...) -#endif #include #include #include +//#define ENABLE_FASTSERIAL_DEBUG + +#ifdef ENABLE_FASTSERIAL_DEBUG +# include +# define serialDebug(fmt, args...) if (FastSerial::getInitialized(0)) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args); delay(0); } while(0) +#else +# define serialDebug(fmt, args...) +#endif + // Global constants exported for general use. // AP_Float AP_Float_unity ( 1.0, AP_Var::k_key_none, NULL, AP_Var::k_flag_unlisted); @@ -71,7 +73,11 @@ AP_Var::AP_Var(AP_Var_group *pGroup, Key index, const prog_char_t *name, Flags f // to the pointer to the node that we are considering inserting in front of. // vp = &_grouped_variables; + size_t loopCount = 0; while (*vp != NULL) { + + if (loopCount++>k_num_max) return; + if ((*vp)->_key >= _key) { break; } @@ -98,12 +104,19 @@ AP_Var::~AP_Var(void) } // Scan the list and remove this if we find it - while (*vp) { - if (*vp == this) { - *vp = _link; - break; - } - vp = &((*vp)->_link); + + { + size_t loopCount = 0; + while (*vp) { + + if (loopCount++>k_num_max) return; + + if (*vp == this) { + *vp = _link; + break; + } + vp = &((*vp)->_link); + } } // If we are destroying a group, remove all its variables from the list @@ -112,8 +125,12 @@ AP_Var::~AP_Var(void) // Scan the list and remove any variable that has this as its group vp = &_grouped_variables; + size_t loopCount = 0; + while (*vp) { + if (loopCount++>k_num_max) return; + // Does the variable claim us as its group? if ((*vp)->_group == this) { *vp = (*vp)->_link; @@ -145,7 +162,12 @@ AP_Var::find(const char *name) { AP_Var *vp; + size_t loopCount = 0; + for (vp = first(); vp; vp = vp->next()) { + + if (loopCount++>k_num_max) return NULL; + char name_buffer[32]; // copy the variable's name into our scratch buffer @@ -165,9 +187,9 @@ AP_Var * AP_Var::find(Key key) { AP_Var *vp; - + size_t loopCount = 0; for (vp = first(); vp; vp = vp->next()) { - + if (loopCount++>k_num_max) return NULL; if (key == vp->key()) { return vp; } @@ -188,11 +210,11 @@ bool AP_Var::save(void) return _group->save(); } - debug("save: %S", _name ? _name : PSTR("??")); + serialDebug("save: %S", _name ? _name : PSTR("??")); // locate the variable in EEPROM, allocating space as required if (!_EEPROM_locate(true)) { - debug("locate failed"); + serialDebug("locate failed"); return false; } @@ -200,13 +222,13 @@ bool AP_Var::save(void) size = serialize(vbuf, sizeof(vbuf)); if (0 == size) { // variable cannot be serialised into the buffer - debug("cannot save (too big or not supported)"); + serialDebug("cannot save (too big or not supported)"); return false; } // if it fit in the buffer, save it to EEPROM if (size <= sizeof(vbuf)) { - debug("saving %u to %u", size, _key); + serialDebug("saving %u to %u", size, _key); // XXX this should use eeprom_update_block if/when Arduino moves to // avr-libc >= 1.7 uint8_t *ep = (uint8_t *)_key; @@ -219,7 +241,7 @@ bool AP_Var::save(void) // now read it back newv = eeprom_read_byte(ep); if (newv != vbuf[i]) { - debug("readback failed at offset %p: got %u, expected %u", + serialDebug("readback failed at offset %p: got %u, expected %u", ep, newv, vbuf[i]); return false; } @@ -241,11 +263,11 @@ bool AP_Var::load(void) return _group->load(); } - debug("load: %S", _name ? _name : PSTR("??")); + serialDebug("load: %S", _name ? _name : PSTR("??")); // locate the variable in EEPROM, but do not allocate space if (!_EEPROM_locate(false)) { - debug("locate failed"); + serialDebug("locate failed"); return false; } @@ -255,7 +277,7 @@ bool AP_Var::load(void) // size = serialize(NULL, 0); if (0 == size) { - debug("cannot load (too big or not supported)"); + serialDebug("cannot load (too big or not supported)"); return false; } @@ -263,7 +285,7 @@ bool AP_Var::load(void) // has converted _key into an EEPROM address. // if (size <= sizeof(vbuf)) { - debug("loading %u from %u", size, _key); + serialDebug("loading %u from %u", size, _key); eeprom_read_block(vbuf, (void *)_key, size); return unserialize(vbuf, size); } @@ -278,7 +300,12 @@ bool AP_Var::save_all(void) bool result = true; AP_Var *vp = _variables; + size_t loopCount = 0; + while (vp) { + + if (loopCount++>k_num_max) return false; + if (!vp->has_flags(k_flag_no_auto_load) && // not opted out of autosave (vp->_key != k_key_none)) { // has a key @@ -298,7 +325,12 @@ bool AP_Var::load_all(void) bool result = true; AP_Var *vp = _variables; + size_t loopCount = 0; + while (vp) { + + if (loopCount++>k_num_max) return false; + if (!vp->has_flags(k_flag_no_auto_load) && // not opted out of autoload (vp->_key != k_key_none)) { // has a key @@ -322,13 +354,19 @@ AP_Var::erase_all() AP_Var *vp; uint16_t i; - debug("erase EEPROM"); + serialDebug("erase EEPROM"); // Scan the list of variables/groups, fetching their key values and // reverting them to their not-located state. // vp = _variables; + + size_t loopCount = 0; + while (vp) { + + if (loopCount++>k_num_max) return; + vp->_key = vp->key() | k_key_not_located; vp = vp->_link; } @@ -405,9 +443,15 @@ AP_Var::first_member(AP_Var_group *group) vp = &_grouped_variables; - debug("seeking %p", group); + serialDebug("seeking %p", group); + + size_t loopCount = 0; + while (*vp) { - debug("consider %p with %p", *vp, (*vp)->_group); + + if (loopCount++>k_num_max) return NULL; + + serialDebug("consider %p with %p", *vp, (*vp)->_group); if ((*vp)->_group == group) { return *vp; } @@ -423,7 +467,12 @@ AP_Var::next_member() AP_Var *vp; vp = _link; + size_t loopCount = 0; + while (vp) { + + if (loopCount++>k_num_max) return NULL; + if (vp->_group == _group) { return vp; } @@ -451,7 +500,7 @@ bool AP_Var::_EEPROM_scan(void) if ((ee_header.magic != k_EEPROM_magic) || (ee_header.revision != k_EEPROM_revision)) { - debug("no header, magic 0x%x revision %u", ee_header.magic, ee_header.revision); + serialDebug("no header, magic 0x%x revision %u", ee_header.magic, ee_header.revision); return false; } @@ -460,17 +509,22 @@ bool AP_Var::_EEPROM_scan(void) // Avoid trying to read a header when there isn't enough space left. // eeprom_address = sizeof(ee_header); + + size_t loopCount = 0; + while (eeprom_address < (k_EEPROM_size - sizeof(var_header) - 1)) { + if (loopCount++>k_num_max) return NULL; + // Read a variable header // - debug("reading header from %u", eeprom_address); + serialDebug("reading header from %u", eeprom_address); eeprom_read_block(&var_header, (void *)eeprom_address, sizeof(var_header)); // If the header is for the sentinel, scanning is complete // if (var_header.key == k_key_sentinel) { - debug("found tail sentinel"); + serialDebug("found tail sentinel"); break; } @@ -482,23 +536,25 @@ bool AP_Var::_EEPROM_scan(void) var_header.size + 1 + // data for this variable sizeof(var_header))) { // header for sentinel - debug("header overruns EEPROM"); + serialDebug("header overruns EEPROM"); return false; } // look for a variable with this key vp = _variables; - while (vp) { + size_t loopCount2 = 0; + while(vp) { + if (loopCount2++>k_num_max) return false; if (vp->key() == var_header.key) { // adjust the variable's key to point to this entry vp->_key = eeprom_address + sizeof(var_header); - debug("update %p with key %u -> %u", vp, var_header.key, vp->_key); + serialDebug("update %p with key %u -> %u", vp, var_header.key, vp->_key); break; } vp = vp->_link; } if (!vp) { - debug("key %u not claimed (already scanned or unknown)", var_header.key); + serialDebug("key %u not claimed (already scanned or unknown)", var_header.key); } // move to the next variable header @@ -514,16 +570,18 @@ bool AP_Var::_EEPROM_scan(void) // mark all the rest as not allocated. // vp = _variables; - while (vp) { + size_t loopCount3 = 0; + while(vp) { + if (loopCount3++>k_num_max) return false; if (vp->_key & k_key_not_located) { vp->_key |= k_key_not_allocated; - debug("key %u not allocated", vp->key()); + serialDebug("key %u not allocated", vp->key()); } vp = vp->_link; } // Scanning is complete - debug("scan done"); + serialDebug("scan done"); _tail_sentinel = eeprom_address; return true; } @@ -539,7 +597,7 @@ bool AP_Var::_EEPROM_locate(bool allocate) // Is it a group member, or does it have a no-location key? // if (_group || (_key == k_key_none)) { - debug("not addressable"); + serialDebug("not addressable"); return false; // it is/does, and thus it has no location } @@ -554,7 +612,7 @@ bool AP_Var::_EEPROM_locate(bool allocate) // try scanning to see if we can locate it. // if (!(_key & k_key_not_allocated)) { - debug("need scan"); + serialDebug("need scan"); _EEPROM_scan(); // Has the variable now been located? @@ -569,7 +627,7 @@ bool AP_Var::_EEPROM_locate(bool allocate) if (!allocate) { return false; } - debug("needs allocation"); + serialDebug("needs allocation"); // Ask the serializer for the size of the thing we are allocating, and fail // if it is too large or if it has no size, as we will not be able to allocate @@ -577,7 +635,7 @@ bool AP_Var::_EEPROM_locate(bool allocate) // size = serialize(NULL, 0); if ((size == 0) || (size > k_size_max)) { - debug("size %u out of bounds", size); + serialDebug("size %u out of bounds", size); return false; } @@ -585,7 +643,7 @@ bool AP_Var::_EEPROM_locate(bool allocate) // header and the new tail sentinel. // if ((_tail_sentinel + size + sizeof(Var_header) * 2) > k_EEPROM_size) { - debug("no space in EEPROM"); + serialDebug("no space in EEPROM"); return false; } @@ -595,7 +653,7 @@ bool AP_Var::_EEPROM_locate(bool allocate) if (0 == _tail_sentinel) { uint8_t pad_size; - debug("writing header"); + serialDebug("writing header"); EEPROM_header ee_header; ee_header.magic = k_EEPROM_magic; @@ -621,7 +679,7 @@ bool AP_Var::_EEPROM_locate(bool allocate) // new_location = _tail_sentinel; _tail_sentinel += sizeof(var_header) + size; - debug("allocated %u/%u for key %u new sentinel %u", new_location, size, key(), _tail_sentinel); + serialDebug("allocated %u/%u for key %u new sentinel %u", new_location, size, key(), _tail_sentinel); // Write the new sentinel first. If we are interrupted during this operation // the old sentinel will still correctly terminate the EEPROM image. @@ -670,17 +728,22 @@ AP_Var_group::_serialize_unserialize(void *buf, size_t buf_size, bool do_seriali // Traverse the list of group members, serializing each in order // vp = first_member(this); - debug("starting with %p", vp); + serialDebug("starting with %p", vp); total_size = 0; + + size_t loopCount = 0; + while (vp) { + if (loopCount++>k_num_max) return false; + // (un)serialise the group member if (do_serialize) { size = vp->serialize(buf, buf_size); - debug("serialize %p -> %u", vp, size); + serialDebug("serialize %p -> %u", vp, size); } else { size = vp->unserialize(buf, buf_size); - debug("unserialize %p -> %u", vp, size); + serialDebug("unserialize %p -> %u", vp, size); } // Unserialize will return zero if the buffer is too small @@ -688,7 +751,7 @@ AP_Var_group::_serialize_unserialize(void *buf, size_t buf_size, bool do_seriali // Either case is fatal for any operation we might be trying. // if (0 == size) { - debug("group (un)serialize failed, buffer too small or not supported"); + serialDebug("group (un)serialize failed, buffer too small or not supported"); return 0; } @@ -704,7 +767,7 @@ AP_Var_group::_serialize_unserialize(void *buf, size_t buf_size, bool do_seriali // and the calling function will have to treat it as an error. // total_size += size; - debug("used %u", total_size); + serialDebug("used %u", total_size); if (size <= buf_size) { // there was space for this one, account for it buf_size -= size; diff --git a/libraries/AP_Common/AP_Var.h b/libraries/AP_Common/AP_Var.h index 6ed680d7ab..4fd5dc1d49 100644 --- a/libraries/AP_Common/AP_Var.h +++ b/libraries/AP_Common/AP_Var.h @@ -128,6 +128,10 @@ public: /// static const size_t k_size_max = 64; + /// The maximum number of keys + /// + static const size_t k_num_max = 255; + /// Optional flags affecting the behavior and usage of the variable. /// typedef uint8_t Flags; diff --git a/libraries/AP_Common/CMakeLists.txt b/libraries/AP_Common/CMakeLists.txt index 7d89ba1e40..d86ce08985 100644 --- a/libraries/AP_Common/CMakeLists.txt +++ b/libraries/AP_Common/CMakeLists.txt @@ -28,6 +28,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/FastSerial/CMakeLists.txt b/libraries/FastSerial/CMakeLists.txt index 78e94e9023..1ace8cb81c 100644 --- a/libraries/FastSerial/CMakeLists.txt +++ b/libraries/FastSerial/CMakeLists.txt @@ -23,6 +23,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/FastSerial/FastSerial.cpp b/libraries/FastSerial/FastSerial.cpp index 5ee8ad3ee8..5312f3d6bf 100644 --- a/libraries/FastSerial/FastSerial.cpp +++ b/libraries/FastSerial/FastSerial.cpp @@ -45,6 +45,7 @@ FastSerial::Buffer __FastSerial__rxBuffer[FS_MAX_PORTS]; FastSerial::Buffer __FastSerial__txBuffer[FS_MAX_PORTS]; +uint8_t FastSerial::_serialInitialized = 0; // Constructor ///////////////////////////////////////////////////////////////// @@ -61,6 +62,8 @@ FastSerial::FastSerial(const uint8_t portNumber, volatile uint8_t *ubrrh, volati _rxBuffer(&__FastSerial__rxBuffer[portNumber]), _txBuffer(&__FastSerial__txBuffer[portNumber]) { + setInitialized(portNumber); + begin(57600); } // Public Methods ////////////////////////////////////////////////////////////// diff --git a/libraries/FastSerial/FastSerial.h b/libraries/FastSerial/FastSerial.h index 920554555f..bc0417a7c0 100644 --- a/libraries/FastSerial/FastSerial.h +++ b/libraries/FastSerial/FastSerial.h @@ -55,6 +55,7 @@ #include "BetterStream.h" + /// @file FastSerial.h /// @brief An enhanced version of the Arduino HardwareSerial class /// implementing interrupt-driven transmission and flexible @@ -101,6 +102,7 @@ extern class FastSerial Serial3; /// class FastSerial: public BetterStream { public: + /// Constructor FastSerial(const uint8_t portNumber, volatile uint8_t *ubrrh, volatile uint8_t *ubrrl, volatile uint8_t *ucsra, volatile uint8_t *ucsrb, const uint8_t u2x, const uint8_t portEnableBits, const uint8_t portTxBits); @@ -149,7 +151,21 @@ public: uint8_t *bytes; ///< pointer to allocated buffer }; + /// Tell if the serial port has been initialized + static bool getInitialized(uint8_t port) { + return (1< Date: Thu, 29 Sep 2011 14:12:15 -0400 Subject: [PATCH 2/7] Added ArduBoat/ ArduRover/ and APO library. --- ArduBoat/BoatGeneric.h | 68 ++ ArduBoat/CMakeLists.txt | 80 ++ ArduBoat/ControllerBoat.h | 125 +++ ArduCopter/CMakeLists.txt | 2 +- ArduPlane/CMakeLists.txt | 2 +- ArduRover/CMakeLists.txt | 80 ++ ArduRover/CarStampede.h | 69 ++ ArduRover/ControllerCar.h | 125 +++ ArduRover/ControllerTank.h | 133 +++ ArduRover/TankGeneric.h | 68 ++ apo/CMakeLists.txt | 80 ++ apo/ControllerPlane.h | 215 +++++ apo/ControllerQuad.h | 250 ++++++ apo/Makefile | 1 + apo/PlaneEasystar.h | 113 +++ apo/QuadArducopter.h | 98 +++ apo/QuadMikrokopter.h | 98 +++ apo/apo.pde | 184 ++++ libraries/APM_BMP085/CMakeLists.txt | 2 +- libraries/APM_PI/CMakeLists.txt | 2 +- libraries/APM_RC/CMakeLists.txt | 2 +- libraries/APO/APO.cpp | 8 + libraries/APO/APO.h | 20 + libraries/APO/AP_Autopilot.cpp | 246 ++++++ libraries/APO/AP_Autopilot.h | 165 ++++ libraries/APO/AP_CommLink.cpp | 15 + libraries/APO/AP_CommLink.h | 790 ++++++++++++++++++ libraries/APO/AP_Controller.cpp | 8 + libraries/APO/AP_Controller.h | 304 +++++++ libraries/APO/AP_Guide.cpp | 8 + libraries/APO/AP_Guide.h | 359 ++++++++ libraries/APO/AP_HardwareAbstractionLayer.cpp | 8 + libraries/APO/AP_HardwareAbstractionLayer.h | 166 ++++ libraries/APO/AP_MavlinkCommand.cpp | 175 ++++ libraries/APO/AP_MavlinkCommand.h | 359 ++++++++ libraries/APO/AP_Navigator.cpp | 8 + libraries/APO/AP_Navigator.h | 391 +++++++++ libraries/APO/AP_RcChannel.cpp | 110 +++ libraries/APO/AP_RcChannel.h | 69 ++ libraries/APO/AP_Var_keys.h | 24 + libraries/APO/CMakeLists.txt | 39 + libraries/APO/constants.h | 23 + libraries/APO/examples/MavlinkTest/Makefile | 2 + .../APO/examples/MavlinkTest/MavlinkTest.pde | 49 ++ libraries/APO/examples/ServoManual/Makefile | 2 + .../APO/examples/ServoManual/ServoManual.pde | 109 +++ libraries/APO/examples/ServoSweep/Makefile | 2 + .../APO/examples/ServoSweep/ServoSweep.pde | 125 +++ libraries/APO/template.h | 32 + libraries/AP_ADC/CMakeLists.txt | 2 +- libraries/AP_Common/AP_Var.cpp | 161 ++-- libraries/AP_Common/AP_Var.h | 4 + libraries/AP_Common/CMakeLists.txt | 2 +- libraries/AP_Compass/CMakeLists.txt | 2 +- libraries/AP_DCM/CMakeLists.txt | 2 +- libraries/AP_GPS/CMakeLists.txt | 2 +- libraries/AP_IMU/CMakeLists.txt | 2 +- libraries/AP_Math/CMakeLists.txt | 2 +- libraries/AP_OpticalFlow/CMakeLists.txt | 2 +- libraries/AP_PID/CMakeLists.txt | 2 +- libraries/AP_RangeFinder/CMakeLists.txt | 2 +- libraries/DataFlash/CMakeLists.txt | 2 +- libraries/FastSerial/CMakeLists.txt | 2 +- libraries/FastSerial/FastSerial.cpp | 3 + libraries/FastSerial/FastSerial.h | 16 + libraries/GCS_MAVLink/CMakeLists.txt | 2 +- libraries/ModeFilter/CMakeLists.txt | 2 +- libraries/PID/CMakeLists.txt | 2 +- libraries/RC_Channel/CMakeLists.txt | 2 +- libraries/memcheck/CMakeLists.txt | 2 +- 70 files changed, 5560 insertions(+), 71 deletions(-) create mode 100644 ArduBoat/BoatGeneric.h create mode 100644 ArduBoat/CMakeLists.txt create mode 100644 ArduBoat/ControllerBoat.h create mode 100644 ArduRover/CMakeLists.txt create mode 100644 ArduRover/CarStampede.h create mode 100644 ArduRover/ControllerCar.h create mode 100644 ArduRover/ControllerTank.h create mode 100644 ArduRover/TankGeneric.h create mode 100644 apo/CMakeLists.txt create mode 100644 apo/ControllerPlane.h create mode 100644 apo/ControllerQuad.h create mode 100644 apo/Makefile create mode 100644 apo/PlaneEasystar.h create mode 100644 apo/QuadArducopter.h create mode 100644 apo/QuadMikrokopter.h create mode 100644 apo/apo.pde create mode 100644 libraries/APO/APO.cpp create mode 100644 libraries/APO/APO.h create mode 100644 libraries/APO/AP_Autopilot.cpp create mode 100644 libraries/APO/AP_Autopilot.h create mode 100644 libraries/APO/AP_CommLink.cpp create mode 100644 libraries/APO/AP_CommLink.h create mode 100644 libraries/APO/AP_Controller.cpp create mode 100644 libraries/APO/AP_Controller.h create mode 100644 libraries/APO/AP_Guide.cpp create mode 100644 libraries/APO/AP_Guide.h create mode 100644 libraries/APO/AP_HardwareAbstractionLayer.cpp create mode 100644 libraries/APO/AP_HardwareAbstractionLayer.h create mode 100644 libraries/APO/AP_MavlinkCommand.cpp create mode 100644 libraries/APO/AP_MavlinkCommand.h create mode 100644 libraries/APO/AP_Navigator.cpp create mode 100644 libraries/APO/AP_Navigator.h create mode 100644 libraries/APO/AP_RcChannel.cpp create mode 100644 libraries/APO/AP_RcChannel.h create mode 100644 libraries/APO/AP_Var_keys.h create mode 100644 libraries/APO/CMakeLists.txt create mode 100644 libraries/APO/constants.h create mode 100644 libraries/APO/examples/MavlinkTest/Makefile create mode 100644 libraries/APO/examples/MavlinkTest/MavlinkTest.pde create mode 100644 libraries/APO/examples/ServoManual/Makefile create mode 100644 libraries/APO/examples/ServoManual/ServoManual.pde create mode 100644 libraries/APO/examples/ServoSweep/Makefile create mode 100644 libraries/APO/examples/ServoSweep/ServoSweep.pde create mode 100644 libraries/APO/template.h diff --git a/ArduBoat/BoatGeneric.h b/ArduBoat/BoatGeneric.h new file mode 100644 index 0000000000..5e47672847 --- /dev/null +++ b/ArduBoat/BoatGeneric.h @@ -0,0 +1,68 @@ +/* + * BoatGeneric.h + * + * Created on: May 1, 2011 + * Author: jgoppert + */ + +#ifndef BOATGENERIC_H_ +#define BOATGENERIC_H_ + +// vehicle options +static const apo::vehicle_t vehicle = apo::VEHICLE_BOAT; +static const apo::halMode_t halMode = apo::MODE_LIVE; +static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560; +static const uint8_t heartBeatTimeout = 3; + +// algorithm selection +#define CONTROLLER_CLASS ControllerBoat +#define GUIDE_CLASS MavlinkGuide +#define NAVIGATOR_CLASS DcmNavigator +#define COMMLINK_CLASS MavlinkComm + +// hardware selection +#define ADC_CLASS AP_ADC_ADS7844 +#define COMPASS_CLASS AP_Compass_HMC5843 +#define BARO_CLASS APM_BMP085_Class +#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL +#define DEBUG_BAUD 57600 +#define TELEM_BAUD 57600 +#define GPS_BAUD 38400 +#define HIL_BAUD 57600 + +// optional sensors +static bool gpsEnabled = false; +static bool baroEnabled = true; +static bool compassEnabled = true; + +static bool rangeFinderFrontEnabled = true; +static bool rangeFinderBackEnabled = true; +static bool rangeFinderLeftEnabled = true; +static bool rangeFinderRightEnabled = true; +static bool rangeFinderUpEnabled = true; +static bool rangeFinderDownEnabled = true; + +// 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 loop4Rate = 0.1; + +// gains +const float steeringP = 1.0; +const float steeringI = 0.0; +const float steeringD = 0.0; +const float steeringIMax = 0.0; +const float steeringYMax = 3.0; + +const float throttleP = 0.0; +const float throttleI = 0.0; +const float throttleD = 0.0; +const float throttleIMax = 0.0; +const float throttleYMax = 0.0; +const float throttleDFCut = 3.0; + +#include "ControllerBoat.h" + +#endif /* BOATGENERIC_H_ */ diff --git a/ArduBoat/CMakeLists.txt b/ArduBoat/CMakeLists.txt new file mode 100644 index 0000000000..95c4c1289c --- /dev/null +++ b/ArduBoat/CMakeLists.txt @@ -0,0 +1,80 @@ +#=============================================================================# +# Author: Sebastian Rohde # +# Date: 30.08.2011 # +#=============================================================================# + + +#====================================================================# +# Settings # +#====================================================================# +set(FIRMWARE_NAME ardupilotone) + +set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board + +set(${FIRMWARE_NAME}_SKETCHES + ardupilotone.pde + ) # Firmware sketches + +set(${FIRMWARE_NAME}_SRCS + ) # Firmware sources + +set(${FIRMWARE_NAME}_HDRS + BoatGeneric.h + CarStampede.h + #CNIRover.h + #CNIBoat.h + ControllerBoat.h + ControllerCar.h + ControllerPlane.h + ControllerQuad.h + PlaneEasystar.h + QuadArducopter.h + QuadMikrokopter.h + ) # Firmware sources + +set(${FIRMWARE_NAME}_LIBS + m + APO + FastSerial + AP_Common + GCS_MAVLink + AP_GPS + APM_RC + AP_DCM + AP_ADC + AP_Compass + AP_IMU + AP_RangeFinder + APM_BMP085 + ModeFilter +) + + +#${CONSOLE_PORT} +set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port +set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd + +include_directories( +${CMAKE_SOURCE_DIR}/libraries/APO +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +) +#====================================================================# +# Target generation # +#====================================================================# +generate_arduino_firmware(${FIRMWARE_NAME}) + +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/${FIRMWARE_NAME}.hex + DESTINATION bin + ) diff --git a/ArduBoat/ControllerBoat.h b/ArduBoat/ControllerBoat.h new file mode 100644 index 0000000000..e25560085c --- /dev/null +++ b/ArduBoat/ControllerBoat.h @@ -0,0 +1,125 @@ +/* + * ControllerBoat.h + * + * Created on: Jun 30, 2011 + * Author: jgoppert + */ + +#ifndef CONTROLLERBOAT_H_ +#define CONTROLLERBOAT_H_ + +#include "../APO/AP_Controller.h" + +namespace apo { + +class ControllerBoat: public AP_Controller { +private: + AP_Var_group _group; + AP_Uint8 _mode; + enum { + k_chMode = k_radioChannelsStart, k_chStr, k_chThr + }; + enum { + k_pidStr = k_controllersStart, k_pidThr + }; + enum { + CH_MODE = 0, CH_STR, CH_THR + }; + BlockPIDDfb pidStr; + BlockPID pidThr; +public: + ControllerBoat(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + AP_Controller(nav, guide, hal), + _group(k_cntrl, PSTR("CNTRL_")), + _mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")), + pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, + steeringI, steeringD, steeringIMax, steeringYMax), + pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP, + throttleI, throttleD, throttleIMax, throttleYMax, + throttleDFCut) { + _hal->debug->println_P(PSTR("initializing boat controller")); + + _hal->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7, 1100, + 1500, 1900)); + _hal->rc.push_back( + new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1540, + 1900)); + _hal->rc.push_back( + new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500, + 1900)); + } + virtual MAV_MODE getMode() { + return (MAV_MODE) _mode.get(); + } + virtual void update(const float & dt) { + + // check for heartbeat + if (_hal->heartBeatLost()) { + _mode = MAV_MODE_FAILSAFE; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n")); + return; + // if throttle less than 5% cut motor power + } else if (_hal->rc[CH_THR]->getRadioPosition() < 0.05) { + _mode = MAV_MODE_LOCKED; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_STANDBY); + return; + // if in live mode then set state to active + } else if (_hal->getMode() == MODE_LIVE) { + _hal->setState(MAV_STATE_ACTIVE); + // if in hardware in the loop (control) mode, set to hilsim + } else if (_hal->getMode() == MODE_HIL_CNTL) { + _hal->setState(MAV_STATE_HILSIM); + } + + // read switch to set mode + if (_hal->rc[CH_MODE]->getRadioPosition() > 0) { + _mode = MAV_MODE_MANUAL; + } else { + _mode = MAV_MODE_AUTO; + } + + // manual mode + switch (_mode) { + + case MAV_MODE_MANUAL: { + setAllRadioChannelsManually(); + //_hal->debug->println("manual"); + break; + } + case MAV_MODE_AUTO: { + float headingError = _guide->getHeadingCommand() + - _nav->getYaw(); + if (headingError > 180 * deg2Rad) + headingError -= 360 * deg2Rad; + if (headingError < -180 * deg2Rad) + headingError += 360 * deg2Rad; + _hal->rc[CH_STR]->setPosition( + pidStr.update(headingError, _nav->getYawRate(), dt)); + _hal->rc[CH_THR]->setPosition( + pidThr.update( + _guide->getGroundSpeedCommand() + - _nav->getGroundSpeed(), dt)); + //_hal->debug->println("automode"); + break; + } + + default: { + setAllRadioChannelsToNeutral(); + _mode = MAV_MODE_FAILSAFE; + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("unknown controller mode\n")); + break; + } + + } + } +}; + +} // namespace apo + +#endif /* CONTROLLERBOAT_H_ */ diff --git a/ArduCopter/CMakeLists.txt b/ArduCopter/CMakeLists.txt index 4e282df19b..f13af158d8 100644 --- a/ArduCopter/CMakeLists.txt +++ b/ArduCopter/CMakeLists.txt @@ -42,7 +42,7 @@ PRINT_BOARD_SETTINGS(mega2560) #====================================================================# set(FIRMWARE_NAME arducopter) -set(${FIRMWARE_NAME}_BOARD mega2560) # Arduino Target board +set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board set(${FIRMWARE_NAME}_SKETCHES ArduCopter.pde diff --git a/ArduPlane/CMakeLists.txt b/ArduPlane/CMakeLists.txt index d2c7506d88..19a3b35a50 100644 --- a/ArduPlane/CMakeLists.txt +++ b/ArduPlane/CMakeLists.txt @@ -42,7 +42,7 @@ PRINT_BOARD_SETTINGS(mega2560) #====================================================================# set(FIRMWARE_NAME ArduPlane) -set(${FIRMWARE_NAME}_BOARD mega2560) # Arduino Target board +set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board set(${FIRMWARE_NAME}_SKETCHES ArduPlane.pde diff --git a/ArduRover/CMakeLists.txt b/ArduRover/CMakeLists.txt new file mode 100644 index 0000000000..95c4c1289c --- /dev/null +++ b/ArduRover/CMakeLists.txt @@ -0,0 +1,80 @@ +#=============================================================================# +# Author: Sebastian Rohde # +# Date: 30.08.2011 # +#=============================================================================# + + +#====================================================================# +# Settings # +#====================================================================# +set(FIRMWARE_NAME ardupilotone) + +set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board + +set(${FIRMWARE_NAME}_SKETCHES + ardupilotone.pde + ) # Firmware sketches + +set(${FIRMWARE_NAME}_SRCS + ) # Firmware sources + +set(${FIRMWARE_NAME}_HDRS + BoatGeneric.h + CarStampede.h + #CNIRover.h + #CNIBoat.h + ControllerBoat.h + ControllerCar.h + ControllerPlane.h + ControllerQuad.h + PlaneEasystar.h + QuadArducopter.h + QuadMikrokopter.h + ) # Firmware sources + +set(${FIRMWARE_NAME}_LIBS + m + APO + FastSerial + AP_Common + GCS_MAVLink + AP_GPS + APM_RC + AP_DCM + AP_ADC + AP_Compass + AP_IMU + AP_RangeFinder + APM_BMP085 + ModeFilter +) + + +#${CONSOLE_PORT} +set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port +set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd + +include_directories( +${CMAKE_SOURCE_DIR}/libraries/APO +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +) +#====================================================================# +# Target generation # +#====================================================================# +generate_arduino_firmware(${FIRMWARE_NAME}) + +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/${FIRMWARE_NAME}.hex + DESTINATION bin + ) diff --git a/ArduRover/CarStampede.h b/ArduRover/CarStampede.h new file mode 100644 index 0000000000..3ed8a0b2b1 --- /dev/null +++ b/ArduRover/CarStampede.h @@ -0,0 +1,69 @@ +/* + * CarStampede.h + * + * Created on: May 1, 2011 + * Author: jgoppert + * + */ + +#ifndef CARSTAMPEDE_H_ +#define CARSTAMPEDE_H_ + +// vehicle options +static const apo::vehicle_t vehicle = apo::VEHICLE_CAR; +static const apo::halMode_t halMode = apo::MODE_LIVE; +static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560; +static const uint8_t heartBeatTimeout = 3; + +// algorithm selection +#define CONTROLLER_CLASS ControllerCar +#define GUIDE_CLASS MavlinkGuide +#define NAVIGATOR_CLASS DcmNavigator +#define COMMLINK_CLASS MavlinkComm + +// hardware selection +#define ADC_CLASS AP_ADC_ADS7844 +#define COMPASS_CLASS AP_Compass_HMC5843 +#define BARO_CLASS APM_BMP085_Class +#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL +#define DEBUG_BAUD 57600 +#define TELEM_BAUD 57600 +#define GPS_BAUD 38400 +#define HIL_BAUD 57600 + +// optional sensors +static bool gpsEnabled = false; +static bool baroEnabled = true; +static bool compassEnabled = true; + +static bool rangeFinderFrontEnabled = true; +static bool rangeFinderBackEnabled = true; +static bool rangeFinderLeftEnabled = true; +static bool rangeFinderRightEnabled = true; +static bool rangeFinderUpEnabled = true; +static bool rangeFinderDownEnabled = true; + +// 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 loop4Rate = 0.1; + +// gains +static const float steeringP = 1.0; +static const float steeringI = 0.0; +static const float steeringD = 0.0; +static const float steeringIMax = 0.0; +static const float steeringYMax = 3.0; + +static const float throttleP = 0.0; +static const float throttleI = 0.0; +static const float throttleD = 0.0; +static const float throttleIMax = 0.0; +static const float throttleYMax = 0.0; +static const float throttleDFCut = 3.0; + +#include "ControllerCar.h" + +#endif /* CARSTAMPEDE_H_ */ diff --git a/ArduRover/ControllerCar.h b/ArduRover/ControllerCar.h new file mode 100644 index 0000000000..5a846df99b --- /dev/null +++ b/ArduRover/ControllerCar.h @@ -0,0 +1,125 @@ +/* + * ControllerCar.h + * + * Created on: Jun 30, 2011 + * Author: jgoppert + */ + +#ifndef CONTROLLERCAR_H_ +#define CONTROLLERCAR_H_ + +#include "../APO/AP_Controller.h" + +namespace apo { + +class ControllerCar: public AP_Controller { +private: + AP_Var_group _group; + AP_Uint8 _mode; + enum { + k_chMode = k_radioChannelsStart, k_chStr, k_chThr + }; + enum { + k_pidStr = k_controllersStart, k_pidThr + }; + enum { + CH_MODE = 0, CH_STR, CH_THR + }; + BlockPIDDfb pidStr; + BlockPID pidThr; +public: + ControllerCar(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + AP_Controller(nav, guide, hal), + _group(k_cntrl, PSTR("CNTRL_")), + _mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")), + pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, + steeringI, steeringD, steeringIMax, steeringYMax), + pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP, + throttleI, throttleD, throttleIMax, throttleYMax, + throttleDFCut) { + _hal->debug->println_P(PSTR("initializing car controller")); + + _hal->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7, 1100, + 1500, 1900)); + _hal->rc.push_back( + new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1540, + 1900)); + _hal->rc.push_back( + new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500, + 1900)); + } + virtual MAV_MODE getMode() { + return (MAV_MODE) _mode.get(); + } + virtual void update(const float & dt) { + + // check for heartbeat + if (_hal->heartBeatLost()) { + _mode = MAV_MODE_FAILSAFE; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n")); + return; + // if throttle less than 5% cut motor power + } else if (_hal->rc[CH_THR]->getRadioPosition() < 0.05) { + _mode = MAV_MODE_LOCKED; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_STANDBY); + return; + // if in live mode then set state to active + } else if (_hal->getMode() == MODE_LIVE) { + _hal->setState(MAV_STATE_ACTIVE); + // if in hardware in the loop (control) mode, set to hilsim + } else if (_hal->getMode() == MODE_HIL_CNTL) { + _hal->setState(MAV_STATE_HILSIM); + } + + // read switch to set mode + if (_hal->rc[CH_MODE]->getRadioPosition() > 0) { + _mode = MAV_MODE_MANUAL; + } else { + _mode = MAV_MODE_AUTO; + } + + // manual mode + switch (_mode) { + + case MAV_MODE_MANUAL: { + setAllRadioChannelsManually(); + //_hal->debug->println("manual"); + break; + } + case MAV_MODE_AUTO: { + float headingError = _guide->getHeadingCommand() + - _nav->getYaw(); + if (headingError > 180 * deg2Rad) + headingError -= 360 * deg2Rad; + if (headingError < -180 * deg2Rad) + headingError += 360 * deg2Rad; + _hal->rc[CH_STR]->setPosition( + pidStr.update(headingError, _nav->getYawRate(), dt)); + _hal->rc[CH_THR]->setPosition( + pidThr.update( + _guide->getGroundSpeedCommand() + - _nav->getGroundSpeed(), dt)); + //_hal->debug->println("automode"); + break; + } + + default: { + setAllRadioChannelsToNeutral(); + _mode = MAV_MODE_FAILSAFE; + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("unknown controller mode\n")); + break; + } + + } + } +}; + +} // namespace apo + +#endif /* CONTROLLERCAR_H_ */ diff --git a/ArduRover/ControllerTank.h b/ArduRover/ControllerTank.h new file mode 100644 index 0000000000..b0bb3f1ef7 --- /dev/null +++ b/ArduRover/ControllerTank.h @@ -0,0 +1,133 @@ +/* + * ControllerTank.h + * + * Created on: Jun 30, 2011 + * Author: jgoppert + */ + +#ifndef CONTROLLERTANK_H_ +#define CONTROLLERTANK_H_ + +#include "../APO/AP_Controller.h" + +namespace apo { + +class ControllerTank: public AP_Controller { +private: + AP_Var_group _group; + AP_Uint8 _mode; + enum { + k_chMode = k_radioChannelsStart, k_chLeft, k_chRight, k_chStr, k_chThr + }; + enum { + k_pidStr = k_controllersStart, k_pidThr + }; + enum { + CH_MODE = 0, CH_LEFT, CH_RIGHT, CH_STR, CH_THR + }; + BlockPIDDfb pidStr; + BlockPID pidThr; +public: + ControllerTank(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + AP_Controller(nav, guide, hal), + _group(k_cntrl, PSTR("CNTRL_")), + _mode(&_group, 1, MAV_MODE_UNINIT, PSTR("MODE")), + pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP, + steeringI, steeringD, steeringIMax, steeringYMax), + pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP, + throttleI, throttleD, throttleIMax, throttleYMax, + throttleDFCut) { + _hal->debug->println_P(PSTR("initializing tank controller")); + + _hal->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100, + 1500, 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 0, 1100, 1500, + 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 1, 1100, 1500, + 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0, 1100, 1500, + 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1, 1100, 1500, + 1900, RC_MODE_IN)); + } + virtual MAV_MODE getMode() { + return (MAV_MODE) _mode.get(); + } + void mix(float headingOutput, float throttleOutput) { + _hal->rc[CH_LEFT]->setPosition(throttleOutput + headingOutput); + _hal->rc[CH_RIGHT]->setPosition(throttleOutput - headingOutput); + } + virtual void update(const float & dt) { + + // check for heartbeat + if (_hal->heartBeatLost()) { + _mode = MAV_MODE_FAILSAFE; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n")); + return; + // if throttle less than 5% cut motor power + } else if (_hal->rc[CH_THR]->getRadioPosition() < 0.05) { + _mode = MAV_MODE_LOCKED; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_STANDBY); + return; + // if in live mode then set state to active + } else if (_hal->getMode() == MODE_LIVE) { + _hal->setState(MAV_STATE_ACTIVE); + // if in hardware in the loop (control) mode, set to hilsim + } else if (_hal->getMode() == MODE_HIL_CNTL) { + _hal->setState(MAV_STATE_HILSIM); + } + + // read switch to set mode + if (_hal->rc[CH_MODE]->getRadioPosition() > 0) { + _mode = MAV_MODE_MANUAL; + } else { + _mode = MAV_MODE_AUTO; + } + + // manual mode + switch (_mode) { + + case MAV_MODE_MANUAL: { + setAllRadioChannelsManually(); + mix(_hal->rc[CH_STR]->getPosition(), + _hal->rc[CH_THR]->getPosition()); + break; + } + case MAV_MODE_AUTO: { + float headingError = _guide->getHeadingCommand() + - _nav->getYaw(); + if (headingError > 180 * deg2Rad) + headingError -= 360 * deg2Rad; + if (headingError < -180 * deg2Rad) + headingError += 360 * deg2Rad; + mix(pidStr.update(headingError, _nav->getYawRate(), dt), + pidThr.update(_guide->getGroundSpeedCommand() + - _nav->getGroundSpeed(), dt)); + //_hal->debug->println("automode"); + break; + } + + default: { + setAllRadioChannelsToNeutral(); + _mode = MAV_MODE_FAILSAFE; + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("unknown controller mode\n")); + break; + } + + } + } +}; + +} // namespace apo + +#endif /* CONTROLLERTANK_H_ */ diff --git a/ArduRover/TankGeneric.h b/ArduRover/TankGeneric.h new file mode 100644 index 0000000000..30d7118582 --- /dev/null +++ b/ArduRover/TankGeneric.h @@ -0,0 +1,68 @@ +/* + * TankGeneric.h + * + * Created on: Sep 26, 2011 + * Author: jgoppert + */ + +#ifndef TANKGENERIC_H_ +#define TANKGENERIC_H_ + +// vehicle options +static const apo::vehicle_t vehicle = apo::VEHICLE_TANK; +static const apo::halMode_t halMode = apo::MODE_LIVE; +static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560; +static const uint8_t heartBeatTimeout = 3; + +// algorithm selection +#define CONTROLLER_CLASS ControllerTank +#define GUIDE_CLASS MavlinkGuide +#define NAVIGATOR_CLASS DcmNavigator +#define COMMLINK_CLASS MavlinkComm + +// hardware selection +#define ADC_CLASS AP_ADC_ADS7844 +#define COMPASS_CLASS AP_Compass_HMC5843 +#define BARO_CLASS APM_BMP085_Class +#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL +#define DEBUG_BAUD 57600 +#define TELEM_BAUD 57600 +#define GPS_BAUD 38400 +#define HIL_BAUD 57600 + +// optional sensors +static bool gpsEnabled = false; +static bool baroEnabled = true; +static bool compassEnabled = true; + +static bool rangeFinderFrontEnabled = true; +static bool rangeFinderBackEnabled = true; +static bool rangeFinderLeftEnabled = true; +static bool rangeFinderRightEnabled = true; +static bool rangeFinderUpEnabled = true; +static bool rangeFinderDownEnabled = true; + +// 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 loop4Rate = 0.1; + +// gains +const float steeringP = 1.0; +const float steeringI = 0.0; +const float steeringD = 0.0; +const float steeringIMax = 0.0; +const float steeringYMax = 3.0; + +const float throttleP = 0.0; +const float throttleI = 0.0; +const float throttleD = 0.0; +const float throttleIMax = 0.0; +const float throttleYMax = 0.0; +const float throttleDFCut = 3.0; + +#include "ControllerTank.h" + +#endif /* TANKGENERIC_H_ */ diff --git a/apo/CMakeLists.txt b/apo/CMakeLists.txt new file mode 100644 index 0000000000..95c4c1289c --- /dev/null +++ b/apo/CMakeLists.txt @@ -0,0 +1,80 @@ +#=============================================================================# +# Author: Sebastian Rohde # +# Date: 30.08.2011 # +#=============================================================================# + + +#====================================================================# +# Settings # +#====================================================================# +set(FIRMWARE_NAME ardupilotone) + +set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board + +set(${FIRMWARE_NAME}_SKETCHES + ardupilotone.pde + ) # Firmware sketches + +set(${FIRMWARE_NAME}_SRCS + ) # Firmware sources + +set(${FIRMWARE_NAME}_HDRS + BoatGeneric.h + CarStampede.h + #CNIRover.h + #CNIBoat.h + ControllerBoat.h + ControllerCar.h + ControllerPlane.h + ControllerQuad.h + PlaneEasystar.h + QuadArducopter.h + QuadMikrokopter.h + ) # Firmware sources + +set(${FIRMWARE_NAME}_LIBS + m + APO + FastSerial + AP_Common + GCS_MAVLink + AP_GPS + APM_RC + AP_DCM + AP_ADC + AP_Compass + AP_IMU + AP_RangeFinder + APM_BMP085 + ModeFilter +) + + +#${CONSOLE_PORT} +set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port +set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd + +include_directories( +${CMAKE_SOURCE_DIR}/libraries/APO +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +) +#====================================================================# +# Target generation # +#====================================================================# +generate_arduino_firmware(${FIRMWARE_NAME}) + +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/${FIRMWARE_NAME}.hex + DESTINATION bin + ) diff --git a/apo/ControllerPlane.h b/apo/ControllerPlane.h new file mode 100644 index 0000000000..539253f8d9 --- /dev/null +++ b/apo/ControllerPlane.h @@ -0,0 +1,215 @@ +/* + * ControllerPlane.h + * + * Created on: Jun 30, 2011 + * Author: jgoppert + */ + +#ifndef CONTROLLERPLANE_H_ +#define CONTROLLERPLANE_H_ + +#include "../APO/AP_Controller.h" + +namespace apo { + +class ControllerPlane: public AP_Controller { +private: + AP_Var_group _group; + AP_Var_group _trimGroup; + AP_Uint8 _mode; + AP_Uint8 _rdrAilMix; + bool _needsTrim; + AP_Float _ailTrim; + AP_Float _elvTrim; + AP_Float _rdrTrim; + AP_Float _thrTrim; + enum { + ch_mode = 0, ch_roll, ch_pitch, ch_thr, ch_yaw + }; + enum { + k_chMode = k_radioChannelsStart, + k_chRoll, + k_chPitch, + k_chYaw, + k_chThr, + + k_pidBnkRll = k_controllersStart, + k_pidSpdPit, + k_pidPitPit, + k_pidYwrYaw, + k_pidHdgBnk, + k_pidAltThr, + + k_trim = k_customStart + }; + BlockPID pidBnkRll; // bank error to roll servo deflection + BlockPID pidSpdPit; // speed error to pitch command + BlockPID pidPitPit; // pitch error to pitch servo deflection + BlockPID pidYwrYaw; // yaw rate error to yaw servo deflection + BlockPID pidHdgBnk; // heading error to bank command + BlockPID pidAltThr; // altitude error to throttle deflection + bool requireRadio; + +public: + ControllerPlane(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + AP_Controller(nav, guide, hal), + _group(k_cntrl, PSTR("cntrl_")), + _trimGroup(k_trim, PSTR("trim_")), + _mode(&_group, 1, MAV_MODE_UNINIT, PSTR("mode")), + _rdrAilMix(&_group, 2, rdrAilMix, PSTR("rdrAilMix")), + _needsTrim(false), + _ailTrim(&_trimGroup, 1, ailTrim, PSTR("ail")), + _elvTrim(&_trimGroup, 2, elvTrim, PSTR("elv")), + _rdrTrim(&_trimGroup, 3, rdrTrim, PSTR("rdr")), + _thrTrim(&_trimGroup, 4, thrTrim, PSTR("thr")), + pidBnkRll(new AP_Var_group(k_pidBnkRll, PSTR("bnkRll_")), 1, + pidBnkRllP, pidBnkRllI, pidBnkRllD, pidBnkRllAwu, + pidBnkRllLim, pidBnkRllDFCut), + pidPitPit(new AP_Var_group(k_pidPitPit, PSTR("pitPit_")), 1, + pidPitPitP, pidPitPitI, pidPitPitD, pidPitPitAwu, + pidPitPitLim, pidPitPitDFCut), + pidSpdPit(new AP_Var_group(k_pidSpdPit, PSTR("spdPit_")), 1, + pidSpdPitP, pidSpdPitI, pidSpdPitD, pidSpdPitAwu, + pidSpdPitLim, pidSpdPitDFCut), + pidYwrYaw(new AP_Var_group(k_pidYwrYaw, PSTR("ywrYaw_")), 1, + pidYwrYawP, pidYwrYawI, pidYwrYawD, pidYwrYawAwu, + pidYwrYawLim, pidYwrYawDFCut), + pidHdgBnk(new AP_Var_group(k_pidHdgBnk, PSTR("hdgBnk_")), 1, + pidHdgBnkP, pidHdgBnkI, pidHdgBnkD, pidHdgBnkAwu, + pidHdgBnkLim, pidHdgBnkDFCut), + pidAltThr(new AP_Var_group(k_pidAltThr, PSTR("altThr_")), 1, + pidAltThrP, pidAltThrI, pidAltThrD, pidAltThrAwu, + pidAltThrLim, pidAltThrDFCut), + requireRadio(false) { + + _hal->debug->println_P(PSTR("initializing car controller")); + + _hal->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("mode_"), APM_RC, 5, 1100, + 1500, 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chRoll, PSTR("roll_"), APM_RC, 0, 1200, + 1500, 1800, RC_MODE_INOUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chPitch, PSTR("pitch_"), APM_RC, 1, 1200, + 1500, 1800, RC_MODE_INOUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chThr, PSTR("thr_"), APM_RC, 2, 1100, 1100, + 1900, RC_MODE_INOUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chYaw, PSTR("yaw_"), APM_RC, 3, 1200, 1500, + 1800, RC_MODE_INOUT)); + } + virtual MAV_MODE getMode() { + return (MAV_MODE) _mode.get(); + } + virtual void update(const float & dt) { + + // check for heartbeat + if (_hal->heartBeatLost()) { + _mode = MAV_MODE_FAILSAFE; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n")); + return; + // if the value of the throttle is less than 5% cut motor power + } else if (requireRadio && _hal->rc[ch_thr]->getRadioPosition() < 0.05) { + _mode = MAV_MODE_LOCKED; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_STANDBY); + return; + // if in live mode then set state to active + } else if (_hal->getMode() == MODE_LIVE) { + _hal->setState(MAV_STATE_ACTIVE); + // if in hardware in the loop (control) mode, set to hilsim + } else if (_hal->getMode() == MODE_HIL_CNTL) { + _hal->setState(MAV_STATE_HILSIM); + } + + // read switch to set mode + if (_hal->rc[ch_mode]->getRadioPosition() > 0) { + _mode = MAV_MODE_MANUAL; + } else { + _mode = MAV_MODE_AUTO; + } + + // manual mode + switch (_mode) { + + case MAV_MODE_MANUAL: { + setAllRadioChannelsManually(); + + // force auto to read new manual trim + if (_needsTrim == false) + _needsTrim = true; + //_hal->debug->println("manual"); + break; + } + case MAV_MODE_AUTO: { + float headingError = _guide->getHeadingCommand() + - _nav->getYaw(); + if (headingError > 180 * deg2Rad) + headingError -= 360 * deg2Rad; + if (headingError < -180 * deg2Rad) + headingError += 360 * deg2Rad; + + float aileron = pidBnkRll.update( + pidHdgBnk.update(headingError, dt) - _nav->getRoll(), dt); + float elevator = pidPitPit.update( + -pidSpdPit.update( + _guide->getAirSpeedCommand() - _nav->getAirSpeed(), + dt) - _nav->getPitch(), dt); + float rudder = pidYwrYaw.update(-_nav->getYawRate(), dt); + // desired yaw rate is zero, needs washout + float throttle = pidAltThr.update( + _guide->getAltitudeCommand() - _nav->getAlt(), dt); + + // if needs trim + if (_needsTrim) { + // need to subtract current controller deflections so control + // surfaces are actually at the same position as manual flight + _ailTrim = _hal->rc[ch_roll]->getRadioPosition() - aileron; + _elvTrim = _hal->rc[ch_pitch]->getRadioPosition() - elevator; + _rdrTrim = _hal->rc[ch_yaw]->getRadioPosition() - rudder; + _thrTrim = _hal->rc[ch_thr]->getRadioPosition() - throttle; + _needsTrim = false; + } + + // actuator mixing/ output + _hal->rc[ch_roll]->setPosition( + aileron + _rdrAilMix * rudder + _ailTrim); + _hal->rc[ch_yaw]->setPosition(rudder + _rdrTrim); + _hal->rc[ch_pitch]->setPosition(elevator + _elvTrim); + _hal->rc[ch_thr]->setPosition(throttle + _thrTrim); + + //_hal->debug->println("automode"); + + + // heading debug +// Serial.print("heading command: "); Serial.println(_guide->getHeadingCommand()); +// Serial.print("heading: "); Serial.println(_nav->getYaw()); +// Serial.print("heading error: "); Serial.println(headingError); + + // alt debug +// Serial.print("alt command: "); Serial.println(_guide->getAltitudeCommand()); +// Serial.print("alt: "); Serial.println(_nav->getAlt()); +// Serial.print("alt error: "); Serial.println(_guide->getAltitudeCommand() - _nav->getAlt()); + break; + } + + default: { + setAllRadioChannelsToNeutral(); + _mode = MAV_MODE_FAILSAFE; + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("unknown controller mode\n")); + break; + } + + } + } +}; + +} // namespace apo + +#endif /* CONTROLLERPLANE_H_ */ diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h new file mode 100644 index 0000000000..3f74739b00 --- /dev/null +++ b/apo/ControllerQuad.h @@ -0,0 +1,250 @@ +/* + * ControllerQuad.h + * + * Created on: Jun 30, 2011 + * Author: jgoppert + */ + +#ifndef CONTROLLERQUAD_H_ +#define CONTROLLERQUAD_H_ + +#include "../APO/AP_Controller.h" + +namespace apo { + +class ControllerQuad: public AP_Controller { +public: + + /** + * note that these are not the controller radio channel numbers, they are just + * unique keys so they can be reaccessed from the hal rc vector + */ + enum { + CH_MODE = 0, // note scicoslab channels set mode, left, right, front, back order + CH_LEFT, // this enum must match this + CH_RIGHT, + CH_FRONT, + CH_BACK, + CH_ROLL, + CH_PITCH, + CH_YAW, + CH_THRUST + }; + + enum { + k_chMode = k_radioChannelsStart, + k_chLeft, + k_chRight, + k_chFront, + k_chBack, + k_chRoll, + k_chPitch, + k_chYaw, + k_chThr + }; + + enum { + k_pidGroundSpeed2Throttle = k_controllersStart, + k_pidStr, + k_pidPN, + k_pidPE, + k_pidPD, + k_pidRoll, + k_pidPitch, + k_pidYawRate, + k_pidYaw, + }; + + ControllerQuad(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + AP_Controller(nav, guide, hal), + pidRoll(new AP_Var_group(k_pidRoll, PSTR("ROLL_")), 1, + PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU, + PID_ATT_LIM), + pidPitch(new AP_Var_group(k_pidPitch, PSTR("PITCH_")), 1, + PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU, + PID_ATT_LIM), + pidYaw(new AP_Var_group(k_pidYaw, PSTR("YAW_")), 1, + PID_YAWPOS_P, PID_YAWPOS_I, PID_YAWPOS_D, + PID_YAWPOS_AWU, PID_YAWPOS_LIM), + pidYawRate(new AP_Var_group(k_pidYawRate, PSTR("YAWRT_")), 1, + PID_YAWSPEED_P, PID_YAWSPEED_I, PID_YAWSPEED_D, + PID_YAWSPEED_AWU, PID_YAWSPEED_LIM, PID_YAWSPEED_DFCUT), + pidPN(new AP_Var_group(k_pidPN, PSTR("NORTH_")), 1, PID_POS_P, + PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM), + pidPE(new AP_Var_group(k_pidPE, PSTR("EAST_")), 1, PID_POS_P, + PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM), + pidPD(new AP_Var_group(k_pidPD, PSTR("DOWN_")), 1, PID_POS_Z_P, + PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM) { + /* + * allocate radio channels + * the order of the channels has to match the enumeration above + */ + _hal->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 5, 1100, + 1500, 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 0, 1100, + 1100, 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 1, 1100, + 1100, 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100, + 1100, 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chBack, PSTR("BACK_"), APM_RC, 3, 1100, + 1100, 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chRoll, PSTR("ROLL_"), APM_RC, 0, 1100, + 1500, 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100, + 1500, 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 2, 1100, 1500, + 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 3, 1100, + 1100, 1900, RC_MODE_IN)); + } + + virtual void update(const float & dt) { + + // check for heartbeat + if (_hal->heartBeatLost()) { + _mode = MAV_MODE_FAILSAFE; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_EMERGENCY); + _hal->debug->printf_P(PSTR("comm lost, send heartbeat from gcs\n")); + return; + // if throttle less than 5% cut motor power + } else if (_hal->rc[CH_THRUST]->getRadioPosition() < 0.05) { + _mode = MAV_MODE_LOCKED; + setAllRadioChannelsToNeutral(); + _hal->setState(MAV_STATE_STANDBY); + return; + // if in live mode then set state to active + } else if (_hal->getMode() == MODE_LIVE) { + _hal->setState(MAV_STATE_ACTIVE); + // if in hardware in the loop (control) mode, set to hilsim + } else if (_hal->getMode() == MODE_HIL_CNTL) { + _hal->setState(MAV_STATE_HILSIM); + } + + // manual mode + float mixRemoteWeight = 0; + if (_hal->rc[CH_MODE]->getRadioPosition() > 0) { + mixRemoteWeight = 1; + _mode = MAV_MODE_MANUAL; + } else { + _mode = MAV_MODE_AUTO; + } + + // commands for inner loop + float cmdRoll = 0; + float cmdPitch = 0; + float cmdYawRate = 0; + float thrustMix = 0; + + switch(_mode) { + + case MAV_MODE_MANUAL: { + setAllRadioChannelsManually(); + // "mix manual" + cmdRoll = 0.5 * _hal->rc[CH_ROLL]->getPosition() + * mixRemoteWeight; + cmdPitch = 0.5 * _hal->rc[CH_PITCH]->getPosition() + * mixRemoteWeight; + cmdYawRate = 0.5 * _hal->rc[CH_YAW]->getPosition() + * mixRemoteWeight; + thrustMix = _hal->rc[CH_THRUST]->getPosition() * mixRemoteWeight; + break; + } + + case MAV_MODE_AUTO: { + + // XXX kills all commands, + // auto not currently implemented + + // position loop + /* + 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); + + // "transform-to-body" + { + float trigSin = sin(-yaw); + float trigCos = cos(-yaw); + _cmdPitch = _cmdEastTilt * trigCos + - _cmdNorthTilt * trigSin; + _cmdRoll = -_cmdEastTilt * trigSin + + _cmdNorthTilt * trigCos; + // note that the north tilt is negative of the pitch + } + + //thrustMix += THRUST_HOVER_OFFSET; + + // "thrust-trim-adjust" + if (fabs(_cmdRoll) > 0.5) { + _thrustMix *= 1.13949393; + } else { + _thrustMix /= cos(_cmdRoll); + } + if (fabs(_cmdPitch) > 0.5) { + _thrustMix *= 1.13949393; + } else { + _thrustMix /= cos(_cmdPitch); + } + */ + } + + } + + // attitude loop + // XXX negative sign added to nav roll, not sure why this is necessary + // XXX negative sign added to nav roll rate, not sure why this is necessary + float rollMix = pidRoll.update(cmdRoll + _nav->getRoll(), + -_nav->getRollRate(), dt); + // XXX negative sign added to cmdPitch, not sure why this is necessary + float pitchMix = pidPitch.update(-cmdPitch - _nav->getPitch(), + _nav->getPitchRate(), dt); + // XXX negative sign added to cmdYawRate, not sure why this is necessary + float yawMix = pidYawRate.update(-cmdYawRate - _nav->getYawRate(), dt); + + _hal->rc[CH_LEFT]->setPosition(thrustMix + rollMix + yawMix); + _hal->rc[CH_RIGHT]->setPosition(thrustMix - rollMix + yawMix); + _hal->rc[CH_FRONT]->setPosition(thrustMix + pitchMix - yawMix); + _hal->rc[CH_BACK]->setPosition(thrustMix - pitchMix - yawMix); + + // _hal->debug->printf("L: %f\t R: %f\t F: %f\t B: %f\n", + // _hal->rc[CH_LEFT]->getPosition(), + // _hal->rc[CH_RIGHT]->getPosition(), + // _hal->rc[CH_FRONT]->getPosition(), + // _hal->rc[CH_BACK]->getPosition()); + + _hal->debug->printf( + "rollMix: %f\t pitchMix: %f\t yawMix: %f\t thrustMix: %f\n", + rollMix, pitchMix, yawMix, thrustMix); + + // _hal->debug->printf("roll pwm: %d\t pitch pwm: %d\t yaw pwm: %d\t thrust pwm: %d\n", + // _hal->rc[CH_ROLL]->readRadio(), + // _hal->rc[CH_PITCH]->readRadio(), + // _hal->rc[CH_YAW]->readRadio(), + // _hal->rc[CH_THRUST]->readRadio()); + } + virtual MAV_MODE getMode() { + return (MAV_MODE) _mode.get(); + } +private: + AP_Uint8 _mode; + BlockPIDDfb pidRoll, pidPitch, pidYaw; + BlockPID pidYawRate; + BlockPIDDfb pidPN, pidPE, pidPD; + +}; + +} // namespace apo + +#endif /* CONTROLLERQUAD_H_ */ diff --git a/apo/Makefile b/apo/Makefile new file mode 100644 index 0000000000..1d9b6a022d --- /dev/null +++ b/apo/Makefile @@ -0,0 +1 @@ +include ../libraries/AP_Common/Arduino.mk diff --git a/apo/PlaneEasystar.h b/apo/PlaneEasystar.h new file mode 100644 index 0000000000..6acf27b403 --- /dev/null +++ b/apo/PlaneEasystar.h @@ -0,0 +1,113 @@ +/* + * PlaneEasystar.h + * + * Created on: May 1, 2011 + * Author: jgoppert + */ + +#ifndef PLANEEASYSTAR_H_ +#define PLANEEASYSTAR_H_ + + +// vehicle options +static const apo::vehicle_t vehicle = apo::VEHICLE_PLANE; +//static const apo::halMode_t halMode = apo::MODE_LIVE; // live mode, actual flight +static const apo::halMode_t halMode = apo::MODE_HIL_CNTL; // hardware in the loop, control level +static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; +static const uint8_t heartBeatTimeout = 3; + +// algorithm selection +#define CONTROLLER_CLASS ControllerPlane +#define GUIDE_CLASS MavlinkGuide +#define NAVIGATOR_CLASS DcmNavigator +#define COMMLINK_CLASS MavlinkComm + +// hardware selection +#define ADC_CLASS AP_ADC_ADS7844 +#define COMPASS_CLASS AP_Compass_HMC5843 +#define BARO_CLASS APM_BMP085_Class +#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL +#define DEBUG_BAUD 57600 +#define TELEM_BAUD 57600 +#define GPS_BAUD 38400 +#define HIL_BAUD 57600 + +// optional sensors +static bool gpsEnabled = false; +static bool baroEnabled = true; +static bool compassEnabled = true; + +static bool rangeFinderFrontEnabled = true; +static bool rangeFinderBackEnabled = true; +static bool rangeFinderLeftEnabled = true; +static bool rangeFinderRightEnabled = true; +static bool rangeFinderUpEnabled = true; +static bool rangeFinderDownEnabled = true; + +// 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 loop4Rate = 0.1; + +// gains +static const float rdrAilMix = 1.0; // since there are no ailerons + +// bank error to roll servo +static const float pidBnkRllP = -0.5; +static const float pidBnkRllI = 0.0; +static const float pidBnkRllD = 0.0; +static const float pidBnkRllAwu = 0.0; +static const float pidBnkRllLim = 1.0; +static const float pidBnkRllDFCut = 0.0; + +// pitch error to pitch servo +static const float pidPitPitP = -1; +static const float pidPitPitI = 0.0; +static const float pidPitPitD = 0.0; +static const float pidPitPitAwu = 0.0; +static const float pidPitPitLim = 1.0; +static const float pidPitPitDFCut = 0.0; + +// speed error to pitch command +static const float pidSpdPitP = 0.1; +static const float pidSpdPitI = 0.0; +static const float pidSpdPitD = 0.0; +static const float pidSpdPitAwu = 0.0; +static const float pidSpdPitLim = 1.0; +static const float pidSpdPitDFCut = 0.0; + +// yaw rate error to yaw servo +static const float pidYwrYawP = -0.2; +static const float pidYwrYawI = 0.0; +static const float pidYwrYawD = 0.0; +static const float pidYwrYawAwu = 0.0; +static const float pidYwrYawLim = 1.0; +static const float pidYwrYawDFCut = 0.0; + +// heading error to bank angle command +static const float pidHdgBnkP = 1.0; +static const float pidHdgBnkI = 0.0; +static const float pidHdgBnkD = 0.0; +static const float pidHdgBnkAwu = 0.0; +static const float pidHdgBnkLim = 0.5; +static const float pidHdgBnkDFCut = 0.0; + +// altitude error to throttle command +static const float pidAltThrP = .01; +static const float pidAltThrI = 0.0; +static const float pidAltThrD = 0.0; +static const float pidAltThrAwu = 0.0; +static const float pidAltThrLim = 1; +static const float pidAltThrDFCut = 0.0; + +// trim control positions (-1,1) +static const float ailTrim = 0.0; +static const float elvTrim = 0.0; +static const float rdrTrim = 0.0; +static const float thrTrim = 0.5; + +#include "ControllerPlane.h" + +#endif /* PLANEEASYSTAR_H_ */ diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h new file mode 100644 index 0000000000..5151b224fe --- /dev/null +++ b/apo/QuadArducopter.h @@ -0,0 +1,98 @@ +/* + * QuadArducopter.h + * + * Created on: May 1, 2011 + * Author: jgoppert + */ + +#ifndef QUADARDUCOPTER_H_ +#define QUADARDUCOPTER_H_ + +// 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_2560; +static const uint8_t heartBeatTimeout = 3; + +// algorithm selection +#define CONTROLLER_CLASS ControllerQuad +#define GUIDE_CLASS MavlinkGuide +#define NAVIGATOR_CLASS DcmNavigator +#define COMMLINK_CLASS MavlinkComm + +// hardware selection +#define ADC_CLASS AP_ADC_ADS7844 +#define COMPASS_CLASS AP_Compass_HMC5843 +#define BARO_CLASS APM_BMP085_Class +#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL +#define DEBUG_BAUD 57600 +#define TELEM_BAUD 57600 +#define GPS_BAUD 38400 +#define HIL_BAUD 57600 + +// optional sensors +static bool gpsEnabled = false; +static bool baroEnabled = true; +static bool compassEnabled = true; + +static bool rangeFinderFrontEnabled = true; +static bool rangeFinderBackEnabled = true; +static bool rangeFinderLeftEnabled = true; +static bool rangeFinderRightEnabled = true; +static bool rangeFinderUpEnabled = true; +static bool rangeFinderDownEnabled = true; + +// 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 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; +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_I = 0; +static const float PID_POS_Z_D = 0; +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_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_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_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" + +#endif /* QUADARDUCOPTER_H_ */ diff --git a/apo/QuadMikrokopter.h b/apo/QuadMikrokopter.h new file mode 100644 index 0000000000..7aeeacd5e6 --- /dev/null +++ b/apo/QuadMikrokopter.h @@ -0,0 +1,98 @@ +/* + * QuadMikrokopter + * + * Created on: May 1, 2011 + * Author: jgoppert + */ + +#ifndef QUADMIKROKOPTER_H_ +#define QUADMIKROKOPTER_H_ + +// 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_2560; +static const uint8_t heartBeatTimeout = 3; + +// algorithm selection +#define CONTROLLER_CLASS ControllerQuad +#define GUIDE_CLASS MavlinkGuide +#define NAVIGATOR_CLASS DcmNavigator +#define COMMLINK_CLASS MavlinkComm + +// hardware selection +#define ADC_CLASS AP_ADC_ADS7844 +#define COMPASS_CLASS AP_Compass_HMC5843 +#define BARO_CLASS APM_BMP085_Class +#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL +#define DEBUG_BAUD 57600 +#define TELEM_BAUD 57600 +#define GPS_BAUD 38400 +#define HIL_BAUD 57600 + +// optional sensors +static bool gpsEnabled = false; +static bool baroEnabled = true; +static bool compassEnabled = true; + +static bool rangeFinderFrontEnabled = true; +static bool rangeFinderBackEnabled = true; +static bool rangeFinderLeftEnabled = true; +static bool rangeFinderRightEnabled = true; +static bool rangeFinderUpEnabled = true; +static bool rangeFinderDownEnabled = true; + +// 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 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; +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_I = 0; +static const float PID_POS_Z_D = 0; +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_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_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_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" + +#endif /* QUADMIKROKOPTER_H_ */ diff --git a/apo/apo.pde b/apo/apo.pde new file mode 100644 index 0000000000..7e79ec1a9e --- /dev/null +++ b/apo/apo.pde @@ -0,0 +1,184 @@ +/* + * ardupilotone + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#define ENABLE_FASTSERIAL_DEBUG + +// Libraries +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +FastSerialPort0(Serial); +FastSerialPort1(Serial1); +FastSerialPort2(Serial2); +FastSerialPort3(Serial3); + +// Vehicle Configuration +#include "PlaneEasystar.h" + +/* + * Required Global Declarations + */ + +static apo::AP_Autopilot * autoPilot; + +void setup() { + + using namespace apo; + + AP_Var::load_all(); + + /* + * Communications + */ + Serial.begin(DEBUG_BAUD, 128, 128); // debug + if (board==BOARD_ARDUPILOTMEGA_2) Serial2.begin(TELEM_BAUD, 128, 128); // gcs + else Serial3.begin(TELEM_BAUD, 128, 128); // gcs + + // hardware abstraction layer + AP_HardwareAbstractionLayer * hal = new AP_HardwareAbstractionLayer( + halMode, board, vehicle, heartBeatTimeout); + + // debug serial + hal->debug = &Serial; + hal->debug->println_P(PSTR("initializing debug line")); + + /* + * Initialize Comm Channels + */ + hal->debug->println_P(PSTR("initializing comm channels")); + if (hal->getMode() == MODE_LIVE) { + Serial1.begin(GPS_BAUD, 128, 16); // gps + } else { // hil + Serial1.begin(HIL_BAUD, 128, 128); + } + + /* + * Sensor initialization + */ + if (hal->getMode() == MODE_LIVE) { + hal->debug->println_P(PSTR("initializing adc")); + hal->adc = new ADC_CLASS; + hal->adc->Init(); + + if (gpsEnabled) { + hal->debug->println_P(PSTR("initializing gps")); + AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps)); + hal->gps = &gpsDriver; + hal->gps->init(); + } + + if (baroEnabled) { + hal->debug->println_P(PSTR("initializing baro")); + hal->baro = new BARO_CLASS; + hal->baro->Init(); + } + + if (compassEnabled) { + hal->debug->println_P(PSTR("initializing compass")); + hal->compass = new COMPASS_CLASS; + hal->compass->set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD); + hal->compass->init(); + } + + /** + * Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not + * initialize them and NULL will be assigned to those corresponding pointers. + * On detecting NULL assigned to any ultrasonic sensor, its corresponding block of code + * will not be executed by the navigator. + * The coordinate system is assigned by the right hand rule with the thumb pointing down. + * In set_orientation, it is defind as (front/back,left/right,down,up) + */ + + if (rangeFinderFrontEnabled) { + hal->debug->println_P(PSTR("initializing front range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(1); + rangeFinder->set_orientation(1, 0, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderBackEnabled) { + hal->debug->println_P(PSTR("initializing back range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(2); + rangeFinder->set_orientation(-1, 0, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderLeftEnabled) { + hal->debug->println_P(PSTR("initializing left range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(3); + rangeFinder->set_orientation(0, -1, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderRightEnabled) { + hal->debug->println_P(PSTR("initializing right range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(4); + rangeFinder->set_orientation(0, 1, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderUpEnabled) { + hal->debug->println_P(PSTR("initializing up range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(5); + rangeFinder->set_orientation(0, 0, -1); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderDownEnabled) { + hal->debug->println_P(PSTR("initializing down range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(6); + rangeFinder->set_orientation(0, 0, 1); + hal->rangeFinders.push_back(rangeFinder); + } + + } + + /* + * Select guidance, navigation, control algorithms + */ + AP_Navigator * navigator = new NAVIGATOR_CLASS(hal); + AP_Guide * guide = new GUIDE_CLASS(navigator, hal); + AP_Controller * controller = new CONTROLLER_CLASS(navigator, guide, hal); + + /* + * CommLinks + */ + if (board==BOARD_ARDUPILOTMEGA_2) hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal); + else hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal); + + hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal); + + /* + * Start the autopilot + */ + hal->debug->printf_P(PSTR("initializing arduplane\n")); + hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); + autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal, + loop0Rate, loop1Rate, loop2Rate, loop3Rate); +} + +void loop() { + autoPilot->update(); +} diff --git a/libraries/APM_BMP085/CMakeLists.txt b/libraries/APM_BMP085/CMakeLists.txt index 342fe4668f..77b83a3b66 100644 --- a/libraries/APM_BMP085/CMakeLists.txt +++ b/libraries/APM_BMP085/CMakeLists.txt @@ -17,6 +17,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/APM_PI/CMakeLists.txt b/libraries/APM_PI/CMakeLists.txt index e65e76f6a4..3ccdd81ce6 100644 --- a/libraries/APM_PI/CMakeLists.txt +++ b/libraries/APM_PI/CMakeLists.txt @@ -19,6 +19,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/APM_RC/CMakeLists.txt b/libraries/APM_RC/CMakeLists.txt index 3e4aab0011..4c3748159f 100644 --- a/libraries/APM_RC/CMakeLists.txt +++ b/libraries/APM_RC/CMakeLists.txt @@ -17,6 +17,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/APO/APO.cpp b/libraries/APO/APO.cpp new file mode 100644 index 0000000000..2592d1140c --- /dev/null +++ b/libraries/APO/APO.cpp @@ -0,0 +1,8 @@ +/* + * APO.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "APO.h" diff --git a/libraries/APO/APO.h b/libraries/APO/APO.h new file mode 100644 index 0000000000..0e0efafbd7 --- /dev/null +++ b/libraries/APO/APO.h @@ -0,0 +1,20 @@ +/* + * APO.h + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#ifndef APO_H_ +#define APO_H_ + +#include "AP_Autopilot.h" +#include "AP_Guide.h" +#include "AP_Controller.h" +#include "AP_HardwareAbstractionLayer.h" +#include "AP_MavlinkCommand.h" +#include "AP_Navigator.h" +#include "AP_RcChannel.h" +//#include "AP_Var_keys.h" + +#endif /* APO_H_ */ diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp new file mode 100644 index 0000000000..96dab80e12 --- /dev/null +++ b/libraries/APO/AP_Autopilot.cpp @@ -0,0 +1,246 @@ +/* + * AP_Autopilot.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_Autopilot.h" + +namespace apo { + +class AP_HardwareAbstractionLayer; + +AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, + AP_Controller * controller, AP_HardwareAbstractionLayer * hal, + float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate) : + Loop(loop0Rate, callback0, this), _navigator(navigator), _guide(guide), + _controller(controller), _hal(hal), _loop0Rate(loop0Rate), + _loop1Rate(loop1Rate), _loop2Rate(loop2Rate), _loop3Rate(loop3Rate), + _loop4Rate(loop3Rate) { + + hal->setState(MAV_STATE_BOOT); + hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + + /* + * Calibration + */ + hal->setState(MAV_STATE_CALIBRATING); + hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + navigator->calibrate(); + + // start clock + //uint32_t timeStart = millis(); + //uint16_t gpsWaitTime = 5000; // 5 second wait for gps + + /* + * Look for valid initial state + */ + while (1) { + // letc gcs known we are alive + hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + hal->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + delay(1000); + if (hal->getMode() == MODE_LIVE) { + _navigator->updateSlow(0); + if (_hal->gps) { + if (hal->gps->fix) { + break; + } else { + hal->gcs->sendText(SEVERITY_LOW, + PSTR("waiting for gps lock\n")); + hal->debug->printf_P(PSTR("waiting for gps lock\n")); + } + } else { // no gps, can skip + break; + } + } else if (hal->getMode() == MODE_HIL_CNTL) { // hil + _hal->hil->receive(); + Serial.println("HIL Recieve Called"); + if (_navigator->getTimeStamp() != 0) { + // give hil a chance to send some packets + for (int i = 0; i < 5; i++) { + hal->debug->println_P(PSTR("reading initial hil packets")); + hal->gcs->sendText(SEVERITY_LOW, + PSTR("reading initial hil packets")); + delay(1000); + } + break; + } + hal->debug->println_P(PSTR("waiting for hil packet")); + } + } + + AP_MavlinkCommand::home.setAlt(_navigator->getAlt()); + AP_MavlinkCommand::home.setLat(_navigator->getLat()); + AP_MavlinkCommand::home.setLon(_navigator->getLon()); + AP_MavlinkCommand::home.save(); + _hal->debug->printf_P(PSTR("\nhome before load lat: %f deg, lon: %f deg\n"), + AP_MavlinkCommand::home.getLat()*rad2Deg, + AP_MavlinkCommand::home.getLon()*rad2Deg); + AP_MavlinkCommand::home.load(); + _hal->debug->printf_P(PSTR("home after load lat: %f deg, lon: %f deg\n"), + AP_MavlinkCommand::home.getLat()*rad2Deg, + AP_MavlinkCommand::home.getLon()*rad2Deg); + + /* + * Attach loops + */ + hal->debug->println_P(PSTR("attaching loops")); + subLoops().push_back(new Loop(getLoopRate(1), callback1, this)); + subLoops().push_back(new Loop(getLoopRate(2), callback2, this)); + subLoops().push_back(new Loop(getLoopRate(3), callback3, this)); + subLoops().push_back(new Loop(getLoopRate(4), callback4, this)); + + hal->debug->println_P(PSTR("running")); + hal->gcs->sendText(SEVERITY_LOW, PSTR("running")); + + if (hal->getMode() == MODE_LIVE) { + hal->setState(MAV_STATE_ACTIVE); + } else { + hal->setState(MAV_STATE_HILSIM); + } + + /* + * Radio setup + */ + hal->debug->println_P(PSTR("initializing radio")); + APM_RC.Init(); // APM Radio initialization, + // start this after control loop is running +} + +void AP_Autopilot::callback0(void * data) { + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->hal()->debug->println_P(PSTR("callback 0")); + + /* + * ahrs update + */ + if (apo->getNavigator()) + apo->getNavigator()->updateFast(1.0 / apo->getLoopRate(0)); +} + +void AP_Autopilot::callback1(void * data) { + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->getHal()->debug->println_P(PSTR("callback 1")); + + /* + * hardware in the loop + */ + if (apo->getHal()->hil && apo->getHal()->getMode() != MODE_LIVE) { + apo->getHal()->hil->receive(); + apo->getHal()->hil->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); + } + + /* + * update guidance laws + */ + if (apo->getGuide()) + { + //apo->getHal()->debug->println_P(PSTR("updating guide")); + apo->getGuide()->update(); + } + + /* + * update control laws + */ + if (apo->getController()) { + //apo->getHal()->debug->println_P(PSTR("updating controller")); + apo->getController()->update(1. / apo->getLoopRate(1)); + } + /* + char msg[50]; + sprintf(msg, "c_hdg: %f, c_thr: %f", apo->guide()->headingCommand, apo->guide()->groundSpeedCommand); + apo->hal()->gcs->sendText(AP_CommLink::SEVERITY_LOW, msg); + */ +} + +void AP_Autopilot::callback2(void * data) { + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->getHal()->debug->println_P(PSTR("callback 2")); + + /* + * 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 + */ + if (apo->getNavigator()) { + apo->getNavigator()->updateSlow(1.0 / apo->getLoopRate(2)); + } + + /* + * handle ground control station communication + */ + if (apo->getHal()->gcs) { + // send messages + apo->getHal()->gcs->requestCmds(); + apo->getHal()->gcs->sendParameters(); + + // receive messages + apo->getHal()->gcs->receive(); + } + + /* + * navigator debug + */ + /* + if (apo->navigator()) { + apo->getHal()->debug->printf_P(PSTR("roll: %f deg\tpitch: %f deg\tyaw: %f deg\n"), + apo->navigator()->getRoll()*rad2Deg, + apo->navigator()->getPitch()*rad2Deg, + apo->navigator()->getYaw()*rad2Deg); + apo->getHal()->debug->printf_P(PSTR("lat: %f deg\tlon: %f deg\talt: %f m\n"), + apo->navigator()->getLat()*rad2Deg, + apo->navigator()->getLon()*rad2Deg, + apo->navigator()->getAlt()); + } + */ +} + +void AP_Autopilot::callback3(void * data) { + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->getHal()->debug->println_P(PSTR("callback 3")); + + /* + * send heartbeat + */ + apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + + /* + * load/loop rate/ram debug + */ + apo->getHal()->load = apo->load(); + apo->getHal()->debug->printf_P(PSTR("load: %d%%\trate: %f Hz\tfree ram: %d bytes\n"), + apo->load(),1.0/apo->dt(),freeMemory()); + + apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + + /* + * adc debug + */ + //apo->getDebug().printf_P(PSTR("adc: %d %d %d %d %d %d %d %d\n"), + //apo->adc()->Ch(0), apo->adc()->Ch(1), apo->adc()->Ch(2), + //apo->adc()->Ch(3), apo->adc()->Ch(4), apo->adc()->Ch(5), + //apo->adc()->Ch(6), apo->adc()->Ch(7), apo->adc()->Ch(8)); +} + +void AP_Autopilot::callback4(void * data) { + //AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->getHal()->debug->println_P(PSTR("callback 4")); +} + +} // apo diff --git a/libraries/APO/AP_Autopilot.h b/libraries/APO/AP_Autopilot.h new file mode 100644 index 0000000000..ceb7be2b76 --- /dev/null +++ b/libraries/APO/AP_Autopilot.h @@ -0,0 +1,165 @@ +/* + * AP_Autopilot.h + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#ifndef AP_AUTOPILOT_H_ +#define AP_AUTOPILOT_H_ + +/* + * AVR runtime + */ +#include +#include +#include +#include +/* + * Libraries + */ +#include "../AP_Common/AP_Common.h" +#include "../FastSerial/FastSerial.h" +#include "../AP_GPS/GPS.h" +#include "../APM_RC/APM_RC.h" +#include "../AP_ADC/AP_ADC.h" +#include "../APM_BMP085/APM_BMP085.h" +#include "../AP_Compass/AP_Compass.h" +#include "../AP_Math/AP_Math.h" +#include "../AP_IMU/AP_IMU.h" +#include "../AP_DCM/AP_DCM.h" +#include "../AP_Common/AP_Loop.h" +#include "../GCS_MAVLink/GCS_MAVLink.h" +#include "../AP_RangeFinder/AP_RangeFinder.h" +/* + * Local Modules + */ +#include "AP_HardwareAbstractionLayer.h" +#include "AP_RcChannel.h" +#include "AP_Controller.h" +#include "AP_Navigator.h" +#include "AP_Guide.h" +#include "AP_CommLink.h" + +/** + * ArduPilotOne namespace to protect varibles + * from overlap with avr and libraries etc. + * ArduPilotOne does not use any global + * variables. + */ +namespace apo { + +// forward declarations +class AP_CommLink; + +/** + * This class encapsulates the entire autopilot system + * The constructor takes guide, navigator, and controller + * as well as the hardware abstraction layer. + * + * It inherits from loop to manage + * the subloops and sets the overall + * frequency for the autopilot. + * + + */ +class AP_Autopilot: public Loop { +public: + /** + * Default constructor + */ + AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, + AP_Controller * controller, AP_HardwareAbstractionLayer * hal, + float loop0Rate, float loop1Rate, float loop2Rate, float loop3Rate); + + /** + * Accessors + */ + AP_Navigator * getNavigator() { + return _navigator; + } + AP_Guide * getGuide() { + return _guide; + } + AP_Controller * getController() { + return _controller; + } + AP_HardwareAbstractionLayer * getHal() { + return _hal; + } + + float getLoopRate(uint8_t i) { + switch(i) { + case 0: return _loop0Rate; + case 1: return _loop1Rate; + case 2: return _loop2Rate; + case 3: return _loop3Rate; + case 4: return _loop4Rate; + default: return 0; + } + } + +private: + + /** + * Loop 0 Callbacks (fastest) + * - inertial navigation + * @param data A void pointer used to pass the apo class + * so that the apo public interface may be accessed. + */ + static void callback0(void * data); + float _loop0Rate; + + /** + * Loop 1 Callbacks + * - control + * - compass reading + * @see callback0 + */ + static void callback1(void * data); + float _loop1Rate; + + /** + * Loop 2 Callbacks + * - gps sensor fusion + * - compass sensor fusion + * @see callback0 + */ + static void callback2(void * data); + float _loop2Rate; + + /** + * Loop 3 Callbacks + * - slow messages + * @see callback0 + */ + static void callback3(void * data); + float _loop3Rate; + + /** + * Loop 4 Callbacks + * - super slow mesages + * - log writing + * @see callback0 + */ + static void callback4(void * data); + float _loop4Rate; + + /** + * Components + */ + AP_Navigator * _navigator; + AP_Guide * _guide; + AP_Controller * _controller; + AP_HardwareAbstractionLayer * _hal; + + /** + * Constants + */ + static const float deg2rad = M_PI / 180; + static const float rad2deg = 180 / M_PI; +}; + +} // namespace apo + +#endif /* AP_AUTOPILOT_H_ */ diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp new file mode 100644 index 0000000000..92b9d0fdde --- /dev/null +++ b/libraries/APO/AP_CommLink.cpp @@ -0,0 +1,15 @@ +/* + * AP_CommLink.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_CommLink.h" + +namespace apo { + +uint8_t MavlinkComm::_nChannels = 0; +uint8_t MavlinkComm::_paramNameLengthMax = 13; + +} // apo diff --git a/libraries/APO/AP_CommLink.h b/libraries/APO/AP_CommLink.h new file mode 100644 index 0000000000..08d20be57f --- /dev/null +++ b/libraries/APO/AP_CommLink.h @@ -0,0 +1,790 @@ +/* + * AP_CommLink.h + * Copyright (C) James Goppert 2010 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#ifndef AP_CommLink_H +#define AP_CommLink_H + +#include "AP_HardwareAbstractionLayer.h" +#include "AP_MavlinkCommand.h" +#include "AP_Controller.h" + +namespace apo { + +class AP_Controller; +class AP_Navigator; +class AP_Guide; +class AP_HardwareAbstractionLayer; + +enum { + SEVERITY_LOW, SEVERITY_MED, SEVERITY_HIGH +}; + +// forward declarations +//class ArduPilotOne; +//class AP_Controller; + +/// CommLink class +class AP_CommLink { +public: + + AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide, + AP_Controller * controller, AP_HardwareAbstractionLayer * hal) : + _link(link), _navigator(navigator), _guide(guide), + _controller(controller), _hal(hal) { + } + virtual void send() = 0; + virtual void receive() = 0; + virtual void sendMessage(uint8_t id, uint32_t param = 0) = 0; + virtual void sendText(uint8_t severity, const char *str) = 0; + virtual void sendText(uint8_t severity, const prog_char_t *str) = 0; + virtual void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) = 0; + virtual void sendParameters() = 0; + virtual void requestCmds() = 0; + +protected: + FastSerial * _link; + AP_Navigator * _navigator; + AP_Guide * _guide; + AP_Controller * _controller; + AP_HardwareAbstractionLayer * _hal; +}; + +class MavlinkComm: public AP_CommLink { +public: + MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide, + AP_Controller * controller, AP_HardwareAbstractionLayer * hal) : + AP_CommLink(link, nav, guide, controller, hal), + + // options + _useRelativeAlt(true), + + // commands + _sendingCmds(false), _receivingCmds(false), + _cmdTimeLastSent(millis()), _cmdTimeLastReceived(millis()), + _cmdDestSysId(0), _cmdDestCompId(0), _cmdRequestIndex(0), + _cmdMax(30), _cmdNumberRequested(0), + + // parameters + _parameterCount(0), _queuedParameter(NULL), + _queuedParameterIndex(0) { + + switch (_nChannels) { + case 0: + mavlink_comm_0_port = link; + _channel = MAVLINK_COMM_0; + _nChannels++; + break; + case 1: + mavlink_comm_1_port = link; + _channel = MAVLINK_COMM_1; + _nChannels++; + break; + default: + // signal that number of channels exceeded + _channel = MAVLINK_COMM_3; + break; + } + } + + virtual void send() { + // if number of channels exceeded return + if (_channel == MAVLINK_COMM_3) + return; + } + + void sendMessage(uint8_t id, uint32_t param = 0) { + //_hal->debug->printf_P(PSTR("send message\n")); + + // if number of channels exceeded return + if (_channel == MAVLINK_COMM_3) + return; + + uint64_t timeStamp = micros(); + + switch (id) { + + case MAVLINK_MSG_ID_HEARTBEAT: { + mavlink_msg_heartbeat_send(_channel, mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); + break; + } + + case MAVLINK_MSG_ID_ATTITUDE: { + mavlink_msg_attitude_send(_channel, timeStamp, + _navigator->getRoll(), _navigator->getPitch(), + _navigator->getYaw(), _navigator->getRollRate(), + _navigator->getPitchRate(), _navigator->getYawRate()); + break; + } + + case MAVLINK_MSG_ID_GLOBAL_POSITION: { + mavlink_msg_global_position_send(_channel, timeStamp, + _navigator->getLat() * rad2Deg, + _navigator->getLon() * rad2Deg, _navigator->getAlt(), + _navigator->getVN(), _navigator->getVE(), + _navigator->getVD()); + break; + } + + case MAVLINK_MSG_ID_GPS_RAW: { + mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(), + _navigator->getLat() * rad2Deg, + _navigator->getLon() * rad2Deg, _navigator->getAlt(), 0, 0, + _navigator->getGroundSpeed(), + _navigator->getYaw() * rad2Deg); + break; + } + + /* + case MAVLINK_MSG_ID_GPS_RAW_INT: { + mavlink_msg_gps_raw_int_send(_channel,timeStamp,_hal->gps->status(), + _navigator->getLat_degInt(), _navigator->getLon_degInt(),_navigator->getAlt_intM(), 0,0, + _navigator->getGroundSpeed(),_navigator->getYaw()*rad2Deg); + break; + } + */ + + 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 + } + + case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: { + int16_t ch[8]; + for (int i = 0; i < 8; i++) + ch[i] = 0; + for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) { + ch[i] = 10000 * _hal->rc[i]->getPosition(); + //_hal->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]); + } + mavlink_msg_rc_channels_scaled_send(_channel, ch[0], ch[1], ch[2], + ch[3], ch[4], ch[5], ch[6], ch[7], 255); + break; + } + + case MAVLINK_MSG_ID_RC_CHANNELS_RAW: { + int16_t ch[8]; + for (int i = 0; i < 8; i++) + ch[i] = 0; + for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) + ch[i] = _hal->rc[i]->getPwm(); + mavlink_msg_rc_channels_raw_send(_channel, ch[0], ch[1], ch[2], + ch[3], ch[4], ch[5], ch[6], ch[7], 255); + break; + } + + case MAVLINK_MSG_ID_SYS_STATUS: { + + float batteryVoltage, temp; + temp = analogRead(0); + batteryVoltage = ((temp * 5 / 1023) / 0.28); + + mavlink_msg_sys_status_send(_channel, _controller->getMode(), + _guide->getMode(), _hal->getState(), _hal->load * 10, + batteryVoltage * 1000, + (batteryVoltage - 3.3) / (4.2 - 3.3) * 1000, _packetDrops); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: { + sendText(SEVERITY_LOW, PSTR("waypoint ack")); + //mavlink_waypoint_ack_t packet; + uint8_t type = 0; // ok (0), error(1) + mavlink_msg_waypoint_ack_send(_channel, _cmdDestSysId, + _cmdDestCompId, type); + + // turn off waypoint send + _receivingCmds = false; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CURRENT: { + mavlink_msg_waypoint_current_send(_channel, + _guide->getCurrentIndex()); + break; + } + + default: { + char msg[50]; + sprintf(msg, "autopilot sending unknown command with id: %d", id); + sendText(SEVERITY_HIGH, msg); + } + + } // switch + } // send message + + virtual void receive() { + //_hal->debug->printf_P(PSTR("receive\n")); + // if number of channels exceeded return + // + if (_channel == MAVLINK_COMM_3) + return; + + // receive new packets + mavlink_message_t msg; + mavlink_status_t status; + + // process received bytes + while (comm_get_available(_channel)) { + uint8_t c = comm_receive_ch(_channel); + + // Try to get a new message + if (mavlink_parse_char(_channel, c, &msg, &status)) + _handleMessage(&msg); + } + + // Update packet drops counter + _packetDrops += status.packet_rx_drop_count; + } + + void sendText(uint8_t severity, const char *str) { + mavlink_msg_statustext_send(_channel, severity, (const int8_t*) str); + } + + void sendText(uint8_t severity, const prog_char_t *str) { + mavlink_statustext_t m; + uint8_t i; + for (i = 0; i < sizeof(m.text); i++) { + m.text[i] = pgm_read_byte((const prog_char *) (str++)); + } + if (i < sizeof(m.text)) + m.text[i] = 0; + sendText(severity, (const char *) m.text); + } + + void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) { + } + + /** + * sends parameters one at a time + */ + void sendParameters() { + //_hal->debug->printf_P(PSTR("send parameters\n")); + // Check to see if we are sending parameters + while (NULL != _queuedParameter) { + AP_Var *vp; + float value; + + // copy the current parameter and prepare to move to the next + vp = _queuedParameter; + _queuedParameter = _queuedParameter->next(); + + // if the parameter can be cast to float, report it here and break out of the loop + value = vp->cast_to_float(); + if (!isnan(value)) { + + char paramName[_paramNameLengthMax]; + vp->copy_name(paramName, sizeof(paramName)); + + mavlink_msg_param_value_send(_channel, (int8_t*) paramName, + value, _countParameters(), _queuedParameterIndex); + + _queuedParameterIndex++; + break; + } + } + + } + + /** + * request commands one at a time + */ + void requestCmds() { + //_hal->debug->printf_P(PSTR("requesting commands\n")); + // request cmds one by one + if (_receivingCmds && _cmdRequestIndex <= _cmdNumberRequested) { + mavlink_msg_waypoint_request_send(_channel, _cmdDestSysId, + _cmdDestCompId, _cmdRequestIndex); + } + } + +private: + + // options + bool _useRelativeAlt; + + // commands + bool _sendingCmds; + bool _receivingCmds; + uint16_t _cmdTimeLastSent; + uint16_t _cmdTimeLastReceived; + uint16_t _cmdDestSysId; + uint16_t _cmdDestCompId; + uint16_t _cmdRequestIndex; + uint16_t _cmdNumberRequested; + uint16_t _cmdMax; + Vector _cmdList; + + // parameters + static uint8_t _paramNameLengthMax; + uint16_t _parameterCount; + AP_Var * _queuedParameter; + uint16_t _queuedParameterIndex; + + // channel + mavlink_channel_t _channel; + uint16_t _packetDrops; + static uint8_t _nChannels; + + void _handleMessage(mavlink_message_t * msg) { + + uint32_t timeStamp = micros(); + + switch (msg->msgid) { + //_hal->debug->printf_P(PSTR("message received: %d"), msg->msgid); + + case MAVLINK_MSG_ID_HEARTBEAT: { + mavlink_heartbeat_t packet; + mavlink_msg_heartbeat_decode(msg, &packet); + _hal->lastHeartBeat = micros(); + break; + } + + case MAVLINK_MSG_ID_GPS_RAW: { + // decode + mavlink_gps_raw_t packet; + mavlink_msg_gps_raw_decode(msg, &packet); + + _navigator->setTimeStamp(timeStamp); + _navigator->setLat(packet.lat * deg2Rad); + _navigator->setLon(packet.lon * deg2Rad); + _navigator->setAlt(packet.alt); + _navigator->setYaw(packet.hdg * deg2Rad); + _navigator->setGroundSpeed(packet.v); + _navigator->setAirSpeed(packet.v); + //_hal->debug->printf_P(PSTR("received hil gps raw packet\n")); + /* + _hal->debug->printf_P(PSTR("received lat: %f deg\tlon: %f deg\talt: %f m\n"), + packet.lat, + packet.lon, + packet.alt); + */ + break; + } + + case MAVLINK_MSG_ID_ATTITUDE: { + // decode + mavlink_attitude_t packet; + mavlink_msg_attitude_decode(msg, &packet); + + // set dcm hil sensor + _navigator->setTimeStamp(timeStamp); + _navigator->setRoll(packet.roll); + _navigator->setPitch(packet.pitch); + _navigator->setYaw(packet.yaw); + _navigator->setRollRate(packet.rollspeed); + _navigator->setPitchRate(packet.pitchspeed); + _navigator->setYawRate(packet.yawspeed); + //_hal->debug->printf_P(PSTR("received hil attitude packet\n")); + break; + } + + case MAVLINK_MSG_ID_ACTION: { + // decode + mavlink_action_t packet; + mavlink_msg_action_decode(msg, &packet); + if (_checkTarget(packet.target, packet.target_component)) + break; + + // do action + sendText(SEVERITY_LOW, PSTR("action received")); + switch (packet.action) { + + case MAV_ACTION_STORAGE_READ: + AP_Var::load_all(); + break; + + case MAV_ACTION_STORAGE_WRITE: + AP_Var::save_all(); + break; + + case MAV_ACTION_CALIBRATE_RC: + case MAV_ACTION_CALIBRATE_GYRO: + case MAV_ACTION_CALIBRATE_MAG: + case MAV_ACTION_CALIBRATE_ACC: + case MAV_ACTION_CALIBRATE_PRESSURE: + 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: + sendText(SEVERITY_LOW, PSTR("unknown action")); + break; + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: { + sendText(SEVERITY_LOW, PSTR("waypoint request list")); + + // decode + mavlink_waypoint_request_list_t packet; + mavlink_msg_waypoint_request_list_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // Start sending waypoints + mavlink_msg_waypoint_count_send(_channel, msg->sysid, msg->compid, + _guide->getNumberOfCommands()); + + _cmdTimeLastSent = millis(); + _cmdTimeLastReceived = millis(); + _sendingCmds = true; + _receivingCmds = false; + _cmdDestSysId = msg->sysid; + _cmdDestCompId = msg->compid; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: { + sendText(SEVERITY_LOW, PSTR("waypoint request")); + + // Check if sending waypiont + if (!_sendingCmds) + break; + + // decode + mavlink_waypoint_request_t packet; + mavlink_msg_waypoint_request_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + _hal->debug->printf_P(PSTR("sequence: %d\n"),packet.seq); + AP_MavlinkCommand cmd(packet.seq); + cmd.load(); + + mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex()); + mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId, + wp.seq, wp.frame, wp.command, wp.current, wp.autocontinue, + wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y, + wp.z); + + // update last waypoint comm stamp + _cmdTimeLastSent = millis(); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: { + sendText(SEVERITY_LOW, PSTR("waypoint ack")); + + // decode + mavlink_waypoint_ack_t packet; + mavlink_msg_waypoint_ack_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // check for error + //uint8_t type = packet.type; // ok (0), error(1) + + // turn off waypoint send + _sendingCmds = false; + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + sendText(SEVERITY_LOW, PSTR("param request list")); + + // decode + mavlink_param_request_list_t packet; + mavlink_msg_param_request_list_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // Start sending parameters - next call to ::update will kick the first one out + + _queuedParameter = AP_Var::first(); + _queuedParameterIndex = 0; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: { + sendText(SEVERITY_LOW, PSTR("waypoint clear all")); + + // decode + mavlink_waypoint_clear_all_t packet; + mavlink_msg_waypoint_clear_all_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // clear all waypoints + uint8_t type = 0; // ok (0), error(1) + _guide->setNumberOfCommands(1); + _guide->setCurrentIndex(0); + + // send acknowledgement 3 times to makes sure it is received + for (int i = 0; i < 3; i++) + mavlink_msg_waypoint_ack_send(_channel, msg->sysid, + msg->compid, type); + + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: { + sendText(SEVERITY_LOW, PSTR("waypoint set current")); + + // decode + mavlink_waypoint_set_current_t packet; + mavlink_msg_waypoint_set_current_decode(msg, &packet); + Serial.print("Packet Sequence:"); + Serial.println(packet.seq); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // set current waypoint + Serial.print("Current Index:"); + Serial.println(_guide->getCurrentIndex()); + Serial.flush(); + _guide->setCurrentIndex(packet.seq); + mavlink_msg_waypoint_current_send(_channel, + _guide->getCurrentIndex()); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_COUNT: { + sendText(SEVERITY_LOW, PSTR("waypoint count")); + + // decode + mavlink_waypoint_count_t packet; + mavlink_msg_waypoint_count_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // start waypoint receiving + if (packet.count > _cmdMax) { + packet.count = _cmdMax; + } + _cmdNumberRequested = packet.count; + _cmdTimeLastReceived = millis(); + _receivingCmds = true; + _sendingCmds = false; + _cmdRequestIndex = 0; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT: { + sendText(SEVERITY_LOW, PSTR("waypoint")); + + // Check if receiving waypiont + if (!_receivingCmds) { + //sendText(SEVERITY_HIGH, PSTR("not receiving commands")); + break; + } + + // decode + mavlink_waypoint_t packet; + mavlink_msg_waypoint_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // check if this is the requested waypoint + if (packet.seq != _cmdRequestIndex) { + char warningMsg[50]; + sprintf(warningMsg, + "waypoint request out of sequence: (packet) %d / %d (ap)", + packet.seq, _cmdRequestIndex); + sendText(SEVERITY_HIGH, warningMsg); + break; + } + + _hal->debug->printf_P(PSTR("received waypoint x: %f\ty: %f\tz: %f\n"), + packet.x, + packet.y, + packet.z); + + // store waypoint + AP_MavlinkCommand command(packet); + //sendText(SEVERITY_HIGH, PSTR("waypoint stored")); + _cmdRequestIndex++; + if (_cmdRequestIndex == _cmdNumberRequested) { + sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK); + _receivingCmds = false; + _guide->setNumberOfCommands(_cmdNumberRequested); + //sendText(SEVERITY_LOW, PSTR("waypoint ack sent")); + } else if (_cmdRequestIndex > _cmdNumberRequested) { + _receivingCmds = false; + } + _cmdTimeLastReceived = millis(); + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: { + sendText(SEVERITY_LOW, PSTR("param set")); + AP_Var *vp; + AP_Meta_class::Type_id var_type; + + // decode + mavlink_param_set_t packet; + mavlink_msg_param_set_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // set parameter + + char key[_paramNameLengthMax + 1]; + strncpy(key, (char *) packet.param_id, _paramNameLengthMax); + key[_paramNameLengthMax] = 0; + + // find the requested parameter + vp = AP_Var::find(key); + if ((NULL != vp) && // exists + !isnan(packet.param_value) && // not nan + !isinf(packet.param_value)) { // not inf + + // add a small amount before casting parameter values + // from float to integer to avoid truncating to the + // next lower integer value. + const float rounding_addition = 0.01; + + // fetch the variable type ID + var_type = vp->meta_type_id(); + + // handle variables with standard type IDs + if (var_type == AP_Var::k_typeid_float) { + ((AP_Float *) vp)->set_and_save(packet.param_value); + + } else if (var_type == AP_Var::k_typeid_float16) { + ((AP_Float16 *) vp)->set_and_save(packet.param_value); + + } else if (var_type == AP_Var::k_typeid_int32) { + ((AP_Int32 *) vp)->set_and_save( + packet.param_value + rounding_addition); + + } else if (var_type == AP_Var::k_typeid_int16) { + ((AP_Int16 *) vp)->set_and_save( + packet.param_value + rounding_addition); + + } else if (var_type == AP_Var::k_typeid_int8) { + ((AP_Int8 *) vp)->set_and_save( + packet.param_value + rounding_addition); + } else { + // we don't support mavlink set on this parameter + break; + } + + // Report back the new value if we accepted the change + // we send the value we actually set, which could be + // different from the value sent, in case someone sent + // a fractional value to an integer type + mavlink_msg_param_value_send(_channel, (int8_t *) key, + vp->cast_to_float(), _countParameters(), -1); // XXX we don't actually know what its index is... + } + + break; + } // end case + + + } + } + + uint16_t _countParameters() { + // if we haven't cached the parameter count yet... + if (0 == _parameterCount) { + AP_Var *vp; + + vp = AP_Var::first(); + do { + // if a parameter responds to cast_to_float then we are going to be able to report it + if (!isnan(vp->cast_to_float())) { + _parameterCount++; + } + } while (NULL != (vp = vp->next())); + } + return _parameterCount; + } + + AP_Var * _findParameter(uint16_t index) { + AP_Var *vp; + + vp = AP_Var::first(); + while (NULL != vp) { + + // if the parameter is reportable + if (!(isnan(vp->cast_to_float()))) { + // if we have counted down to the index we want + if (0 == index) { + // return the parameter + return vp; + } + // count off this parameter, as it is reportable but not + // the one we want + index--; + } + // and move to the next parameter + vp = vp->next(); + } + return NULL; + } + + // check the target + uint8_t _checkTarget(uint8_t sysid, uint8_t compid) { + /* + char msg[50]; + sprintf(msg, "target = %d / %d\tcomp = %d / %d", sysid, + mavlink_system.sysid, compid, mavlink_system.compid); + sendText(SEVERITY_LOW, msg); + */ + if (sysid != mavlink_system.sysid) { + //sendText(SEVERITY_LOW, PSTR("system id mismatch")); + return 1; + + } else if (compid != mavlink_system.compid) { + //sendText(SEVERITY_LOW, PSTR("component id mismatch")); + return 0; // XXX currently not receiving correct compid from gcs + + } else { + return 0; // no error + } + } + +}; + +} // namespace apo + +#endif // AP_CommLink_H +// vim:ts=4:sw=4:tw=78:expandtab diff --git a/libraries/APO/AP_Controller.cpp b/libraries/APO/AP_Controller.cpp new file mode 100644 index 0000000000..ebbd5ed524 --- /dev/null +++ b/libraries/APO/AP_Controller.cpp @@ -0,0 +1,8 @@ +/* + * AP_Controller.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_Controller.h" diff --git a/libraries/APO/AP_Controller.h b/libraries/APO/AP_Controller.h new file mode 100644 index 0000000000..d7dac0c60c --- /dev/null +++ b/libraries/APO/AP_Controller.h @@ -0,0 +1,304 @@ +/* + * AP_Controller.h + * Copyright (C) James Goppert 2010 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#ifndef AP_Controller_H +#define AP_Controller_H + +#include "AP_Navigator.h" +#include "AP_Guide.h" +#include "AP_HardwareAbstractionLayer.h" +#include "../AP_Common/AP_Vector.h" +#include "../AP_Common/AP_Var.h" + +namespace apo { + +/// Controller class +class AP_Controller { +public: + AP_Controller(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + _nav(nav), _guide(guide), _hal(hal) { + } + + virtual void update(const float & dt) = 0; + + virtual MAV_MODE getMode() = 0; + + void setAllRadioChannelsToNeutral() { + for (uint8_t i = 0; i < _hal->rc.getSize(); i++) { + _hal->rc[i]->setPosition(0.0); + } + } + + void setAllRadioChannelsManually() { + for (uint8_t i = 0; i < _hal->rc.getSize(); i++) { + _hal->rc[i]->setUsingRadio(); + } + } + +protected: + AP_Navigator * _nav; + AP_Guide * _guide; + AP_HardwareAbstractionLayer * _hal; +}; + +class AP_ControllerBlock { +public: + AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart, + uint8_t groupLength) : + _group(group), _groupStart(groupStart), + _groupEnd(groupStart + groupLength) { + } + uint8_t getGroupEnd() { + return _groupEnd; + } +protected: + AP_Var_group * _group; /// helps with parameter management + uint8_t _groupStart; + uint8_t _groupEnd; +}; + +class BlockLowPass: public AP_ControllerBlock { +public: + BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut, + const prog_char_t * fCutLabel = NULL) : + AP_ControllerBlock(group, groupStart, 1), + _fCut(group, groupStart, fCut, fCutLabel ? : PSTR("fCut")), + _y(0) { + } + float update(const float & input, const float & dt) { + float RC = 1 / (2 * M_PI * _fCut); // low pass filter + _y = _y + (input - _y) * (dt / (dt + RC)); + return _y; + } +protected: + AP_Float _fCut; + float _y; +}; + +class BlockSaturation: public AP_ControllerBlock { +public: + BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax, + const prog_char_t * yMaxLabel = NULL) : + AP_ControllerBlock(group, groupStart, 1), + _yMax(group, groupStart, yMax, yMaxLabel ? : PSTR("yMax")) { + } + float update(const float & input) { + + // pid sum + float y = input; + + // saturation + if (y > _yMax) + y = _yMax; + if (y < -_yMax) + y = -_yMax; + return y; + } +protected: + AP_Float _yMax; /// output saturation +}; + +class BlockDerivative { +public: + BlockDerivative() : + _lastInput(0), firstRun(true) { + } + float update(const float & input, const float & dt) { + float derivative = (input - _lastInput) / dt; + _lastInput = input; + if (firstRun) { + firstRun = false; + return 0; + } else + return derivative; + } +protected: + float _lastInput; /// last input + bool firstRun; +}; + +class BlockIntegral { +public: + BlockIntegral() : + _i(0) { + } + float update(const float & input, const float & dt) { + _i += input * dt; + return _i; + } +protected: + float _i; /// integral +}; + +class BlockP: public AP_ControllerBlock { +public: + BlockP(AP_Var_group * group, uint8_t groupStart, float kP, + const prog_char_t * kPLabel = NULL) : + AP_ControllerBlock(group, groupStart, 1), + _kP(group, groupStart, kP, kPLabel ? : PSTR("p")) { + } + + float update(const float & input) { + return _kP * input; + } +protected: + AP_Float _kP; /// proportional gain +}; + +class BlockI: public AP_ControllerBlock { +public: + BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax, + const prog_char_t * kILabel = NULL, + const prog_char_t * iMaxLabel = NULL) : + AP_ControllerBlock(group, groupStart, 2), + _kI(group, groupStart, kI, kILabel ? : PSTR("i")), + _blockSaturation(group, groupStart + 1, iMax, iMaxLabel ? : PSTR("iMax")), + _eI(0) { + } + + float update(const float & input, const float & dt) { + // integral + _eI += input * dt; + _eI = _blockSaturation.update(_eI); + return _kI * _eI; + } +protected: + AP_Float _kI; /// integral gain + BlockSaturation _blockSaturation; /// integrator saturation + float _eI; /// integral of input +}; + +class BlockD: public AP_ControllerBlock { +public: + BlockD(AP_Var_group * group, uint8_t groupStart, float kD, uint8_t dFCut, + const prog_char_t * kDLabel = NULL, + const prog_char_t * dFCutLabel = NULL) : + AP_ControllerBlock(group, groupStart, 2), + _blockLowPass(group, groupStart, dFCut, + dFCutLabel ? : PSTR("dFCut")), + _kD(group, _blockLowPass.getGroupEnd(), kD, + kDLabel ? : PSTR("d")), _eP(0) { + } + float update(const float & input, const float & dt) { + // derivative with low pass + float _eD = _blockLowPass.update((_eP - input) / dt, dt); + // proportional, note must come after derivative + // because derivatve uses _eP as previous value + _eP = input; + return _kD * _eD; + } +protected: + BlockLowPass _blockLowPass; + AP_Float _kD; /// derivative gain + float _eP; /// input +}; + +class BlockPID: public AP_ControllerBlock { +public: + BlockPID(AP_Var_group * group, uint8_t groupStart, float kP, float kI, + float kD, float iMax, float yMax, uint8_t dFcut) : + AP_ControllerBlock(group, groupStart, 6), + _blockP(group, groupStart, kP), + _blockI(group, _blockP.getGroupEnd(), kI, iMax), + _blockD(group, _blockI.getGroupEnd(), kD, dFcut), + _blockSaturation(group, _blockD.getGroupEnd(), yMax) { + } + + float update(const float & input, const float & dt) { + return _blockSaturation.update( + _blockP.update(input) + _blockI.update(input, dt) + + _blockD.update(input, dt)); + } +protected: + BlockP _blockP; + BlockI _blockI; + BlockD _blockD; + BlockSaturation _blockSaturation; +}; + +class BlockPI: public AP_ControllerBlock { +public: + BlockPI(AP_Var_group * group, uint8_t groupStart, float kP, float kI, + float iMax, float yMax) : + AP_ControllerBlock(group, groupStart, 4), + _blockP(group, groupStart, kP), + _blockI(group, _blockP.getGroupEnd(), kI, iMax), + _blockSaturation(group, _blockI.getGroupEnd(), yMax) { + } + + float update(const float & input, const float & dt) { + + float y = _blockP.update(input) + _blockI.update(input, dt); + return _blockSaturation.update(y); + } +protected: + BlockP _blockP; + BlockI _blockI; + BlockSaturation _blockSaturation; +}; + +class BlockPD: public AP_ControllerBlock { +public: + BlockPD(AP_Var_group * group, uint8_t groupStart, float kP, float kI, + float kD, float iMax, float yMax, uint8_t dFcut) : + AP_ControllerBlock(group, groupStart, 4), + _blockP(group, groupStart, kP), + _blockD(group, _blockP.getGroupEnd(), kD, dFcut), + _blockSaturation(group, _blockD.getGroupEnd(), yMax) { + } + + float update(const float & input, const float & dt) { + + float y = _blockP.update(input) + _blockD.update(input, dt); + return _blockSaturation.update(y); + } +protected: + BlockP _blockP; + BlockD _blockD; + BlockSaturation _blockSaturation; +}; + +/// PID with derivative feedback (ignores reference derivative) +class BlockPIDDfb: public AP_ControllerBlock { +public: + BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI, + float kD, float iMax, float yMax, const prog_char_t * dLabel = NULL) : + AP_ControllerBlock(group, groupStart, 5), + _blockP(group, groupStart, kP), + _blockI(group, _blockP.getGroupEnd(), kI, iMax), + _blockSaturation(group, _blockI.getGroupEnd(), yMax), + _kD(group, _blockSaturation.getGroupEnd(), kD, dLabel ? : PSTR("d")) { + } + float update(const float & input, const float & derivative, + const float & dt) { + float y = _blockP.update(input) + _blockI.update(input, dt) - _kD + * derivative; + return _blockSaturation.update(y); + } +protected: + BlockP _blockP; + BlockI _blockI; + BlockSaturation _blockSaturation; + AP_Float _kD; /// derivative gain +}; + +} // apo + +#endif // AP_Controller_H +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp new file mode 100644 index 0000000000..488b746610 --- /dev/null +++ b/libraries/APO/AP_Guide.cpp @@ -0,0 +1,8 @@ +/* + * AP_Guide.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_Guide.h" diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h new file mode 100644 index 0000000000..fb4352f830 --- /dev/null +++ b/libraries/APO/AP_Guide.h @@ -0,0 +1,359 @@ +/* + * AP_Guide.h + * Copyright (C) James Goppert 2010 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#ifndef AP_Guide_H +#define AP_Guide_H + +#include "../GCS_MAVLink/GCS_MAVLink.h" +#include "AP_HardwareAbstractionLayer.h" +#include "AP_Navigator.h" +#include "../AP_Common/AP_Common.h" +#include "../AP_Common/AP_Vector.h" +#include "AP_MavlinkCommand.h" +#include "constants.h" + +//#include "AP_CommLink.h" + +namespace apo { + +/// Guide class +class AP_Guide { +public: + + /** + * This is the constructor, which requires a link to the navigator. + * @param navigator This is the navigator pointer. + */ + AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal) : + _navigator(navigator), _hal(hal), _command(0), _previousCommand(0), + _headingCommand(0), _airSpeedCommand(0), + _groundSpeedCommand(0), _altitudeCommand(0), _pNCmd(0), + _pECmd(0), _pDCmd(0), _mode(MAV_NAV_LOST), + _numberOfCommands(1), _cmdIndex(0), _nextCommandCalls(0), + _nextCommandTimer(0) { + } + + virtual void update() = 0; + + virtual void nextCommand() = 0; + + MAV_NAV getMode() const { + return _mode; + } + uint8_t getCurrentIndex() { + return _cmdIndex; + } + + void setCurrentIndex(uint8_t val) { + _cmdIndex.set_and_save(val); + _command = AP_MavlinkCommand(getCurrentIndex()); + _command.load(); + _previousCommand = AP_MavlinkCommand(getPreviousIndex()); + _previousCommand.load(); + //_hal->gcs->sendMessage(MAVLINK_MSG_ID_WAYPOINT_CURRENT); + } + + uint8_t getNumberOfCommands() { + return _numberOfCommands; + } + + void setNumberOfCommands(uint8_t val) { + _numberOfCommands.set_and_save(val); + } + + uint8_t getPreviousIndex() { + // find previous waypoint, TODO, handle non-nav commands + int16_t prevIndex = int16_t(getCurrentIndex()) - 1; + if (prevIndex < 0) + prevIndex = getNumberOfCommands() - 1; + return (uint8_t) prevIndex; + } + + uint8_t getNextIndex() { + // find previous waypoint, TODO, handle non-nav commands + int16_t nextIndex = int16_t(getCurrentIndex()) + 1; + if (nextIndex > (getNumberOfCommands() - 1)) + nextIndex = 0; + return nextIndex; + } + + float getHeadingCommand() { + return _headingCommand; + } + float getAirSpeedCommand() { + return _airSpeedCommand; + } + float getGroundSpeedCommand() { + return _groundSpeedCommand; + } + float getAltitudeCommand() { + return _altitudeCommand; + } + float getPNCmd() { + return _pNCmd; + } + float getPECmd() { + return _pECmd; + } + float getPDCmd() { + return _pDCmd; + } + MAV_NAV getMode() { + return _mode; + } + uint8_t getCommandIndex() { + return _cmdIndex; + } + +protected: + AP_Navigator * _navigator; + AP_HardwareAbstractionLayer * _hal; + AP_MavlinkCommand _command, _previousCommand; + float _headingCommand; + float _airSpeedCommand; + float _groundSpeedCommand; + float _altitudeCommand; + float _pNCmd; + float _pECmd; + float _pDCmd; + MAV_NAV _mode; + AP_Uint8 _numberOfCommands; + AP_Uint8 _cmdIndex; + uint16_t _nextCommandCalls; + uint16_t _nextCommandTimer; +}; + +class MavlinkGuide: public AP_Guide { +public: + MavlinkGuide(AP_Navigator * navigator, + AP_HardwareAbstractionLayer * hal) : + AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(), + _rangeFinderLeft(), _rangeFinderRight(), + _group(k_guide, PSTR("guide_")), + _velocityCommand(&_group, 1, 1, PSTR("velCmd")), + _crossTrackGain(&_group, 2, 2, PSTR("xt")), + _crossTrackLim(&_group, 3, 10, PSTR("xtLim")) { + + for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) { + RangeFinder * rF = _hal->rangeFinders[i]; + if (rF == NULL) + continue; + if (rF->orientation_x == 1 && rF->orientation_y == 0 + && rF->orientation_z == 0) + _rangeFinderFront = rF; + else if (rF->orientation_x == -1 && rF->orientation_y == 0 + && rF->orientation_z == 0) + _rangeFinderBack = rF; + else if (rF->orientation_x == 0 && rF->orientation_y == 1 + && rF->orientation_z == 0) + _rangeFinderRight = rF; + else if (rF->orientation_x == 0 && rF->orientation_y == -1 + && rF->orientation_z == 0) + _rangeFinderLeft = rF; + + } + } + + virtual void update() { +// _hal->debug->printf_P( +// PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"), +// getNumberOfCommands(), +// getCurrentIndex(), +// getPreviousIndex()); + + // if we don't have enough waypoint for cross track calcs + // go home + if (_numberOfCommands == 1) { + _mode = MAV_NAV_RETURNING; + _altitudeCommand = AP_MavlinkCommand::home.getAlt(); + _headingCommand = AP_MavlinkCommand::home.bearingTo( + _navigator->getLat_degInt(), _navigator->getLon_degInt()) + + 180 * deg2Rad; + if (_headingCommand > 360 * deg2Rad) + _headingCommand -= 360 * deg2Rad; + +// _hal->debug->printf_P(PSTR("going home: bearing: %f distance: %f\n"), +// headingCommand,AP_MavlinkCommand::home.distanceTo(_navigator->getLat_degInt(),_navigator->getLon_degInt())); + } else { + _mode = MAV_NAV_WAYPOINT; + _altitudeCommand = _command.getAlt(); + // TODO wrong behavior if 0 selected as waypoint, says previous 0 + float dXt = _command.crossTrack(_previousCommand, + _navigator->getLat_degInt(), + _navigator->getLon_degInt()); + float temp = dXt * _crossTrackGain * deg2Rad; // crosstrack gain, rad/m + if (temp > _crossTrackLim * deg2Rad) + temp = _crossTrackLim * deg2Rad; + if (temp < -_crossTrackLim * deg2Rad) + temp = -_crossTrackLim * deg2Rad; + float bearing = _previousCommand.bearingTo(_command); + _headingCommand = bearing - temp; + float alongTrack = _command.alongTrack(_previousCommand, + _navigator->getLat_degInt(), + _navigator->getLon_degInt()); + float distanceToNext = _command.distanceTo( + _navigator->getLat_degInt(), _navigator->getLon_degInt()); + float segmentLength = _previousCommand.distanceTo(_command); + if (distanceToNext < _command.getRadius() || alongTrack + > segmentLength) + { + Serial.println("radius reached"); + nextCommand(); + } +// _hal->debug->printf_P( +// PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\n"), +// bearing * rad2Deg, dXt, _headingCommand * rad2Deg, distanceToNext, alongTrack); + } + + _groundSpeedCommand = _velocityCommand; + + // TODO : calculate pN,pE,pD from home and gps coordinates + _pNCmd = _command.getPN(_navigator->getLat_degInt(), + _navigator->getLon_degInt()); + _pECmd = _command.getPE(_navigator->getLat_degInt(), + _navigator->getLon_degInt()); + _pDCmd = _command.getPD(_navigator->getAlt_intM()); + + // process mavlink commands + //handleCommand(); + + // obstacle avoidance overrides + // stop if your going to drive into something in front of you + for (uint8_t i = 0; i < _hal->rangeFinders.getSize(); i++) + _hal->rangeFinders[i]->read(); + float frontDistance = _rangeFinderFront->distance / 200.0; //convert for other adc + if (_rangeFinderFront && frontDistance < 2) { + _mode = MAV_NAV_VECTOR; + //airSpeedCommand = 0; + //groundSpeedCommand = 0; +// _headingCommand -= 45 * deg2Rad; +// _hal->debug->print("Obstacle Distance (m): "); +// _hal->debug->println(frontDistance); +// _hal->debug->print("Obstacle avoidance Heading Command: "); +// _hal->debug->println(headingCommand); +// _hal->debug->printf_P( +// PSTR("Front Distance, %f\n"), +// frontDistance); + } + if (_rangeFinderBack && _rangeFinderBack->distance < 5) { + _airSpeedCommand = 0; + _groundSpeedCommand = 0; + + } + + if (_rangeFinderLeft && _rangeFinderLeft->distance < 5) { + _airSpeedCommand = 0; + _groundSpeedCommand = 0; + } + + if (_rangeFinderRight && _rangeFinderRight->distance < 5) { + _airSpeedCommand = 0; + _groundSpeedCommand = 0; + } + } + + void nextCommand() { + // within 1 seconds, check if more than 5 calls to next command occur + // if they do, go to home waypoint + if (millis() - _nextCommandTimer < 1000) { + if (_nextCommandCalls > 5) { + Serial.println("commands loading too fast, returning home"); + setCurrentIndex(0); + setNumberOfCommands(1); + _nextCommandCalls = 0; + _nextCommandTimer = millis(); + return; + } + _nextCommandCalls++; + } else { + _nextCommandTimer = millis(); + _nextCommandCalls = 0; + } + + _cmdIndex = getNextIndex(); + Serial.print("cmd : "); Serial.println(int(_cmdIndex)); + Serial.print("cmd prev : "); Serial.println(int(getPreviousIndex())); + Serial.print("cmd num : "); Serial.println(int(getNumberOfCommands())); + _command = AP_MavlinkCommand(getCurrentIndex()); + _previousCommand = AP_MavlinkCommand(getPreviousIndex()); + } + + void handleCommand(AP_MavlinkCommand command, + AP_MavlinkCommand previousCommand) { + // TODO handle more commands + switch (command.getCommand()) { + case MAV_CMD_NAV_WAYPOINT: { + // if within radius, increment + float d = previousCommand.distanceTo(_navigator->getLat_degInt(), + _navigator->getLon_degInt()); + if (d < command.getRadius()) { + nextCommand(); + Serial.println("radius reached"); + } + break; + } +// case MAV_CMD_CONDITION_CHANGE_ALT: +// case MAV_CMD_CONDITION_DELAY: +// case MAV_CMD_CONDITION_DISTANCE: +// case MAV_CMD_CONDITION_LAST: +// case MAV_CMD_CONDITION_YAW: +// case MAV_CMD_DO_CHANGE_SPEED: +// case MAV_CMD_DO_CONTROL_VIDEO: +// case MAV_CMD_DO_JUMP: +// case MAV_CMD_DO_LAST: +// case MAV_CMD_DO_LAST: +// case MAV_CMD_DO_REPEAT_RELAY: +// case MAV_CMD_DO_REPEAT_SERVO: +// case MAV_CMD_DO_SET_HOME: +// case MAV_CMD_DO_SET_MODE: +// case MAV_CMD_DO_SET_PARAMETER: +// case MAV_CMD_DO_SET_RELAY: +// case MAV_CMD_DO_SET_SERVO: +// case MAV_CMD_PREFLIGHT_CALIBRATION: +// case MAV_CMD_PREFLIGHT_STORAGE: +// case MAV_CMD_NAV_LAND: +// case MAV_CMD_NAV_LAST: +// case MAV_CMD_NAV_LOITER_TIME: +// case MAV_CMD_NAV_LOITER_TURNS: +// case MAV_CMD_NAV_LOITER_UNLIM: +// case MAV_CMD_NAV_ORIENTATION_TARGET: +// case MAV_CMD_NAV_PATHPLANNING: +// case MAV_CMD_NAV_RETURN_TO_LAUNCH: +// case MAV_CMD_NAV_TAKEOFF: + default: + // unhandled command, skip + Serial.println("unhandled command"); + nextCommand(); + break; + } + } +private: + RangeFinder * _rangeFinderFront; + RangeFinder * _rangeFinderBack; + RangeFinder * _rangeFinderLeft; + RangeFinder * _rangeFinderRight; + AP_Var_group _group; + AP_Float _velocityCommand; + AP_Float _crossTrackGain; + AP_Float _crossTrackLim; +}; + +} // namespace apo + +#endif // AP_Guide_H +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_HardwareAbstractionLayer.cpp b/libraries/APO/AP_HardwareAbstractionLayer.cpp new file mode 100644 index 0000000000..afa06769ab --- /dev/null +++ b/libraries/APO/AP_HardwareAbstractionLayer.cpp @@ -0,0 +1,8 @@ +/* + * AP_HardwareAbstractionLayer.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_HardwareAbstractionLayer.h" diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_HardwareAbstractionLayer.h new file mode 100644 index 0000000000..45e7daaf75 --- /dev/null +++ b/libraries/APO/AP_HardwareAbstractionLayer.h @@ -0,0 +1,166 @@ +/* + * AP_HardwareAbstractionLayer.h + * + * Created on: Apr 4, 2011 + * + */ + +#ifndef AP_HARDWAREABSTRACTIONLAYER_H_ +#define AP_HARDWAREABSTRACTIONLAYER_H_ + +#include "../AP_Common/AP_Common.h" +#include "../FastSerial/FastSerial.h" +#include "../AP_Common/AP_Vector.h" +#include "../GCS_MAVLink/GCS_MAVLink.h" + +#include "../AP_ADC/AP_ADC.h" +#include "../AP_IMU/AP_IMU.h" +#include "../AP_GPS/GPS.h" +#include "../APM_BMP085/APM_BMP085.h" +#include "../AP_Compass/AP_Compass.h" +#include "AP_RcChannel.h" +#include "../AP_RangeFinder/AP_RangeFinder.h" +#include "../GCS_MAVLink/GCS_MAVLink.h" + +class AP_ADC; +class IMU; +class GPS; +class APM_BMP085_Class; +class Compass; +class BetterStream; +class RangeFinder; + +namespace apo { + +class AP_RcChannel; +class AP_CommLink; + +// enumerations +enum halMode_t { + MODE_LIVE, MODE_HIL_CNTL, +/*MODE_HIL_NAV*/}; +enum board_t { + BOARD_ARDUPILOTMEGA_1280, BOARD_ARDUPILOTMEGA_2560, BOARD_ARDUPILOTMEGA_2 +}; +enum vehicle_t { + VEHICLE_CAR, VEHICLE_QUAD, VEHICLE_PLANE, VEHICLE_BOAT, VEHICLE_TANK +}; + +class AP_HardwareAbstractionLayer { + +public: + + AP_HardwareAbstractionLayer(halMode_t mode, board_t board, + vehicle_t vehicle, uint8_t heartBeatTimeout) : + adc(), gps(), baro(), compass(), rangeFinders(), imu(), rc(), gcs(), + hil(), debug(), load(), lastHeartBeat(), + _heartBeatTimeout(heartBeatTimeout), _mode(mode), + _board(board), _vehicle(vehicle), _state(MAV_STATE_UNINIT) { + + /* + * Board specific hardware initialization + */ + if (board == BOARD_ARDUPILOTMEGA_1280) { + slideSwitchPin = 40; + pushButtonPin = 41; + aLedPin = 37; + bLedPin = 36; + cLedPin = 35; + eepromMaxAddr = 1024; + pinMode(aLedPin, OUTPUT); // extra led + pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; + pinMode(cLedPin, OUTPUT); // gps led + pinMode(slideSwitchPin, INPUT); + pinMode(pushButtonPin, INPUT); + DDRL |= B00000100; // set port L, pint 2 to output for the relay + } else if (board == BOARD_ARDUPILOTMEGA_2560) { + slideSwitchPin = 40; + pushButtonPin = 41; + aLedPin = 37; + bLedPin = 36; + cLedPin = 35; + eepromMaxAddr = 2048; + pinMode(aLedPin, OUTPUT); // extra led + pinMode(bLedPin, OUTPUT); // imu ledclass AP_CommLink; + pinMode(cLedPin, OUTPUT); // gps led + pinMode(slideSwitchPin, INPUT); + pinMode(pushButtonPin, INPUT); + DDRL |= B00000100; // set port L, pint 2 to output for the relay + } + } + + /** + * Sensors + */ + AP_ADC * adc; + GPS * gps; + APM_BMP085_Class * baro; + Compass * compass; + Vector rangeFinders; + IMU * imu; + + /** + * Radio Channels + */ + Vector rc; + + /** + * Communication Channels + */ + AP_CommLink * gcs; + AP_CommLink * hil; + FastSerial * debug; + + /** + * data + */ + uint8_t load; + uint32_t lastHeartBeat; + + /** + * settings + */ + uint8_t slideSwitchPin; + uint8_t pushButtonPin; + uint8_t aLedPin; + uint8_t bLedPin; + uint8_t cLedPin; + uint16_t eepromMaxAddr; + + // accessors + halMode_t getMode() { + return _mode; + } + board_t getBoard() { + return _board; + } + vehicle_t getVehicle() { + return _vehicle; + } + MAV_STATE getState() { + return _state; + } + void setState(MAV_STATE state) { + _state = state; + } + + bool heartBeatLost() { + if (_heartBeatTimeout == 0) + return false; + else + return ((micros() - lastHeartBeat) / 1e6) > _heartBeatTimeout; + } + +private: + + // enumerations + uint8_t _heartBeatTimeout; + halMode_t _mode; + board_t _board; + vehicle_t _vehicle; + MAV_STATE _state; +}; + +} + +#endif /* AP_HARDWAREABSTRACTIONLAYER_H_ */ diff --git a/libraries/APO/AP_MavlinkCommand.cpp b/libraries/APO/AP_MavlinkCommand.cpp new file mode 100644 index 0000000000..1109a3641b --- /dev/null +++ b/libraries/APO/AP_MavlinkCommand.cpp @@ -0,0 +1,175 @@ +/* + * AP_MavlinkCommand.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_MavlinkCommand.h" + +namespace apo { + +AP_MavlinkCommand::AP_MavlinkCommand(const AP_MavlinkCommand & v) : + _data(v._data), _seq(v._seq) { + //if (FastSerial::getInitialized(0)) Serial.println("copy ctor"); +} + +AP_MavlinkCommand::AP_MavlinkCommand(uint16_t index, bool doLoad) : + _data(k_commands + index), _seq(index) { + + if (FastSerial::getInitialized(0)) { + Serial.println("index ctor"); + Serial.println("++"); + Serial.print("index: "); Serial.println(index); + Serial.print("key: "); Serial.println(k_commands + index); + Serial.println("++"); + } + // This is a failsafe measure to stop trying to load a command if it can't load + if (doLoad && !load()) { + Serial.println("load failed, reverting to home waypoint"); + _data = AP_MavlinkCommand::home._data; + _seq = AP_MavlinkCommand::home._seq; + } +} + +AP_MavlinkCommand::AP_MavlinkCommand(mavlink_waypoint_t cmd) : + _data(k_commands + cmd.seq), _seq(cmd.seq) { + setCommand(MAV_CMD(cmd.command)); + setAutocontinue(cmd.autocontinue); + setFrame(MAV_FRAME(cmd.frame)); + setParam1(cmd.param1); + setParam2(cmd.param2); + setParam3(cmd.param3); + setParam4(cmd.param4); + setX(cmd.x); + setY(cmd.y); + setZ(cmd.z); + save(); + + // reload home if sent + if (cmd.seq == 0) home.load(); + + Serial.println("============================================================"); + Serial.println("storing new command from mavlink_waypoint_t"); + Serial.print("key: "); Serial.println(_data.key(),DEC); + Serial.print("number: "); Serial.println(cmd.seq,DEC); + Serial.print("command: "); Serial.println(getCommand()); + Serial.print("autocontinue: "); Serial.println(getAutocontinue(),DEC); + Serial.print("frame: "); Serial.println(getFrame(),DEC); + Serial.print("1000*param1: "); Serial.println(int(1000*getParam1()),DEC); + Serial.print("1000*param2: "); Serial.println(int(1000*getParam2()),DEC); + Serial.print("1000*param3: "); Serial.println(int(1000*getParam3()),DEC); + Serial.print("1000*param4: "); Serial.println(int(1000*getParam4()),DEC); + Serial.print("1000*x0: "); Serial.println(int(1000*cmd.x),DEC); + Serial.print("1000*y0: "); Serial.println(int(1000*cmd.y),DEC); + Serial.print("1000*z0: "); Serial.println(int(1000*cmd.z),DEC); + Serial.print("1000*x: "); Serial.println(int(1000*getX()),DEC); + Serial.print("1000*y: "); Serial.println(int(1000*getY()),DEC); + Serial.print("1000*z: "); Serial.println(int(1000*getZ()),DEC); + + load(); + + Serial.print("1000*x1: "); Serial.println(int(1000*getX()),DEC); + Serial.print("1000*y1: "); Serial.println(int(1000*getY()),DEC); + Serial.print("1000*z1: "); Serial.println(int(1000*getZ()),DEC); + Serial.println("============================================================"); + Serial.flush(); +} + +mavlink_waypoint_t AP_MavlinkCommand::convert(uint8_t current) { + mavlink_waypoint_t mavCmd; + mavCmd.seq = getSeq(); + mavCmd.command = getCommand(); + mavCmd.frame = getFrame(); + mavCmd.param1 = getParam1(); + mavCmd.param2 = getParam2(); + mavCmd.param3 = getParam3(); + mavCmd.param4 = getParam4(); + mavCmd.x = getX(); + mavCmd.y = getY(); + mavCmd.z = getZ(); + mavCmd.autocontinue = getAutocontinue(); + mavCmd.current = (getSeq() == current); + mavCmd.target_component = mavlink_system.compid; + mavCmd.target_system = mavlink_system.sysid; + return mavCmd; +} + +float AP_MavlinkCommand::bearingTo(AP_MavlinkCommand next) const { + float deltaLon = next.getLon() - getLon(); + /* + Serial.print("Lon: "); Serial.println(getLon()); + Serial.print("nextLon: "); Serial.println(next.getLon()); + Serial.print("deltaLonDeg * 1e7: "); Serial.println(deltaLon*rad2DegInt); + */ + float bearing = atan2( + sin(deltaLon) * cos(next.getLat()), + cos(getLat()) * sin(next.getLat()) - sin(getLat()) * cos( + next.getLat()) * cos(deltaLon)); + return bearing; +} + +float AP_MavlinkCommand::bearingTo(int32_t latDegInt, int32_t lonDegInt) const { + // have to be careful to maintain the precision of the gps coordinate + float deltaLon = (lonDegInt - getLon_degInt()) * degInt2Rad; + float nextLat = latDegInt * degInt2Rad; + float bearing = atan2( + sin(deltaLon) * cos(nextLat), + cos(getLat()) * sin(nextLat) - sin(getLat()) * cos(nextLat) + * cos(deltaLon)); + if (bearing < 0) + bearing += 2 * M_PI; + return bearing; +} + +float AP_MavlinkCommand::distanceTo(AP_MavlinkCommand next) const { + float sinDeltaLat2 = sin((getLat() - next.getLat()) / 2); + float sinDeltaLon2 = sin((getLon() - next.getLon()) / 2); + float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos( + next.getLat()) * sinDeltaLon2 * sinDeltaLon2; + float c = 2 * atan2(sqrt(a), sqrt(1 - a)); + return rEarth * c; +} + +float AP_MavlinkCommand::distanceTo(int32_t lat_degInt, int32_t lon_degInt) const { + float sinDeltaLat2 = sin( + (lat_degInt - getLat_degInt()) * degInt2Rad / 2); + float sinDeltaLon2 = sin( + (lon_degInt - getLon_degInt()) * degInt2Rad / 2); + float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos( + lat_degInt * degInt2Rad) * sinDeltaLon2 * sinDeltaLon2; + float c = 2 * atan2(sqrt(a), sqrt(1 - a)); + /* + Serial.print("wp lat_degInt: "); Serial.println(getLat_degInt()); + Serial.print("wp lon_degInt: "); Serial.println(getLon_degInt()); + Serial.print("lat_degInt: "); Serial.println(lat_degInt); + Serial.print("lon_degInt: "); Serial.println(lon_degInt); + Serial.print("sinDeltaLat2: "); Serial.println(sinDeltaLat2); + Serial.print("sinDeltaLon2: "); Serial.println(sinDeltaLon2); + */ + return rEarth * c; +} + +//calculates cross track of a current location +float AP_MavlinkCommand::crossTrack(AP_MavlinkCommand previous, + int32_t lat_degInt, int32_t lon_degInt) { + float d = previous.distanceTo(lat_degInt, lon_degInt); + float bCurrent = previous.bearingTo(lat_degInt, lon_degInt); + float bNext = previous.bearingTo(*this); + return asin(sin(d / rEarth) * sin(bCurrent - bNext)) * rEarth; + return 0; +} + +// calculates along track distance of a current location +float AP_MavlinkCommand::alongTrack(AP_MavlinkCommand previous, + int32_t lat_degInt, int32_t lon_degInt) { + // ignores lat/lon since single prec. + float dXt = this->crossTrack(previous,lat_degInt, lon_degInt); + float d = previous.distanceTo(lat_degInt, lon_degInt); + return dXt / tan(asin(dXt / d)); +} + + +AP_MavlinkCommand AP_MavlinkCommand::home = AP_MavlinkCommand(0,false); + +} diff --git a/libraries/APO/AP_MavlinkCommand.h b/libraries/APO/AP_MavlinkCommand.h new file mode 100644 index 0000000000..6cb10557e3 --- /dev/null +++ b/libraries/APO/AP_MavlinkCommand.h @@ -0,0 +1,359 @@ +/* + * AP_MavlinkCommand.h + * + * Created on: Apr 4, 2011 + * Author: jgoppert + */ + +#ifndef AP_MAVLINKCOMMAND_H_ +#define AP_MAVLINKCOMMAND_H_ + +#include "../GCS_MAVLink/GCS_MAVLink.h" +#include "../AP_Common/AP_Common.h" +#include "AP_Var_keys.h" +#include "constants.h" +#include "../FastSerial/FastSerial.h" + +namespace apo { + +class AP_MavlinkCommand { +private: + struct CommandStorage { + MAV_CMD command; + bool autocontinue; + MAV_FRAME frame; + float param1; + float param2; + float param3; + float param4; + float x; + float y; + float z; + }; + AP_VarS _data; + uint16_t _seq; +public: + static AP_MavlinkCommand home; + + /** + * Copy Constructor + */ + AP_MavlinkCommand(const AP_MavlinkCommand & v); + + /** + * Basic Constructor + * @param index Start at zero. + */ + AP_MavlinkCommand(uint16_t index, bool doLoad = true); + + /** + * Constructor for copying/ saving from a mavlink waypoint. + * @param cmd The mavlink_waopint_t structure for the command. + */ + AP_MavlinkCommand(mavlink_waypoint_t cmd); + + bool save() { + return _data.save(); + } + bool load() { + return _data.load(); + } + uint8_t getSeq() const { + return _seq; + } + bool getAutocontinue() const { + return _data.get().autocontinue; + } + void setAutocontinue(bool val) { + _data.get().autocontinue = val; + } + void setSeq(uint8_t val) { + _seq = val; + } + MAV_CMD getCommand() const { + return _data.get().command; + } + void setCommand(MAV_CMD val) { + _data.get().command = val; + } + MAV_FRAME getFrame() const { + return _data.get().frame; + } + void setFrame(MAV_FRAME val) { + _data.get().frame = val; + } + float getParam1() const { + return _data.get().param1; + } + void setParam1(float val) { + _data.get().param1 = val; + } + float getParam2() const { + return _data.get().param2; + } + void setParam2(float val) { + _data.get().param2 = val; + } + float getParam3() const { + return _data.get().param3; + } + void setParam3(float val) { + _data.get().param3 = val; + } + float getParam4() const { + return _data.get().param4; + } + void setParam4(float val) { + _data.get().param4 = val; + } + float getX() const { + return _data.get().x; + } + void setX(float val) { + _data.get().x = val; + } + float getY() const { + return _data.get().y; + } + void setY(float val) { + _data.get().y = val; + } + float getZ() const { + return _data.get().z; + } + void setZ(float val) { + _data.get().z = val; + } + float getLatDeg() const { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + return getX(); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + void setLatDeg(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + setX(val); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_MISSION: + default: + break; + } + } + float getLonDeg() const { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + return getY(); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + void setLonDeg(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + setY(val); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_MISSION: + default: + break; + } + } + void setLon(float val) { + setLonDeg(val * rad2Deg); + } + void setLon_degInt(int32_t val) { + setLonDeg(val / 1.0e7); + } + void setLat_degInt(int32_t val) { + setLatDeg(val / 1.0e7); + } + int32_t getLon_degInt() const { + return getLonDeg() * 1e7; + } + int32_t getLat_degInt() const { + return getLatDeg() * 1e7; + } + float getLon() const { + return getLonDeg() * deg2Rad; + } + float getLat() const { + return getLatDeg() * deg2Rad; + } + void setLat(float val) { + setLatDeg(val * rad2Deg); + } + float getAlt() const { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + return getZ(); + break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_LOCAL: + return getZ() + home.getAlt(); + break; + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + /** + * set the altitude in meters + */ + void setAlt(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + setZ(val); + break; + case MAV_FRAME_LOCAL: + setZ(val - home.getLonDeg()); + break; + case MAV_FRAME_MISSION: + default: + break; + } + } + /** + * Get the relative altitude to home + * @return relative altitude in meters + */ + float getRelAlt() const { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + return getZ() - home.getAlt(); + break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_LOCAL: + return getZ(); + break; + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + /** + * set the relative altitude in meters from home + */ + void setRelAlt(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + setZ(val + home.getAlt()); + break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_LOCAL: + setZ(val); + break; + case MAV_FRAME_MISSION: + break; + } + } + + float getRadius() const { + return getParam2(); + } + + void setRadius(float val) { + setParam2(val); + } + + /** + * conversion for outbound packets to ground station + * @return output the mavlink_waypoint_t packet + */ + mavlink_waypoint_t convert(uint8_t current); + + /** + * Calculate the bearing from this command to the next command + * @param next The command to calculate the bearing to. + * @return the bearing + */ + float bearingTo(AP_MavlinkCommand next) const; + + /** + * Bearing form this command to a gps coordinate in integer units + * @param latDegInt latitude in degrees E-7 + * @param lonDegInt longitude in degrees E-7 + * @return + */ + float bearingTo(int32_t latDegInt, int32_t lonDegInt) const; + + /** + * Distance to another command + * @param next The command to measure to. + * @return The distance in meters. + */ + float distanceTo(AP_MavlinkCommand next) const; + + /** + * Distance to a gps coordinate in integer units + * @param latDegInt latitude in degrees E-7 + * @param lonDegInt longitude in degrees E-7 + * @return The distance in meters. + */ + float distanceTo(int32_t lat_degInt, int32_t lon_degInt) const; + + float getPN(int32_t lat_degInt, int32_t lon_degInt) const { + // local tangent approximation at this waypoint + float deltaLat = (lat_degInt - getLat_degInt()) * degInt2Rad; + return deltaLat * rEarth; + } + + float getPE(int32_t lat_degInt, int32_t lon_degInt) const { + // local tangent approximation at this waypoint + float deltaLon = (lon_degInt - getLon_degInt()) * degInt2Rad; + return cos(getLat()) * deltaLon * rEarth; + } + + float getPD(int32_t alt_intM) const { + return -(alt_intM / scale_m - getAlt()); + } + + float getLat(float pN) const { + + return pN / rEarth + getLat(); + } + + float getLon(float pE) const { + + return pE / rEarth / cos(getLat()) + getLon(); + } + + /** + * Gets altitude in meters + * @param pD alt in meters + * @return + */ + float getAlt(float pD) const { + + return getAlt() - pD; + } + + //calculates cross track of a current location + float crossTrack(AP_MavlinkCommand previous, int32_t lat_degInt, int32_t lon_degInt); + + // calculates along track distance of a current location + float alongTrack(AP_MavlinkCommand previous, int32_t lat_degInt, int32_t lon_degInt); +}; + +} // namespace apo + + +#endif /* AP_MAVLINKCOMMAND_H_ */ diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp new file mode 100644 index 0000000000..ff33e8ed2c --- /dev/null +++ b/libraries/APO/AP_Navigator.cpp @@ -0,0 +1,8 @@ +/* + * AP_Navigator.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_Navigator.h" diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h new file mode 100644 index 0000000000..9b87561946 --- /dev/null +++ b/libraries/APO/AP_Navigator.h @@ -0,0 +1,391 @@ +/* + * AP_Navigator.h + * Copyright (C) James Goppert 2010 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#ifndef AP_Navigator_H +#define AP_Navigator_H + +#include "AP_HardwareAbstractionLayer.h" +#include "../AP_DCM/AP_DCM.h" +#include "../AP_Math/AP_Math.h" +#include "../AP_Compass/AP_Compass.h" +#include "AP_MavlinkCommand.h" +#include "constants.h" +#include "AP_Var_keys.h" +#include "../AP_RangeFinder/AP_RangeFinder.h" +#include "../AP_IMU/AP_IMU.h" + +namespace apo { + +/// Navigator class +class AP_Navigator { +public: + AP_Navigator(AP_HardwareAbstractionLayer * hal) : + _hal(hal), _timeStamp(0), _roll(0), _rollRate(0), _pitch(0), + _pitchRate(0), _yaw(0), _yawRate(0), _airSpeed(0), + _groundSpeed(0), _vD(0), _lat_degInt(0), + _lon_degInt(0), _alt_intM(0) { + } + virtual void calibrate() { + } + virtual void updateFast(float dt) = 0; + virtual void updateSlow(float dt) = 0; + float getPD() const { + return AP_MavlinkCommand::home.getPD(getAlt_intM()); + } + + float getPE() const { + return AP_MavlinkCommand::home.getPE(getLat_degInt(), getLon_degInt()); + } + + float getPN() const { + return AP_MavlinkCommand::home.getPN(getLat_degInt(), getLon_degInt()); + } + + void setPD(float _pD) { + setAlt(AP_MavlinkCommand::home.getAlt(_pD)); + } + + void setPE(float _pE) { + setLat(AP_MavlinkCommand::home.getLat(_pE)); + } + + void setPN(float _pN) { + setLon(AP_MavlinkCommand::home.getLon(_pN)); + } + + float getAirSpeed() const { + return _airSpeed; + } + + int32_t getAlt_intM() const { + return _alt_intM; + } + + float getAlt() const { + return _alt_intM / scale_m; + } + + void setAlt(float _alt) { + this->_alt_intM = _alt * scale_m; + } + + float getLat() const { + //Serial.print("getLatfirst"); + //Serial.println(_lat_degInt * degInt2Rad); + return _lat_degInt * degInt2Rad; + } + + void setLat(float _lat) { + //Serial.print("setLatfirst"); + //Serial.println(_lat * rad2DegInt); + setLat_degInt(_lat*rad2DegInt); + } + + float getLon() const { + return _lon_degInt * degInt2Rad; + } + + void setLon(float _lon) { + this->_lon_degInt = _lon * rad2DegInt; + } + + float getVD() const { + return _vD; + } + + float getVE() const { + return sin(getYaw()) * getGroundSpeed(); + } + + float getGroundSpeed() const { + return _groundSpeed; + } + + int32_t getLat_degInt() const { + //Serial.print("getLat_degInt"); + //Serial.println(_lat_degInt); + return _lat_degInt; + + } + + int32_t getLon_degInt() const { + return _lon_degInt; + } + + float getVN() const { + return cos(getYaw()) * getGroundSpeed(); + } + + float getPitch() const { + return _pitch; + } + + float getPitchRate() const { + return _pitchRate; + } + + float getRoll() const { + return _roll; + } + + float getRollRate() const { + return _rollRate; + } + + float getYaw() const { + return _yaw; + } + + float getYawRate() const { + return _yawRate; + } + + void setAirSpeed(float airSpeed) { + _airSpeed = airSpeed; + } + + void setAlt_intM(int32_t alt_intM) { + _alt_intM = alt_intM; + } + + void setVD(float vD) { + _vD = vD; + } + + void setGroundSpeed(float groundSpeed) { + _groundSpeed = groundSpeed; + } + + void setLat_degInt(int32_t lat_degInt) { + _lat_degInt = lat_degInt; + //Serial.print("setLat_degInt"); + //Serial.println(_lat_degInt); + } + + void setLon_degInt(int32_t lon_degInt) { + _lon_degInt = lon_degInt; + } + + void setPitch(float pitch) { + _pitch = pitch; + } + + void setPitchRate(float pitchRate) { + _pitchRate = pitchRate; + } + + void setRoll(float roll) { + _roll = roll; + } + + void setRollRate(float rollRate) { + _rollRate = rollRate; + } + + void setYaw(float yaw) { + _yaw = yaw; + } + + void setYawRate(float yawRate) { + _yawRate = yawRate; + } + void setTimeStamp(int32_t timeStamp) { + _timeStamp = timeStamp; + } + int32_t getTimeStamp() const { + return _timeStamp; + } + +protected: + AP_HardwareAbstractionLayer * _hal; +private: + int32_t _timeStamp; // micros clock + float _roll; // rad + float _rollRate; //rad/s + float _pitch; // rad + float _pitchRate; // rad/s + float _yaw; // rad + float _yawRate; // rad/s + float _airSpeed; // m/s + float _groundSpeed; // m/s + float _vD; // m/s + int32_t _lat_degInt; // deg / 1e7 + int32_t _lon_degInt; // deg / 1e7 + int32_t _alt_intM; // meters / 1e3 +}; + +class DcmNavigator: public AP_Navigator { +private: + /** + * Sensors + */ + + RangeFinder * _rangeFinderDown; + AP_DCM * _dcm; + IMU * _imu; + uint16_t _imuOffsetAddress; + +public: + DcmNavigator(AP_HardwareAbstractionLayer * hal) : + AP_Navigator(hal), _dcm(), _imuOffsetAddress(0) { + + // if orientation equal to front, store as front + /** + * rangeFinder is assigned values based on orientation which + * is specified in ArduPilotOne.pde. + */ + for (uint8_t i = 0; i < _hal-> rangeFinders.getSize(); i++) { + if (_hal->rangeFinders[i] == NULL) + continue; + if (_hal->rangeFinders[i]->orientation_x == 0 + && _hal->rangeFinders[i]->orientation_y == 0 + && _hal->rangeFinders[i]->orientation_z == 1) + _rangeFinderDown = _hal->rangeFinders[i]; + } + + if (_hal->getMode() == MODE_LIVE) { + if (_hal->adc) + _hal->imu = new AP_IMU_Oilpan(_hal->adc, k_sensorCalib); + if (_hal->imu) + _dcm = new AP_DCM(_hal->imu, _hal->gps, _hal->compass); + if (_hal->compass) { + _dcm->set_compass(_hal->compass); + + } + } + } + virtual void calibrate() { + + AP_Navigator::calibrate(); + + // TODO: handle cold restart + if (_hal->imu) { + /* + * Gyro has built in warm up cycle and should + * run first */ + _hal->imu->init_gyro(NULL); + _hal->imu->init_accel(NULL); + } + } + virtual void updateFast(float dt) { + if (_hal->getMode() != MODE_LIVE) + return; + + setTimeStamp(micros()); // if running in live mode, record new time stamp + + + //_hal->debug->println_P(PSTR("nav loop")); + + /** + * The altitued is read off the barometer by implementing the following formula: + * altitude (in m) = 44330*(1-(p/po)^(1/5.255)), + * where, po is pressure in Pa at sea level (101325 Pa). + * See http://www.sparkfun.com/tutorials/253 or type this formula + * in a search engine for more information. + * altInt contains the altitude in meters. + */ + if (_hal->baro) { + + if (_rangeFinderDown != NULL && _rangeFinderDown->distance <= 695) + setAlt(_rangeFinderDown->distance); + + else { + float tmp = (_hal->baro->Press / 101325.0); + tmp = pow(tmp, 0.190295); + //setAlt(44330 * (1.0 - tmp)); //sets the altitude in meters XXX wrong, baro reads 0 press + setAlt(0.0); + } + } + + // dcm class for attitude + if (_dcm) { + _dcm->update_DCM(); + setRoll(_dcm->roll); + setPitch(_dcm->pitch); + setYaw(_dcm->yaw); + setRollRate(_dcm->get_gyro().x); + setPitchRate(_dcm->get_gyro().y); + setYawRate(_dcm->get_gyro().z); + + /* + * 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); + */ + } + } + virtual void updateSlow(float dt) { + if (_hal->getMode() != MODE_LIVE) + return; + + setTimeStamp(micros()); // if running in live mode, record new time stamp + + if (_hal->gps) { + _hal->gps->update(); + updateGpsLight(); + if (_hal->gps->fix && _hal->gps->new_data) { + setLat_degInt(_hal->gps->latitude); + setLon_degInt(_hal->gps->longitude); + setAlt_intM(_hal->gps->altitude * 10); // gps in cm, intM in mm + setGroundSpeed(_hal->gps->ground_speed / 100.0); // gps is in cm/s + } + } + + if (_hal->compass) { + _hal->compass->read(); + _hal->compass->calculate(getRoll(), getPitch()); + _hal->compass->null_offsets(_dcm->get_dcm_matrix()); + } + } + void updateGpsLight(void) { + // GPS LED on if we have a fix or Blink GPS LED if we are receiving data + // --------------------------------------------------------------------- + static bool GPS_light = false; + switch (_hal->gps->status()) { + case (2): + //digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. + break; + + case (1): + if (_hal->gps->valid_read == true) { + GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock + if (GPS_light) { + digitalWrite(_hal->cLedPin, LOW); + } else { + digitalWrite(_hal->cLedPin, HIGH); + } + _hal->gps->valid_read = false; + } + break; + + default: + digitalWrite(_hal->cLedPin, LOW); + break; + } + } + +}; + +} // namespace apo + +#endif // AP_Navigator_H +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_RcChannel.cpp b/libraries/APO/AP_RcChannel.cpp new file mode 100644 index 0000000000..c2adbc5c52 --- /dev/null +++ b/libraries/APO/AP_RcChannel.cpp @@ -0,0 +1,110 @@ +/* + AP_RcChannel.cpp - Radio library for Arduino + Code by Jason Short, James Goppert. DIYDrones.com + + This library is free software; you can redistribute it and / or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + */ + +#include +#include +#include "AP_RcChannel.h" +#include "../AP_Common/AP_Common.h" +#include + +namespace apo { + +AP_RcChannel::AP_RcChannel(AP_Var::Key keyValue, const prog_char_t * name, + APM_RC_Class & rc, const uint8_t & ch, const uint16_t & pwmMin, + const uint16_t & pwmNeutral, const uint16_t & pwmMax, + const rcMode_t & rcMode, const bool & reverse) : + AP_Var_group(keyValue, name), _ch(this, 1, ch, PSTR("ch")), + _pwmMin(this, 2, pwmMin, PSTR("pMin")), + _pwmNeutral(this, 3, pwmNeutral, PSTR("pNtrl")), + _pwmMax(this, 4, pwmMax, PSTR("pMax")), _rcMode(rcMode), + _reverse(this, 5, reverse, PSTR("rev")), _rc(rc), _pwm(pwmNeutral) { + //Serial.print("pwm after ctor: "); Serial.println(pwmNeutral); + if (rcMode == RC_MODE_IN) + return; + //Serial.print("pwm set for ch: "); Serial.println(int(ch)); + rc.OutputCh(ch, pwmNeutral); + +} + +uint16_t AP_RcChannel::getRadioPwm() { + if (_rcMode == RC_MODE_OUT) { + Serial.print("tryng to read from output channel: "); + Serial.println(int(_ch)); + return _pwmNeutral; // if this happens give a safe value of neutral + } + return _rc.InputCh(_ch); +} + +void AP_RcChannel::setUsingRadio() { + setPwm(getRadioPwm()); +} + +void AP_RcChannel::setPwm(uint16_t pwm) { + //Serial.printf("pwm in setPwm: %d\n", pwm); + //Serial.printf("reverse: %s\n", (reverse)?"true":"false"); + + // apply reverse + if (_reverse) + pwm = int16_t(_pwmNeutral - pwm) + _pwmNeutral; + //Serial.printf("pwm after reverse: %d\n", pwm); + + // apply saturation + if (_pwm > uint8_t(_pwmMax)) + _pwm = _pwmMax; + if (_pwm < uint8_t(_pwmMin)) + _pwm = _pwmMin; + _pwm = pwm; + + //Serial.print("ch: "); Serial.print(ch); Serial.print(" pwm: "); Serial.println(pwm); + if (_rcMode == RC_MODE_IN) + return; + _rc.OutputCh(_ch, _pwm); +} + +void AP_RcChannel::setPosition(float position) { + if (position > 1.0) + position = 1.0; + else if (position < -1.0) + position = -1.0; + setPwm(_positionToPwm(position)); +} + +uint16_t AP_RcChannel::_positionToPwm(const float & position) { + uint16_t pwm; + //Serial.printf("position: %f\n", position); + if (position < 0) + pwm = position * int16_t(_pwmNeutral - _pwmMin) + _pwmNeutral; + else + pwm = position * int16_t(_pwmMax - _pwmNeutral) + _pwmNeutral; + + if (pwm > uint16_t(_pwmMax)) + pwm = _pwmMax; + if (pwm < uint16_t(_pwmMin)) + pwm = _pwmMin; + return pwm; +} + +float AP_RcChannel::_pwmToPosition(const uint16_t & pwm) { + float position; + if (pwm < uint8_t(_pwmNeutral)) + position = 1.0 * int16_t(pwm - _pwmNeutral) / int16_t( + _pwmNeutral - _pwmMin); + else + position = 1.0 * int16_t(pwm - _pwmNeutral) / int16_t( + _pwmMax - _pwmNeutral); + if (position > 1) + position = 1; + if (position < -1) + position = -1; + return position; +} + +} // namespace apo diff --git a/libraries/APO/AP_RcChannel.h b/libraries/APO/AP_RcChannel.h new file mode 100644 index 0000000000..b12489a257 --- /dev/null +++ b/libraries/APO/AP_RcChannel.h @@ -0,0 +1,69 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AP_RcChannel.h +/// @brief AP_RcChannel manager + +#ifndef AP_RCCHANNEL_H +#define AP_RCCHANNEL_H + +#include +#include "../APM_RC/APM_RC.h" +#include "../AP_Common/AP_Common.h" +#include "../AP_Common/AP_Var.h" + +namespace apo { + +enum rcMode_t { + RC_MODE_IN, RC_MODE_OUT, RC_MODE_INOUT +}; + +/// @class AP_RcChannel +/// @brief Object managing one RC channel +class AP_RcChannel: public AP_Var_group { + +public: + + /// Constructor + AP_RcChannel(AP_Var::Key key, const prog_char_t * name, APM_RC_Class & rc, + const uint8_t & ch, const uint16_t & pwmMin, + const uint16_t & pwmNeutral, const uint16_t & pwmMax, + const rcMode_t & rcMode = RC_MODE_INOUT, + const bool & reverse = false); + + // configuration + AP_Uint8 _ch;AP_Uint16 _pwmMin;AP_Uint16 _pwmNeutral;AP_Uint16 _pwmMax; + rcMode_t _rcMode;AP_Bool _reverse; + + // set + void setUsingRadio(); + void setPwm(uint16_t pwm); + void setPosition(float position); + + // get + uint16_t getPwm() { + return _pwm; + } + uint16_t getRadioPwm(); + float getPosition() { + return _pwmToPosition(_pwm); + } + float getRadioPosition() { + return _pwmToPosition(getRadioPwm()); + } + +private: + + // configuration + APM_RC_Class & _rc; + + // internal states + uint16_t _pwm; // this is the internal state, position is just created when needed + + // private methods + uint16_t _positionToPwm(const float & position); + float _pwmToPosition(const uint16_t & pwm); +}; + +} // apo + +#endif // AP_RCCHANNEL_H diff --git a/libraries/APO/AP_Var_keys.h b/libraries/APO/AP_Var_keys.h new file mode 100644 index 0000000000..58a715b591 --- /dev/null +++ b/libraries/APO/AP_Var_keys.h @@ -0,0 +1,24 @@ +#ifndef AP_Var_keys_H +#define AP_Var_keys_H + +enum keys { + + // general + k_config = 0, + k_cntrl, + k_guide, + k_sensorCalib, + + k_radioChannelsStart=10, + + k_controllersStart=30, + + k_customStart=100, + + // 200-256 reserved for commands + k_commands = 200 +}; + +// max 256 keys + +#endif diff --git a/libraries/APO/CMakeLists.txt b/libraries/APO/CMakeLists.txt new file mode 100644 index 0000000000..ee964b337b --- /dev/null +++ b/libraries/APO/CMakeLists.txt @@ -0,0 +1,39 @@ +set(LIB_NAME APO) + +set(${LIB_NAME}_SRCS + AP_Autopilot.cpp + AP_CommLink.cpp + AP_Controller.cpp + AP_Guide.cpp + AP_HardwareAbstractionLayer.cpp + AP_MavlinkCommand.cpp + AP_Navigator.cpp + AP_RcChannel.cpp + APO.cpp + ) # Firmware sources + +set(${LIB_NAME}_HDRS + AP_Autopilot.h + AP_CommLink.h + AP_Controller.h + AP_Guide.h + AP_HardwareAbstractionLayer.h + AP_MavlinkCommand.h + AP_Navigator.h + AP_RcChannel.h + AP_Var_keys.h + APO.h + constants.h + template.h +) + +include_directories( +# ${CMAKE_SOURCE_DIR}/libraries/AP_Common + ${CMAKE_SOURCE_DIR}/libraries/FastSerial + ${CMAKE_SOURCE_DIR}/libraries/ModeFilter +# + ) + +set(${LIB_NAME}_BOARD ${BOARD}) + +generate_arduino_library(${LIB_NAME}) diff --git a/libraries/APO/constants.h b/libraries/APO/constants.h new file mode 100644 index 0000000000..2a4316bcb3 --- /dev/null +++ b/libraries/APO/constants.h @@ -0,0 +1,23 @@ +/* + * constants.h + * + * Created on: Apr 7, 2011 + * Author: nkgentry + */ + +#ifndef CONSTANTS_H_ +#define CONSTANTS_H_ + +#include "math.h" + +const float scale_deg = 1e7; // scale of integer degrees/ degree +const float scale_m = 1e3; // scale of integer meters/ meter +const float rEarth = 6371000; // radius of earth, meters +const float rad2Deg = 180 / M_PI; // radians to degrees +const float deg2Rad = M_PI / 180; // degrees to radians +const float rad2DegInt = rad2Deg * scale_deg; // radians to degrees x 1e7 +const float degInt2Rad = deg2Rad / scale_deg; // degrees x 1e7 to radians + +#define MAV_MODE_FAILSAFE MAV_MODE_TEST3 + +#endif /* CONSTANTS_H_ */ diff --git a/libraries/APO/examples/MavlinkTest/Makefile b/libraries/APO/examples/MavlinkTest/Makefile new file mode 100644 index 0000000000..85b4d8856b --- /dev/null +++ b/libraries/APO/examples/MavlinkTest/Makefile @@ -0,0 +1,2 @@ +BOARD = mega +include ../../../AP_Common/Arduino.mk diff --git a/libraries/APO/examples/MavlinkTest/MavlinkTest.pde b/libraries/APO/examples/MavlinkTest/MavlinkTest.pde new file mode 100644 index 0000000000..d64be33822 --- /dev/null +++ b/libraries/APO/examples/MavlinkTest/MavlinkTest.pde @@ -0,0 +1,49 @@ +/* + AnalogReadSerial + Reads an analog input on pin 0, prints the result to the serial monitor + + This example code is in the public domain. + */ + +#include + +int packetDrops = 0; + +void handleMessage(mavlink_message_t * msg) { + Serial.print(", received mavlink message: "); + Serial.print(msg->msgid, DEC); +} + +void setup() { + Serial.begin(57600); + Serial3.begin(57600); + mavlink_comm_0_port = &Serial3; + packetDrops = 0; +} + +void loop() { + mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); + Serial.print("heartbeat sent"); + + // receive new packets + mavlink_message_t msg; + mavlink_status_t status; + + Serial.print(", bytes available: "); + Serial.print(comm_get_available(MAVLINK_COMM_0)); + while (comm_get_available( MAVLINK_COMM_0)) { + uint8_t c = comm_receive_ch(MAVLINK_COMM_0); + + // Try to get a new message + if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) + handleMessage(&msg); + } + + // Update packet drops counter + packetDrops += status.packet_rx_drop_count; + + Serial.print(", dropped packets: "); + Serial.println(packetDrops); + delay(1000); +} diff --git a/libraries/APO/examples/ServoManual/Makefile b/libraries/APO/examples/ServoManual/Makefile new file mode 100644 index 0000000000..85b4d8856b --- /dev/null +++ b/libraries/APO/examples/ServoManual/Makefile @@ -0,0 +1,2 @@ +BOARD = mega +include ../../../AP_Common/Arduino.mk diff --git a/libraries/APO/examples/ServoManual/ServoManual.pde b/libraries/APO/examples/ServoManual/ServoManual.pde new file mode 100644 index 0000000000..8be346f18a --- /dev/null +++ b/libraries/APO/examples/ServoManual/ServoManual.pde @@ -0,0 +1,109 @@ +/* + Example of RC_Channel library. + Code by James Goppert/ Jason Short. 2010 + DIYDrones.com + + */ +#include +#include +#include +#include // ArduPilot Mega RC Library +#include +#include +FastSerialPort0(Serial) +; // make sure this proceeds variable declarations + +// test settings + +using namespace apo; + +class RadioTest { +private: + float testPosition; + int8_t testSign; + enum { + version, + rollKey, + pitchKey, + thrKey, + yawKey, + ch5Key, + ch6Key, + ch7Key, + ch8Key + }; + Vector ch; +public: + RadioTest() : + testPosition(2), testSign(1) { + ch.push_back( + new AP_RcChannel(rollKey, PSTR("ROLL"), APM_RC, 0, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(pitchKey, PSTR("PITCH"), APM_RC, 1, 1100, + 1500, 1900)); + ch.push_back( + new AP_RcChannel(thrKey, PSTR("THR"), APM_RC, 2, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(yawKey, PSTR("YAW"), APM_RC, 3, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch5Key, PSTR("CH5"), APM_RC, 4, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch6Key, PSTR("CH6"), APM_RC, 5, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch7Key, PSTR("CH7"), APM_RC, 6, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch8Key, PSTR("CH8"), APM_RC, 7, 1100, 1500, + 1900)); + + Serial.begin(115200); + delay(2000); + Serial.println("ArduPilot RC Channel test"); + APM_RC.Init(); // APM Radio initialization + delay(2000); + } + + void update() { + // read the radio + for (uint8_t i = 0; i < ch.getSize(); i++) + ch[i]->setUsingRadio(); + + // print channel names + Serial.print("\t\t"); + static char name[7]; + for (uint8_t i = 0; i < ch.getSize(); i++) { + ch[i]->copy_name(name, 7); + Serial.printf("%7s\t", name); + } + Serial.println(); + + // print pwm + Serial.printf("pwm :\t"); + for (uint8_t i = 0; i < ch.getSize(); i++) + Serial.printf("%7d\t", ch[i]->getPwm()); + Serial.println(); + + // print position + Serial.printf("position :\t"); + for (uint8_t i = 0; i < ch.getSize(); i++) + Serial.printf("%7.2f\t", ch[i]->getPosition()); + Serial.println(); + + delay(500); + } +}; + +RadioTest * test; + +void setup() { + test = new RadioTest; +} + +void loop() { + test->update(); +} diff --git a/libraries/APO/examples/ServoSweep/Makefile b/libraries/APO/examples/ServoSweep/Makefile new file mode 100644 index 0000000000..85b4d8856b --- /dev/null +++ b/libraries/APO/examples/ServoSweep/Makefile @@ -0,0 +1,2 @@ +BOARD = mega +include ../../../AP_Common/Arduino.mk diff --git a/libraries/APO/examples/ServoSweep/ServoSweep.pde b/libraries/APO/examples/ServoSweep/ServoSweep.pde new file mode 100644 index 0000000000..75827d4f09 --- /dev/null +++ b/libraries/APO/examples/ServoSweep/ServoSweep.pde @@ -0,0 +1,125 @@ +/* + Example of RC_Channel library. + Code by James Goppert/ Jason Short. 2010 + DIYDrones.com + + */ +#include +#include +#include +#include // ArduPilot Mega RC Library +#include +#include +FastSerialPort0(Serial) +; // make sure this proceeds variable declarations + +// test settings + +using namespace apo; + +class RadioTest { +private: + float testPosition; + int8_t testSign; + enum { + version, + rollKey, + pitchKey, + thrKey, + yawKey, + ch5Key, + ch6Key, + ch7Key, + ch8Key + }; + Vector ch; +public: + RadioTest() : + testPosition(2), testSign(1) { + ch.push_back( + new AP_RcChannel(rollKey, PSTR("ROLL"), APM_RC, 0, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(pitchKey, PSTR("PITCH"), APM_RC, 1, 1100, + 1500, 1900)); + ch.push_back( + new AP_RcChannel(thrKey, PSTR("THR"), APM_RC, 2, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(yawKey, PSTR("YAW"), APM_RC, 3, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch5Key, PSTR("CH5"), APM_RC, 4, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch6Key, PSTR("CH6"), APM_RC, 5, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch7Key, PSTR("CH7"), APM_RC, 6, 1100, 1500, + 1900)); + ch.push_back( + new AP_RcChannel(ch8Key, PSTR("CH8"), APM_RC, 7, 1100, 1500, + 1900)); + + Serial.begin(115200); + delay(2000); + Serial.println("ArduPilot RC Channel test"); + APM_RC.Init(); // APM Radio initialization + delay(2000); + } + + void update() { + // update test value + testPosition += testSign * .1; + if (testPosition > 1) { + //eepromRegistry.print(Serial); // show eeprom map + testPosition = 1; + testSign = -1; + } else if (testPosition < -1) { + testPosition = -1; + testSign = 1; + } + + // set channel positions + for (uint8_t i = 0; i < ch.getSize(); i++) + ch[i]->setPosition(testPosition); + + // print test position + Serial.printf("\nnormalized position (%f)\n", testPosition); + + // print channel names + Serial.print("\t\t"); + static char name[7]; + for (uint8_t i = 0; i < ch.getSize(); i++) { + ch[i]->copy_name(name, 7); + Serial.printf("%7s\t", name); + } + Serial.println(); + + // print pwm + Serial.printf("pwm :\t"); + for (uint8_t i = 0; i < ch.getSize(); i++) + Serial.printf("%7d\t", ch[i]->getRadioPwm()); + Serial.println(); + + // print position + Serial.printf("position :\t"); + for (uint8_t i = 0; i < ch.getSize(); i++) + Serial.printf("%7.2f\t", ch[i]->getRadioPosition()); + Serial.println(); + + delay(500); + } +}; + +RadioTest * test; + +void setup() { + test = new RadioTest; +} + +void loop() { + test->update(); +} + +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/template.h b/libraries/APO/template.h new file mode 100644 index 0000000000..4bed52d485 --- /dev/null +++ b/libraries/APO/template.h @@ -0,0 +1,32 @@ +/* + * Class.h + * Copyright (C) Author 2011 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#ifndef Class_H +#define Class_H + +namespace apo { + +/// Class desciprtion +class Class { +public: +} + +} // namespace apo + +#endif // Class_H +// vim:ts=4:sw=4:expandtab diff --git a/libraries/AP_ADC/CMakeLists.txt b/libraries/AP_ADC/CMakeLists.txt index df85645fb2..4c72c51e9f 100644 --- a/libraries/AP_ADC/CMakeLists.txt +++ b/libraries/AP_ADC/CMakeLists.txt @@ -21,6 +21,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_Common/AP_Var.cpp b/libraries/AP_Common/AP_Var.cpp index 956f527a3a..efc590743b 100644 --- a/libraries/AP_Common/AP_Var.cpp +++ b/libraries/AP_Common/AP_Var.cpp @@ -9,19 +9,21 @@ /// @file AP_Var.cpp /// @brief The AP variable store. -#if 0 -# include -extern "C" { extern void delay(unsigned long); } -# define debug(fmt, args...) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args); delay(100); } while(0) -#else -# define debug(fmt, args...) -#endif #include #include #include +#define ENABLE_FASTSERIAL_DEBUG + +#ifdef ENABLE_FASTSERIAL_DEBUG +# include +# define serialDebug(fmt, args...) if (FastSerial::getInitialized(0)) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args); delay(0); } while(0) +#else +# define serialDebug(fmt, args...) +#endif + // Global constants exported for general use. // AP_Float AP_Float_unity ( 1.0, AP_Var::k_key_none, NULL, AP_Var::k_flag_unlisted); @@ -71,7 +73,11 @@ AP_Var::AP_Var(AP_Var_group *pGroup, Key index, const prog_char_t *name, Flags f // to the pointer to the node that we are considering inserting in front of. // vp = &_grouped_variables; + size_t loopCount = 0; while (*vp != NULL) { + + if (loopCount++>k_num_max) return; + if ((*vp)->_key >= _key) { break; } @@ -98,12 +104,19 @@ AP_Var::~AP_Var(void) } // Scan the list and remove this if we find it - while (*vp) { - if (*vp == this) { - *vp = _link; - break; - } - vp = &((*vp)->_link); + + { + size_t loopCount = 0; + while (*vp) { + + if (loopCount++>k_num_max) return; + + if (*vp == this) { + *vp = _link; + break; + } + vp = &((*vp)->_link); + } } // If we are destroying a group, remove all its variables from the list @@ -112,8 +125,12 @@ AP_Var::~AP_Var(void) // Scan the list and remove any variable that has this as its group vp = &_grouped_variables; + size_t loopCount = 0; + while (*vp) { + if (loopCount++>k_num_max) return; + // Does the variable claim us as its group? if ((*vp)->_group == this) { *vp = (*vp)->_link; @@ -145,7 +162,12 @@ AP_Var::find(const char *name) { AP_Var *vp; + size_t loopCount = 0; + for (vp = first(); vp; vp = vp->next()) { + + if (loopCount++>k_num_max) return NULL; + char name_buffer[32]; // copy the variable's name into our scratch buffer @@ -165,9 +187,9 @@ AP_Var * AP_Var::find(Key key) { AP_Var *vp; - + size_t loopCount = 0; for (vp = first(); vp; vp = vp->next()) { - + if (loopCount++>k_num_max) return NULL; if (key == vp->key()) { return vp; } @@ -188,11 +210,11 @@ bool AP_Var::save(void) return _group->save(); } - debug("save: %S", _name ? _name : PSTR("??")); + serialDebug("save: %S", _name ? _name : PSTR("??")); // locate the variable in EEPROM, allocating space as required if (!_EEPROM_locate(true)) { - debug("locate failed"); + serialDebug("locate failed"); return false; } @@ -200,13 +222,13 @@ bool AP_Var::save(void) size = serialize(vbuf, sizeof(vbuf)); if (0 == size) { // variable cannot be serialised into the buffer - debug("cannot save (too big or not supported)"); + serialDebug("cannot save (too big or not supported)"); return false; } // if it fit in the buffer, save it to EEPROM if (size <= sizeof(vbuf)) { - debug("saving %u to %u", size, _key); + serialDebug("saving %u to %u", size, _key); // XXX this should use eeprom_update_block if/when Arduino moves to // avr-libc >= 1.7 uint8_t *ep = (uint8_t *)_key; @@ -219,7 +241,7 @@ bool AP_Var::save(void) // now read it back newv = eeprom_read_byte(ep); if (newv != vbuf[i]) { - debug("readback failed at offset %p: got %u, expected %u", + serialDebug("readback failed at offset %p: got %u, expected %u", ep, newv, vbuf[i]); return false; } @@ -241,11 +263,11 @@ bool AP_Var::load(void) return _group->load(); } - debug("load: %S", _name ? _name : PSTR("??")); + serialDebug("load: %S", _name ? _name : PSTR("??")); // locate the variable in EEPROM, but do not allocate space if (!_EEPROM_locate(false)) { - debug("locate failed"); + serialDebug("locate failed"); return false; } @@ -255,7 +277,7 @@ bool AP_Var::load(void) // size = serialize(NULL, 0); if (0 == size) { - debug("cannot load (too big or not supported)"); + serialDebug("cannot load (too big or not supported)"); return false; } @@ -263,7 +285,7 @@ bool AP_Var::load(void) // has converted _key into an EEPROM address. // if (size <= sizeof(vbuf)) { - debug("loading %u from %u", size, _key); + serialDebug("loading %u from %u", size, _key); eeprom_read_block(vbuf, (void *)_key, size); return unserialize(vbuf, size); } @@ -278,7 +300,12 @@ bool AP_Var::save_all(void) bool result = true; AP_Var *vp = _variables; + size_t loopCount = 0; + while (vp) { + + if (loopCount++>k_num_max) return false; + if (!vp->has_flags(k_flag_no_auto_load) && // not opted out of autosave (vp->_key != k_key_none)) { // has a key @@ -298,7 +325,12 @@ bool AP_Var::load_all(void) bool result = true; AP_Var *vp = _variables; + size_t loopCount = 0; + while (vp) { + + if (loopCount++>k_num_max) return false; + if (!vp->has_flags(k_flag_no_auto_load) && // not opted out of autoload (vp->_key != k_key_none)) { // has a key @@ -322,13 +354,19 @@ AP_Var::erase_all() AP_Var *vp; uint16_t i; - debug("erase EEPROM"); + serialDebug("erase EEPROM"); // Scan the list of variables/groups, fetching their key values and // reverting them to their not-located state. // vp = _variables; + + size_t loopCount = 0; + while (vp) { + + if (loopCount++>k_num_max) return; + vp->_key = vp->key() | k_key_not_located; vp = vp->_link; } @@ -405,9 +443,15 @@ AP_Var::first_member(AP_Var_group *group) vp = &_grouped_variables; - debug("seeking %p", group); + serialDebug("seeking %p", group); + + size_t loopCount = 0; + while (*vp) { - debug("consider %p with %p", *vp, (*vp)->_group); + + if (loopCount++>k_num_max) return NULL; + + serialDebug("consider %p with %p", *vp, (*vp)->_group); if ((*vp)->_group == group) { return *vp; } @@ -423,7 +467,12 @@ AP_Var::next_member() AP_Var *vp; vp = _link; + size_t loopCount = 0; + while (vp) { + + if (loopCount++>k_num_max) return NULL; + if (vp->_group == _group) { return vp; } @@ -451,7 +500,7 @@ bool AP_Var::_EEPROM_scan(void) if ((ee_header.magic != k_EEPROM_magic) || (ee_header.revision != k_EEPROM_revision)) { - debug("no header, magic 0x%x revision %u", ee_header.magic, ee_header.revision); + serialDebug("no header, magic 0x%x revision %u", ee_header.magic, ee_header.revision); return false; } @@ -460,17 +509,22 @@ bool AP_Var::_EEPROM_scan(void) // Avoid trying to read a header when there isn't enough space left. // eeprom_address = sizeof(ee_header); + + size_t loopCount = 0; + while (eeprom_address < (k_EEPROM_size - sizeof(var_header) - 1)) { + if (loopCount++>k_num_max) return NULL; + // Read a variable header // - debug("reading header from %u", eeprom_address); + serialDebug("reading header from %u", eeprom_address); eeprom_read_block(&var_header, (void *)eeprom_address, sizeof(var_header)); // If the header is for the sentinel, scanning is complete // if (var_header.key == k_key_sentinel) { - debug("found tail sentinel"); + serialDebug("found tail sentinel"); break; } @@ -482,23 +536,25 @@ bool AP_Var::_EEPROM_scan(void) var_header.size + 1 + // data for this variable sizeof(var_header))) { // header for sentinel - debug("header overruns EEPROM"); + serialDebug("header overruns EEPROM"); return false; } // look for a variable with this key vp = _variables; - while (vp) { + size_t loopCount2 = 0; + while(vp) { + if (loopCount2++>k_num_max) return false; if (vp->key() == var_header.key) { // adjust the variable's key to point to this entry vp->_key = eeprom_address + sizeof(var_header); - debug("update %p with key %u -> %u", vp, var_header.key, vp->_key); + serialDebug("update %p with key %u -> %u", vp, var_header.key, vp->_key); break; } vp = vp->_link; } if (!vp) { - debug("key %u not claimed (already scanned or unknown)", var_header.key); + serialDebug("key %u not claimed (already scanned or unknown)", var_header.key); } // move to the next variable header @@ -514,16 +570,18 @@ bool AP_Var::_EEPROM_scan(void) // mark all the rest as not allocated. // vp = _variables; - while (vp) { + size_t loopCount3 = 0; + while(vp) { + if (loopCount3++>k_num_max) return false; if (vp->_key & k_key_not_located) { vp->_key |= k_key_not_allocated; - debug("key %u not allocated", vp->key()); + serialDebug("key %u not allocated", vp->key()); } vp = vp->_link; } // Scanning is complete - debug("scan done"); + serialDebug("scan done"); _tail_sentinel = eeprom_address; return true; } @@ -539,7 +597,7 @@ bool AP_Var::_EEPROM_locate(bool allocate) // Is it a group member, or does it have a no-location key? // if (_group || (_key == k_key_none)) { - debug("not addressable"); + serialDebug("not addressable"); return false; // it is/does, and thus it has no location } @@ -554,7 +612,7 @@ bool AP_Var::_EEPROM_locate(bool allocate) // try scanning to see if we can locate it. // if (!(_key & k_key_not_allocated)) { - debug("need scan"); + serialDebug("need scan"); _EEPROM_scan(); // Has the variable now been located? @@ -569,7 +627,7 @@ bool AP_Var::_EEPROM_locate(bool allocate) if (!allocate) { return false; } - debug("needs allocation"); + serialDebug("needs allocation"); // Ask the serializer for the size of the thing we are allocating, and fail // if it is too large or if it has no size, as we will not be able to allocate @@ -577,7 +635,7 @@ bool AP_Var::_EEPROM_locate(bool allocate) // size = serialize(NULL, 0); if ((size == 0) || (size > k_size_max)) { - debug("size %u out of bounds", size); + serialDebug("size %u out of bounds", size); return false; } @@ -585,7 +643,7 @@ bool AP_Var::_EEPROM_locate(bool allocate) // header and the new tail sentinel. // if ((_tail_sentinel + size + sizeof(Var_header) * 2) > k_EEPROM_size) { - debug("no space in EEPROM"); + serialDebug("no space in EEPROM"); return false; } @@ -595,7 +653,7 @@ bool AP_Var::_EEPROM_locate(bool allocate) if (0 == _tail_sentinel) { uint8_t pad_size; - debug("writing header"); + serialDebug("writing header"); EEPROM_header ee_header; ee_header.magic = k_EEPROM_magic; @@ -621,7 +679,7 @@ bool AP_Var::_EEPROM_locate(bool allocate) // new_location = _tail_sentinel; _tail_sentinel += sizeof(var_header) + size; - debug("allocated %u/%u for key %u new sentinel %u", new_location, size, key(), _tail_sentinel); + serialDebug("allocated %u/%u for key %u new sentinel %u", new_location, size, key(), _tail_sentinel); // Write the new sentinel first. If we are interrupted during this operation // the old sentinel will still correctly terminate the EEPROM image. @@ -670,17 +728,22 @@ AP_Var_group::_serialize_unserialize(void *buf, size_t buf_size, bool do_seriali // Traverse the list of group members, serializing each in order // vp = first_member(this); - debug("starting with %p", vp); + serialDebug("starting with %p", vp); total_size = 0; + + size_t loopCount = 0; + while (vp) { + if (loopCount++>k_num_max) return false; + // (un)serialise the group member if (do_serialize) { size = vp->serialize(buf, buf_size); - debug("serialize %p -> %u", vp, size); + serialDebug("serialize %p -> %u", vp, size); } else { size = vp->unserialize(buf, buf_size); - debug("unserialize %p -> %u", vp, size); + serialDebug("unserialize %p -> %u", vp, size); } // Unserialize will return zero if the buffer is too small @@ -688,7 +751,7 @@ AP_Var_group::_serialize_unserialize(void *buf, size_t buf_size, bool do_seriali // Either case is fatal for any operation we might be trying. // if (0 == size) { - debug("group (un)serialize failed, buffer too small or not supported"); + serialDebug("group (un)serialize failed, buffer too small or not supported"); return 0; } @@ -704,7 +767,7 @@ AP_Var_group::_serialize_unserialize(void *buf, size_t buf_size, bool do_seriali // and the calling function will have to treat it as an error. // total_size += size; - debug("used %u", total_size); + serialDebug("used %u", total_size); if (size <= buf_size) { // there was space for this one, account for it buf_size -= size; diff --git a/libraries/AP_Common/AP_Var.h b/libraries/AP_Common/AP_Var.h index 6ed680d7ab..4fd5dc1d49 100644 --- a/libraries/AP_Common/AP_Var.h +++ b/libraries/AP_Common/AP_Var.h @@ -128,6 +128,10 @@ public: /// static const size_t k_size_max = 64; + /// The maximum number of keys + /// + static const size_t k_num_max = 255; + /// Optional flags affecting the behavior and usage of the variable. /// typedef uint8_t Flags; diff --git a/libraries/AP_Common/CMakeLists.txt b/libraries/AP_Common/CMakeLists.txt index 7d89ba1e40..d86ce08985 100644 --- a/libraries/AP_Common/CMakeLists.txt +++ b/libraries/AP_Common/CMakeLists.txt @@ -28,6 +28,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_Compass/CMakeLists.txt b/libraries/AP_Compass/CMakeLists.txt index 613b751ef4..e12ac20fdb 100644 --- a/libraries/AP_Compass/CMakeLists.txt +++ b/libraries/AP_Compass/CMakeLists.txt @@ -22,6 +22,6 @@ include_directories( #${CMAKE_SOURCE_DIR}/libraries/FastSerial # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_DCM/CMakeLists.txt b/libraries/AP_DCM/CMakeLists.txt index 0131eeb495..2f0ecd2f78 100644 --- a/libraries/AP_DCM/CMakeLists.txt +++ b/libraries/AP_DCM/CMakeLists.txt @@ -20,6 +20,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_GPS/CMakeLists.txt b/libraries/AP_GPS/CMakeLists.txt index 57409c6d24..56570d2c9c 100644 --- a/libraries/AP_GPS/CMakeLists.txt +++ b/libraries/AP_GPS/CMakeLists.txt @@ -39,6 +39,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_IMU/CMakeLists.txt b/libraries/AP_IMU/CMakeLists.txt index 2447a078c4..4685ea4511 100644 --- a/libraries/AP_IMU/CMakeLists.txt +++ b/libraries/AP_IMU/CMakeLists.txt @@ -20,6 +20,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_Math/CMakeLists.txt b/libraries/AP_Math/CMakeLists.txt index f4a83b3102..262bbfd8cc 100644 --- a/libraries/AP_Math/CMakeLists.txt +++ b/libraries/AP_Math/CMakeLists.txt @@ -19,6 +19,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_OpticalFlow/CMakeLists.txt b/libraries/AP_OpticalFlow/CMakeLists.txt index 74d7457ac4..117d84c00d 100644 --- a/libraries/AP_OpticalFlow/CMakeLists.txt +++ b/libraries/AP_OpticalFlow/CMakeLists.txt @@ -19,6 +19,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_PID/CMakeLists.txt b/libraries/AP_PID/CMakeLists.txt index 14e9782e51..c38fbea5d3 100644 --- a/libraries/AP_PID/CMakeLists.txt +++ b/libraries/AP_PID/CMakeLists.txt @@ -19,6 +19,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/AP_RangeFinder/CMakeLists.txt b/libraries/AP_RangeFinder/CMakeLists.txt index add2e2928e..05b5750e33 100644 --- a/libraries/AP_RangeFinder/CMakeLists.txt +++ b/libraries/AP_RangeFinder/CMakeLists.txt @@ -22,6 +22,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/DataFlash/CMakeLists.txt b/libraries/DataFlash/CMakeLists.txt index 2e13294932..d36f3fa8a7 100644 --- a/libraries/DataFlash/CMakeLists.txt +++ b/libraries/DataFlash/CMakeLists.txt @@ -16,6 +16,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/FastSerial/CMakeLists.txt b/libraries/FastSerial/CMakeLists.txt index 78e94e9023..1ace8cb81c 100644 --- a/libraries/FastSerial/CMakeLists.txt +++ b/libraries/FastSerial/CMakeLists.txt @@ -23,6 +23,6 @@ include_directories( # ) -set(${LIB_NAME}_BOARD mega2560) +set(${LIB_NAME}_BOARD ${BOARD}) generate_arduino_library(${LIB_NAME}) diff --git a/libraries/FastSerial/FastSerial.cpp b/libraries/FastSerial/FastSerial.cpp index 5ee8ad3ee8..5312f3d6bf 100644 --- a/libraries/FastSerial/FastSerial.cpp +++ b/libraries/FastSerial/FastSerial.cpp @@ -45,6 +45,7 @@ FastSerial::Buffer __FastSerial__rxBuffer[FS_MAX_PORTS]; FastSerial::Buffer __FastSerial__txBuffer[FS_MAX_PORTS]; +uint8_t FastSerial::_serialInitialized = 0; // Constructor ///////////////////////////////////////////////////////////////// @@ -61,6 +62,8 @@ FastSerial::FastSerial(const uint8_t portNumber, volatile uint8_t *ubrrh, volati _rxBuffer(&__FastSerial__rxBuffer[portNumber]), _txBuffer(&__FastSerial__txBuffer[portNumber]) { + setInitialized(portNumber); + begin(57600); } // Public Methods ////////////////////////////////////////////////////////////// diff --git a/libraries/FastSerial/FastSerial.h b/libraries/FastSerial/FastSerial.h index 920554555f..bc0417a7c0 100644 --- a/libraries/FastSerial/FastSerial.h +++ b/libraries/FastSerial/FastSerial.h @@ -55,6 +55,7 @@ #include "BetterStream.h" + /// @file FastSerial.h /// @brief An enhanced version of the Arduino HardwareSerial class /// implementing interrupt-driven transmission and flexible @@ -101,6 +102,7 @@ extern class FastSerial Serial3; /// class FastSerial: public BetterStream { public: + /// Constructor FastSerial(const uint8_t portNumber, volatile uint8_t *ubrrh, volatile uint8_t *ubrrl, volatile uint8_t *ucsra, volatile uint8_t *ucsrb, const uint8_t u2x, const uint8_t portEnableBits, const uint8_t portTxBits); @@ -149,7 +151,21 @@ public: uint8_t *bytes; ///< pointer to allocated buffer }; + /// Tell if the serial port has been initialized + static bool getInitialized(uint8_t port) { + return (1< Date: Thu, 29 Sep 2011 14:16:36 -0400 Subject: [PATCH 3/7] Added cmake files. --- ArduBoat/ArduBoat.pde | 21 + ArduBoat/CMakeLists.txt | 12 +- ArduBoat/Makefile | 1 + ArduRover/ArduRover.pde | 21 + ArduRover/CMakeLists.txt | 12 +- ArduRover/Makefile | 1 + cmake/arkcmake/.gitignore | 4 + cmake/arkcmake/COPYING | 674 ++++++++++++++++++ cmake/arkcmake/DefineCMakeDefaults.cmake | 27 + cmake/arkcmake/DefineCompilerFlags.cmake | 71 ++ cmake/arkcmake/DefineInstallationPaths.cmake | 104 +++ cmake/arkcmake/DefinePlatformDefaults.cmake | 28 + cmake/arkcmake/FindARKCOMM.cmake | 36 + cmake/arkcmake/FindARKMATH.cmake | 43 ++ cmake/arkcmake/FindARKOSG.cmake | 43 ++ cmake/arkcmake/FindARKSCICOS.cmake | 36 + cmake/arkcmake/FindBOOSTNUMERICBINDINGS.cmake | 21 + cmake/arkcmake/FindEASYSTAR.cmake | 21 + cmake/arkcmake/FindJSBSIM.cmake | 39 + cmake/arkcmake/FindMAVLINK.cmake | 21 + cmake/arkcmake/FindPLIB.cmake | 70 ++ cmake/arkcmake/FindSCICOSLAB.cmake | 72 ++ cmake/arkcmake/FindSIMGEAR.cmake | 29 + cmake/arkcmake/LibFindMacros.cmake | 98 +++ cmake/arkcmake/MacroAddCompileFlags.cmake | 21 + cmake/arkcmake/MacroAddLinkFlags.cmake | 20 + cmake/arkcmake/MacroAddPlugin.cmake | 30 + .../arkcmake/MacroCheckCCompilerFlagSSP.cmake | 26 + cmake/arkcmake/MacroCommonPaths.cmake | 51 ++ cmake/arkcmake/MacroCopyFile.cmake | 33 + .../MacroEnsureOutOfSourceBuild.cmake | 19 + cmake/arkcmake/MacroFindOrBuild.cmake | 18 + cmake/arkcmake/MacroSetDefault.cmake | 6 + cmake/arkcmake/README.md | 10 + cmake/arkcmake/UseDoxygen.cmake | 100 +++ cmake/arkcmake/autobuild.py | 205 ++++++ cmake/arkcmake/get_build_path.py | 62 ++ cmake/arkcmake/language_support_v2.cmake | 65 ++ cmake/arkcmake/updateArkcmake.py | 19 + libraries/APO/APO_DefaultSetup.h | 161 +++++ libraries/AP_Common/AP_Var.cpp | 2 +- 41 files changed, 2332 insertions(+), 21 deletions(-) create mode 100644 ArduBoat/ArduBoat.pde create mode 100644 ArduBoat/Makefile create mode 100644 ArduRover/ArduRover.pde create mode 100644 ArduRover/Makefile create mode 100644 cmake/arkcmake/.gitignore create mode 100644 cmake/arkcmake/COPYING create mode 100644 cmake/arkcmake/DefineCMakeDefaults.cmake create mode 100644 cmake/arkcmake/DefineCompilerFlags.cmake create mode 100644 cmake/arkcmake/DefineInstallationPaths.cmake create mode 100644 cmake/arkcmake/DefinePlatformDefaults.cmake create mode 100644 cmake/arkcmake/FindARKCOMM.cmake create mode 100644 cmake/arkcmake/FindARKMATH.cmake create mode 100644 cmake/arkcmake/FindARKOSG.cmake create mode 100644 cmake/arkcmake/FindARKSCICOS.cmake create mode 100644 cmake/arkcmake/FindBOOSTNUMERICBINDINGS.cmake create mode 100644 cmake/arkcmake/FindEASYSTAR.cmake create mode 100644 cmake/arkcmake/FindJSBSIM.cmake create mode 100644 cmake/arkcmake/FindMAVLINK.cmake create mode 100644 cmake/arkcmake/FindPLIB.cmake create mode 100644 cmake/arkcmake/FindSCICOSLAB.cmake create mode 100644 cmake/arkcmake/FindSIMGEAR.cmake create mode 100644 cmake/arkcmake/LibFindMacros.cmake create mode 100644 cmake/arkcmake/MacroAddCompileFlags.cmake create mode 100644 cmake/arkcmake/MacroAddLinkFlags.cmake create mode 100644 cmake/arkcmake/MacroAddPlugin.cmake create mode 100644 cmake/arkcmake/MacroCheckCCompilerFlagSSP.cmake create mode 100644 cmake/arkcmake/MacroCommonPaths.cmake create mode 100644 cmake/arkcmake/MacroCopyFile.cmake create mode 100644 cmake/arkcmake/MacroEnsureOutOfSourceBuild.cmake create mode 100644 cmake/arkcmake/MacroFindOrBuild.cmake create mode 100644 cmake/arkcmake/MacroSetDefault.cmake create mode 100644 cmake/arkcmake/README.md create mode 100644 cmake/arkcmake/UseDoxygen.cmake create mode 100755 cmake/arkcmake/autobuild.py create mode 100755 cmake/arkcmake/get_build_path.py create mode 100644 cmake/arkcmake/language_support_v2.cmake create mode 100755 cmake/arkcmake/updateArkcmake.py create mode 100644 libraries/APO/APO_DefaultSetup.h diff --git a/ArduBoat/ArduBoat.pde b/ArduBoat/ArduBoat.pde new file mode 100644 index 0000000000..58d159dd05 --- /dev/null +++ b/ArduBoat/ArduBoat.pde @@ -0,0 +1,21 @@ +// Libraries +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Vehicle Configuration +#include "BoatGeneric.h" + +// ArduPilotOne Default Setup +#include "APO_DefaultSetup.h" diff --git a/ArduBoat/CMakeLists.txt b/ArduBoat/CMakeLists.txt index 95c4c1289c..4686c63bd9 100644 --- a/ArduBoat/CMakeLists.txt +++ b/ArduBoat/CMakeLists.txt @@ -7,12 +7,12 @@ #====================================================================# # Settings # #====================================================================# -set(FIRMWARE_NAME ardupilotone) +set(FIRMWARE_NAME ArduBoat) set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board set(${FIRMWARE_NAME}_SKETCHES - ardupilotone.pde + ArduBoat.pde ) # Firmware sketches set(${FIRMWARE_NAME}_SRCS @@ -20,16 +20,8 @@ set(${FIRMWARE_NAME}_SRCS set(${FIRMWARE_NAME}_HDRS BoatGeneric.h - CarStampede.h - #CNIRover.h #CNIBoat.h ControllerBoat.h - ControllerCar.h - ControllerPlane.h - ControllerQuad.h - PlaneEasystar.h - QuadArducopter.h - QuadMikrokopter.h ) # Firmware sources set(${FIRMWARE_NAME}_LIBS diff --git a/ArduBoat/Makefile b/ArduBoat/Makefile new file mode 100644 index 0000000000..1d9b6a022d --- /dev/null +++ b/ArduBoat/Makefile @@ -0,0 +1 @@ +include ../libraries/AP_Common/Arduino.mk diff --git a/ArduRover/ArduRover.pde b/ArduRover/ArduRover.pde new file mode 100644 index 0000000000..f6a1f17ac3 --- /dev/null +++ b/ArduRover/ArduRover.pde @@ -0,0 +1,21 @@ +// Libraries +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Vehicle Configuration +#include "CarStampede.h" + +// ArduPilotOne Default Setup +#include "APO_DefaultSetup.h" diff --git a/ArduRover/CMakeLists.txt b/ArduRover/CMakeLists.txt index 95c4c1289c..7e5ba77a24 100644 --- a/ArduRover/CMakeLists.txt +++ b/ArduRover/CMakeLists.txt @@ -7,29 +7,21 @@ #====================================================================# # Settings # #====================================================================# -set(FIRMWARE_NAME ardupilotone) +set(FIRMWARE_NAME ArduRover) set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board set(${FIRMWARE_NAME}_SKETCHES - ardupilotone.pde + ArduRover.pde ) # Firmware sketches set(${FIRMWARE_NAME}_SRCS ) # Firmware sources set(${FIRMWARE_NAME}_HDRS - BoatGeneric.h CarStampede.h #CNIRover.h - #CNIBoat.h - ControllerBoat.h ControllerCar.h - ControllerPlane.h - ControllerQuad.h - PlaneEasystar.h - QuadArducopter.h - QuadMikrokopter.h ) # Firmware sources set(${FIRMWARE_NAME}_LIBS diff --git a/ArduRover/Makefile b/ArduRover/Makefile new file mode 100644 index 0000000000..1d9b6a022d --- /dev/null +++ b/ArduRover/Makefile @@ -0,0 +1 @@ +include ../libraries/AP_Common/Arduino.mk diff --git a/cmake/arkcmake/.gitignore b/cmake/arkcmake/.gitignore new file mode 100644 index 0000000000..b8959c7221 --- /dev/null +++ b/cmake/arkcmake/.gitignore @@ -0,0 +1,4 @@ +*.swp +*.pyc +.DS_Store +arkcmake diff --git a/cmake/arkcmake/COPYING b/cmake/arkcmake/COPYING new file mode 100644 index 0000000000..94a9ed024d --- /dev/null +++ b/cmake/arkcmake/COPYING @@ -0,0 +1,674 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/cmake/arkcmake/DefineCMakeDefaults.cmake b/cmake/arkcmake/DefineCMakeDefaults.cmake new file mode 100644 index 0000000000..72893c3c9b --- /dev/null +++ b/cmake/arkcmake/DefineCMakeDefaults.cmake @@ -0,0 +1,27 @@ +# Always include srcdir and builddir in include path +# This saves typing ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY} in +# about every subdir +# since cmake 2.4.0 +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +# Put the include dirs which are in the source or build tree +# before all other include dirs, so the headers in the sources +# are prefered over the already installed ones +# since cmake 2.4.1 +set(CMAKE_INCLUDE_DIRECTORIES_PROJECT_BEFORE ON) + +# Use colored output +# since cmake 2.4.0 +set(CMAKE_COLOR_MAKEFILE ON) + +# Define the generic version of the libraries here +set(GENERIC_LIB_VERSION "0.1.0") +set(GENERIC_LIB_SOVERSION "0") + +# Set the default build type to release with debug info +if (NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE RelWithDebInfo + CACHE STRING + "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." + ) +endif (NOT CMAKE_BUILD_TYPE) diff --git a/cmake/arkcmake/DefineCompilerFlags.cmake b/cmake/arkcmake/DefineCompilerFlags.cmake new file mode 100644 index 0000000000..d43c454bef --- /dev/null +++ b/cmake/arkcmake/DefineCompilerFlags.cmake @@ -0,0 +1,71 @@ +# define system dependent compiler flags + +include(CheckCCompilerFlag) +include(MacroCheckCCompilerFlagSSP) + +if (UNIX AND NOT WIN32) + # + # Define GNUCC compiler flags + # + if (${CMAKE_C_COMPILER_ID} MATCHES GNU) + # add -Wconversion ? + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -pedantic -pedantic-errors") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra -Wshadow -Wmissing-prototypes -Wdeclaration-after-statement") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wunused -Wfloat-equal -Wpointer-arith -Wwrite-strings -Wformat-security") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wmissing-format-attribute") + + # with -fPIC + check_c_compiler_flag("-fPIC" WITH_FPIC) + if (WITH_FPIC) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC") + endif (WITH_FPIC) + + check_c_compiler_flag_ssp("-fstack-protector" WITH_STACK_PROTECTOR) + if (WITH_STACK_PROTECTOR) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fstack-protector") + endif (WITH_STACK_PROTECTOR) + + check_c_compiler_flag("-D_FORTIFY_SOURCE=2" WITH_FORTIFY_SOURCE) + if (WITH_FORTIFY_SOURCE) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -D_FORTIFY_SOURCE=2") + endif (WITH_FORTIFY_SOURCE) + endif (${CMAKE_C_COMPILER_ID} MATCHES GNU) + + # + # Check for large filesystem support + # + if (CMAKE_SIZEOF_VOID_P MATCHES "8") + # with large file support + execute_process( + COMMAND + getconf LFS64_CFLAGS + OUTPUT_VARIABLE + _lfs_CFLAGS + ERROR_QUIET + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + else (CMAKE_SIZEOF_VOID_P MATCHES "8") + # with large file support + execute_process( + COMMAND + getconf LFS_CFLAGS + OUTPUT_VARIABLE + _lfs_CFLAGS + ERROR_QUIET + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + endif (CMAKE_SIZEOF_VOID_P MATCHES "8") + if (_lfs_CFLAGS) + string(REGEX REPLACE "[\r\n]" " " "${_lfs_CFLAGS}" "${${_lfs_CFLAGS}}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${_lfs_CFLAGS}") + endif (_lfs_CFLAGS) + +endif (UNIX AND NOT WIN32) + +if (MSVC) + # Use secure functions by defaualt and suppress warnings about + #"deprecated" functions + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /D _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES=1") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /D _CRT_SECURE_CPP_OVERLOAD_STANDARD_NAMES_COUNT=1") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /D _CRT_NONSTDC_NO_WARNINGS=1 /D _CRT_SECURE_NO_WARNINGS=1") +endif (MSVC) diff --git a/cmake/arkcmake/DefineInstallationPaths.cmake b/cmake/arkcmake/DefineInstallationPaths.cmake new file mode 100644 index 0000000000..d857871e83 --- /dev/null +++ b/cmake/arkcmake/DefineInstallationPaths.cmake @@ -0,0 +1,104 @@ +if (WIN32) + # Same same + set(BIN_INSTALL_DIR "bin" CACHE PATH "-") + set(SBIN_INSTALL_DIR "." CACHE PATH "-") + set(LIB_INSTALL_DIR "lib" CACHE PATH "-") + set(INCLUDE_INSTALL_DIR "include" CACHE PATH "-") + set(PLUGIN_INSTALL_DIR "plugins" CACHE PATH "-") + set(HTML_INSTALL_DIR "doc/HTML" CACHE PATH "-") + set(ICON_INSTALL_DIR "." CACHE PATH "-") + set(SOUND_INSTALL_DIR "." CACHE PATH "-") + set(LOCALE_INSTALL_DIR "lang" CACHE PATH "-") +elseif (UNIX OR OS2) + IF (NOT APPLICATION_NAME) + MESSAGE(STATUS "${PROJECT_NAME} is used as APPLICATION_NAME") + SET(APPLICATION_NAME ${PROJECT_NAME}) + ENDIF (NOT APPLICATION_NAME) + + # Suffix for Linux + SET(LIB_SUFFIX + CACHE STRING "Define suffix of directory name (32/64)" + ) + + SET(EXEC_INSTALL_PREFIX + "${CMAKE_INSTALL_PREFIX}" + CACHE PATH "Base directory for executables and libraries" + ) + SET(SHARE_INSTALL_PREFIX + "${CMAKE_INSTALL_PREFIX}/share" + CACHE PATH "Base directory for files which go to share/" + ) + SET(DATA_INSTALL_PREFIX + "${SHARE_INSTALL_PREFIX}/${APPLICATION_NAME}" + CACHE PATH "The parent directory where applications can install their data") + + # The following are directories where stuff will be installed to + SET(BIN_INSTALL_DIR + "${EXEC_INSTALL_PREFIX}/bin" + CACHE PATH "The ${APPLICATION_NAME} binary install dir (default prefix/bin)" + ) + SET(SBIN_INSTALL_DIR + "${EXEC_INSTALL_PREFIX}/sbin" + CACHE PATH "The ${APPLICATION_NAME} sbin install dir (default prefix/sbin)" + ) + SET(LIB_INSTALL_DIR + "${EXEC_INSTALL_PREFIX}/lib${LIB_SUFFIX}" + CACHE PATH "The subdirectory relative to the install prefix where libraries will be installed (default is prefix/lib)" + ) + SET(LIBEXEC_INSTALL_DIR + "${EXEC_INSTALL_PREFIX}/libexec" + CACHE PATH "The subdirectory relative to the install prefix where libraries will be installed (default is prefix/libexec)" + ) + SET(PLUGIN_INSTALL_DIR + "${LIB_INSTALL_DIR}/${APPLICATION_NAME}" + CACHE PATH "The subdirectory relative to the install prefix where plugins will be installed (default is prefix/lib/${APPLICATION_NAME})" + ) + SET(INCLUDE_INSTALL_DIR + "${CMAKE_INSTALL_PREFIX}/include" + CACHE PATH "The subdirectory to the header prefix (default prefix/include)" + ) + + SET(DATA_INSTALL_DIR + "${DATA_INSTALL_PREFIX}" + CACHE PATH "The parent directory where applications can install their data (default prefix/share/${APPLICATION_NAME})" + ) + SET(HTML_INSTALL_DIR + "${DATA_INSTALL_PREFIX}/doc/HTML" + CACHE PATH "The HTML install dir for documentation (default data/doc/html)" + ) + SET(ICON_INSTALL_DIR + "${DATA_INSTALL_PREFIX}/icons" + CACHE PATH "The icon install dir (default data/icons/)" + ) + SET(SOUND_INSTALL_DIR + "${DATA_INSTALL_PREFIX}/sounds" + CACHE PATH "The install dir for sound files (default data/sounds)" + ) + + SET(LOCALE_INSTALL_DIR + "${SHARE_INSTALL_PREFIX}/locale" + CACHE PATH "The install dir for translations (default prefix/share/locale)" + ) + + SET(XDG_APPS_DIR + "${SHARE_INSTALL_PREFIX}/applications/" + CACHE PATH "The XDG apps dir" + ) + SET(XDG_DIRECTORY_DIR + "${SHARE_INSTALL_PREFIX}/desktop-directories" + CACHE PATH "The XDG directory" + ) + + SET(SYSCONF_INSTALL_DIR + "${EXEC_INSTALL_PREFIX}/etc" + CACHE PATH "The ${APPLICATION_NAME} sysconfig install dir (default prefix/etc)" + ) + SET(MAN_INSTALL_DIR + "${SHARE_INSTALL_PREFIX}/man" + CACHE PATH "The ${APPLICATION_NAME} man install dir (default prefix/man)" + ) + SET(INFO_INSTALL_DIR + "${SHARE_INSTALL_PREFIX}/info" + CACHE PATH "The ${APPLICATION_NAME} info install dir (default prefix/info)" + ) +endif () diff --git a/cmake/arkcmake/DefinePlatformDefaults.cmake b/cmake/arkcmake/DefinePlatformDefaults.cmake new file mode 100644 index 0000000000..502d936b6f --- /dev/null +++ b/cmake/arkcmake/DefinePlatformDefaults.cmake @@ -0,0 +1,28 @@ +# Set system vars + +if (CMAKE_SYSTEM_NAME MATCHES "Linux") + set(LINUX TRUE) +endif(CMAKE_SYSTEM_NAME MATCHES "Linux") + +if (CMAKE_SYSTEM_NAME MATCHES "FreeBSD") + set(FREEBSD TRUE) + set(BSD TRUE) +endif (CMAKE_SYSTEM_NAME MATCHES "FreeBSD") + +if (CMAKE_SYSTEM_NAME MATCHES "OpenBSD") + set(OPENBSD TRUE) + set(BSD TRUE) +endif (CMAKE_SYSTEM_NAME MATCHES "OpenBSD") + +if (CMAKE_SYSTEM_NAME MATCHES "NetBSD") + set(NETBSD TRUE) + set(BSD TRUE) +endif (CMAKE_SYSTEM_NAME MATCHES "NetBSD") + +if (CMAKE_SYSTEM_NAME MATCHES "(Solaris|SunOS)") + set(SOLARIS TRUE) +endif (CMAKE_SYSTEM_NAME MATCHES "(Solaris|SunOS)") + +if (CMAKE_SYSTEM_NAME MATCHES "OS2") + set(OS2 TRUE) +endif (CMAKE_SYSTEM_NAME MATCHES "OS2") diff --git a/cmake/arkcmake/FindARKCOMM.cmake b/cmake/arkcmake/FindARKCOMM.cmake new file mode 100644 index 0000000000..a25fe0e5a4 --- /dev/null +++ b/cmake/arkcmake/FindARKCOMM.cmake @@ -0,0 +1,36 @@ +# - Try to find ARKCOMM +# Once done, this will define +# +# ARKCOMM_FOUND - system has scicoslab +# ARKCOMM_INCLUDE_DIRS - the scicoslab include directories +# ARKCOMM_LIBRARIES - libraries to link to + +include(LibFindMacros) +include(MacroCommonPaths) + +MacroCommonPaths(ARKCOMM) + +# Include dir +find_path(ARKCOMM_INCLUDE_DIR + NAMES arkcomm/AsyncSerial.hpp + PATHS ${COMMON_INCLUDE_PATHS_ARKCOMM} +) + +# the library itself +find_library(ARKCOMM_LIBRARY + NAMES arkcomm + PATHS ${COMMON_LIBRARY_PATHS_ARKCOMM} +) + +# the import file +find_path(ARKCOMM_LIBRARY_DIR + NAMES arkcomm/arkcomm-targets.cmake + PATHS ${COMMON_LIBRARY_PATHS_ARKCOMM} +) +set(ARKCOMM_LIB_IMPORT ${ARKCOMM_LIBRARY_DIR}/arkcomm/arkcomm-targets.cmake) + +# Set the include dir variables and the libraries and let libfind_process do the rest. +# NOTE: Singular variables for this library, plural for libraries this this lib depends on. +set(ARKCOMM_PROCESS_INCLUDES ARKCOMM_INCLUDE_DIR) +set(ARKCOMM_PROCESS_LIBS ARKCOMM_LIBRARY ARKCOMM_LIBRARIES) +libfind_process(ARKCOMM) diff --git a/cmake/arkcmake/FindARKMATH.cmake b/cmake/arkcmake/FindARKMATH.cmake new file mode 100644 index 0000000000..1ad3a51103 --- /dev/null +++ b/cmake/arkcmake/FindARKMATH.cmake @@ -0,0 +1,43 @@ +# - Try to find ARKMATH +# Once done, this will define +# +# ARKMATH_FOUND - system has scicoslab +# ARKMATH_INCLUDE_DIRS - the scicoslab include directories +# ARKMATH_LIBRARIES - libraries to link to + +include(LibFindMacros) +include(MacroCommonPaths) + +MacroCommonPaths(ARKMATH) + +# Include dir +find_path(ARKMATH_INCLUDE_DIR + NAMES arkmath/storage_adaptors.hpp + PATHS ${COMMON_INCLUDE_PATHS_ARKMATH} +) + +# data dir +find_path(ARKMATH_DATA_DIR_SEARCH + NAMES arkmath/data/WMM.COF + PATHS ${COMMON_DATA_PATHS_ARKMATH} +) +set(ARKMATH_DATA_DIR ${ARKMATH_DATA_DIR_SEARCH}/arkmath/data) + +# the library itself +find_library(ARKMATH_LIBRARY + NAMES arkmath + PATHS ${COMMON_LIBRARY_PATHS_ARKMATH} +) + +# the import file +find_path(ARKMATH_LIBRARY_DIR + NAMES arkmath/arkmath-targets.cmake + PATHS ${COMMON_LIBRARY_PATHS_ARKMATH} +) +set(ARKMATH_LIB_IMPORT ${ARKMATH_LIBRARY_DIR}/arkmath/arkmath-targets.cmake) + +# Set the include dir variables and the libraries and let libfind_process do the rest. +# NOTE: Singular variables for this library, plural for libraries this this lib depends on. +set(ARKMATH_PROCESS_INCLUDES ARKMATH_INCLUDE_DIR) +set(ARKMATH_PROCESS_LIBS ARKMATH_LIBRARY ARKMATH_LIBRARIES) +libfind_process(ARKMATH) diff --git a/cmake/arkcmake/FindARKOSG.cmake b/cmake/arkcmake/FindARKOSG.cmake new file mode 100644 index 0000000000..51f641af19 --- /dev/null +++ b/cmake/arkcmake/FindARKOSG.cmake @@ -0,0 +1,43 @@ +# - Try to find ARKOSG +# Once done, this will define +# +# ARKOSG_FOUND - system has scicoslab +# ARKOSG_INCLUDE_DIRS - the scicoslab include directories +# ARKOSG_LIBRARIES - libraries to link to + +include(LibFindMacros) +include(MacroCommonPaths) + +MacroCommonPaths(ARKOSG) + +# Include dir +find_path(ARKOSG_INCLUDE_DIR + NAMES arkosg/osgUtils.hpp + PATHS ${COMMON_INCLUDE_PATHS_ARKOSG} +) + +# data dir +find_path(ARKOSG_DATA_DIR_SEARCH + NAMES arkosg/data/models/plane.ac + PATHS ${COMMON_DATA_PATHS_ARKOSG} +) +set(ARKOSG_DATA_DIR ${ARKOSG_DATA_DIR_SEARCH}/arkosg/data) + +# the library itself +find_library(ARKOSG_LIBRARY + NAMES arkosg + PATHS ${COMMON_LIBRARY_PATHS_ARKOSG} +) + +# the import file +find_path(ARKOSG_LIBRARY_DIR + NAMES arkosg/arkosg-targets.cmake + PATHS ${COMMON_LIBRARY_PATHS_ARKOSG} +) +set(ARKOSG_LIB_IMPORT ${ARKOSG_LIBRARY_DIR}/arkosg/arkosg-targets.cmake) + +# Set the include dir variables and the libraries and let libfind_process do the rest. +# NOTE: Singular variables for this library, plural for libraries this this lib depends on. +set(ARKOSG_PROCESS_INCLUDES ARKOSG_INCLUDE_DIR) +set(ARKOSG_PROCESS_LIBS ARKOSG_LIBRARY ARKOSG_LIBRARIES) +libfind_process(ARKOSG) diff --git a/cmake/arkcmake/FindARKSCICOS.cmake b/cmake/arkcmake/FindARKSCICOS.cmake new file mode 100644 index 0000000000..3128557112 --- /dev/null +++ b/cmake/arkcmake/FindARKSCICOS.cmake @@ -0,0 +1,36 @@ +# - Try to find ARKSCICOS +# Once done, this will define +# +# ARKSCICOS_FOUND - system has scicoslab +# ARKSCICOS_INCLUDE_DIRS - the scicoslab include directories +# ARKSCICOS_LIBRARIES - libraries to link to + +include(LibFindMacroos) +include(MacroCommonPaths) + +MacroCommonPaths(ARKSCICOS) + +# Include dir +find_path(ARKSCICOS_INCLUDE_DIR + NAMES definiotions.hpp + PATHS ${COMMON_INCLUDE_PATHS_ARKSCICOS} +) + +# the library itself +find_library(ARKSCICOS_LIBRARY + NAMES arkscicos + PATHS ${COMMON_LIBRARY_PATHS_ARKSCICOS} +) + +# the import file +find_path(ARKSCICOS_LIBRARY_DIR + NAMES arkscicos/arkscicos-targets.cmake + PATHS ${COMMON_LIBRARY_PATHS_ARKSCICOS} +) +set(ARKSCICOS_LIB_IMPORT ${ARKSCICOS_LIBRARY_DIR}/arkscicos/arkscicos-targets.cmake) + +# Set the include dir variables and the libraries and let libfind_process do the rest. +# NOTE: Singular variables for this library, plural for libraries this this lib depends on. +set(ARKSCICOS_PROCESS_INCLUDES ARKSCICOS_INCLUDE_DIR) +set(ARKSCICOS_PROCESS_LIBS ARKSCICOS_LIBRARY ARKSCICOS_LIBRARIES) +libfind_process(ARKSCICOS) diff --git a/cmake/arkcmake/FindBOOSTNUMERICBINDINGS.cmake b/cmake/arkcmake/FindBOOSTNUMERICBINDINGS.cmake new file mode 100644 index 0000000000..d2074a54d9 --- /dev/null +++ b/cmake/arkcmake/FindBOOSTNUMERICBINDINGS.cmake @@ -0,0 +1,21 @@ +# - Try to find BOOSTNUMERICBINDINGS +# Once done, this will define +# +# BOOSTNUMERICBINDINGS_FOUND - system has scicoslab +# BOOSTNUMERICBINDINGS_INCLUDE_DIRS - the scicoslab include directories + +include(LibFindMacros) +include(MacroCommonPaths) + +MacroCommonPaths(BOOSTNUMERICBINDINGS) + +# Include dir +find_path(BOOSTNUMERICBINDINGS_INCLUDE_DIR + NAMES boost/numeric/bindings/lapack/lapack.h + PATHS ${COMMON_INCLUDE_PATHS_BOOSTNUMERICBINDINGS} +) + +# Set the include dir variables and the libraries and let libfind_process do the rest. +# NOTE: Singular variables for this library, plural for libraries this this lib depends on. +set(BOOSTNUMERICBINDINGS_PROCESS_INCLUDES BOOSTNUMERICBINDINGS_INCLUDE_DIR) +libfind_process(BOOSTNUMERICBINDINGS) diff --git a/cmake/arkcmake/FindEASYSTAR.cmake b/cmake/arkcmake/FindEASYSTAR.cmake new file mode 100644 index 0000000000..7e3de7d3b4 --- /dev/null +++ b/cmake/arkcmake/FindEASYSTAR.cmake @@ -0,0 +1,21 @@ +# - Try to find EASYSTAR +# Once done, this will define +# +# EASYSTAR_FOUND - system has easystar +# EASYSTAR_INCLUDE_DIRS - the easystar include directories + +include(LibFindMacros) +include(MacroCommonPaths) + +MacroCommonPaths(EASYSTAR) + +# Include dir +find_path(EASYSTAR_INCLUDE_DIR + NAMES easystar/easystar.xml + PATHS ${COMMON_INCLUDE_PATHS_EASYSTAR} +) + +# Set the include dir variables and the libraries and let libfind_process do the rest. +# NOTE: Singular variables for this library, plural for libraries this this lib depends on. +set(EASYSTAR_PROCESS_INCLUDES EASYSTAR_INCLUDE_DIR) +libfind_process(EASYSTAR) diff --git a/cmake/arkcmake/FindJSBSIM.cmake b/cmake/arkcmake/FindJSBSIM.cmake new file mode 100644 index 0000000000..56c376946a --- /dev/null +++ b/cmake/arkcmake/FindJSBSIM.cmake @@ -0,0 +1,39 @@ +# - Try to find JSBSIM +# Once done, this will define +# +# JSBSIM_FOUND - system has scicoslab +# JSBSIM_INCLUDE_DIRS - the scicoslab include directories +# JSBSIM_LIBRARIES - libraries to link to + +include(LibFindMacros) +include(MacroCommonPaths) + +MacroCommonPaths(JSBSIM) + +# Include dir +find_path(JSBSIM_INCLUDE_DIR + NAMES JSBSim/initialization/FGTrimmer.h + PATHS ${COMMON_INCLUDE_PATHS_JSBSIM} +) + +# data dir +find_path(JSBSIM_DATA_DIR_SEARCH + NAMES jsbsim/aircraft/aircraft_template.xml + PATHS ${COMMON_DATA_PATHS_JSBSIM} +) +set(JSBSIM_DATA_DIR ${JSBSIM_DATA_DIR_SEARCH}/jsbsim) + +# Finally the library itself +find_library(JSBSIM_LIBRARY + NAMES JSBSim + PATHS ${COMMON_LIBRARY_PATHS_JSBSIM} +) + +# Set the include dir variables and the libraries and let libfind_process do the rest. +# NOTE: Singular variables for this library, plural for libraries this this lib depends on. +set(JSBSIM_PROCESS_INCLUDES JSBSIM_INCLUDE_DIR) +set(JSBSIM_PROCESS_LIBS JSBSIM_LIBRARY JSBSIM_LIBRARIES) +set(JSBSIM_INCLUDE_DIR ${JSBSIM_INCLUDE_DIR} ${JSBSIM_INCLUDE_DIR}/JSBSim) +set(JSBSIM_INCLUDES ${JSBSIM_INCLUDES} ${JSBSIM_INCLUDE_DIR}/JSBSim) + +libfind_process(JSBSIM) diff --git a/cmake/arkcmake/FindMAVLINK.cmake b/cmake/arkcmake/FindMAVLINK.cmake new file mode 100644 index 0000000000..24a949c130 --- /dev/null +++ b/cmake/arkcmake/FindMAVLINK.cmake @@ -0,0 +1,21 @@ +# - Try to find MAVLINK +# Once done, this will define +# +# MAVLINK_FOUND - system has scicoslab +# MAVLINK_INCLUDE_DIRS - the scicoslab include directories + +include(LibFindMacros) +include(MacroCommonPaths) + +MacroCommonPaths(MAVLINK) + +# Include dir +find_path(MAVLINK_INCLUDE_DIR + NAMES mavlink/mavlink_types.h + PATHS ${COMMON_INCLUDE_PATHS_MAVLINK} +) + +# Set the include dir variables and the libraries and let libfind_process do the rest. +# NOTE: Singular variables for this library, plural for libraries this this lib depends on. +set(MAVLINK_PROCESS_INCLUDES MAVLINK_INCLUDE_DIR) +libfind_process(MAVLINK) diff --git a/cmake/arkcmake/FindPLIB.cmake b/cmake/arkcmake/FindPLIB.cmake new file mode 100644 index 0000000000..8e5de7f8f5 --- /dev/null +++ b/cmake/arkcmake/FindPLIB.cmake @@ -0,0 +1,70 @@ +# Locate plib +# This module defines +# PLIB_LIBRARY +# PLIB_FOUND, if false, do not try to link to plib +# PLIB_INCLUDE_DIR, where to find the headers +# +# $PLIB_DIR is an environment variable that would +# correspond to the ./configure --prefix=$PLIB_DIR +# +# Created David Guthrie with code by Robert Osfield. + +FIND_PATH(PLIB_INCLUDE_DIR plib/js.h + $ENV{PLIB_DIR}/include + $ENV{PLIB_DIR} + $ENV{PLIB_ROOT}/include + ${DELTA3D_EXT_DIR}/inc + $ENV{DELTA_ROOT}/ext/inc + ~/Library/Frameworks + /Library/Frameworks + /usr/local/include + /usr/include + /sw/include # Fink + /opt/local/include # DarwinPorts + /opt/csw/include # Blastwave + /opt/include + [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\ Manager\\Environment;PLIB_ROOT]/include + /usr/freeware/include +) + +MACRO(FIND_PLIB_LIBRARY MYLIBRARY MYLIBRARYNAMES) + + FIND_LIBRARY(${MYLIBRARY} + NAMES ${MYLIBRARYNAMES} + PATHS + $ENV{PLIB_DIR}/lib + $ENV{PLIB_DIR} + $ENV{OSGDIR}/lib + $ENV{OSGDIR} + $ENV{PLIB_ROOT}/lib + ${DELTA3D_EXT_DIR}/lib + $ENV{DELTA_ROOT}/ext/lib + ~/Library/Frameworks + /Library/Frameworks + /usr/local/lib + /usr/lib + /sw/lib + /opt/local/lib + /opt/csw/lib + /opt/lib + [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\ Manager\\Environment;PLIB_ROOT]/lib + /usr/freeware/lib64 + ) + +ENDMACRO(FIND_PLIB_LIBRARY MYLIBRARY MYLIBRARYNAMES) + +SET(PLIB_RELEASE_JS_LIB_NAMES js plibjs) +SET(PLIB_RELEASE_UL_LIB_NAMES ul plibul) +SET(PLIB_DEBUG_JS_LIB_NAMES js_d plibjs_d) +SET(PLIB_DEBUG_UL_LIB_NAMES ul_d plibul_d) + + +FIND_PLIB_LIBRARY(PLIB_JS_LIBRARY "${PLIB_RELEASE_JS_LIB_NAMES}") +FIND_PLIB_LIBRARY(PLIB_JS_LIBRARY_DEBUG "${PLIB_DEBUG_JS_LIB_NAMES}") +FIND_PLIB_LIBRARY(PLIB_UL_LIBRARY "${PLIB_RELEASE_UL_LIB_NAMES}") +FIND_PLIB_LIBRARY(PLIB_UL_LIBRARY_DEBUG "${PLIB_DEBUG_UL_LIB_NAMES}") + +SET(PLIB_FOUND "NO") +IF(PLIB_JS_LIBRARY AND PLIB_INCLUDE_DIR) + SET(PLIB_FOUND "YES") +ENDIF(PLIB_JS_LIBRARY AND PLIB_INCLUDE_DIR) diff --git a/cmake/arkcmake/FindSCICOSLAB.cmake b/cmake/arkcmake/FindSCICOSLAB.cmake new file mode 100644 index 0000000000..1ba3a3e4a8 --- /dev/null +++ b/cmake/arkcmake/FindSCICOSLAB.cmake @@ -0,0 +1,72 @@ +# - Try to find SCICOSLAB +# Once done, this will define +# +# SCICOSLAB_FOUND - system has scicoslab +# SCICOSLAB_INCLUDE_DIRS - the scicoslab include directories +# SCICOSLAB_CONTRIB_DIR - the scicoslab contrib directory + +include(LibFindMacros) + +# find scicos +if (APPLE) + execute_process(COMMAND mdfind "kMDItemKind == Application && kMDItemDisplayName == ScicosLabGtk" + COMMAND head -1 + RESULT_VARIABLE RESULT + OUTPUT_VARIABLE SCICOS_APP_BUNDLE + ERROR_VARIABLE ERROR_MESSAGE + OUTPUT_STRIP_TRAILING_WHITESPACE) + if (RESULT) + MESSAGE(FATAL_ERROR "Could not locate 'ScicosLabGtk.app' - ${ERROR_MESSAGE}") + endif (RESULT) + execute_process(COMMAND find ${SCICOS_APP_BUNDLE} -name routines + COMMAND head -1 + RESULT_VARIABLE RESULT + OUTPUT_VARIABLE SCICOSLAB_GUESS_INCLUDE_DIRS + ERROR_VARIABLE ERROR_MESSAGE + OUTPUT_STRIP_TRAILING_WHITESPACE) + if (RESULT) + MESSAGE(FATAL_ERROR "Could not locate 'scicos_block4.h' in ScicosLabGtk.app - ${ERROR_MESSAGE}") + endif (RESULT) + execute_process(COMMAND find ${SCICOS_APP_BUNDLE} -name contrib + COMMAND head -1 + RESULT_VARIABLE RESULT + OUTPUT_VARIABLE SCICOSLAB_GUESS_CONTRIB_DIRS + ERROR_VARIABLE ERROR_MESSAGE + OUTPUT_STRIP_TRAILING_WHITESPACE) + if (RESULT) + MESSAGE(FATAL_ERROR "Could not locate 'loader.sce' in ScicosLabGtk.app - ${ERROR_MESSAGE}") + endif (RESULT) +elseif(UNIX) + set(SCICOSLAB_GUESS_INCLUDE_DIRS + /usr/lib/scicoslab-gtk-4.4b7/routines + /usr/lib/scicoslab-gtk-4.4/routines + /usr/lib/scicoslab-gtk-4.4.1/routines + ) + set(SCICOSLAB_GUESS_CONTRIB_DIRS + /usr/lib/scicoslab-gtk-4.4b7/contrib + /usr/lib/scicoslab-gtk-4.4/contrib + /usr/lib/scicoslab-gtk-4.4.1/contrib + ) +elseif(WIN32) + message(FATAL_ERROR "scicoslab cmake find module doesn't work for windows") +endif() + + +# Include dir +find_path(SCICOSLAB_INCLUDE_DIR + NAMES scicos/scicos_block4.h + PATHS ${SCICOSLAB_GUESS_INCLUDE_DIRS} +) + +# Contrib dir +find_path(SCICOSLAB_CONTRIB_DIR + NAMES loader.sce + PATHS ${SCICOSLAB_GUESS_CONTRIB_DIRS} +) +message(STATUS "contrib dir ${SCICOSLAB_CONTRIB_DIR}") + +# Set the include dir variables and the libraries and let libfind_process do the rest. +# NOTE: Singular variables for this library, plural for libraries this this lib depends on. +set(SCICOSLAB_PROCESS_INCLUDES SCICOSLAB_INCLUDE_DIR) +libfind_process(SCICOSLAB) +# vim:ts=4:sw=4:expandtab diff --git a/cmake/arkcmake/FindSIMGEAR.cmake b/cmake/arkcmake/FindSIMGEAR.cmake new file mode 100644 index 0000000000..e86de2aa93 --- /dev/null +++ b/cmake/arkcmake/FindSIMGEAR.cmake @@ -0,0 +1,29 @@ +# - Try to find SIMGEAR +# Once done, this will define +# +# SIMGEAR_FOUND - system has scicoslab +# SIMGEAR_INCLUDE_DIRS - the scicoslab include directories +# SIMGEAR_LIBRARIES - libraries to link to + +include(LibFindMacros) +include(MacroCommonPaths) + +MacroCommonPaths(SIMGEAR) + +# Include dir +find_path(SIMGEAR_INCLUDE_DIR + NAMES simgear/version.h + PATHS ${COMMON_INCLUDE_PATHS_SIMGEAR} +) + +# Finally the library itself +find_library(SIMGEAR_LIBRARY + NAMES sgio + PATHS ${COMMON_LIBRARY_PATHS_SIMGEAR} +) + +# Set the include dir variables and the libraries and let libfind_process do the rest. +# NOTE: Singular variables for this library, plural for libraries this this lib depends on. +set(SIMGEAR_PROCESS_INCLUDES SIMGEAR_INCLUDE_DIR) +set(SIMGEAR_PROCESS_LIBS SIMGEAR_LIBRARY SIMGEAR_LIBRARIES) +libfind_process(SIMGEAR) diff --git a/cmake/arkcmake/LibFindMacros.cmake b/cmake/arkcmake/LibFindMacros.cmake new file mode 100644 index 0000000000..ff9233a6c8 --- /dev/null +++ b/cmake/arkcmake/LibFindMacros.cmake @@ -0,0 +1,98 @@ +# Works the same as find_package, but forwards the "REQUIRED" and "QUIET" arguments +# used for the current package. For this to work, the first parameter must be the +# prefix of the current package, then the prefix of the new package etc, which are +# passed to find_package. +macro (libfind_package PREFIX) + set (LIBFIND_PACKAGE_ARGS ${ARGN}) + if (${PREFIX}_FIND_QUIETLY) + set (LIBFIND_PACKAGE_ARGS ${LIBFIND_PACKAGE_ARGS} QUIET) + endif (${PREFIX}_FIND_QUIETLY) + if (${PREFIX}_FIND_REQUIRED) + set (LIBFIND_PACKAGE_ARGS ${LIBFIND_PACKAGE_ARGS} REQUIRED) + endif (${PREFIX}_FIND_REQUIRED) + find_package(${LIBFIND_PACKAGE_ARGS}) +endmacro (libfind_package) + +# CMake developers made the UsePkgConfig system deprecated in the same release (2.6) +# where they added pkg_check_modules. Consequently I need to support both in my scripts +# to avoid those deprecated warnings. Here's a helper that does just that. +# Works identically to pkg_check_modules, except that no checks are needed prior to use. +macro (libfind_pkg_check_modules PREFIX PKGNAME) + if (${CMAKE_MAJOR_VERSION} EQUAL 2 AND ${CMAKE_MINOR_VERSION} EQUAL 4) + include(UsePkgConfig) + pkgconfig(${PKGNAME} ${PREFIX}_INCLUDE_DIRS ${PREFIX}_LIBRARY_DIRS ${PREFIX}_LDFLAGS ${PREFIX}_CFLAGS) + else (${CMAKE_MAJOR_VERSION} EQUAL 2 AND ${CMAKE_MINOR_VERSION} EQUAL 4) + find_package(PkgConfig) + if (PKG_CONFIG_FOUND) + pkg_check_modules(${PREFIX} ${PKGNAME}) + endif (PKG_CONFIG_FOUND) + endif (${CMAKE_MAJOR_VERSION} EQUAL 2 AND ${CMAKE_MINOR_VERSION} EQUAL 4) +endmacro (libfind_pkg_check_modules) + +# Do the final processing once the paths have been detected. +# If include dirs are needed, ${PREFIX}_PROCESS_INCLUDES should be set to contain +# all the variables, each of which contain one include directory. +# Ditto for ${PREFIX}_PROCESS_LIBS and library files. +# Will set ${PREFIX}_FOUND, ${PREFIX}_INCLUDE_DIRS and ${PREFIX}_LIBRARIES. +# Also handles errors in case library detection was required, etc. +macro (libfind_process PREFIX) + # Skip processing if already processed during this run + if (NOT ${PREFIX}_FOUND) + # Start with the assumption that the library was found + set (${PREFIX}_FOUND TRUE) + + # Process all includes and set _FOUND to false if any are missing + foreach (i ${${PREFIX}_PROCESS_INCLUDES}) + if (${i}) + set (${PREFIX}_INCLUDE_DIRS ${${PREFIX}_INCLUDE_DIRS} ${${i}}) + mark_as_advanced(${i}) + else (${i}) + set (${PREFIX}_FOUND FALSE) + endif (${i}) + endforeach (i) + + # Process all libraries and set _FOUND to false if any are missing + foreach (i ${${PREFIX}_PROCESS_LIBS}) + if (${i}) + set (${PREFIX}_LIBRARIES ${${PREFIX}_LIBRARIES} ${${i}}) + mark_as_advanced(${i}) + else (${i}) + set (${PREFIX}_FOUND FALSE) + endif (${i}) + endforeach (i) + + # Print message and/or exit on fatal error + if (${PREFIX}_FOUND) + if (NOT ${PREFIX}_FIND_QUIETLY) + message (STATUS "Found ${PREFIX} ${${PREFIX}_VERSION}") + endif (NOT ${PREFIX}_FIND_QUIETLY) + else (${PREFIX}_FOUND) + if (${PREFIX}_FIND_REQUIRED) + foreach (i ${${PREFIX}_PROCESS_INCLUDES} ${${PREFIX}_PROCESS_LIBS}) + message("${i}=${${i}}") + endforeach (i) + message (FATAL_ERROR "Required library ${PREFIX} NOT FOUND.\nInstall the library (dev version) and try again. If the library is already installed, use ccmake to set the missing variables manually.") + endif (${PREFIX}_FIND_REQUIRED) + endif (${PREFIX}_FOUND) + endif (NOT ${PREFIX}_FOUND) +endmacro (libfind_process) + +macro(libfind_library PREFIX basename) + set(TMP "") + if(MSVC80) + set(TMP -vc80) + endif(MSVC80) + if(MSVC90) + set(TMP -vc90) + endif(MSVC90) + set(${PREFIX}_LIBNAMES ${basename}${TMP}) + if(${ARGC} GREATER 2) + set(${PREFIX}_LIBNAMES ${basename}${TMP}-${ARGV2}) + string(REGEX REPLACE "\\." "_" TMP ${${PREFIX}_LIBNAMES}) + set(${PREFIX}_LIBNAMES ${${PREFIX}_LIBNAMES} ${TMP}) + endif(${ARGC} GREATER 2) + find_library(${PREFIX}_LIBRARY + NAMES ${${PREFIX}_LIBNAMES} + PATHS ${${PREFIX}_PKGCONF_LIBRARY_DIRS} + ) +endmacro(libfind_library) diff --git a/cmake/arkcmake/MacroAddCompileFlags.cmake b/cmake/arkcmake/MacroAddCompileFlags.cmake new file mode 100644 index 0000000000..a866689da6 --- /dev/null +++ b/cmake/arkcmake/MacroAddCompileFlags.cmake @@ -0,0 +1,21 @@ +# - MACRO_ADD_COMPILE_FLAGS(target_name flag1 ... flagN) + +# Copyright (c) 2006, Oswald Buddenhagen, +# Copyright (c) 2006, Andreas Schneider, +# +# Redistribution and use is allowed according to the terms of the BSD license. +# For details see the accompanying COPYING-CMAKE-SCRIPTS file. + + +macro (MACRO_ADD_COMPILE_FLAGS _target) + + get_target_property(_flags ${_target} COMPILE_FLAGS) + if (_flags) + set(_flags ${_flags} ${ARGN}) + else (_flags) + set(_flags ${ARGN}) + endif (_flags) + + set_target_properties(${_target} PROPERTIES COMPILE_FLAGS ${_flags}) + +endmacro (MACRO_ADD_COMPILE_FLAGS) diff --git a/cmake/arkcmake/MacroAddLinkFlags.cmake b/cmake/arkcmake/MacroAddLinkFlags.cmake new file mode 100644 index 0000000000..91cad30625 --- /dev/null +++ b/cmake/arkcmake/MacroAddLinkFlags.cmake @@ -0,0 +1,20 @@ +# - MACRO_ADD_LINK_FLAGS(target_name flag1 ... flagN) + +# Copyright (c) 2006, Oswald Buddenhagen, +# Copyright (c) 2006, Andreas Schneider, +# +# Redistribution and use is allowed according to the terms of the BSD license. +# For details see the accompanying COPYING-CMAKE-SCRIPTS file. + +macro (MACRO_ADD_LINK_FLAGS _target) + + get_target_property(_flags ${_target} LINK_FLAGS) + if (_flags) + set(_flags "${_flags} ${ARGN}") + else (_flags) + set(_flags "${ARGN}") + endif (_flags) + + set_target_properties(${_target} PROPERTIES LINK_FLAGS "${_flags}") + +endmacro (MACRO_ADD_LINK_FLAGS) diff --git a/cmake/arkcmake/MacroAddPlugin.cmake b/cmake/arkcmake/MacroAddPlugin.cmake new file mode 100644 index 0000000000..36b5e57e01 --- /dev/null +++ b/cmake/arkcmake/MacroAddPlugin.cmake @@ -0,0 +1,30 @@ +# - MACRO_ADD_PLUGIN(name [WITH_PREFIX] file1 .. fileN) +# +# Create a plugin from the given source files. +# If WITH_PREFIX is given, the resulting plugin will have the +# prefix "lib", otherwise it won't. +# +# Copyright (c) 2006, Alexander Neundorf, +# Copyright (c) 2006, Laurent Montel, +# Copyright (c) 2006, Andreas Schneider, +# +# Redistribution and use is allowed according to the terms of the BSD license. +# For details see the accompanying COPYING-CMAKE-SCRIPTS file. + + +macro (MACRO_ADD_PLUGIN _target_NAME _with_PREFIX) + + if (${_with_PREFIX} STREQUAL "WITH_PREFIX") + set(_first_SRC) + else (${_with_PREFIX} STREQUAL "WITH_PREFIX") + set(_first_SRC ${_with_PREFIX}) + endif (${_with_PREFIX} STREQUAL "WITH_PREFIX") + + add_library(${_target_NAME} MODULE ${_first_SRC} ${ARGN}) + + if (_first_SRC) + set_target_properties(${_target_NAME} PROPERTIES PREFIX "") + endif (_first_SRC) + +endmacro (MACRO_ADD_PLUGIN _name _sources) + diff --git a/cmake/arkcmake/MacroCheckCCompilerFlagSSP.cmake b/cmake/arkcmake/MacroCheckCCompilerFlagSSP.cmake new file mode 100644 index 0000000000..b64fb453a4 --- /dev/null +++ b/cmake/arkcmake/MacroCheckCCompilerFlagSSP.cmake @@ -0,0 +1,26 @@ +# - Check whether the C compiler supports a given flag in the +# context of a stack checking compiler option. +# CHECK_C_COMPILER_FLAG_SSP(FLAG VARIABLE) +# +# FLAG - the compiler flag +# VARIABLE - variable to store the result +# +# This actually calls the check_c_source_compiles macro. +# See help for CheckCSourceCompiles for a listing of variables +# that can modify the build. + +# Copyright (c) 2006, Alexander Neundorf, +# +# Redistribution and use is allowed according to the terms of the BSD license. +# For details see the accompanying COPYING-CMAKE-SCRIPTS file. + + +INCLUDE(CheckCSourceCompiles) + +MACRO (CHECK_C_COMPILER_FLAG_SSP _FLAG _RESULT) + SET(SAFE_CMAKE_REQUIRED_DEFINITIONS "${CMAKE_REQUIRED_DEFINITIONS}") + SET(CMAKE_REQUIRED_DEFINITIONS "${_FLAG}") + CHECK_C_SOURCE_COMPILES("int main(int argc, char **argv) { char buffer[256]; return buffer[argc]=0;}" ${_RESULT}) + SET (CMAKE_REQUIRED_DEFINITIONS "${SAFE_CMAKE_REQUIRED_DEFINITIONS}") +ENDMACRO (CHECK_C_COMPILER_FLAG_SSP) + diff --git a/cmake/arkcmake/MacroCommonPaths.cmake b/cmake/arkcmake/MacroCommonPaths.cmake new file mode 100644 index 0000000000..03f1d89867 --- /dev/null +++ b/cmake/arkcmake/MacroCommonPaths.cmake @@ -0,0 +1,51 @@ +macro(MacroCommonPaths NAME) + set(COMMON_INCLUDE_PATHS_${NAME} + $ENV{${NAME}_DIR}/include + $ENV{${NAME}_DIR} + $ENV{${NAME}_ROOT}/include + $ENV{${NAME}_ROOT} + ~/Library/Frameworks + /Library/Frameworks + /usr/local/include + /usr/include + /sw/include # Fink + /opt/local/include # DarwinPorts + /opt/csw/include # Blastwave + /opt/include + [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\\Manager\\Environment;${NAME}_DIR]/include + [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\\Manager\\Environment;${NAME}_ROOT]/include + /usr/freeware/include + ) + set(COMMON_LIB_PATHS_${NAME} + $ENV{${NAME}_DIR}/lib + $ENV{${NAME}_DIR} + ~/Library/Frameworks + /Library/Frameworks + /usr/local/lib + /usr/lib + /sw/lib + /opt/local/lib + /opt/csw/lib + /opt/lib + [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\\Manager\\Environment;${NAME}_DIR]/lib + [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\\Manager\\Environment;${NAME}_ROOT]/lib + /usr/freeware/lib64 + ) + set(COMMON_DATA_PATHS_${NAME} + $ENV{${NAME}_DIR}/share + $ENV{${NAME}_DIR} + ~/Library/Frameworks + /Library/Frameworks + /usr/local/share + /usr/share + /sw/share + /opt/local/share + /opt/csw/share + /opt/share + [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\\Manager\\Environment;${NAME}_DIR]/share + [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\\Manager\\Environment;${NAME}_ROOT]/share + /usr/freeware/share64 + ) +endmacro(MacroCommonPaths) + +# vim:ts=4:sw=4:expandtab diff --git a/cmake/arkcmake/MacroCopyFile.cmake b/cmake/arkcmake/MacroCopyFile.cmake new file mode 100644 index 0000000000..cee1cae37e --- /dev/null +++ b/cmake/arkcmake/MacroCopyFile.cmake @@ -0,0 +1,33 @@ +# - macro_copy_file(_src _dst) +# Copies a file to ${_dst} only if ${_src} is different (newer) than ${_dst} +# +# Example: +# macro_copy_file(${CMAKE_CURRENT_SOURCE_DIR}/icon.png ${CMAKE_CURRENT_BINARY_DIR}/.) +# Copies file icon.png to ${CMAKE_CURRENT_BINARY_DIR} directory +# +# Copyright (c) 2006-2007 Wengo +# Copyright (c) 2006-2008 Andreas Schneider +# +# Redistribution and use is allowed according to the terms of the BSD license. +# For details see the accompanying COPYING file. + + +macro (macro_copy_file _src _dst) + # Removes all path containing .svn or CVS or CMakeLists.txt during the copy + if (NOT ${_src} MATCHES ".*\\.svn|CVS|CMakeLists\\.txt.*") + + if (CMAKE_VERBOSE_MAKEFILE) + message(STATUS "Copy file from ${_src} to ${_dst}") + endif (CMAKE_VERBOSE_MAKEFILE) + + # Creates directory if necessary + get_filename_component(_path ${_dst} PATH) + file(MAKE_DIRECTORY ${_path}) + + execute_process( + COMMAND + ${CMAKE_COMMAND} -E copy_if_different ${_src} ${_dst} + OUTPUT_QUIET + ) + endif (NOT ${_src} MATCHES ".*\\.svn|CVS|CMakeLists\\.txt.*") +endmacro (macro_copy_file) diff --git a/cmake/arkcmake/MacroEnsureOutOfSourceBuild.cmake b/cmake/arkcmake/MacroEnsureOutOfSourceBuild.cmake new file mode 100644 index 0000000000..3ff891b512 --- /dev/null +++ b/cmake/arkcmake/MacroEnsureOutOfSourceBuild.cmake @@ -0,0 +1,19 @@ +# - MACRO_ENSURE_OUT_OF_SOURCE_BUILD() +# MACRO_ENSURE_OUT_OF_SOURCE_BUILD() + +# Copyright (c) 2006, Alexander Neundorf, +# +# Redistribution and use is allowed according to the terms of the BSD license. +# For details see the accompanying COPYING-CMAKE-SCRIPTS file. + +macro (MACRO_ENSURE_OUT_OF_SOURCE_BUILD _errorMessage) + + string(COMPARE EQUAL "${CMAKE_SOURCE_DIR}" "${CMAKE_BINARY_DIR}" _insource) + if (_insource) + file(REMOVE [CMakeCache.txt CMakeFiles]) + message(FATAL_ERROR "${_errorMessage}") + endif (_insource) + +endmacro (MACRO_ENSURE_OUT_OF_SOURCE_BUILD) + +# vim:ts=4:sw=4:expandtab diff --git a/cmake/arkcmake/MacroFindOrBuild.cmake b/cmake/arkcmake/MacroFindOrBuild.cmake new file mode 100644 index 0000000000..973472d7c8 --- /dev/null +++ b/cmake/arkcmake/MacroFindOrBuild.cmake @@ -0,0 +1,18 @@ +macro(MacroFindOrBuild PACKAGE PACKAGE_PATH IS_GIT_SUBMODULE) + add_custom_target(${PACKAGE}) + if (NOT ${PACKAGE}_BUILD_FROM_SOURCE) + find_package(${PACKAGE}) + endif() + if (NOT ${PACKAGE}_FOUND) + set(${PACKAGE}_BUILD_FROM_SOURCE TRUE) + message(STATUS "could not find package ${PACKAGE}, building from source") + add_custom_target(${PACKAGE}_BUILD DEPENDS ${PACKAGE}_BUILD.stamp) + add_dependencies(${PACKAGE} ${PACKAGE}_BUILD) + set(${PACKAGE}_FOUND TRUE) + if (${IS_GIT_SUBMODULE}) + message(STATUS "${PACKAGE} detected as git submodule, will attempt to initialize it") + list(APPEND GIT_SUBMODULES ${PACKAGE_PATH}) + add_dependencies(${PACKAGE}_BUILD GIT) + endif() + endif() +endmacro(MacroFindOrBuild) diff --git a/cmake/arkcmake/MacroSetDefault.cmake b/cmake/arkcmake/MacroSetDefault.cmake new file mode 100644 index 0000000000..acb1e66698 --- /dev/null +++ b/cmake/arkcmake/MacroSetDefault.cmake @@ -0,0 +1,6 @@ +macro(MacroSetDefault VAR DEFAULT) + if (NOT DEFINED ${VAR}) + set(${VAR} ${DEFAULT}) + endif() +endmacro(MacroSetDefault) + diff --git a/cmake/arkcmake/README.md b/cmake/arkcmake/README.md new file mode 100644 index 0000000000..5eaeb70b42 --- /dev/null +++ b/cmake/arkcmake/README.md @@ -0,0 +1,10 @@ +arktools cmake modules +============= + +This repository contains the cmake modules used by arktools. +The python scripts do not function unless they are in an arktool. +From an arktool (ex. arktools/arkscicos) run: + +```console +./cmake/updateArkcmake.py +``` diff --git a/cmake/arkcmake/UseDoxygen.cmake b/cmake/arkcmake/UseDoxygen.cmake new file mode 100644 index 0000000000..c4ab7cccf3 --- /dev/null +++ b/cmake/arkcmake/UseDoxygen.cmake @@ -0,0 +1,100 @@ +# - Run Doxygen +# +# Adds a doxygen target that runs doxygen to generate the html +# and optionally the LaTeX API documentation. +# The doxygen target is added to the doc target as dependency. +# i.e.: the API documentation is built with: +# make doc +# +# USAGE: INCLUDE IN PROJECT +# +# set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}) +# include(UseDoxygen) +# Add the Doxyfile.in and UseDoxygen.cmake files to the projects source directory. +# +# +# Variables you may define are: +# DOXYFILE_OUTPUT_DIR - Path where the Doxygen output is stored. Defaults to "doc". +# +# DOXYFILE_LATEX_DIR - Directory where the Doxygen LaTeX output is stored. Defaults to "latex". +# +# DOXYFILE_HTML_DIR - Directory where the Doxygen html output is stored. Defaults to "html". +# + +# +# Copyright (c) 2009-2010 Tobias Rautenkranz +# Copyright (c) 2010 Andreas Schneider +# +# Redistribution and use is allowed according to the terms of the New +# BSD license. +# For details see the accompanying COPYING-CMAKE-SCRIPTS file. +# + +macro(usedoxygen_set_default name value) + if(NOT DEFINED "${name}") + set("${name}" "${value}") + endif() +endmacro() + +find_package(Doxygen) + +if(DOXYGEN_FOUND) + find_file(DOXYFILE_IN + NAMES + doxy.config.in + PATHS + ${CMAKE_CURRENT_SOURCE_DIR} + ${CMAKE_ROOT}/Modules/ + NO_DEFAULT_PATH) + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(DOXYFILE_IN DEFAULT_MSG "DOXYFILE_IN") +endif() + +if(DOXYGEN_FOUND AND DOXYFILE_IN_FOUND) + add_custom_target(doxygen ${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/doxy.config) + + usedoxygen_set_default(DOXYFILE_OUTPUT_DIR "${CMAKE_CURRENT_BINARY_DIR}") + usedoxygen_set_default(DOXYFILE_HTML_DIR "html") + + set_property(DIRECTORY APPEND PROPERTY + ADDITIONAL_MAKE_CLEAN_FILES "${DOXYFILE_OUTPUT_DIR}/${DOXYFILE_HTML_DIR}") + + set(DOXYFILE_LATEX FALSE) + set(DOXYFILE_PDFLATEX FALSE) + set(DOXYFILE_DOT FALSE) + + find_package(LATEX) + if(LATEX_COMPILER AND MAKEINDEX_COMPILER) + set(DOXYFILE_LATEX TRUE) + usedoxygen_set_default(DOXYFILE_LATEX_DIR "latex") + + set_property(DIRECTORY APPEND PROPERTY + ADDITIONAL_MAKE_CLEAN_FILES + "${DOXYFILE_OUTPUT_DIR}/${DOXYFILE_LATEX_DIR}") + + if(PDFLATEX_COMPILER) + set(DOXYFILE_PDFLATEX TRUE) + endif() + if(DOXYGEN_DOT_EXECUTABLE) + set(DOXYFILE_DOT TRUE) + endif() + + add_custom_command(TARGET doxygen + POST_BUILD + COMMAND ${CMAKE_MAKE_PROGRAM} + WORKING_DIRECTORY "${DOXYFILE_OUTPUT_DIR}/${DOXYFILE_LATEX_DIR}") + endif() + + configure_file(${DOXYFILE_IN} ${CMAKE_CURRENT_BINARY_DIR}/doxy.config ESCAPE_QUOTES IMMEDIATE @ONLY) + if (EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/doxy.trac.in) + configure_file(${CMAKE_CURRENT_SOURCE_DIR}/doxy.trac.in ${CMAKE_CURRENT_BINARY_DIR}/doxy.trac ESCAPE_QUOTES IMMEDIATE @ONLY) + add_custom_target(doxygen-trac ${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/doxy.trac) + endif() + + get_target_property(DOC_TARGET doc TYPE) + if(NOT DOC_TARGET) + add_custom_target(doc) + endif() + + add_dependencies(doc doxygen) +endif() diff --git a/cmake/arkcmake/autobuild.py b/cmake/arkcmake/autobuild.py new file mode 100755 index 0000000000..c5eed1e22d --- /dev/null +++ b/cmake/arkcmake/autobuild.py @@ -0,0 +1,205 @@ +#!/usr/bin/python +# Author: Lenna X. Peterson (github.com/lennax) +# based on bash script by James Goppert (github.com/jgoppert) + +# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # +# USAGE: # +# $ ./autobuild.py [1-9] # +# Then follow menu # +# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # + +# TODO: Error handling: +## check cmake success, etc. +## catch CMake Warning: + #Manually-specified variables were not used by the project: + #BUILD_TYPE +# (missing gprof flags) + +import sys # for sys.argv[] and sys.platform +import os # for chdir() +import subprocess # for check_call() +import shutil # for rmtree() +from optparse import OptionParser # for parsing options +try: + from get_build_path import get_build_path +except ImportError: + print "Could not find 'get_build_path.py' " + print "in '%s'" % os.path.dirname(os.path.abspath(__file__)) + print "This module is required." + raise SystemExit + +## Move to directory containing CMakeLists.txt and src/ +build_path = get_build_path() +if build_path: + os.chdir(build_path) +else: + print "The script was unable to find a build directory." + raise SystemExit + +makeargs = "-j8" +cmakecall = ["cmake", ".."] +build_dir = "build" + +## Parse command line options +## TODO: add makeargs/cmakeargs etc. +## (with 'action="append"' to append arg(s) to list) +usage = "usage: %prog [options] [1-9]" +parser = OptionParser(usage=usage) +parser.set_defaults(verbose=False, makeargs="-j8") +parser.add_option("-v", "--verbose", + action="store_true", dest="verbose", + help="Verbose mode") +parser.add_option("-c", "--cmake", + action="store", dest="cm", + help="Specify one or more arguments for CMake") +#parser.add_option("--makeargs", +# action="store", type="string", dest="makeargs", +# help="Argument to `make` [default '-j8']") +(options, args) = parser.parse_args() +if options.verbose: + os.environ['VERBOSE'] = "1" +### Split cmakeargs, reassemble, and insert into cmake call +if options.cm: + cm_raw = options.cm + cm_list = cm_raw.split("-D") + if cm_list[0] == "": + cm_list.pop(0) + for cm in cm_list: + cm = cm.lstrip() + cmakearg = "-D" + cm + cmakecall.insert(1, cmakearg) +#if options.makeargs: +# makeargs = options.makeargs + + +def install_build(cmakecall, exitVal=True): + if not os.path.isdir(build_dir): + os.mkdir(build_dir) + os.chdir(build_dir) + subprocess.check_call(cmakecall) + subprocess.check_call(["make", makeargs]) + if exitVal == True: + raise SystemExit + +def dev_build(): + cmakecall.insert(1, "-DDEV_MODE::bool=TRUE") + install_build(cmakecall) + +def grab_deps(): + if 'linux' in sys.platform: + try: + subprocess.check_call('sudo apt-get install cmake', shell=True) + except: + print "Error installing dependencies: ", sys.exc_info()[0] + print "apt-get is available on Debian and Ubuntu" + raise SystemExit + elif 'darwin' in sys.platform: + try: + subprocess.check_call('sudo port install cmake', shell=True) + except: + print "Error installing dependencies: ", sys.exc_info()[0] + print "Please install Macports (http://www.macports.org)" + raise SystemExit + else: + print "Platform not recognized (did not match linux or darwin)" + print "Script doesn't download dependencies for this platform" + raise SystemExit + +def package_source(): + install_build(cmakecall, exitVal=False) + subprocess.check_call(["make", "package_source"]) + raise SystemExit + +def package(): + install_build(cmakecall, exitVal=False) + subprocess.check_call(["make", "package"]) + raise SystemExit + +def remake(): + if not os.path.isdir(build_dir): + print "Directory '%s' does not exist" % build_dir + print "You must make before you can remake." + return 1 + os.chdir(build_dir) + subprocess.check_call(["make", makeargs]) + raise SystemExit + +def clean(): + if 'posix' in os.name: + print "Cleaning '%s' with rm -rf" % build_dir + subprocess.check_call(["rm", "-rf", build_dir]) + else: + print "Cleaning '%s' with shutil.rmtree()" % build_dir + print "(may be very slow)" + shutil.rmtree(build_dir, ignore_errors=True) + print "Build cleaned" + +# requires PROFILE definition in CMakeLists.txt: +# set(CMAKE_BUILD_TYPE PROFILE) +# set(CMAKE_CXX_FLAGS_PROFILE "-g -pg") +# set(CMAKE_C_FLAGS_PROFILE "-g -pg") +def profile(): + cmakecall.insert(1, "-DDEV_MODE::bool=TRUE") + cmakecall.insert(2, "-DBUILD_TYPE=PROFILE") + install_build(cmakecall) + +def menu(): + print "1. developer build: used for development." + print "2. install build: used for building before final installation to the system." + print "3. grab dependencies: installs all the required packages for debian based systems (ubuntu maverick/ debian squeeze,lenny) or darwin with macports." + print "4. package source: creates a source package for distribution." + print "5. package: creates binary packages for distribution." + print "6. remake: calls make again after project has been configured as install or in source build." + print "7. clean: removes the build directory." + print "8. profile: compiles for gprof." + print "9. end." + opt = raw_input("Please choose an option: ") + return opt + +try: + loop_num = 0 + # continues until a function raises system exit or ^C + while (1): + if len(args) == 1 and loop_num == 0: + opt = args[0] + loop_num += 1 + else: + opt = menu() + + try: + opt = int(opt) + except ValueError: + pass + + if opt == 1: + print "You chose developer build" + dev_build() + elif opt == 2: + print "You chose install build" + install_build(cmakecall) + elif opt == 3: + print "You chose to install dependencies" + grab_deps() + elif opt == 4: + print "You chose to package the source" + package_source() + elif opt == 5: + print "You chose to package the binary" + package() + elif opt == 6: + print "You chose to re-call make on the previously configured build" + remake() + elif opt == 7: + print "You chose to clean the build" + clean() + elif opt == 8: + # requires definition in CMakeLists.txt (see def above) + print "You chose to compile for gprof" + profile() + elif opt == 9: + raise SystemExit + else: + print "Invalid option. Please try again: " + +except KeyboardInterrupt: + print "\n" diff --git a/cmake/arkcmake/get_build_path.py b/cmake/arkcmake/get_build_path.py new file mode 100755 index 0000000000..c313476a9f --- /dev/null +++ b/cmake/arkcmake/get_build_path.py @@ -0,0 +1,62 @@ +#!/usr/bin/python +# Author: Lenna X. Peterson (github.com/lennax) +# Determines appropriate path for CMake +# Looks for "CMakeLists.txt" and "src/" +# ( BUILDFILE and SRCDIR in find_build_dir() ) +# Searches the following paths: +# 1 Path of call +# 2 Path where script is located +# 3 Path above 2 (parent directory) +# 4 Path above 3 (grandparent directory) + +import os # for getcwd(), os.path + +def get_build_path(): + + build_dir="" + + ## Initialize search paths + call_dir = os.getcwd() + script_dir = os.path.dirname(os.path.abspath(__file__)) + script_mom = os.path.abspath(script_dir + os.sep + os.pardir) + script_grandma = os.path.abspath(script_mom + os.sep + os.pardir) + if script_mom == call_dir: + script_mom = "" + if script_grandma == call_dir: + script_grandma = "" + + ## Define function to search for required components for build + def find_build_dir(search_dir): + BUILDFILE = "CMakeLists.txt" + #SRCDIR = "src" + os.chdir(search_dir) + if os.path.isfile(BUILDFILE): + return search_dir + return False + + ## Class to emulate if temp = x + # (checking equality of x while assigning it to temp) + # Borrowed from Alex Martelli + class Holder(object): + def set(self, value): + self.value = value + return value + def get(self): + return self.value + + temp = Holder() + + ## Search paths for build components + if temp.set(find_build_dir(call_dir)): + build_dir = temp.get() + elif temp.set(find_build_dir(script_dir)): + build_dir = temp.get() + elif script_mom and temp.set(find_build_dir(script_mom)): + build_dir = temp.get() + elif script_grandma and temp.set(find_build_dir(script_grandma)): + build_dir = temp.get() + else: + return 0 + + print "I go now. Good luck, everybody!" + return build_dir diff --git a/cmake/arkcmake/language_support_v2.cmake b/cmake/arkcmake/language_support_v2.cmake new file mode 100644 index 0000000000..aa5870bb09 --- /dev/null +++ b/cmake/arkcmake/language_support_v2.cmake @@ -0,0 +1,65 @@ +# cmake/modules/language_support.cmake +# +# Temporary additional general language support is contained within this +# file. + +# This additional function definition is needed to provide a workaround for +# CMake bug 9220. + +# On debian testing (cmake 2.6.2), I get return code zero when calling +# cmake the first time, but cmake crashes when running a second time +# as follows: +# +# -- The Fortran compiler identification is unknown +# CMake Error at /usr/share/cmake-2.6/Modules/CMakeFortranInformation.cmake:7 (GET_FILENAME_COMPONENT): +# get_filename_component called with incorrect number of arguments +# Call Stack (most recent call first): +# CMakeLists.txt:3 (enable_language) +# +# My workaround is to invoke cmake twice. If both return codes are zero, +# it is safe to invoke ENABLE_LANGUAGE(Fortran OPTIONAL) + +function(workaround_9220 language language_works) + #message("DEBUG: language = ${language}") + set(text + "project(test NONE) +cmake_minimum_required(VERSION 2.6.0) +enable_language(${language} OPTIONAL) +" + ) + file(REMOVE_RECURSE ${CMAKE_BINARY_DIR}/language_tests/${language}) + file(MAKE_DIRECTORY ${CMAKE_BINARY_DIR}/language_tests/${language}) + file(WRITE ${CMAKE_BINARY_DIR}/language_tests/${language}/CMakeLists.txt + ${text}) + execute_process( + COMMAND ${CMAKE_COMMAND} . + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/language_tests/${language} + RESULT_VARIABLE return_code + OUTPUT_QUIET + ERROR_QUIET + ) + + if(return_code EQUAL 0) + # Second run + execute_process ( + COMMAND ${CMAKE_COMMAND} . + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/language_tests/${language} + RESULT_VARIABLE return_code + OUTPUT_QUIET + ERROR_QUIET + ) + if(return_code EQUAL 0) + set(${language_works} ON PARENT_SCOPE) + else(return_code EQUAL 0) + set(${language_works} OFF PARENT_SCOPE) + endif(return_code EQUAL 0) + else(return_code EQUAL 0) + set(${language_works} OFF PARENT_SCOPE) + endif(return_code EQUAL 0) +endfunction(workaround_9220) + +# Temporary tests of the above function. +#workaround_9220(CXX CXX_language_works) +#message("CXX_language_works = ${CXX_language_works}") +#workaround_9220(CXXp CXXp_language_works) +#message("CXXp_language_works = ${CXXp_language_works}") diff --git a/cmake/arkcmake/updateArkcmake.py b/cmake/arkcmake/updateArkcmake.py new file mode 100755 index 0000000000..df75104ac9 --- /dev/null +++ b/cmake/arkcmake/updateArkcmake.py @@ -0,0 +1,19 @@ +#!/usr/bin/python +# Author: Lenna X. Peterson (github.com/lennax) +# Based on bash script by James Goppert (github.com/jgoppert) +# +# script used to update cmake modules from git repo, can't make this +# a submodule otherwise it won't know how to interpret the CMakeLists.txt +# # # # # # subprocess# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # + +import os # for os.path +import subprocess # for check_call() + +clone_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +print clone_path +os.chdir(clone_path) +subprocess.check_call(["git", "clone", "git://github.com/arktools/arkcmake.git","arkcmake_tmp"]) +subprocess.check_call(["rm", "-rf", "arkcmake_tmp/.git"]) +if os.path.isdir("arkcmake"): + subprocess.check_call(["rm", "-rf", "arkcmake"]) +subprocess.check_call(["mv", "arkcmake_tmp", "arkcmake"]) diff --git a/libraries/APO/APO_DefaultSetup.h b/libraries/APO/APO_DefaultSetup.h new file mode 100644 index 0000000000..a270081d94 --- /dev/null +++ b/libraries/APO/APO_DefaultSetup.h @@ -0,0 +1,161 @@ +#ifndef _APO_COMMON_H +#define _APO_COMMON_H + +FastSerialPort0(Serial); +FastSerialPort1(Serial1); +FastSerialPort2(Serial2); +FastSerialPort3(Serial3); + +/* + * Required Global Declarations + */ + +static apo::AP_Autopilot * autoPilot; + +void setup() { + + using namespace apo; + + AP_Var::load_all(); + + /* + * Communications + */ + Serial.begin(DEBUG_BAUD, 128, 128); // debug + if (board==BOARD_ARDUPILOTMEGA_2) Serial2.begin(TELEM_BAUD, 128, 128); // gcs + else Serial3.begin(TELEM_BAUD, 128, 128); // gcs + + // hardware abstraction layer + AP_HardwareAbstractionLayer * hal = new AP_HardwareAbstractionLayer( + halMode, board, vehicle, heartBeatTimeout); + + // debug serial + hal->debug = &Serial; + hal->debug->println_P(PSTR("initializing debug line")); + + /* + * Initialize Comm Channels + */ + hal->debug->println_P(PSTR("initializing comm channels")); + if (hal->getMode() == MODE_LIVE) { + Serial1.begin(GPS_BAUD, 128, 16); // gps + } else { // hil + Serial1.begin(HIL_BAUD, 128, 128); + } + + /* + * Sensor initialization + */ + if (hal->getMode() == MODE_LIVE) { + hal->debug->println_P(PSTR("initializing adc")); + hal->adc = new ADC_CLASS; + hal->adc->Init(); + + if (gpsEnabled) { + hal->debug->println_P(PSTR("initializing gps")); + AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps)); + hal->gps = &gpsDriver; + hal->gps->init(); + } + + if (baroEnabled) { + hal->debug->println_P(PSTR("initializing baro")); + hal->baro = new BARO_CLASS; + hal->baro->Init(); + } + + if (compassEnabled) { + hal->debug->println_P(PSTR("initializing compass")); + hal->compass = new COMPASS_CLASS; + hal->compass->set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD); + hal->compass->init(); + } + + /** + * Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not + * initialize them and NULL will be assigned to those corresponding pointers. + * On detecting NULL assigned to any ultrasonic sensor, its corresponding block of code + * will not be executed by the navigator. + * The coordinate system is assigned by the right hand rule with the thumb pointing down. + * In set_orientation, it is defind as (front/back,left/right,down,up) + */ + + if (rangeFinderFrontEnabled) { + hal->debug->println_P(PSTR("initializing front range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(1); + rangeFinder->set_orientation(1, 0, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderBackEnabled) { + hal->debug->println_P(PSTR("initializing back range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(2); + rangeFinder->set_orientation(-1, 0, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderLeftEnabled) { + hal->debug->println_P(PSTR("initializing left range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(3); + rangeFinder->set_orientation(0, -1, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderRightEnabled) { + hal->debug->println_P(PSTR("initializing right range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(4); + rangeFinder->set_orientation(0, 1, 0); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderUpEnabled) { + hal->debug->println_P(PSTR("initializing up range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(5); + rangeFinder->set_orientation(0, 0, -1); + hal->rangeFinders.push_back(rangeFinder); + } + + if (rangeFinderDownEnabled) { + hal->debug->println_P(PSTR("initializing down range finder")); + RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); + rangeFinder->set_analog_port(6); + rangeFinder->set_orientation(0, 0, 1); + hal->rangeFinders.push_back(rangeFinder); + } + + } + + /* + * Select guidance, navigation, control algorithms + */ + AP_Navigator * navigator = new NAVIGATOR_CLASS(hal); + AP_Guide * guide = new GUIDE_CLASS(navigator, hal); + AP_Controller * controller = new CONTROLLER_CLASS(navigator, guide, hal); + + /* + * CommLinks + */ + if (board==BOARD_ARDUPILOTMEGA_2) hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal); + else hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal); + + hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal); + + /* + * Start the autopilot + */ + hal->debug->printf_P(PSTR("initializing arduplane\n")); + hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); + autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal, + loop0Rate, loop1Rate, loop2Rate, loop3Rate); +} + +void loop() { + autoPilot->update(); +} + +#endif _APO_COMMON_H diff --git a/libraries/AP_Common/AP_Var.cpp b/libraries/AP_Common/AP_Var.cpp index efc590743b..474dcb8541 100644 --- a/libraries/AP_Common/AP_Var.cpp +++ b/libraries/AP_Common/AP_Var.cpp @@ -15,7 +15,7 @@ #include #include -#define ENABLE_FASTSERIAL_DEBUG +//#define ENABLE_FASTSERIAL_DEBUG #ifdef ENABLE_FASTSERIAL_DEBUG # include From a0db0fc25f01962afadd6f5f5561c749ff15b927 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 29 Sep 2011 14:18:05 -0400 Subject: [PATCH 4/7] Added missing files. --- .gitignore | 3 + ArduPlane/CMakeLists.txt | 8 +- ArduPlane/Makefile | 516 ++++++++++++++++++++++++++++++++++++++- apo/CMakeLists.txt | 10 +- apo/apo.pde | 167 +------------ 5 files changed, 519 insertions(+), 185 deletions(-) diff --git a/.gitignore b/.gitignore index 99b43f7898..ebd471e7e0 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,6 @@ Tools/ArdupilotMegaPlanner/bin/Release/logs/ ArduCopter/Debug/ config.mk serialsent.raw +CMakeFiles +CMakeCache.txt +cmake_install.cmake diff --git a/ArduPlane/CMakeLists.txt b/ArduPlane/CMakeLists.txt index 19a3b35a50..3b6d496b97 100644 --- a/ArduPlane/CMakeLists.txt +++ b/ArduPlane/CMakeLists.txt @@ -62,7 +62,7 @@ set(${FIRMWARE_NAME}_SKETCHES #GCS_Standard.pde #GCS_Xplane.pde #heli.pde - HIL_Xplane.pde + #HIL_Xplane.pde #leds.pde Log.pde #motors_hexa.pde @@ -92,12 +92,12 @@ set(${FIRMWARE_NAME}_SRCS set(${FIRMWARE_NAME}_HDRS APM_Config.h APM_Config_mavlink_hil.h - APM_Config_xplane.h + #APM_Config_xplane.h config.h defines.h GCS.h - HIL.h - Mavlink_Common.h + #HIL.h + #Mavlink_Common.h Parameters.h ) # Firmware sources diff --git a/ArduPlane/Makefile b/ArduPlane/Makefile index 34f8baffd9..0ea3e7a873 100644 --- a/ArduPlane/Makefile +++ b/ArduPlane/Makefile @@ -1,10 +1,510 @@ -# -# Trivial makefile for building APM -# +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 -# -# Select 'mega' for the original APM, or 'mega2560' for the V2 APM. -# -BOARD = mega +# Default target executed when no arguments are given to make. +default_target: all +.PHONY : default_target + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canoncical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /hsl/homes/jgoppert/Projects/ardupilot-mega/ArduPlane + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /hsl/homes/jgoppert/Projects/ardupilot-mega/ArduPlane + +#============================================================================= +# Targets provided globally by CMake. + +# Special rule for the target edit_cache +edit_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..." + /usr/bin/cmake -i . +.PHONY : edit_cache + +# Special rule for the target edit_cache +edit_cache/fast: edit_cache +.PHONY : edit_cache/fast + +# Special rule for the target rebuild_cache +rebuild_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." + /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) +.PHONY : rebuild_cache + +# Special rule for the target rebuild_cache +rebuild_cache/fast: rebuild_cache +.PHONY : rebuild_cache/fast + +# The main all target +all: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /hsl/homes/jgoppert/Projects/ardupilot-mega/ArduPlane/CMakeFiles /hsl/homes/jgoppert/Projects/ardupilot-mega/ArduPlane/CMakeFiles/progress.marks + $(MAKE) -f CMakeFiles/Makefile2 all + $(CMAKE_COMMAND) -E cmake_progress_start /hsl/homes/jgoppert/Projects/ardupilot-mega/ArduPlane/CMakeFiles 0 +.PHONY : all + +# The main clean target +clean: + $(MAKE) -f CMakeFiles/Makefile2 clean +.PHONY : clean + +# The main clean target +clean/fast: clean +.PHONY : clean/fast + +# Prepare targets for installation. +preinstall: all + $(MAKE) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall + +# Prepare targets for installation. +preinstall/fast: + $(MAKE) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall/fast + +# clear depends +depend: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 +.PHONY : depend + +#============================================================================= +# Target rules for targets named ArduPlane + +# Build rule for target. +ArduPlane: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ArduPlane +.PHONY : ArduPlane + +# fast build rule for target. +ArduPlane/fast: + $(MAKE) -f CMakeFiles/ArduPlane.dir/build.make CMakeFiles/ArduPlane.dir/build +.PHONY : ArduPlane/fast + +#============================================================================= +# Target rules for targets named ArduPlane-serial + +# Build rule for target. +ArduPlane-serial: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ArduPlane-serial +.PHONY : ArduPlane-serial + +# fast build rule for target. +ArduPlane-serial/fast: + $(MAKE) -f CMakeFiles/ArduPlane-serial.dir/build.make CMakeFiles/ArduPlane-serial.dir/build +.PHONY : ArduPlane-serial/fast + +#============================================================================= +# Target rules for targets named ArduPlane-upload + +# Build rule for target. +ArduPlane-upload: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ArduPlane-upload +.PHONY : ArduPlane-upload + +# fast build rule for target. +ArduPlane-upload/fast: + $(MAKE) -f CMakeFiles/ArduPlane-upload.dir/build.make CMakeFiles/ArduPlane-upload.dir/build +.PHONY : ArduPlane-upload/fast + +#============================================================================= +# Target rules for targets named upload + +# Build rule for target. +upload: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 upload +.PHONY : upload + +# fast build rule for target. +upload/fast: + $(MAKE) -f CMakeFiles/upload.dir/build.make CMakeFiles/upload.dir/build +.PHONY : upload/fast + +#============================================================================= +# Target rules for targets named DataFlash + +# Build rule for target. +DataFlash: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 DataFlash +.PHONY : DataFlash + +# fast build rule for target. +DataFlash/fast: + $(MAKE) -f libs/DataFlash/CMakeFiles/DataFlash.dir/build.make libs/DataFlash/CMakeFiles/DataFlash.dir/build +.PHONY : DataFlash/fast + +#============================================================================= +# Target rules for targets named mega_CORE + +# Build rule for target. +mega_CORE: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 mega_CORE +.PHONY : mega_CORE + +# fast build rule for target. +mega_CORE/fast: + $(MAKE) -f libs/DataFlash/CMakeFiles/mega_CORE.dir/build.make libs/DataFlash/CMakeFiles/mega_CORE.dir/build +.PHONY : mega_CORE/fast + +#============================================================================= +# Target rules for targets named mega_SPI + +# Build rule for target. +mega_SPI: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 mega_SPI +.PHONY : mega_SPI + +# fast build rule for target. +mega_SPI/fast: + $(MAKE) -f libs/DataFlash/CMakeFiles/mega_SPI.dir/build.make libs/DataFlash/CMakeFiles/mega_SPI.dir/build +.PHONY : mega_SPI/fast + +#============================================================================= +# Target rules for targets named AP_Math + +# Build rule for target. +AP_Math: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 AP_Math +.PHONY : AP_Math + +# fast build rule for target. +AP_Math/fast: + $(MAKE) -f libs/AP_Math/CMakeFiles/AP_Math.dir/build.make libs/AP_Math/CMakeFiles/AP_Math.dir/build +.PHONY : AP_Math/fast + +#============================================================================= +# Target rules for targets named PID + +# Build rule for target. +PID: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 PID +.PHONY : PID + +# fast build rule for target. +PID/fast: + $(MAKE) -f libs/PID/CMakeFiles/PID.dir/build.make libs/PID/CMakeFiles/PID.dir/build +.PHONY : PID/fast + +#============================================================================= +# Target rules for targets named AP_Common + +# Build rule for target. +AP_Common: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 AP_Common +.PHONY : AP_Common + +# fast build rule for target. +AP_Common/fast: + $(MAKE) -f libs/AP_Common/CMakeFiles/AP_Common.dir/build.make libs/AP_Common/CMakeFiles/AP_Common.dir/build +.PHONY : AP_Common/fast + +#============================================================================= +# Target rules for targets named RC_Channel + +# Build rule for target. +RC_Channel: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 RC_Channel +.PHONY : RC_Channel + +# fast build rule for target. +RC_Channel/fast: + $(MAKE) -f libs/RC_Channel/CMakeFiles/RC_Channel.dir/build.make libs/RC_Channel/CMakeFiles/RC_Channel.dir/build +.PHONY : RC_Channel/fast + +#============================================================================= +# Target rules for targets named AP_OpticalFlow + +# Build rule for target. +AP_OpticalFlow: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 AP_OpticalFlow +.PHONY : AP_OpticalFlow + +# fast build rule for target. +AP_OpticalFlow/fast: + $(MAKE) -f libs/AP_OpticalFlow/CMakeFiles/AP_OpticalFlow.dir/build.make libs/AP_OpticalFlow/CMakeFiles/AP_OpticalFlow.dir/build +.PHONY : AP_OpticalFlow/fast + +#============================================================================= +# Target rules for targets named ModeFilter + +# Build rule for target. +ModeFilter: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ModeFilter +.PHONY : ModeFilter + +# fast build rule for target. +ModeFilter/fast: + $(MAKE) -f libs/ModeFilter/CMakeFiles/ModeFilter.dir/build.make libs/ModeFilter/CMakeFiles/ModeFilter.dir/build +.PHONY : ModeFilter/fast + +#============================================================================= +# Target rules for targets named memcheck + +# Build rule for target. +memcheck: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 memcheck +.PHONY : memcheck + +# fast build rule for target. +memcheck/fast: + $(MAKE) -f libs/memcheck/CMakeFiles/memcheck.dir/build.make libs/memcheck/CMakeFiles/memcheck.dir/build +.PHONY : memcheck/fast + +#============================================================================= +# Target rules for targets named APM_PI + +# Build rule for target. +APM_PI: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 APM_PI +.PHONY : APM_PI + +# fast build rule for target. +APM_PI/fast: + $(MAKE) -f libs/APM_PI/CMakeFiles/APM_PI.dir/build.make libs/APM_PI/CMakeFiles/APM_PI.dir/build +.PHONY : APM_PI/fast + +#============================================================================= +# Target rules for targets named AP_GPS + +# Build rule for target. +AP_GPS: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 AP_GPS +.PHONY : AP_GPS + +# fast build rule for target. +AP_GPS/fast: + $(MAKE) -f libs/AP_GPS/CMakeFiles/AP_GPS.dir/build.make libs/AP_GPS/CMakeFiles/AP_GPS.dir/build +.PHONY : AP_GPS/fast + +#============================================================================= +# Target rules for targets named AP_DCM + +# Build rule for target. +AP_DCM: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 AP_DCM +.PHONY : AP_DCM + +# fast build rule for target. +AP_DCM/fast: + $(MAKE) -f libs/AP_DCM/CMakeFiles/AP_DCM.dir/build.make libs/AP_DCM/CMakeFiles/AP_DCM.dir/build +.PHONY : AP_DCM/fast + +#============================================================================= +# Target rules for targets named AP_Compass + +# Build rule for target. +AP_Compass: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 AP_Compass +.PHONY : AP_Compass + +# fast build rule for target. +AP_Compass/fast: + $(MAKE) -f libs/AP_Compass/CMakeFiles/AP_Compass.dir/build.make libs/AP_Compass/CMakeFiles/AP_Compass.dir/build +.PHONY : AP_Compass/fast + +#============================================================================= +# Target rules for targets named AP_ADC + +# Build rule for target. +AP_ADC: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 AP_ADC +.PHONY : AP_ADC + +# fast build rule for target. +AP_ADC/fast: + $(MAKE) -f libs/AP_ADC/CMakeFiles/AP_ADC.dir/build.make libs/AP_ADC/CMakeFiles/AP_ADC.dir/build +.PHONY : AP_ADC/fast + +#============================================================================= +# Target rules for targets named AP_IMU + +# Build rule for target. +AP_IMU: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 AP_IMU +.PHONY : AP_IMU + +# fast build rule for target. +AP_IMU/fast: + $(MAKE) -f libs/AP_IMU/CMakeFiles/AP_IMU.dir/build.make libs/AP_IMU/CMakeFiles/AP_IMU.dir/build +.PHONY : AP_IMU/fast + +#============================================================================= +# Target rules for targets named AP_RangeFinder + +# Build rule for target. +AP_RangeFinder: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 AP_RangeFinder +.PHONY : AP_RangeFinder + +# fast build rule for target. +AP_RangeFinder/fast: + $(MAKE) -f libs/AP_RangeFinder/CMakeFiles/AP_RangeFinder.dir/build.make libs/AP_RangeFinder/CMakeFiles/AP_RangeFinder.dir/build +.PHONY : AP_RangeFinder/fast + +#============================================================================= +# Target rules for targets named APM_RC + +# Build rule for target. +APM_RC: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 APM_RC +.PHONY : APM_RC + +# fast build rule for target. +APM_RC/fast: + $(MAKE) -f libs/APM_RC/CMakeFiles/APM_RC.dir/build.make libs/APM_RC/CMakeFiles/APM_RC.dir/build +.PHONY : APM_RC/fast + +#============================================================================= +# Target rules for targets named APM_BMP085 + +# Build rule for target. +APM_BMP085: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 APM_BMP085 +.PHONY : APM_BMP085 + +# fast build rule for target. +APM_BMP085/fast: + $(MAKE) -f libs/APM_BMP085/CMakeFiles/APM_BMP085.dir/build.make libs/APM_BMP085/CMakeFiles/APM_BMP085.dir/build +.PHONY : APM_BMP085/fast + +#============================================================================= +# Target rules for targets named mega_Wire + +# Build rule for target. +mega_Wire: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 mega_Wire +.PHONY : mega_Wire + +# fast build rule for target. +mega_Wire/fast: + $(MAKE) -f libs/APM_BMP085/CMakeFiles/mega_Wire.dir/build.make libs/APM_BMP085/CMakeFiles/mega_Wire.dir/build +.PHONY : mega_Wire/fast + +#============================================================================= +# Target rules for targets named FastSerial + +# Build rule for target. +FastSerial: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 FastSerial +.PHONY : FastSerial + +# fast build rule for target. +FastSerial/fast: + $(MAKE) -f libs/FastSerial/CMakeFiles/FastSerial.dir/build.make libs/FastSerial/CMakeFiles/FastSerial.dir/build +.PHONY : FastSerial/fast + +#============================================================================= +# Target rules for targets named GCS_MAVLink + +# Build rule for target. +GCS_MAVLink: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 GCS_MAVLink +.PHONY : GCS_MAVLink + +# fast build rule for target. +GCS_MAVLink/fast: + $(MAKE) -f libs/GCS_MAVLink/CMakeFiles/GCS_MAVLink.dir/build.make libs/GCS_MAVLink/CMakeFiles/GCS_MAVLink.dir/build +.PHONY : GCS_MAVLink/fast + +ArduPlane.obj: ArduPlane.cpp.obj +.PHONY : ArduPlane.obj + +# target to build an object file +ArduPlane.cpp.obj: + $(MAKE) -f CMakeFiles/ArduPlane.dir/build.make CMakeFiles/ArduPlane.dir/ArduPlane.cpp.obj +.PHONY : ArduPlane.cpp.obj + +ArduPlane.i: ArduPlane.cpp.i +.PHONY : ArduPlane.i + +# target to preprocess a source file +ArduPlane.cpp.i: + $(MAKE) -f CMakeFiles/ArduPlane.dir/build.make CMakeFiles/ArduPlane.dir/ArduPlane.cpp.i +.PHONY : ArduPlane.cpp.i + +ArduPlane.s: ArduPlane.cpp.s +.PHONY : ArduPlane.s + +# target to generate assembly for a file +ArduPlane.cpp.s: + $(MAKE) -f CMakeFiles/ArduPlane.dir/build.make CMakeFiles/ArduPlane.dir/ArduPlane.cpp.s +.PHONY : ArduPlane.cpp.s + +# Help Target +help: + @echo "The following are some of the valid targets for this Makefile:" + @echo "... all (the default if no target is provided)" + @echo "... clean" + @echo "... depend" + @echo "... ArduPlane" + @echo "... ArduPlane-serial" + @echo "... ArduPlane-upload" + @echo "... edit_cache" + @echo "... rebuild_cache" + @echo "... upload" + @echo "... DataFlash" + @echo "... mega_CORE" + @echo "... mega_SPI" + @echo "... AP_Math" + @echo "... PID" + @echo "... AP_Common" + @echo "... RC_Channel" + @echo "... AP_OpticalFlow" + @echo "... ModeFilter" + @echo "... memcheck" + @echo "... APM_PI" + @echo "... AP_GPS" + @echo "... AP_DCM" + @echo "... AP_Compass" + @echo "... AP_ADC" + @echo "... AP_IMU" + @echo "... AP_RangeFinder" + @echo "... APM_RC" + @echo "... APM_BMP085" + @echo "... mega_Wire" + @echo "... FastSerial" + @echo "... GCS_MAVLink" + @echo "... ArduPlane.obj" + @echo "... ArduPlane.i" + @echo "... ArduPlane.s" +.PHONY : help + + + +#============================================================================= +# Special targets to cleanup operation of make. + +# Special rule to run CMake to check the build system integrity. +# No rule that depends on this can have commands that come from listfiles +# because they might be regenerated. +cmake_check_build_system: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 +.PHONY : cmake_check_build_system -include ../libraries/AP_Common/Arduino.mk diff --git a/apo/CMakeLists.txt b/apo/CMakeLists.txt index 95c4c1289c..f08d48c262 100644 --- a/apo/CMakeLists.txt +++ b/apo/CMakeLists.txt @@ -7,24 +7,18 @@ #====================================================================# # Settings # #====================================================================# -set(FIRMWARE_NAME ardupilotone) +set(FIRMWARE_NAME apo) set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board set(${FIRMWARE_NAME}_SKETCHES - ardupilotone.pde + apo.pde ) # Firmware sketches set(${FIRMWARE_NAME}_SRCS ) # Firmware sources set(${FIRMWARE_NAME}_HDRS - BoatGeneric.h - CarStampede.h - #CNIRover.h - #CNIBoat.h - ControllerBoat.h - ControllerCar.h ControllerPlane.h ControllerQuad.h PlaneEasystar.h diff --git a/apo/apo.pde b/apo/apo.pde index 7e79ec1a9e..233b9513ee 100644 --- a/apo/apo.pde +++ b/apo/apo.pde @@ -1,12 +1,3 @@ -/* - * ardupilotone - * - * Created on: Apr 30, 2011 - * Author: jgoppert - */ - -#define ENABLE_FASTSERIAL_DEBUG - // Libraries #include #include @@ -23,162 +14,8 @@ #include #include -FastSerialPort0(Serial); -FastSerialPort1(Serial1); -FastSerialPort2(Serial2); -FastSerialPort3(Serial3); - // Vehicle Configuration #include "PlaneEasystar.h" -/* - * Required Global Declarations - */ - -static apo::AP_Autopilot * autoPilot; - -void setup() { - - using namespace apo; - - AP_Var::load_all(); - - /* - * Communications - */ - Serial.begin(DEBUG_BAUD, 128, 128); // debug - if (board==BOARD_ARDUPILOTMEGA_2) Serial2.begin(TELEM_BAUD, 128, 128); // gcs - else Serial3.begin(TELEM_BAUD, 128, 128); // gcs - - // hardware abstraction layer - AP_HardwareAbstractionLayer * hal = new AP_HardwareAbstractionLayer( - halMode, board, vehicle, heartBeatTimeout); - - // debug serial - hal->debug = &Serial; - hal->debug->println_P(PSTR("initializing debug line")); - - /* - * Initialize Comm Channels - */ - hal->debug->println_P(PSTR("initializing comm channels")); - if (hal->getMode() == MODE_LIVE) { - Serial1.begin(GPS_BAUD, 128, 16); // gps - } else { // hil - Serial1.begin(HIL_BAUD, 128, 128); - } - - /* - * Sensor initialization - */ - if (hal->getMode() == MODE_LIVE) { - hal->debug->println_P(PSTR("initializing adc")); - hal->adc = new ADC_CLASS; - hal->adc->Init(); - - if (gpsEnabled) { - hal->debug->println_P(PSTR("initializing gps")); - AP_GPS_Auto gpsDriver(&Serial1, &(hal->gps)); - hal->gps = &gpsDriver; - hal->gps->init(); - } - - if (baroEnabled) { - hal->debug->println_P(PSTR("initializing baro")); - hal->baro = new BARO_CLASS; - hal->baro->Init(); - } - - if (compassEnabled) { - hal->debug->println_P(PSTR("initializing compass")); - hal->compass = new COMPASS_CLASS; - hal->compass->set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD); - hal->compass->init(); - } - - /** - * Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not - * initialize them and NULL will be assigned to those corresponding pointers. - * On detecting NULL assigned to any ultrasonic sensor, its corresponding block of code - * will not be executed by the navigator. - * The coordinate system is assigned by the right hand rule with the thumb pointing down. - * In set_orientation, it is defind as (front/back,left/right,down,up) - */ - - if (rangeFinderFrontEnabled) { - hal->debug->println_P(PSTR("initializing front range finder")); - RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(1); - rangeFinder->set_orientation(1, 0, 0); - hal->rangeFinders.push_back(rangeFinder); - } - - if (rangeFinderBackEnabled) { - hal->debug->println_P(PSTR("initializing back range finder")); - RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(2); - rangeFinder->set_orientation(-1, 0, 0); - hal->rangeFinders.push_back(rangeFinder); - } - - if (rangeFinderLeftEnabled) { - hal->debug->println_P(PSTR("initializing left range finder")); - RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(3); - rangeFinder->set_orientation(0, -1, 0); - hal->rangeFinders.push_back(rangeFinder); - } - - if (rangeFinderRightEnabled) { - hal->debug->println_P(PSTR("initializing right range finder")); - RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(4); - rangeFinder->set_orientation(0, 1, 0); - hal->rangeFinders.push_back(rangeFinder); - } - - if (rangeFinderUpEnabled) { - hal->debug->println_P(PSTR("initializing up range finder")); - RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(5); - rangeFinder->set_orientation(0, 0, -1); - hal->rangeFinders.push_back(rangeFinder); - } - - if (rangeFinderDownEnabled) { - hal->debug->println_P(PSTR("initializing down range finder")); - RangeFinder * rangeFinder = new RANGE_FINDER_CLASS(hal->adc,new ModeFilter); - rangeFinder->set_analog_port(6); - rangeFinder->set_orientation(0, 0, 1); - hal->rangeFinders.push_back(rangeFinder); - } - - } - - /* - * Select guidance, navigation, control algorithms - */ - AP_Navigator * navigator = new NAVIGATOR_CLASS(hal); - AP_Guide * guide = new GUIDE_CLASS(navigator, hal); - AP_Controller * controller = new CONTROLLER_CLASS(navigator, guide, hal); - - /* - * CommLinks - */ - if (board==BOARD_ARDUPILOTMEGA_2) hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal); - else hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal); - - hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal); - - /* - * Start the autopilot - */ - hal->debug->printf_P(PSTR("initializing arduplane\n")); - hal->debug->printf_P(PSTR("free ram: %d bytes\n"),freeMemory()); - autoPilot = new apo::AP_Autopilot(navigator, guide, controller, hal, - loop0Rate, loop1Rate, loop2Rate, loop3Rate); -} - -void loop() { - autoPilot->update(); -} +// ArduPilotOne Default Setup +#include "APO_DefaultSetup.h" From bfe03318035f213ffe5417114f19a47e80f026e9 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 29 Sep 2011 18:57:19 -0400 Subject: [PATCH 5/7] Corrected makefile. --- ArduPlane/Makefile | 516 +-------------------------------------------- 1 file changed, 8 insertions(+), 508 deletions(-) diff --git a/ArduPlane/Makefile b/ArduPlane/Makefile index 0ea3e7a873..34f8baffd9 100644 --- a/ArduPlane/Makefile +++ b/ArduPlane/Makefile @@ -1,510 +1,10 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 2.8 +# +# Trivial makefile for building APM +# -# Default target executed when no arguments are given to make. -default_target: all -.PHONY : default_target - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canoncical targets will work. -.SUFFIXES: - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - -# A target that is always out of date. -cmake_force: -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /hsl/homes/jgoppert/Projects/ardupilot-mega/ArduPlane - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /hsl/homes/jgoppert/Projects/ardupilot-mega/ArduPlane - -#============================================================================= -# Targets provided globally by CMake. - -# Special rule for the target edit_cache -edit_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..." - /usr/bin/cmake -i . -.PHONY : edit_cache - -# Special rule for the target edit_cache -edit_cache/fast: edit_cache -.PHONY : edit_cache/fast - -# Special rule for the target rebuild_cache -rebuild_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." - /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) -.PHONY : rebuild_cache - -# Special rule for the target rebuild_cache -rebuild_cache/fast: rebuild_cache -.PHONY : rebuild_cache/fast - -# The main all target -all: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /hsl/homes/jgoppert/Projects/ardupilot-mega/ArduPlane/CMakeFiles /hsl/homes/jgoppert/Projects/ardupilot-mega/ArduPlane/CMakeFiles/progress.marks - $(MAKE) -f CMakeFiles/Makefile2 all - $(CMAKE_COMMAND) -E cmake_progress_start /hsl/homes/jgoppert/Projects/ardupilot-mega/ArduPlane/CMakeFiles 0 -.PHONY : all - -# The main clean target -clean: - $(MAKE) -f CMakeFiles/Makefile2 clean -.PHONY : clean - -# The main clean target -clean/fast: clean -.PHONY : clean/fast - -# Prepare targets for installation. -preinstall: all - $(MAKE) -f CMakeFiles/Makefile2 preinstall -.PHONY : preinstall - -# Prepare targets for installation. -preinstall/fast: - $(MAKE) -f CMakeFiles/Makefile2 preinstall -.PHONY : preinstall/fast - -# clear depends -depend: - $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 -.PHONY : depend - -#============================================================================= -# Target rules for targets named ArduPlane - -# Build rule for target. -ArduPlane: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 ArduPlane -.PHONY : ArduPlane - -# fast build rule for target. -ArduPlane/fast: - $(MAKE) -f CMakeFiles/ArduPlane.dir/build.make CMakeFiles/ArduPlane.dir/build -.PHONY : ArduPlane/fast - -#============================================================================= -# Target rules for targets named ArduPlane-serial - -# Build rule for target. -ArduPlane-serial: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 ArduPlane-serial -.PHONY : ArduPlane-serial - -# fast build rule for target. -ArduPlane-serial/fast: - $(MAKE) -f CMakeFiles/ArduPlane-serial.dir/build.make CMakeFiles/ArduPlane-serial.dir/build -.PHONY : ArduPlane-serial/fast - -#============================================================================= -# Target rules for targets named ArduPlane-upload - -# Build rule for target. -ArduPlane-upload: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 ArduPlane-upload -.PHONY : ArduPlane-upload - -# fast build rule for target. -ArduPlane-upload/fast: - $(MAKE) -f CMakeFiles/ArduPlane-upload.dir/build.make CMakeFiles/ArduPlane-upload.dir/build -.PHONY : ArduPlane-upload/fast - -#============================================================================= -# Target rules for targets named upload - -# Build rule for target. -upload: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 upload -.PHONY : upload - -# fast build rule for target. -upload/fast: - $(MAKE) -f CMakeFiles/upload.dir/build.make CMakeFiles/upload.dir/build -.PHONY : upload/fast - -#============================================================================= -# Target rules for targets named DataFlash - -# Build rule for target. -DataFlash: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 DataFlash -.PHONY : DataFlash - -# fast build rule for target. -DataFlash/fast: - $(MAKE) -f libs/DataFlash/CMakeFiles/DataFlash.dir/build.make libs/DataFlash/CMakeFiles/DataFlash.dir/build -.PHONY : DataFlash/fast - -#============================================================================= -# Target rules for targets named mega_CORE - -# Build rule for target. -mega_CORE: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 mega_CORE -.PHONY : mega_CORE - -# fast build rule for target. -mega_CORE/fast: - $(MAKE) -f libs/DataFlash/CMakeFiles/mega_CORE.dir/build.make libs/DataFlash/CMakeFiles/mega_CORE.dir/build -.PHONY : mega_CORE/fast - -#============================================================================= -# Target rules for targets named mega_SPI - -# Build rule for target. -mega_SPI: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 mega_SPI -.PHONY : mega_SPI - -# fast build rule for target. -mega_SPI/fast: - $(MAKE) -f libs/DataFlash/CMakeFiles/mega_SPI.dir/build.make libs/DataFlash/CMakeFiles/mega_SPI.dir/build -.PHONY : mega_SPI/fast - -#============================================================================= -# Target rules for targets named AP_Math - -# Build rule for target. -AP_Math: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 AP_Math -.PHONY : AP_Math - -# fast build rule for target. -AP_Math/fast: - $(MAKE) -f libs/AP_Math/CMakeFiles/AP_Math.dir/build.make libs/AP_Math/CMakeFiles/AP_Math.dir/build -.PHONY : AP_Math/fast - -#============================================================================= -# Target rules for targets named PID - -# Build rule for target. -PID: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 PID -.PHONY : PID - -# fast build rule for target. -PID/fast: - $(MAKE) -f libs/PID/CMakeFiles/PID.dir/build.make libs/PID/CMakeFiles/PID.dir/build -.PHONY : PID/fast - -#============================================================================= -# Target rules for targets named AP_Common - -# Build rule for target. -AP_Common: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 AP_Common -.PHONY : AP_Common - -# fast build rule for target. -AP_Common/fast: - $(MAKE) -f libs/AP_Common/CMakeFiles/AP_Common.dir/build.make libs/AP_Common/CMakeFiles/AP_Common.dir/build -.PHONY : AP_Common/fast - -#============================================================================= -# Target rules for targets named RC_Channel - -# Build rule for target. -RC_Channel: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 RC_Channel -.PHONY : RC_Channel - -# fast build rule for target. -RC_Channel/fast: - $(MAKE) -f libs/RC_Channel/CMakeFiles/RC_Channel.dir/build.make libs/RC_Channel/CMakeFiles/RC_Channel.dir/build -.PHONY : RC_Channel/fast - -#============================================================================= -# Target rules for targets named AP_OpticalFlow - -# Build rule for target. -AP_OpticalFlow: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 AP_OpticalFlow -.PHONY : AP_OpticalFlow - -# fast build rule for target. -AP_OpticalFlow/fast: - $(MAKE) -f libs/AP_OpticalFlow/CMakeFiles/AP_OpticalFlow.dir/build.make libs/AP_OpticalFlow/CMakeFiles/AP_OpticalFlow.dir/build -.PHONY : AP_OpticalFlow/fast - -#============================================================================= -# Target rules for targets named ModeFilter - -# Build rule for target. -ModeFilter: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 ModeFilter -.PHONY : ModeFilter - -# fast build rule for target. -ModeFilter/fast: - $(MAKE) -f libs/ModeFilter/CMakeFiles/ModeFilter.dir/build.make libs/ModeFilter/CMakeFiles/ModeFilter.dir/build -.PHONY : ModeFilter/fast - -#============================================================================= -# Target rules for targets named memcheck - -# Build rule for target. -memcheck: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 memcheck -.PHONY : memcheck - -# fast build rule for target. -memcheck/fast: - $(MAKE) -f libs/memcheck/CMakeFiles/memcheck.dir/build.make libs/memcheck/CMakeFiles/memcheck.dir/build -.PHONY : memcheck/fast - -#============================================================================= -# Target rules for targets named APM_PI - -# Build rule for target. -APM_PI: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 APM_PI -.PHONY : APM_PI - -# fast build rule for target. -APM_PI/fast: - $(MAKE) -f libs/APM_PI/CMakeFiles/APM_PI.dir/build.make libs/APM_PI/CMakeFiles/APM_PI.dir/build -.PHONY : APM_PI/fast - -#============================================================================= -# Target rules for targets named AP_GPS - -# Build rule for target. -AP_GPS: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 AP_GPS -.PHONY : AP_GPS - -# fast build rule for target. -AP_GPS/fast: - $(MAKE) -f libs/AP_GPS/CMakeFiles/AP_GPS.dir/build.make libs/AP_GPS/CMakeFiles/AP_GPS.dir/build -.PHONY : AP_GPS/fast - -#============================================================================= -# Target rules for targets named AP_DCM - -# Build rule for target. -AP_DCM: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 AP_DCM -.PHONY : AP_DCM - -# fast build rule for target. -AP_DCM/fast: - $(MAKE) -f libs/AP_DCM/CMakeFiles/AP_DCM.dir/build.make libs/AP_DCM/CMakeFiles/AP_DCM.dir/build -.PHONY : AP_DCM/fast - -#============================================================================= -# Target rules for targets named AP_Compass - -# Build rule for target. -AP_Compass: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 AP_Compass -.PHONY : AP_Compass - -# fast build rule for target. -AP_Compass/fast: - $(MAKE) -f libs/AP_Compass/CMakeFiles/AP_Compass.dir/build.make libs/AP_Compass/CMakeFiles/AP_Compass.dir/build -.PHONY : AP_Compass/fast - -#============================================================================= -# Target rules for targets named AP_ADC - -# Build rule for target. -AP_ADC: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 AP_ADC -.PHONY : AP_ADC - -# fast build rule for target. -AP_ADC/fast: - $(MAKE) -f libs/AP_ADC/CMakeFiles/AP_ADC.dir/build.make libs/AP_ADC/CMakeFiles/AP_ADC.dir/build -.PHONY : AP_ADC/fast - -#============================================================================= -# Target rules for targets named AP_IMU - -# Build rule for target. -AP_IMU: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 AP_IMU -.PHONY : AP_IMU - -# fast build rule for target. -AP_IMU/fast: - $(MAKE) -f libs/AP_IMU/CMakeFiles/AP_IMU.dir/build.make libs/AP_IMU/CMakeFiles/AP_IMU.dir/build -.PHONY : AP_IMU/fast - -#============================================================================= -# Target rules for targets named AP_RangeFinder - -# Build rule for target. -AP_RangeFinder: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 AP_RangeFinder -.PHONY : AP_RangeFinder - -# fast build rule for target. -AP_RangeFinder/fast: - $(MAKE) -f libs/AP_RangeFinder/CMakeFiles/AP_RangeFinder.dir/build.make libs/AP_RangeFinder/CMakeFiles/AP_RangeFinder.dir/build -.PHONY : AP_RangeFinder/fast - -#============================================================================= -# Target rules for targets named APM_RC - -# Build rule for target. -APM_RC: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 APM_RC -.PHONY : APM_RC - -# fast build rule for target. -APM_RC/fast: - $(MAKE) -f libs/APM_RC/CMakeFiles/APM_RC.dir/build.make libs/APM_RC/CMakeFiles/APM_RC.dir/build -.PHONY : APM_RC/fast - -#============================================================================= -# Target rules for targets named APM_BMP085 - -# Build rule for target. -APM_BMP085: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 APM_BMP085 -.PHONY : APM_BMP085 - -# fast build rule for target. -APM_BMP085/fast: - $(MAKE) -f libs/APM_BMP085/CMakeFiles/APM_BMP085.dir/build.make libs/APM_BMP085/CMakeFiles/APM_BMP085.dir/build -.PHONY : APM_BMP085/fast - -#============================================================================= -# Target rules for targets named mega_Wire - -# Build rule for target. -mega_Wire: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 mega_Wire -.PHONY : mega_Wire - -# fast build rule for target. -mega_Wire/fast: - $(MAKE) -f libs/APM_BMP085/CMakeFiles/mega_Wire.dir/build.make libs/APM_BMP085/CMakeFiles/mega_Wire.dir/build -.PHONY : mega_Wire/fast - -#============================================================================= -# Target rules for targets named FastSerial - -# Build rule for target. -FastSerial: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 FastSerial -.PHONY : FastSerial - -# fast build rule for target. -FastSerial/fast: - $(MAKE) -f libs/FastSerial/CMakeFiles/FastSerial.dir/build.make libs/FastSerial/CMakeFiles/FastSerial.dir/build -.PHONY : FastSerial/fast - -#============================================================================= -# Target rules for targets named GCS_MAVLink - -# Build rule for target. -GCS_MAVLink: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 GCS_MAVLink -.PHONY : GCS_MAVLink - -# fast build rule for target. -GCS_MAVLink/fast: - $(MAKE) -f libs/GCS_MAVLink/CMakeFiles/GCS_MAVLink.dir/build.make libs/GCS_MAVLink/CMakeFiles/GCS_MAVLink.dir/build -.PHONY : GCS_MAVLink/fast - -ArduPlane.obj: ArduPlane.cpp.obj -.PHONY : ArduPlane.obj - -# target to build an object file -ArduPlane.cpp.obj: - $(MAKE) -f CMakeFiles/ArduPlane.dir/build.make CMakeFiles/ArduPlane.dir/ArduPlane.cpp.obj -.PHONY : ArduPlane.cpp.obj - -ArduPlane.i: ArduPlane.cpp.i -.PHONY : ArduPlane.i - -# target to preprocess a source file -ArduPlane.cpp.i: - $(MAKE) -f CMakeFiles/ArduPlane.dir/build.make CMakeFiles/ArduPlane.dir/ArduPlane.cpp.i -.PHONY : ArduPlane.cpp.i - -ArduPlane.s: ArduPlane.cpp.s -.PHONY : ArduPlane.s - -# target to generate assembly for a file -ArduPlane.cpp.s: - $(MAKE) -f CMakeFiles/ArduPlane.dir/build.make CMakeFiles/ArduPlane.dir/ArduPlane.cpp.s -.PHONY : ArduPlane.cpp.s - -# Help Target -help: - @echo "The following are some of the valid targets for this Makefile:" - @echo "... all (the default if no target is provided)" - @echo "... clean" - @echo "... depend" - @echo "... ArduPlane" - @echo "... ArduPlane-serial" - @echo "... ArduPlane-upload" - @echo "... edit_cache" - @echo "... rebuild_cache" - @echo "... upload" - @echo "... DataFlash" - @echo "... mega_CORE" - @echo "... mega_SPI" - @echo "... AP_Math" - @echo "... PID" - @echo "... AP_Common" - @echo "... RC_Channel" - @echo "... AP_OpticalFlow" - @echo "... ModeFilter" - @echo "... memcheck" - @echo "... APM_PI" - @echo "... AP_GPS" - @echo "... AP_DCM" - @echo "... AP_Compass" - @echo "... AP_ADC" - @echo "... AP_IMU" - @echo "... AP_RangeFinder" - @echo "... APM_RC" - @echo "... APM_BMP085" - @echo "... mega_Wire" - @echo "... FastSerial" - @echo "... GCS_MAVLink" - @echo "... ArduPlane.obj" - @echo "... ArduPlane.i" - @echo "... ArduPlane.s" -.PHONY : help - - - -#============================================================================= -# Special targets to cleanup operation of make. - -# Special rule to run CMake to check the build system integrity. -# No rule that depends on this can have commands that come from listfiles -# because they might be regenerated. -cmake_check_build_system: - $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 -.PHONY : cmake_check_build_system +# +# Select 'mega' for the original APM, or 'mega2560' for the V2 APM. +# +BOARD = mega +include ../libraries/AP_Common/Arduino.mk From 112d5e953126466ff25beb9f144452721bbc2841 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 29 Sep 2011 21:23:26 -0400 Subject: [PATCH 6/7] Improvements to cmake. --- .gitignore | 1 + ArduCopter/.gitignore | 1 + ArduCopter/CMakeLists.txt | 2 - ArduPlane/.gitignore | 1 + ArduPlane/CMakeLists.txt | 2 - apo/.gitignore | 1 + apo/CMakeLists.txt | 93 ++++++++++++------------- apo/apo.pde | 1 + cmake/modules/ApmMakeLib.cmake | 7 ++ cmake/modules/ApmMakeSketch.cmake | 74 ++++++++++++++++++++ libraries/APM_BMP085/CMakeLists.txt | 23 +----- libraries/APM_PI/CMakeLists.txt | 25 +------ libraries/APM_PerfMon/CMakeLists.txt | 1 + libraries/APM_RC/CMakeLists.txt | 23 +----- libraries/APO/CMakeLists.txt | 40 +---------- libraries/AP_ADC/CMakeLists.txt | 27 +------ libraries/AP_Common/CMakeLists.txt | 34 +-------- libraries/AP_Compass/CMakeLists.txt | 28 +------- libraries/AP_DCM/CMakeLists.txt | 26 +------ libraries/AP_EEPROMB/CMakeLists.txt | 1 + libraries/AP_GPS/CMakeLists.txt | 45 +----------- libraries/AP_IMU/CMakeLists.txt | 26 +------ libraries/AP_Math/CMakeLists.txt | 25 +------ libraries/AP_Navigation/CMakeLists.txt | 1 + libraries/AP_OpticalFlow/CMakeLists.txt | 25 +------ libraries/AP_PID/CMakeLists.txt | 25 +------ libraries/AP_RC/CMakeLists.txt | 1 + libraries/AP_RC_Channel/CMakeLists.txt | 1 + libraries/AP_RangeFinder/CMakeLists.txt | 28 +------- libraries/CMakeLists.txt | 46 +++++++----- libraries/DataFlash/CMakeLists.txt | 22 +----- libraries/FastSerial/CMakeLists.txt | 29 +------- libraries/GCS_MAVLink/CMakeLists.txt | 20 +----- libraries/GCS_SIMPLE/CMakeLists.txt | 1 + libraries/GPS_IMU/CMakeLists.txt | 1 + libraries/GPS_MTK/CMakeLists.txt | 1 + libraries/GPS_NMEA/CMakeLists.txt | 1 + libraries/GPS_UBLOX/CMakeLists.txt | 1 + libraries/ModeFilter/CMakeLists.txt | 25 +------ libraries/PID/CMakeLists.txt | 22 +----- libraries/RC_Channel/CMakeLists.txt | 25 +------ libraries/Trig_LUT/CMakeLists.txt | 1 + libraries/Waypoints/CMakeLists.txt | 1 + libraries/memcheck/CMakeLists.txt | 25 +------ 44 files changed, 192 insertions(+), 617 deletions(-) create mode 100644 ArduCopter/.gitignore create mode 100644 ArduPlane/.gitignore create mode 100644 apo/.gitignore create mode 100644 cmake/modules/ApmMakeLib.cmake create mode 100644 cmake/modules/ApmMakeSketch.cmake create mode 100644 libraries/APM_PerfMon/CMakeLists.txt create mode 100644 libraries/AP_EEPROMB/CMakeLists.txt create mode 100644 libraries/AP_Navigation/CMakeLists.txt create mode 100644 libraries/AP_RC/CMakeLists.txt create mode 100644 libraries/AP_RC_Channel/CMakeLists.txt create mode 100644 libraries/GCS_SIMPLE/CMakeLists.txt create mode 100644 libraries/GPS_IMU/CMakeLists.txt create mode 100644 libraries/GPS_MTK/CMakeLists.txt create mode 100644 libraries/GPS_NMEA/CMakeLists.txt create mode 100644 libraries/GPS_UBLOX/CMakeLists.txt create mode 100644 libraries/Trig_LUT/CMakeLists.txt create mode 100644 libraries/Waypoints/CMakeLists.txt diff --git a/.gitignore b/.gitignore index ebd471e7e0..7028573613 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,4 @@ serialsent.raw CMakeFiles CMakeCache.txt cmake_install.cmake +build diff --git a/ArduCopter/.gitignore b/ArduCopter/.gitignore new file mode 100644 index 0000000000..05fd67446c --- /dev/null +++ b/ArduCopter/.gitignore @@ -0,0 +1 @@ +arducopter.cpp diff --git a/ArduCopter/CMakeLists.txt b/ArduCopter/CMakeLists.txt index f13af158d8..78dedcb342 100644 --- a/ArduCopter/CMakeLists.txt +++ b/ArduCopter/CMakeLists.txt @@ -126,8 +126,6 @@ set(${FIRMWARE_NAME}_LIBS c m ) -SET_TARGET_PROPERTIES(AP_Math PROPERTIES LINKER_LANGUAGE CXX) - #${CONSOLE_PORT} set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port diff --git a/ArduPlane/.gitignore b/ArduPlane/.gitignore new file mode 100644 index 0000000000..733b1fe2ff --- /dev/null +++ b/ArduPlane/.gitignore @@ -0,0 +1 @@ +ArduPlane.cpp diff --git a/ArduPlane/CMakeLists.txt b/ArduPlane/CMakeLists.txt index 3b6d496b97..33bddb959a 100644 --- a/ArduPlane/CMakeLists.txt +++ b/ArduPlane/CMakeLists.txt @@ -126,8 +126,6 @@ set(${FIRMWARE_NAME}_LIBS c m ) -SET_TARGET_PROPERTIES(AP_Math PROPERTIES LINKER_LANGUAGE CXX) - #${CONSOLE_PORT} set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port diff --git a/apo/.gitignore b/apo/.gitignore new file mode 100644 index 0000000000..e31d5d5b64 --- /dev/null +++ b/apo/.gitignore @@ -0,0 +1 @@ +apo.cpp diff --git a/apo/CMakeLists.txt b/apo/CMakeLists.txt index f08d48c262..6471926e01 100644 --- a/apo/CMakeLists.txt +++ b/apo/CMakeLists.txt @@ -1,32 +1,52 @@ -#=============================================================================# -# Author: Sebastian Rohde # -# Date: 30.08.2011 # -#=============================================================================# +cmake_minimum_required(VERSION 2.6) +set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../") +set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) -#====================================================================# -# Settings # -#====================================================================# -set(FIRMWARE_NAME apo) +string(REGEX REPLACE ".*/" "" PROJECT_NAME ${CMAKE_CURRENT_SOURCE_DIR}) +project(${PROJECT_NAME} C CXX) +set(FIRMWARE_NAME ${PROJECT_NAME}) -set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board +set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde) -set(${FIRMWARE_NAME}_SKETCHES - apo.pde - ) # Firmware sketches +find_package(Arduino 22 REQUIRED) + +if (NOT DEFINED BOARD) + message(STATUS "board not defined, assuming mega, use cmake -DBOARD=mega2560 , etc. to specify") + set(BOARD "mega") +endif() +message(STATUS "Board configured as: ${BOARD}") + +# need to configure based on host operating system +set(${PROJECT_NAME}_PORT COM2) +set(${PROJECT_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) -set(${FIRMWARE_NAME}_SRCS - ) # Firmware sources - -set(${FIRMWARE_NAME}_HDRS - ControllerPlane.h - ControllerQuad.h - PlaneEasystar.h - QuadArducopter.h - QuadMikrokopter.h - ) # Firmware sources +include_directories( +${ARDUINO_LIBRARIES_PATH}/Wire +${CMAKE_SOURCE_DIR}/libraries/APO +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +) -set(${FIRMWARE_NAME}_LIBS +add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs") +file(WRITE ${PROJECT_NAME}.cpp "// Do not edit") +set(${PROJECT_NAME}_BOARD ${BOARD}) +file(GLOB ${PROJECT_NAME}_SKETCHES *.pde) +file(GLOB ${PROJECT_NAME}_SRCS *.cpp) +file(GLOB ${PROJECT_NAME}_HDRS *.h) +set(${PROJECT_NAME}_LIBS + c m APO FastSerial @@ -42,33 +62,10 @@ set(${FIRMWARE_NAME}_LIBS APM_BMP085 ModeFilter ) - -#${CONSOLE_PORT} -set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port -set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd - -include_directories( -${CMAKE_SOURCE_DIR}/libraries/APO -${CMAKE_SOURCE_DIR}/libraries/AP_Common -${CMAKE_SOURCE_DIR}/libraries/FastSerial -${CMAKE_SOURCE_DIR}/libraries/ModeFilter -${CMAKE_SOURCE_DIR}/libraries/AP_Compass -${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder -${CMAKE_SOURCE_DIR}/libraries/AP_GPS -${CMAKE_SOURCE_DIR}/libraries/AP_IMU -${CMAKE_SOURCE_DIR}/libraries/AP_ADC -${CMAKE_SOURCE_DIR}/libraries/AP_DCM -${CMAKE_SOURCE_DIR}/libraries/APM_RC -${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink -${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 -) -#====================================================================# -# Target generation # -#====================================================================# -generate_arduino_firmware(${FIRMWARE_NAME}) +generate_arduino_firmware(${PROJECT_NAME}) install(FILES - ${CMAKE_CURRENT_BINARY_DIR}/${FIRMWARE_NAME}.hex + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.hex DESTINATION bin ) diff --git a/apo/apo.pde b/apo/apo.pde index 233b9513ee..44a1edf0ce 100644 --- a/apo/apo.pde +++ b/apo/apo.pde @@ -1,4 +1,5 @@ // Libraries +#include #include #include #include diff --git a/cmake/modules/ApmMakeLib.cmake b/cmake/modules/ApmMakeLib.cmake new file mode 100644 index 0000000000..4077c0f7f3 --- /dev/null +++ b/cmake/modules/ApmMakeLib.cmake @@ -0,0 +1,7 @@ +string(REGEX REPLACE ".*/" "" LIB_NAME ${CMAKE_CURRENT_SOURCE_DIR}) +#message(STATUS "building lib: ${LIB_NAME}") +file(GLOB ${LIB_NAME}_SRCS *.cpp) +file(GLOB ${LIB_NAME}_HDRS *.h) +set(${LIB_NAME}_BOARD ${BOARD}) +generate_arduino_library(${LIB_NAME}) +set_target_properties(${LIB_NAME} PROPERTIES LINKER_LANGUAGE CXX) diff --git a/cmake/modules/ApmMakeSketch.cmake b/cmake/modules/ApmMakeSketch.cmake new file mode 100644 index 0000000000..e5db60aa11 --- /dev/null +++ b/cmake/modules/ApmMakeSketch.cmake @@ -0,0 +1,74 @@ +cmake_minimum_required(VERSION 2.6) + +set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../") +set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) + +string(REGEX REPLACE ".*/" "" PROJECT_NAME ${CMAKE_CURRENT_SOURCE_DIR}) +project(${PROJECT_NAME} C CXX) + +set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde) + +find_package(Arduino 22 REQUIRED) + +if (NOT DEFINED BOARD) + message(STATUS "board not defined, assuming mega, use cmake -DBOARD=mega2560 , etc. to specify") + set(BOARD "mega") +endif() +message(STATUS "Board configured as: ${BOARD}") + +# need to configure based on host operating system +set(${PROJECT_NAME}_PORT COM2) +set(${PROJECT_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) + +include_directories( +${ARDUINO_LIBRARIES_PATH}/Wire +${CMAKE_SOURCE_DIR}/libraries/APO +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +) + +add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs") + +set(${PROJECT_NAME}_BOARD ${BOARD}) +file(GLOB ${PROJECT_NAME}_SKETCHES *.pde) +file(GLOB ${PROJECT_NAME}_SRCS *.cpp) +file(GLOB ${PROJECT_NAME}_HDRS *.h) +set(${PROJECT_NAME}_LIBS + c + m + APO + FastSerial + AP_Common + GCS_MAVLink + AP_GPS + APM_RC + AP_DCM + AP_ADC + AP_Compass + AP_IMU + AP_RangeFinder + APM_BMP085 + ModeFilter +) + + + + + +generate_arduino_firmware(${PROJECT_NAME}) + +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.hex + DESTINATION bin + ) diff --git a/libraries/APM_BMP085/CMakeLists.txt b/libraries/APM_BMP085/CMakeLists.txt index 77b83a3b66..03e7091ff3 100644 --- a/libraries/APM_BMP085/CMakeLists.txt +++ b/libraries/APM_BMP085/CMakeLists.txt @@ -1,22 +1 @@ -set(LIB_NAME APM_BMP085) - -set(${LIB_NAME}_SRCS - APM_BMP085.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - APM_BMP085.h - ) - -include_directories( - - - - #${CMAKE_SOURCE_DIR}/libraries/AP_Math - #${CMAKE_SOURCE_DIR}/libraries/AP_Common - #${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/APM_PI/CMakeLists.txt b/libraries/APM_PI/CMakeLists.txt index 3ccdd81ce6..03e7091ff3 100644 --- a/libraries/APM_PI/CMakeLists.txt +++ b/libraries/APM_PI/CMakeLists.txt @@ -1,24 +1 @@ -set(LIB_NAME APM_PI) - -set(${LIB_NAME}_SRCS - APM_PI.cpp - #AP_OpticalFlow_ADNS3080.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - APM_PI.h - #AP_OpticalFlow_ADNS3080.h - ) - -include_directories( - - - - #${CMAKE_SOURCE_DIR}/libraries/AP_Math - ${CMAKE_SOURCE_DIR}/libraries/AP_Common - #${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/APM_PerfMon/CMakeLists.txt b/libraries/APM_PerfMon/CMakeLists.txt new file mode 100644 index 0000000000..03e7091ff3 --- /dev/null +++ b/libraries/APM_PerfMon/CMakeLists.txt @@ -0,0 +1 @@ +include(ApmMakeLib) diff --git a/libraries/APM_RC/CMakeLists.txt b/libraries/APM_RC/CMakeLists.txt index 4c3748159f..03e7091ff3 100644 --- a/libraries/APM_RC/CMakeLists.txt +++ b/libraries/APM_RC/CMakeLists.txt @@ -1,22 +1 @@ -set(LIB_NAME APM_RC) - -set(${LIB_NAME}_SRCS - APM_RC.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - APM_RC.h - ) - -include_directories( - - - - #${CMAKE_SOURCE_DIR}/libraries/AP_Math - #${CMAKE_SOURCE_DIR}/libraries/AP_Common - #${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/APO/CMakeLists.txt b/libraries/APO/CMakeLists.txt index ee964b337b..03e7091ff3 100644 --- a/libraries/APO/CMakeLists.txt +++ b/libraries/APO/CMakeLists.txt @@ -1,39 +1 @@ -set(LIB_NAME APO) - -set(${LIB_NAME}_SRCS - AP_Autopilot.cpp - AP_CommLink.cpp - AP_Controller.cpp - AP_Guide.cpp - AP_HardwareAbstractionLayer.cpp - AP_MavlinkCommand.cpp - AP_Navigator.cpp - AP_RcChannel.cpp - APO.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - AP_Autopilot.h - AP_CommLink.h - AP_Controller.h - AP_Guide.h - AP_HardwareAbstractionLayer.h - AP_MavlinkCommand.h - AP_Navigator.h - AP_RcChannel.h - AP_Var_keys.h - APO.h - constants.h - template.h -) - -include_directories( -# ${CMAKE_SOURCE_DIR}/libraries/AP_Common - ${CMAKE_SOURCE_DIR}/libraries/FastSerial - ${CMAKE_SOURCE_DIR}/libraries/ModeFilter -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/AP_ADC/CMakeLists.txt b/libraries/AP_ADC/CMakeLists.txt index 4c72c51e9f..03e7091ff3 100644 --- a/libraries/AP_ADC/CMakeLists.txt +++ b/libraries/AP_ADC/CMakeLists.txt @@ -1,26 +1 @@ -set(LIB_NAME AP_ADC) - -set(${LIB_NAME}_SRCS - AP_ADC_HIL.cpp - AP_ADC_ADS7844.cpp - AP_ADC.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - AP_ADC_HIL.h - AP_ADC_ADS7844.h - AP_ADC.h - ) - -include_directories( - - - - #${CMAKE_SOURCE_DIR}/libraries/AP_Math - #${CMAKE_SOURCE_DIR}/libraries/AP_Common - #${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/AP_Common/CMakeLists.txt b/libraries/AP_Common/CMakeLists.txt index d86ce08985..03e7091ff3 100644 --- a/libraries/AP_Common/CMakeLists.txt +++ b/libraries/AP_Common/CMakeLists.txt @@ -1,33 +1 @@ -set(LIB_NAME AP_Common) - -set(${LIB_NAME}_SRCS - AP_Common.cpp - AP_Loop.cpp - AP_MetaClass.cpp - AP_Var.cpp - AP_Var_menufuncs.cpp - c++.cpp - menu.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - AP_Common.h - AP_Loop.h - AP_MetaClass.h - AP_Var.h - AP_Test.h - c++.h - AP_Vector.h -) - -include_directories( - - - - ${CMAKE_SOURCE_DIR}/libraries/AP_Common - ${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/AP_Compass/CMakeLists.txt b/libraries/AP_Compass/CMakeLists.txt index e12ac20fdb..03e7091ff3 100644 --- a/libraries/AP_Compass/CMakeLists.txt +++ b/libraries/AP_Compass/CMakeLists.txt @@ -1,27 +1 @@ -set(LIB_NAME AP_Compass) - -set(${LIB_NAME}_SRCS - AP_Compass_HIL.cpp - AP_Compass_HMC5843.cpp - Compass.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - Compass.h - AP_Compass_HIL.h - AP_Compass_HMC5843.h - AP_Compass.h - ) - - - -include_directories( - - - - ${ARDUINO_LIBRARIES_PATH}/Wire - #${CMAKE_SOURCE_DIR}/libraries/FastSerial - # -) -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/AP_DCM/CMakeLists.txt b/libraries/AP_DCM/CMakeLists.txt index 2f0ecd2f78..03e7091ff3 100644 --- a/libraries/AP_DCM/CMakeLists.txt +++ b/libraries/AP_DCM/CMakeLists.txt @@ -1,25 +1 @@ -set(LIB_NAME AP_DCM) - -set(${LIB_NAME}_SRCS - AP_DCM.cpp - AP_DCM_HIL.cpp - #AP_OpticalFlow_ADNS3080.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - AP_DCM.h - AP_DCM_HIL.h - ) - -include_directories( - - - - ${CMAKE_SOURCE_DIR}/libraries/AP_DCM - #${CMAKE_SOURCE_DIR}/libraries/AP_Common - #${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/AP_EEPROMB/CMakeLists.txt b/libraries/AP_EEPROMB/CMakeLists.txt new file mode 100644 index 0000000000..03e7091ff3 --- /dev/null +++ b/libraries/AP_EEPROMB/CMakeLists.txt @@ -0,0 +1 @@ +include(ApmMakeLib) diff --git a/libraries/AP_GPS/CMakeLists.txt b/libraries/AP_GPS/CMakeLists.txt index 56570d2c9c..03e7091ff3 100644 --- a/libraries/AP_GPS/CMakeLists.txt +++ b/libraries/AP_GPS/CMakeLists.txt @@ -1,44 +1 @@ -set(LIB_NAME AP_GPS) - -set(${LIB_NAME}_SRCS - AP_GPS_406.cpp - AP_GPS_Auto.cpp - AP_GPS_HIL.cpp - AP_GPS_IMU.cpp - AP_GPS_MTK.cpp - AP_GPS_MTK16.cpp - AP_GPS_NMEA.cpp - AP_GPS_SIRF.cpp - AP_GPS_UBLOX.cpp - GPS.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - AP_GPS_406.h - AP_GPS_Auto.h - AP_GPS_HIL.h - AP_GPS_IMU.h - AP_GPS_MTK.h - AP_GPS_MTK_Common.h - AP_GPS_MTK16.h - AP_GPS_NMEA.h - AP_GPS_None.h - AP_GPS_Shim.h - AP_GPS_SIRF.h - AP_GPS_UBLOX.h - AP_GPS.h - GPS.h - ) - -include_directories( - - - - #${CMAKE_SOURCE_DIR}/libraries/AP_Math - ${CMAKE_SOURCE_DIR}/libraries/AP_Common - ${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/AP_IMU/CMakeLists.txt b/libraries/AP_IMU/CMakeLists.txt index 4685ea4511..03e7091ff3 100644 --- a/libraries/AP_IMU/CMakeLists.txt +++ b/libraries/AP_IMU/CMakeLists.txt @@ -1,25 +1 @@ -set(LIB_NAME AP_IMU) - -set(${LIB_NAME}_SRCS - AP_IMU_Oilpan.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - AP_IMU.h - AP_IMU_Shim.h - AP_IMU_Oilpan.h - IMU.h - ) - -include_directories( - - - - #${CMAKE_SOURCE_DIR}/libraries/AP_Math - ${CMAKE_SOURCE_DIR}/libraries/AP_Common - ${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/AP_Math/CMakeLists.txt b/libraries/AP_Math/CMakeLists.txt index 262bbfd8cc..03e7091ff3 100644 --- a/libraries/AP_Math/CMakeLists.txt +++ b/libraries/AP_Math/CMakeLists.txt @@ -1,24 +1 @@ -set(LIB_NAME AP_Math) - -set(${LIB_NAME}_SRCS - - ) # Firmware sources - -set(${LIB_NAME}_HDRS - AP_Math.h - matrix3.h - vector2.h - vector3.h - ) - -include_directories( - - - -# ${CMAKE_SOURCE_DIR}/libraries/AP_Common - #${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/AP_Navigation/CMakeLists.txt b/libraries/AP_Navigation/CMakeLists.txt new file mode 100644 index 0000000000..03e7091ff3 --- /dev/null +++ b/libraries/AP_Navigation/CMakeLists.txt @@ -0,0 +1 @@ +include(ApmMakeLib) diff --git a/libraries/AP_OpticalFlow/CMakeLists.txt b/libraries/AP_OpticalFlow/CMakeLists.txt index 117d84c00d..03e7091ff3 100644 --- a/libraries/AP_OpticalFlow/CMakeLists.txt +++ b/libraries/AP_OpticalFlow/CMakeLists.txt @@ -1,24 +1 @@ -set(LIB_NAME AP_OpticalFlow) - -set(${LIB_NAME}_SRCS - AP_OpticalFlow.cpp - #AP_OpticalFlow_ADNS3080.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - AP_OpticalFlow.h - #AP_OpticalFlow_ADNS3080.h - ) - -include_directories( - - - - ${CMAKE_SOURCE_DIR}/libraries/AP_Math - #${CMAKE_SOURCE_DIR}/libraries/AP_Common - #${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/AP_PID/CMakeLists.txt b/libraries/AP_PID/CMakeLists.txt index c38fbea5d3..03e7091ff3 100644 --- a/libraries/AP_PID/CMakeLists.txt +++ b/libraries/AP_PID/CMakeLists.txt @@ -1,24 +1 @@ -set(LIB_NAME AP_PID) - -set(${LIB_NAME}_SRCS - AP_PID.cpp - #AP_OpticalFlow_ADNS3080.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - AP_PID.h - #AP_OpticalFlow_ADNS3080.h - ) - -include_directories( - - - - #${CMAKE_SOURCE_DIR}/libraries/AP_Math - #${CMAKE_SOURCE_DIR}/libraries/AP_Common - #${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/AP_RC/CMakeLists.txt b/libraries/AP_RC/CMakeLists.txt new file mode 100644 index 0000000000..03e7091ff3 --- /dev/null +++ b/libraries/AP_RC/CMakeLists.txt @@ -0,0 +1 @@ +include(ApmMakeLib) diff --git a/libraries/AP_RC_Channel/CMakeLists.txt b/libraries/AP_RC_Channel/CMakeLists.txt new file mode 100644 index 0000000000..03e7091ff3 --- /dev/null +++ b/libraries/AP_RC_Channel/CMakeLists.txt @@ -0,0 +1 @@ +include(ApmMakeLib) diff --git a/libraries/AP_RangeFinder/CMakeLists.txt b/libraries/AP_RangeFinder/CMakeLists.txt index 05b5750e33..03e7091ff3 100644 --- a/libraries/AP_RangeFinder/CMakeLists.txt +++ b/libraries/AP_RangeFinder/CMakeLists.txt @@ -1,27 +1 @@ -set(LIB_NAME AP_RangeFinder) - -set(${LIB_NAME}_SRCS - AP_RangeFinder_MaxsonarXL.cpp - AP_RangeFinder_SharpGP2Y.cpp - RangeFinder.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - AP_RangeFinder.h - AP_RangeFinder_MaxsonarXL.h - AP_RangeFinder_SharpGP2Y.h - RangeFinder.h - ) - -include_directories( - - - - #${CMAKE_SOURCE_DIR}/libraries/AP_Math - #${CMAKE_SOURCE_DIR}/libraries/AP_Common - #${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/CMakeLists.txt b/libraries/CMakeLists.txt index ed27448a23..0d8ac42886 100644 --- a/libraries/CMakeLists.txt +++ b/libraries/CMakeLists.txt @@ -1,24 +1,34 @@ -add_subdirectory(DataFlash) -add_subdirectory(AP_Math) -add_subdirectory(PID) -add_subdirectory(AP_Common) -add_subdirectory(RC_Channel) -add_subdirectory(AP_OpticalFlow) -add_subdirectory(ModeFilter) -add_subdirectory(memcheck) -add_subdirectory(APM_PI) -add_subdirectory(AP_GPS) -add_subdirectory(AP_DCM) -add_subdirectory(AP_Compass) add_subdirectory(AP_ADC) +add_subdirectory(AP_Common) +add_subdirectory(AP_Compass) +add_subdirectory(AP_DCM) +add_subdirectory(AP_EEPROMB) +add_subdirectory(AP_GPS) add_subdirectory(AP_IMU) -add_subdirectory(AP_RangeFinder) - -add_subdirectory(APM_RC) +add_subdirectory(AP_Math) add_subdirectory(APM_BMP085) - -#add_subdirectory(APO) +add_subdirectory(APM_PerfMon) +add_subdirectory(APM_PI) +add_subdirectory(APM_RC) +add_subdirectory(AP_Navigation) +add_subdirectory(APO) +add_subdirectory(AP_OpticalFlow) +add_subdirectory(AP_PID) +add_subdirectory(AP_RangeFinder) +add_subdirectory(AP_RC) +add_subdirectory(AP_RC_Channel) +add_subdirectory(DataFlash) add_subdirectory(FastSerial) add_subdirectory(GCS_MAVLink) +add_subdirectory(GCS_SIMPLE) +add_subdirectory(GPS_IMU) +add_subdirectory(GPS_MTK) +add_subdirectory(GPS_NMEA) +add_subdirectory(GPS_UBLOX) +add_subdirectory(memcheck) +add_subdirectory(ModeFilter) +add_subdirectory(PID) +add_subdirectory(RC_Channel) +add_subdirectory(Waypoints) +add_subdirectory(Trig_LUT) -#add_subdirectory(playgroundlib) diff --git a/libraries/DataFlash/CMakeLists.txt b/libraries/DataFlash/CMakeLists.txt index d36f3fa8a7..03e7091ff3 100644 --- a/libraries/DataFlash/CMakeLists.txt +++ b/libraries/DataFlash/CMakeLists.txt @@ -1,21 +1 @@ -set(LIB_NAME DataFlash) - -set(${LIB_NAME}_SRCS - DataFlash.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - DataFlash.h - ) - -include_directories( - - - -# ${CMAKE_SOURCE_DIR}/libraries/AP_Common - #${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/FastSerial/CMakeLists.txt b/libraries/FastSerial/CMakeLists.txt index 1ace8cb81c..03e7091ff3 100644 --- a/libraries/FastSerial/CMakeLists.txt +++ b/libraries/FastSerial/CMakeLists.txt @@ -1,28 +1 @@ -set(LIB_NAME FastSerial) - -set(${LIB_NAME}_SRCS - BetterStream.cpp - FastSerial.cpp - vprintf.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - BetterStream.h - FastSerial.h - ftoa_engine.h - ntz.h - xtoa_fast.h - ) - -include_directories( - - - - #${CMAKE_SOURCE_DIR}/libraries/AP_Math - #${CMAKE_SOURCE_DIR}/libraries/AP_Common - #${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/GCS_MAVLink/CMakeLists.txt b/libraries/GCS_MAVLink/CMakeLists.txt index fcd653fead..03e7091ff3 100644 --- a/libraries/GCS_MAVLink/CMakeLists.txt +++ b/libraries/GCS_MAVLink/CMakeLists.txt @@ -1,19 +1 @@ -set(LIB_NAME GCS_MAVLink) - -set(${LIB_NAME}_SRCS - GCS_MAVLink.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - GCS_MAVLink.h -) - -include_directories( - #${CMAKE_SOURCE_DIR}/libraries/ - ${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/GCS_SIMPLE/CMakeLists.txt b/libraries/GCS_SIMPLE/CMakeLists.txt new file mode 100644 index 0000000000..03e7091ff3 --- /dev/null +++ b/libraries/GCS_SIMPLE/CMakeLists.txt @@ -0,0 +1 @@ +include(ApmMakeLib) diff --git a/libraries/GPS_IMU/CMakeLists.txt b/libraries/GPS_IMU/CMakeLists.txt new file mode 100644 index 0000000000..03e7091ff3 --- /dev/null +++ b/libraries/GPS_IMU/CMakeLists.txt @@ -0,0 +1 @@ +include(ApmMakeLib) diff --git a/libraries/GPS_MTK/CMakeLists.txt b/libraries/GPS_MTK/CMakeLists.txt new file mode 100644 index 0000000000..03e7091ff3 --- /dev/null +++ b/libraries/GPS_MTK/CMakeLists.txt @@ -0,0 +1 @@ +include(ApmMakeLib) diff --git a/libraries/GPS_NMEA/CMakeLists.txt b/libraries/GPS_NMEA/CMakeLists.txt new file mode 100644 index 0000000000..03e7091ff3 --- /dev/null +++ b/libraries/GPS_NMEA/CMakeLists.txt @@ -0,0 +1 @@ +include(ApmMakeLib) diff --git a/libraries/GPS_UBLOX/CMakeLists.txt b/libraries/GPS_UBLOX/CMakeLists.txt new file mode 100644 index 0000000000..03e7091ff3 --- /dev/null +++ b/libraries/GPS_UBLOX/CMakeLists.txt @@ -0,0 +1 @@ +include(ApmMakeLib) diff --git a/libraries/ModeFilter/CMakeLists.txt b/libraries/ModeFilter/CMakeLists.txt index cb0c8eb997..03e7091ff3 100644 --- a/libraries/ModeFilter/CMakeLists.txt +++ b/libraries/ModeFilter/CMakeLists.txt @@ -1,24 +1 @@ -set(LIB_NAME ModeFilter) - -set(${LIB_NAME}_SRCS - ModeFilter.cpp - #AP_OpticalFlow_ADNS3080.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - ModeFilter.h - #AP_OpticalFlow_ADNS3080.h - ) - -include_directories( - - - - #${CMAKE_SOURCE_DIR}/libraries/AP_Math - #${CMAKE_SOURCE_DIR}/libraries/AP_Common - #${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/PID/CMakeLists.txt b/libraries/PID/CMakeLists.txt index 6233d35eb3..03e7091ff3 100644 --- a/libraries/PID/CMakeLists.txt +++ b/libraries/PID/CMakeLists.txt @@ -1,21 +1 @@ -set(LIB_NAME PID) - -set(${LIB_NAME}_SRCS - PID.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - PID.h - ) - -include_directories( - - - - ${CMAKE_SOURCE_DIR}/libraries/AP_Common - #${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/RC_Channel/CMakeLists.txt b/libraries/RC_Channel/CMakeLists.txt index 89ff6b9278..03e7091ff3 100644 --- a/libraries/RC_Channel/CMakeLists.txt +++ b/libraries/RC_Channel/CMakeLists.txt @@ -1,24 +1 @@ -set(LIB_NAME RC_Channel) - -set(${LIB_NAME}_SRCS - RC_Channel.cpp - RC_Channel_aux.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - RC_Channel.h - RC_Channel_aux.h - ) - -include_directories( - - - - ${CMAKE_SOURCE_DIR}/libraries/AP_Common - ${CMAKE_SOURCE_DIR}/libraries/APM_RC - #${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) diff --git a/libraries/Trig_LUT/CMakeLists.txt b/libraries/Trig_LUT/CMakeLists.txt new file mode 100644 index 0000000000..03e7091ff3 --- /dev/null +++ b/libraries/Trig_LUT/CMakeLists.txt @@ -0,0 +1 @@ +include(ApmMakeLib) diff --git a/libraries/Waypoints/CMakeLists.txt b/libraries/Waypoints/CMakeLists.txt new file mode 100644 index 0000000000..03e7091ff3 --- /dev/null +++ b/libraries/Waypoints/CMakeLists.txt @@ -0,0 +1 @@ +include(ApmMakeLib) diff --git a/libraries/memcheck/CMakeLists.txt b/libraries/memcheck/CMakeLists.txt index a5c1575256..03e7091ff3 100644 --- a/libraries/memcheck/CMakeLists.txt +++ b/libraries/memcheck/CMakeLists.txt @@ -1,24 +1 @@ -set(LIB_NAME memcheck) - -set(${LIB_NAME}_SRCS - memcheck.cpp - #AP_OpticalFlow_ADNS3080.cpp - ) # Firmware sources - -set(${LIB_NAME}_HDRS - memcheck.h - #AP_OpticalFlow_ADNS3080.h - ) - -include_directories( - - - - #${CMAKE_SOURCE_DIR}/libraries/AP_Math - ${CMAKE_SOURCE_DIR}/libraries/AP_Common - #${CMAKE_SOURCE_DIR}/libraries/FastSerial -# - ) - -set(${LIB_NAME}_BOARD ${BOARD}) - -generate_arduino_library(${LIB_NAME}) +include(ApmMakeLib) From 9a2da8f22f70121c796994b450e83af00dad6844 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 29 Sep 2011 22:10:26 -0400 Subject: [PATCH 7/7] Arduino version directory updated to lib/version.txt Also updated cmake files. --- ArduBoat/CMakeLists.txt | 89 +++++++++++++++---------------- ArduRover/CMakeLists.txt | 89 +++++++++++++++---------------- apo/CMakeLists.txt | 8 ++- apo/apo.pde | 2 +- cmake/modules/ApmMakeSketch.cmake | 6 --- libraries/AP_Common/Arduino.mk | 2 +- 6 files changed, 91 insertions(+), 105 deletions(-) diff --git a/ArduBoat/CMakeLists.txt b/ArduBoat/CMakeLists.txt index 4686c63bd9..aa3fe000f0 100644 --- a/ArduBoat/CMakeLists.txt +++ b/ArduBoat/CMakeLists.txt @@ -1,30 +1,50 @@ -#=============================================================================# -# Author: Sebastian Rohde # -# Date: 30.08.2011 # -#=============================================================================# +cmake_minimum_required(VERSION 2.6) +set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../") +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain +set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) # CMake module search path +string(REGEX REPLACE ".*/" "" PROJECT_NAME ${CMAKE_CURRENT_SOURCE_DIR}) +project(${PROJECT_NAME} C CXX) -#====================================================================# -# Settings # -#====================================================================# -set(FIRMWARE_NAME ArduBoat) +set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde) -set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board +find_package(Arduino 22 REQUIRED) -set(${FIRMWARE_NAME}_SKETCHES - ArduBoat.pde - ) # Firmware sketches +if (NOT DEFINED BOARD) + message(STATUS "board not defined, assuming mega, use cmake -DBOARD=mega2560 , etc. to specify") + set(BOARD "mega") +endif() +message(STATUS "Board configured as: ${BOARD}") + +# need to configure based on host operating system +set(${PROJECT_NAME}_PORT COM2) +set(${PROJECT_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) -set(${FIRMWARE_NAME}_SRCS - ) # Firmware sources - -set(${FIRMWARE_NAME}_HDRS - BoatGeneric.h - #CNIBoat.h - ControllerBoat.h - ) # Firmware sources +include_directories( +${ARDUINO_LIBRARIES_PATH}/Wire +${CMAKE_SOURCE_DIR}/libraries/APO +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +) -set(${FIRMWARE_NAME}_LIBS +add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs") + +set(${PROJECT_NAME}_BOARD ${BOARD}) +file(GLOB ${PROJECT_NAME}_SKETCHES *.pde) +file(GLOB ${PROJECT_NAME}_SRCS *.cpp) +file(GLOB ${PROJECT_NAME}_HDRS *.h) +set(${PROJECT_NAME}_LIBS + c m APO FastSerial @@ -40,33 +60,10 @@ set(${FIRMWARE_NAME}_LIBS APM_BMP085 ModeFilter ) - -#${CONSOLE_PORT} -set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port -set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd - -include_directories( -${CMAKE_SOURCE_DIR}/libraries/APO -${CMAKE_SOURCE_DIR}/libraries/AP_Common -${CMAKE_SOURCE_DIR}/libraries/FastSerial -${CMAKE_SOURCE_DIR}/libraries/ModeFilter -${CMAKE_SOURCE_DIR}/libraries/AP_Compass -${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder -${CMAKE_SOURCE_DIR}/libraries/AP_GPS -${CMAKE_SOURCE_DIR}/libraries/AP_IMU -${CMAKE_SOURCE_DIR}/libraries/AP_ADC -${CMAKE_SOURCE_DIR}/libraries/AP_DCM -${CMAKE_SOURCE_DIR}/libraries/APM_RC -${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink -${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 -) -#====================================================================# -# Target generation # -#====================================================================# -generate_arduino_firmware(${FIRMWARE_NAME}) +generate_arduino_firmware(${PROJECT_NAME}) install(FILES - ${CMAKE_CURRENT_BINARY_DIR}/${FIRMWARE_NAME}.hex + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.hex DESTINATION bin ) diff --git a/ArduRover/CMakeLists.txt b/ArduRover/CMakeLists.txt index 7e5ba77a24..aa3fe000f0 100644 --- a/ArduRover/CMakeLists.txt +++ b/ArduRover/CMakeLists.txt @@ -1,30 +1,50 @@ -#=============================================================================# -# Author: Sebastian Rohde # -# Date: 30.08.2011 # -#=============================================================================# +cmake_minimum_required(VERSION 2.6) +set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../") +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain +set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) # CMake module search path +string(REGEX REPLACE ".*/" "" PROJECT_NAME ${CMAKE_CURRENT_SOURCE_DIR}) +project(${PROJECT_NAME} C CXX) -#====================================================================# -# Settings # -#====================================================================# -set(FIRMWARE_NAME ArduRover) +set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde) -set(${FIRMWARE_NAME}_BOARD ${BOARD}) # Arduino Target board +find_package(Arduino 22 REQUIRED) -set(${FIRMWARE_NAME}_SKETCHES - ArduRover.pde - ) # Firmware sketches +if (NOT DEFINED BOARD) + message(STATUS "board not defined, assuming mega, use cmake -DBOARD=mega2560 , etc. to specify") + set(BOARD "mega") +endif() +message(STATUS "Board configured as: ${BOARD}") + +# need to configure based on host operating system +set(${PROJECT_NAME}_PORT COM2) +set(${PROJECT_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) -set(${FIRMWARE_NAME}_SRCS - ) # Firmware sources - -set(${FIRMWARE_NAME}_HDRS - CarStampede.h - #CNIRover.h - ControllerCar.h - ) # Firmware sources +include_directories( +${ARDUINO_LIBRARIES_PATH}/Wire +${CMAKE_SOURCE_DIR}/libraries/APO +${CMAKE_SOURCE_DIR}/libraries/AP_Common +${CMAKE_SOURCE_DIR}/libraries/FastSerial +${CMAKE_SOURCE_DIR}/libraries/ModeFilter +${CMAKE_SOURCE_DIR}/libraries/AP_Compass +${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder +${CMAKE_SOURCE_DIR}/libraries/AP_GPS +${CMAKE_SOURCE_DIR}/libraries/AP_IMU +${CMAKE_SOURCE_DIR}/libraries/AP_ADC +${CMAKE_SOURCE_DIR}/libraries/AP_DCM +${CMAKE_SOURCE_DIR}/libraries/APM_RC +${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink +${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 +) -set(${FIRMWARE_NAME}_LIBS +add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs") + +set(${PROJECT_NAME}_BOARD ${BOARD}) +file(GLOB ${PROJECT_NAME}_SKETCHES *.pde) +file(GLOB ${PROJECT_NAME}_SRCS *.cpp) +file(GLOB ${PROJECT_NAME}_HDRS *.h) +set(${PROJECT_NAME}_LIBS + c m APO FastSerial @@ -40,33 +60,10 @@ set(${FIRMWARE_NAME}_LIBS APM_BMP085 ModeFilter ) - -#${CONSOLE_PORT} -set(${FIRMWARE_NAME}_PORT COM2) # Serial upload port -set(${FIRMWARE_NAME}_SERIAL putty -serial COM2 -sercfg 57600,8,n,1,X ) # Serial terminal cmd - -include_directories( -${CMAKE_SOURCE_DIR}/libraries/APO -${CMAKE_SOURCE_DIR}/libraries/AP_Common -${CMAKE_SOURCE_DIR}/libraries/FastSerial -${CMAKE_SOURCE_DIR}/libraries/ModeFilter -${CMAKE_SOURCE_DIR}/libraries/AP_Compass -${CMAKE_SOURCE_DIR}/libraries/AP_RangeFinder -${CMAKE_SOURCE_DIR}/libraries/AP_GPS -${CMAKE_SOURCE_DIR}/libraries/AP_IMU -${CMAKE_SOURCE_DIR}/libraries/AP_ADC -${CMAKE_SOURCE_DIR}/libraries/AP_DCM -${CMAKE_SOURCE_DIR}/libraries/APM_RC -${CMAKE_SOURCE_DIR}/libraries/GCS_MAVLink -${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 -) -#====================================================================# -# Target generation # -#====================================================================# -generate_arduino_firmware(${FIRMWARE_NAME}) +generate_arduino_firmware(${PROJECT_NAME}) install(FILES - ${CMAKE_CURRENT_BINARY_DIR}/${FIRMWARE_NAME}.hex + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.hex DESTINATION bin ) diff --git a/apo/CMakeLists.txt b/apo/CMakeLists.txt index 6471926e01..aa3fe000f0 100644 --- a/apo/CMakeLists.txt +++ b/apo/CMakeLists.txt @@ -1,12 +1,10 @@ cmake_minimum_required(VERSION 2.6) - set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../") -set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) # Arduino Toolchain +set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) # CMake module search path string(REGEX REPLACE ".*/" "" PROJECT_NAME ${CMAKE_CURRENT_SOURCE_DIR}) project(${PROJECT_NAME} C CXX) -set(FIRMWARE_NAME ${PROJECT_NAME}) set (CMAKE_CXX_SOURCE_FILE_EXTENSIONS pde) @@ -40,7 +38,7 @@ ${CMAKE_SOURCE_DIR}/libraries/APM_BMP085 ) add_subdirectory(../libraries "${CMAKE_CURRENT_BINARY_DIR}/libs") -file(WRITE ${PROJECT_NAME}.cpp "// Do not edit") + set(${PROJECT_NAME}_BOARD ${BOARD}) file(GLOB ${PROJECT_NAME}_SKETCHES *.pde) file(GLOB ${PROJECT_NAME}_SRCS *.cpp) diff --git a/apo/apo.pde b/apo/apo.pde index 44a1edf0ce..dd67215691 100644 --- a/apo/apo.pde +++ b/apo/apo.pde @@ -1,5 +1,4 @@ // Libraries -#include #include #include #include @@ -14,6 +13,7 @@ #include #include #include +#include // Vehicle Configuration #include "PlaneEasystar.h" diff --git a/cmake/modules/ApmMakeSketch.cmake b/cmake/modules/ApmMakeSketch.cmake index e5db60aa11..6aabc20ff2 100644 --- a/cmake/modules/ApmMakeSketch.cmake +++ b/cmake/modules/ApmMakeSketch.cmake @@ -1,9 +1,3 @@ -cmake_minimum_required(VERSION 2.6) - -set(CMAKE_SOURCE_DIR "${CMAKE_SOURCE_DIR}/../") -set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Arduino.cmake) - string(REGEX REPLACE ".*/" "" PROJECT_NAME ${CMAKE_CURRENT_SOURCE_DIR}) project(${PROJECT_NAME} C CXX) diff --git a/libraries/AP_Common/Arduino.mk b/libraries/AP_Common/Arduino.mk index 0ff6796b31..8f3e97a0c8 100644 --- a/libraries/AP_Common/Arduino.mk +++ b/libraries/AP_Common/Arduino.mk @@ -305,7 +305,7 @@ LIBOBJS := $(SKETCHLIBOBJS) $(ARDUINOLIBOBJS) # Pull the Arduino version from the revisions.txt file # # XXX can we count on this? If not, what? -ARDUINO_VERS := $(shell expr `head -1 $(ARDUINO)/revisions.txt | cut -d ' ' -f 2`) +ARDUINO_VERS := $(shell expr `head -1 $(ARDUINO)/lib/version.txt | cut -d ' ' -f 2`) # Find the hardware directory to use HARDWARE_DIR := $(firstword $(wildcard $(SKETCHBOOK)/hardware/$(HARDWARE) \