diff --git a/ArduBoat/BoatGeneric.h b/ArduBoat/BoatGeneric.h index 5e47672847..46c4de4d4f 100644 --- a/ArduBoat/BoatGeneric.h +++ b/ArduBoat/BoatGeneric.h @@ -11,7 +11,7 @@ // 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 apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; static const uint8_t heartBeatTimeout = 3; // algorithm selection @@ -25,29 +25,40 @@ static const uint8_t heartBeatTimeout = 3; #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 + +// baud rates +static uint32_t debugBaud = 57600; +static uint32_t telemBaud = 57600; +static uint32_t gpsBaud = 38400; +static uint32_t hilBaud = 57600; // optional sensors -static bool gpsEnabled = false; -static bool baroEnabled = true; -static bool compassEnabled = true; +static const bool gpsEnabled = false; +static const bool baroEnabled = false; +static const bool compassEnabled = true; +static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; +// compass orientation: See AP_Compass_HMC5843.h for possible values -static bool rangeFinderFrontEnabled = true; -static bool rangeFinderBackEnabled = true; -static bool rangeFinderLeftEnabled = true; -static bool rangeFinderRightEnabled = true; -static bool rangeFinderUpEnabled = true; -static bool rangeFinderDownEnabled = true; +// battery monitoring +static const bool batteryMonitorEnabled = true; +static const uint8_t batteryPin = 0; +static const float batteryVoltageDivRatio = 6; +static const float batteryMinVolt = 10.0; +static const float batteryMaxVolt = 12.4; + +static const bool rangeFinderFrontEnabled = false; +static const bool rangeFinderBackEnabled = false; +static const bool rangeFinderLeftEnabled = false; +static const bool rangeFinderRightEnabled = false; +static const bool rangeFinderUpEnabled = false; +static const bool rangeFinderDownEnabled = false; // loop rates -static const float loop0Rate = 150; -static const float loop1Rate = 100; -static const float loop2Rate = 10; -static const float loop3Rate = 1; -static const float loop4Rate = 0.1; +static const float loopRate = 150; // attitude nav +static const float loop0Rate = 50; // controller +static const float loop1Rate = 5; // pos nav/ gcs fast +static const float loop2Rate = 1; // gcs slow +static const float loop3Rate = 0.1; // gains const float steeringP = 1.0; @@ -55,6 +66,7 @@ const float steeringI = 0.0; const float steeringD = 0.0; const float steeringIMax = 0.0; const float steeringYMax = 3.0; +const float steeringDFCut = 25; const float throttleP = 0.0; const float throttleI = 0.0; diff --git a/ArduBoat/ControllerBoat.h b/ArduBoat/ControllerBoat.h index 4999569acb..c0ac2c7c6f 100644 --- a/ArduBoat/ControllerBoat.h +++ b/ArduBoat/ControllerBoat.h @@ -34,7 +34,7 @@ public: _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), + steeringI, steeringD, steeringIMax, steeringYMax, steeringDFCut), pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP, throttleI, throttleD, throttleIMax, throttleYMax, throttleDFCut) { diff --git a/ArduRover/ArduRover.cpp b/ArduRover/ArduRover.cpp index 57f0903250..4d4a29a733 100644 --- a/ArduRover/ArduRover.cpp +++ b/ArduRover/ArduRover.cpp @@ -1,4 +1,5 @@ // Libraries +#include #include #include #include @@ -13,6 +14,7 @@ #include #include #include +#include // Vehicle Configuration #include "CarStampede.h" diff --git a/ArduRover/ArduRover.pde b/ArduRover/ArduRover.pde index cc1a79aa0c..14846ca904 100644 --- a/ArduRover/ArduRover.pde +++ b/ArduRover/ArduRover.pde @@ -1,4 +1,5 @@ // Libraries +#include #include #include #include @@ -13,9 +14,10 @@ #include #include #include +#include // Vehicle Configuration -#include "TankGeneric.h" +#include "CarStampede.h" // ArduPilotOne Default Setup #include "APO_DefaultSetup.h" diff --git a/ArduRover/CarStampede.h b/ArduRover/CarStampede.h index 07a4fe20e4..dc67d6730a 100644 --- a/ArduRover/CarStampede.h +++ b/ArduRover/CarStampede.h @@ -26,29 +26,40 @@ static const uint8_t heartBeatTimeout = 3; #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 + +// baud rates +static uint32_t debugBaud = 57600; +static uint32_t telemBaud = 57600; +static uint32_t gpsBaud = 38400; +static uint32_t hilBaud = 57600; // optional sensors -static bool gpsEnabled = false; -static bool baroEnabled = true; -static bool compassEnabled = true; +static const bool gpsEnabled = false; +static const bool baroEnabled = false; +static const bool compassEnabled = true; +static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; +// compass orientation: See AP_Compass_HMC5843.h for possible values -static bool rangeFinderFrontEnabled = true; -static bool rangeFinderBackEnabled = true; -static bool rangeFinderLeftEnabled = true; -static bool rangeFinderRightEnabled = true; -static bool rangeFinderUpEnabled = true; -static bool rangeFinderDownEnabled = true; +// battery monitoring +static const bool batteryMonitorEnabled = true; +static const uint8_t batteryPin = 0; +static const float batteryVoltageDivRatio = 6; +static const float batteryMinVolt = 10.0; +static const float batteryMaxVolt = 12.4; + +static const bool rangeFinderFrontEnabled = false; +static const bool rangeFinderBackEnabled = false; +static const bool rangeFinderLeftEnabled = false; +static const bool rangeFinderRightEnabled = false; +static const bool rangeFinderUpEnabled = false; +static const bool rangeFinderDownEnabled = false; // loop rates -static const float loop0Rate = 150; -static const float loop1Rate = 100; -static const float loop2Rate = 10; -static const float loop3Rate = 1; -static const float loop4Rate = 0.1; +static const float loopRate = 150; // attitude nav +static const float loop0Rate = 50; // controller +static const float loop1Rate = 5; // pos nav/ gcs fast +static const float loop2Rate = 1; // gcs slow +static const float loop3Rate = 0.1; // gains static const float steeringP = 1.0; @@ -56,6 +67,7 @@ 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 steeringDFCut = 25; static const float throttleP = 0.0; static const float throttleI = 0.0; diff --git a/ArduRover/ControllerCar.h b/ArduRover/ControllerCar.h index 8bb7e9afb5..686ea64787 100644 --- a/ArduRover/ControllerCar.h +++ b/ArduRover/ControllerCar.h @@ -34,7 +34,7 @@ public: _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), + steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut), pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP, throttleI, throttleD, throttleIMax, throttleYMax, throttleDFCut) { diff --git a/ArduRover/ControllerTank.h b/ArduRover/ControllerTank.h index ad8a294a2c..2e0958569c 100644 --- a/ArduRover/ControllerTank.h +++ b/ArduRover/ControllerTank.h @@ -34,7 +34,7 @@ public: _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), + steeringI, steeringD, steeringIMax, steeringYMax, steeringDFCut), pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP, throttleI, throttleD, throttleIMax, throttleYMax, throttleDFCut) { diff --git a/ArduRover/TankGeneric.h b/ArduRover/TankGeneric.h index 9af4f9b1d4..10e5af72a3 100644 --- a/ArduRover/TankGeneric.h +++ b/ArduRover/TankGeneric.h @@ -25,29 +25,40 @@ static const uint8_t heartBeatTimeout = 3; #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 + +// baud rates +static uint32_t debugBaud = 57600; +static uint32_t telemBaud = 57600; +static uint32_t gpsBaud = 38400; +static uint32_t hilBaud = 57600; // optional sensors -static bool gpsEnabled = false; -static bool baroEnabled = false; -static bool compassEnabled = false; +static const bool gpsEnabled = false; +static const bool baroEnabled = false; +static const bool compassEnabled = true; +static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; +// compass orientation: See AP_Compass_HMC5843.h for possible values -static bool rangeFinderFrontEnabled = false; -static bool rangeFinderBackEnabled = false; -static bool rangeFinderLeftEnabled = false; -static bool rangeFinderRightEnabled = false; -static bool rangeFinderUpEnabled = false; -static bool rangeFinderDownEnabled = false; +// battery monitoring +static const bool batteryMonitorEnabled = true; +static const uint8_t batteryPin = 0; +static const float batteryVoltageDivRatio = 6; +static const float batteryMinVolt = 10.0; +static const float batteryMaxVolt = 12.4; + +static const bool rangeFinderFrontEnabled = false; +static const bool rangeFinderBackEnabled = false; +static const bool rangeFinderLeftEnabled = false; +static const bool rangeFinderRightEnabled = false; +static const bool rangeFinderUpEnabled = false; +static const bool rangeFinderDownEnabled = false; // loop rates -static const float loop0Rate = 150; -static const float loop1Rate = 100; -static const float loop2Rate = 10; -static const float loop3Rate = 1; -static const float loop4Rate = 0.1; +static const float loopRate = 150; // attitude nav +static const float loop0Rate = 50; // controller +static const float loop1Rate = 5; // pos nav/ gcs fast +static const float loop2Rate = 1; // gcs slow +static const float loop3Rate = 0.1; // gains const float steeringP = 1.0; @@ -55,6 +66,7 @@ const float steeringI = 0.0; const float steeringD = 0.0; const float steeringIMax = 0.0; const float steeringYMax = 3.0; +const float steeringDFCut = 25; const float throttleP = 0.0; const float throttleI = 0.0; diff --git a/apo/PlaneEasystar.h b/apo/PlaneEasystar.h index 812c4ad449..87649f48a3 100644 --- a/apo/PlaneEasystar.h +++ b/apo/PlaneEasystar.h @@ -8,12 +8,10 @@ #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 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 @@ -27,29 +25,40 @@ static const uint8_t heartBeatTimeout = 3; #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 + +// baud rates +static uint32_t debugBaud = 57600; +static uint32_t telemBaud = 57600; +static uint32_t gpsBaud = 38400; +static uint32_t hilBaud = 57600; // optional sensors -static bool gpsEnabled = false; -static bool baroEnabled = true; -static bool compassEnabled = true; +static const bool gpsEnabled = false; +static const bool baroEnabled = false; +static const bool compassEnabled = true; +static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD; +// compass orientation: See AP_Compass_HMC5843.h for possible values -static bool rangeFinderFrontEnabled = true; -static bool rangeFinderBackEnabled = true; -static bool rangeFinderLeftEnabled = true; -static bool rangeFinderRightEnabled = true; -static bool rangeFinderUpEnabled = true; -static bool rangeFinderDownEnabled = true; +// battery monitoring +static const bool batteryMonitorEnabled = true; +static const uint8_t batteryPin = 0; +static const float batteryVoltageDivRatio = 6; +static const float batteryMinVolt = 10.0; +static const float batteryMaxVolt = 12.4; + +static const bool rangeFinderFrontEnabled = false; +static const bool rangeFinderBackEnabled = false; +static const bool rangeFinderLeftEnabled = false; +static const bool rangeFinderRightEnabled = false; +static const bool rangeFinderUpEnabled = false; +static const bool rangeFinderDownEnabled = false; // loop rates -static const float loop0Rate = 150; -static const float loop1Rate = 100; -static const float loop2Rate = 10; -static const float loop3Rate = 1; -static const float loop4Rate = 0.1; +static const float loopRate = 150; // attitude nav +static const float loop0Rate = 50; // controller +static const float loop1Rate = 5; // pos nav/ gcs fast +static const float loop2Rate = 1; // gcs slow +static const float loop3Rate = 0.1; // gains static const float rdrAilMix = 1.0; // since there are no ailerons diff --git a/apo/QuadMikrokopter.h b/apo/QuadMikrokopter.h deleted file mode 100644 index 7aeeacd5e6..0000000000 --- a/apo/QuadMikrokopter.h +++ /dev/null @@ -1,98 +0,0 @@ -/* - * 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 index dfca3d80ef..a413f824e3 100644 --- a/apo/apo.pde +++ b/apo/apo.pde @@ -18,6 +18,7 @@ // Vehicle Configuration #include "QuadArducopter.h" +//#include "PlaneEasystar.h" // ArduPilotOne Default Setup #include "APO_DefaultSetup.h" diff --git a/libraries/APO/APO.h b/libraries/APO/APO.h index 0e0efafbd7..357591c7e2 100644 --- a/libraries/APO/APO.h +++ b/libraries/APO/APO.h @@ -15,6 +15,7 @@ #include "AP_MavlinkCommand.h" #include "AP_Navigator.h" #include "AP_RcChannel.h" +#include "AP_BatteryMonitor.h" //#include "AP_Var_keys.h" #endif /* APO_H_ */