From 71fbdf721ad1ec27338eef822959c98372dfd7b5 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 28 Sep 2011 20:51:12 -0400 Subject: [PATCH] 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<