diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 328afef9b3..28061c4ee9 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -27,7 +27,6 @@ V_FRAME */ - # define CH7_OPTION CH7_DO_NOTHING /* CH7_DO_NOTHING @@ -52,8 +51,7 @@ //#define RATE_ROLL_I 0.18 //#define RATE_PITCH_I 0.18 - - +//#define MOTORS_JD880 // agmatthews USERHOOKS diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 7d4f8a176d..e043d60530 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -361,6 +361,19 @@ ////////////////////////////////////////////////////////////////////////////// // Attitude Control // + +// Extra motor values that are changed from time to time by jani @ jDrones as software +// and charachteristics changes. +#ifdef MOTORS_JD880 +# define STABILIZE_ROLL_P 3.6 +# define STABILIZE_ROLL_I 0.06 +# define STABILIZE_ROLL_IMAX 2.0 // degrees +# define STABILIZE_PITCH_P 3.6 +# define STABILIZE_PITCH_I 0.06 +# define STABILIZE_PITCH_IMAX 2.0 // degrees +#endif + +// Jasons default values that are good for smaller payload motors. #ifndef STABILIZE_ROLL_P # define STABILIZE_ROLL_P 4.6 #endif diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 852178c43f..438bec2d6b 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -92,6 +92,22 @@ static void read_trim_switch() } } + #elif CH7_OPTION == CH7_SAVE_WP + + if (g.rc_7.control_in > 800){ + trim_flag = true; + + }else{ // switch is disengaged + + if(trim_flag){ + // set the next_WP + CH7_wp_index++; + current_loc.id = MAV_CMD_NAV_WAYPOINT; + g.waypoint_total.set_and_save(CH7_wp_index); + set_command_with_index(current_loc, CH7_wp_index); + } + } + #elif CH7_OPTION == CH7_ADC_FILTER if (g.rc_7.control_in > 800){ adc.filter_result = true; diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 19d14c4580..003375460a 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -37,6 +37,7 @@ #define CH7_RTL 4 #define CH7_AUTO_TRIM 5 #define CH7_ADC_FILTER 6 +#define CH7_SAVE_WP 7 // Frame types #define QUAD_FRAME 0 diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index c6f7e397d5..2b287016f5 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -199,6 +199,10 @@ static void init_ardupilot() } #endif +// agmatthews USERHOOKS +#ifdef USERHOOK_INIT + USERHOOK_INIT +#endif // Logging: // -------- // DataFlash log initialization diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.build.log index 8c61efe92c..10604a699c 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.build.log @@ -46,7 +46,7 @@ autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' bu autogenerated:249: warning: 'long int read_barometer()' declared 'static' but never defined autogenerated:250: warning: 'void read_airspeed()' declared 'static' but never defined autogenerated:251: warning: 'void zero_airspeed()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/system.pde:434: warning: 'void set_failsafe(boolean)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/system.pde:438: warning: 'void set_failsafe(boolean)' defined but not used autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined autogenerated:292: warning: 'void fake_out_gps()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.build.log index 8c61efe92c..10604a699c 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.build.log @@ -46,7 +46,7 @@ autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' bu autogenerated:249: warning: 'long int read_barometer()' declared 'static' but never defined autogenerated:250: warning: 'void read_airspeed()' declared 'static' but never defined autogenerated:251: warning: 'void zero_airspeed()' declared 'static' but never defined -/root/apm/ardupilot-mega/ArduCopter/system.pde:434: warning: 'void set_failsafe(boolean)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/system.pde:438: warning: 'void set_failsafe(boolean)' defined but not used autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined autogenerated:292: warning: 'void fake_out_gps()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.build.log index 7131865864..d420f23d8f 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.build.log @@ -9,11 +9,11 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77: /root/apm/ardupilot-mega/ArduCopter/heli.pde: In function 'void heli_init_swash()': /root/apm/ardupilot-mega/ArduCopter/heli.pde:60: warning: comparisons like X<=Y<=Z do not have their mathematical meaning /root/apm/ardupilot-mega/ArduCopter/setup.pde: In function 'int8_t setup_heli(uint8_t, const Menu::arg*)': -/root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_pitch' may be used uninitialized in this function -/root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_tail' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'min_tail' may be used uninitialized in this function +/root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_tail' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_coll' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'min_coll' may be used uninitialized in this function +/root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_pitch' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_roll' may be used uninitialized in this function autogenerated: At global scope: autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but never defined @@ -34,7 +34,7 @@ autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' de autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/sensors.pde:95: warning: 'void read_airspeed()' defined but not used /root/apm/ardupilot-mega/ArduCopter/sensors.pde:100: warning: 'void zero_airspeed()' defined but not used -/root/apm/ardupilot-mega/ArduCopter/system.pde:434: warning: 'void set_failsafe(boolean)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/system.pde:438: warning: 'void set_failsafe(boolean)' defined but not used autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined autogenerated:292: warning: 'void fake_out_gps()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.size.txt index 68bdfdcb08..41607607b2 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.size.txt @@ -677,9 +677,9 @@ 000003a0 t read_battery() 0000045c T update_yaw_mode() 0000046e T update_roll_pitch_mode() -0000052e t heli_move_swash(int, int, int, int) +0000053e t heli_move_swash(int, int, int, int) 000005cc t __static_initialization_and_destruction_0(int, int) -00000640 t init_ardupilot() +00000620 t init_ardupilot() 0000071a t update_nav_wp() 000007ec t setup_heli(unsigned char, Menu::arg const*) 00000870 t process_next_command() diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.build.log b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.build.log index 7131865864..d420f23d8f 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.build.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.build.log @@ -9,11 +9,11 @@ In file included from /root/apm/ardupilot-mega/ArduCopter/ArduCopter.pde:77: /root/apm/ardupilot-mega/ArduCopter/heli.pde: In function 'void heli_init_swash()': /root/apm/ardupilot-mega/ArduCopter/heli.pde:60: warning: comparisons like X<=Y<=Z do not have their mathematical meaning /root/apm/ardupilot-mega/ArduCopter/setup.pde: In function 'int8_t setup_heli(uint8_t, const Menu::arg*)': -/root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_pitch' may be used uninitialized in this function -/root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_tail' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'min_tail' may be used uninitialized in this function +/root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_tail' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_coll' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'min_coll' may be used uninitialized in this function +/root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_pitch' may be used uninitialized in this function /root/apm/ardupilot-mega/ArduCopter/setup.pde:467: warning: 'max_roll' may be used uninitialized in this function autogenerated: At global scope: autogenerated:109: warning: 'void Log_Write_Optflow()' declared 'static' but never defined @@ -34,7 +34,7 @@ autogenerated:238: warning: 'long int get_alt_distance(Location*, Location*)' de autogenerated:248: warning: 'long int read_baro_filtered()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/sensors.pde:95: warning: 'void read_airspeed()' defined but not used /root/apm/ardupilot-mega/ArduCopter/sensors.pde:100: warning: 'void zero_airspeed()' defined but not used -/root/apm/ardupilot-mega/ArduCopter/system.pde:434: warning: 'void set_failsafe(boolean)' defined but not used +/root/apm/ardupilot-mega/ArduCopter/system.pde:438: warning: 'void set_failsafe(boolean)' defined but not used autogenerated:285: warning: 'void init_optflow()' declared 'static' but never defined autogenerated:292: warning: 'void fake_out_gps()' declared 'static' but never defined /root/apm/ardupilot-mega/ArduCopter/test.pde:1028: warning: 'void print_motor_out()' defined but not used diff --git a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.size.txt b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.size.txt index b936dfb003..0f00a831cc 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.size.txt +++ b/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.size.txt @@ -677,9 +677,9 @@ 000003a0 t read_battery() 0000045c T update_yaw_mode() 0000046e T update_roll_pitch_mode() -0000052e t heli_move_swash(int, int, int, int) +0000053e t heli_move_swash(int, int, int, int) 000005cc t __static_initialization_and_destruction_0(int, int) -0000063e t init_ardupilot() +0000061e t init_ardupilot() 0000071a t update_nav_wp() 000007e8 t setup_heli(unsigned char, Menu::arg const*) 00000870 t process_next_command() diff --git a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml index d39b47fa9a..c2dccc3239 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml +++ b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml @@ -99,6 +99,7 @@ #define AUTO_RESET_LOITER 0 #define FRAME_CONFIG HELI_FRAME +#define INSTANT_PWM ENABLED // DEFAULT PIDS diff --git a/Tools/ArdupilotMegaPlanner/Firmware/git.log b/Tools/ArdupilotMegaPlanner/Firmware/git.log index 2a7bfed917..04b288c7e4 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/git.log +++ b/Tools/ArdupilotMegaPlanner/Firmware/git.log @@ -1 +1,36 @@ -Already up-to-date. +From https://code.google.com/p/ardupilot-mega + 8a21477..b0bfa54 APM_Camera -> origin/APM_Camera + 6f44fff..076459c master -> origin/master +Updating 6f44fff..076459c +Fast-forward + ArduCopter/control_modes.pde | 16 + + ArduCopter/defines.h | 1 + + ArduCopter/system.pde | 4 + + ArduPlane/ArduPlane.pde | 10 +- + ArduPlane/Parameters.h | 3 + + ArduPlane/config.h | 8 + + Tools/ArduTracker/tags |148411 -------------------- + Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs | 2 + + Tools/ArdupilotMegaPlanner/Log.cs | 174 +- + Tools/ArdupilotMegaPlanner/MainV2.cs | 9 +- + .../Properties/AssemblyInfo.cs | 2 +- + .../bin/Release/ArdupilotMegaPlanner.application | 2 +- + .../bin/Release/ArdupilotMegaPlanner.exe | Bin 2194432 -> 2194944 bytes + Tools/ArdupilotMegaPlanner/bin/Release/Updater.exe | Bin 8192 -> 8192 bytes + Tools/ArdupilotMegaPlanner/bin/Release/resedit.exe | Bin 20480 -> 20480 bytes + .../ru-RU/ArdupilotMegaPlanner.resources.dll | Bin 53248 -> 53248 bytes + .../zh-Hans/ArdupilotMegaPlanner.resources.dll | Bin 380928 -> 380928 bytes + apo/ControllerQuad.h | 51 +- + apo/QuadArducopter.h | 10 +- + libraries/APO/AP_ArmingMechanism.cpp | 57 + + libraries/APO/AP_ArmingMechanism.h | 67 + + libraries/APO/AP_BatteryMonitor.cpp | 2 + + libraries/APO/AP_BatteryMonitor.h | 58 +- + libraries/APO/AP_Guide.h | 1 + + libraries/APO/AP_HardwareAbstractionLayer.h | 13 + + libraries/APO/AP_Navigator.cpp | 20 +- + libraries/Desktop/support/FastSerial.cpp | 283 +- + 27 files changed, 517 insertions(+), 148687 deletions(-) + delete mode 100644 Tools/ArduTracker/tags + create mode 100644 libraries/APO/AP_ArmingMechanism.cpp + create mode 100644 libraries/APO/AP_ArmingMechanism.h diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 847f5617fd..59b1e9a6ea 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -10,6 +10,7 @@ #include "../APO/AP_Controller.h" #include "../APO/AP_BatteryMonitor.h" +#include "../APO/AP_ArmingMechanism.h" namespace apo { @@ -78,7 +79,7 @@ public: PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_DFCUT), 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, PID_POS_DFCUT), - _armingClock(0), _thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0), + _armingMechanism(hal,CH_THRUST,CH_YAW,0.1,-0.9,0.9), _thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0), _cmdRoll(0), _cmdPitch(0), _cmdYawRate(0), _mode(MAV_MODE_LOCKED) { /* * allocate radio channels @@ -117,51 +118,7 @@ public: virtual void update(const float & dt) { //_hal->debug->printf_P(PSTR("thr: %f, yaw: %f\n"),_hal->rc[CH_THRUST]->getRadioPosition(),_hal->rc[CH_YAW]->getRadioPosition()); - // arming - // - // to arm: put stick to bottom right for 100 controller cycles - // (max yaw, min throttle) - // - // didn't use clock here in case of millis() roll over - // for long runs - if ( (_hal->getState() != MAV_STATE_ACTIVE) & - (_hal->rc[CH_THRUST]->getRadioPosition() < 0.1) && - (_hal->rc[CH_YAW]->getRadioPosition() < -0.9) ) { - - // always start clock at 0 - if (_armingClock<0) _armingClock = 0; - - if (_armingClock++ >= 100) { - _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed")); - _hal->setState(MAV_STATE_ACTIVE); - } else { - _hal->gcs->sendText(SEVERITY_HIGH, PSTR("arming")); - } - } - // disarming - // - // to disarm: put stick to bottom left for 100 controller cycles - // (min yaw, min throttle) - else if ( (_hal->getState() == MAV_STATE_ACTIVE) & - (_hal->rc[CH_THRUST]->getRadioPosition() < 0.1) && - (_hal->rc[CH_YAW]->getRadioPosition() > 0.9) ) { - - // always start clock at 0 - if (_armingClock>0) _armingClock = 0; - - if (_armingClock-- <= -100) { - _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed")); - _hal->setState(MAV_STATE_STANDBY); - } else { - _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarming")); - } - } - // reset arming clock and report status - else if (_armingClock != 0) { - _armingClock = 0; - if (_hal->getState()==MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed")); - else if (_hal->getState()!=MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed")); - } + _armingMechanism.update(dt); // determine flight mode // @@ -170,7 +127,7 @@ public: _mode = MAV_MODE_FAILSAFE; _hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat")); // if battery less than 5%, go to failsafe - } else if (_hal->batteryMonitor->getPercentage() < 5) { + } else if (_hal->batteryMonitor && _hal->batteryMonitor->getPercentage() < 5) { _mode = MAV_MODE_FAILSAFE; _hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery")); // manual/auto switch @@ -232,7 +189,7 @@ private: BlockPIDDfb pidRoll, pidPitch, pidYaw; BlockPID pidYawRate; BlockPIDDfb pidPN, pidPE, pidPD; - int32_t _armingClock; + AP_ArmingMechanism _armingMechanism; float _thrustMix, _pitchMix, _rollMix, _yawMix; float _cmdRoll, _cmdPitch, _cmdYawRate; diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index 32f402035d..80d6c6cb06 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -11,7 +11,7 @@ // vehicle options static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD; static const apo::halMode_t halMode = apo::MODE_LIVE; -static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_2560; +static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280; static const uint8_t heartBeatTimeout = 3; // algorithm selection @@ -54,9 +54,9 @@ static const bool rangeFinderUpEnabled = false; static const bool rangeFinderDownEnabled = false; // loop rates -static const float loopRate = 150; // attitude nav +static const float loopRate = 250; // attitude nav static const float loop0Rate = 50; // controller -static const float loop1Rate = 5; // pos nav/ gcs fast +static const float loop1Rate = 10; // pos nav/ gcs fast static const float loop2Rate = 1; // gcs slow static const float loop3Rate = 0.1; @@ -77,7 +77,7 @@ static const float PID_POS_DFCUT = 10; // cut derivative feedback at 10 hz static const float PID_ATT_P = 0.17; static const float PID_ATT_I = 0.5; static const float PID_ATT_D = 0.06; -static const float PID_ATT_LIM = 0.05; // 10 % +static const float PID_ATT_LIM = 0.05; // 5 % motors static const float PID_ATT_AWU = 0.005; // 0.5 % static const float PID_ATT_DFCUT = 25; // cut derivative feedback at 25 hz static const float PID_YAWPOS_P = 0; @@ -88,7 +88,7 @@ static const float PID_YAWPOS_AWU = 0; // 1 rad/s static const float PID_YAWSPEED_P = 0.5; static const float PID_YAWSPEED_I = 0; static const float PID_YAWSPEED_D = 0; -static const float PID_YAWSPEED_LIM = .1; // 10 % MOTORs +static const float PID_YAWSPEED_LIM = .05; // 5 % motors static const float PID_YAWSPEED_AWU = 0.0; static const float PID_YAWSPEED_DFCUT = 3.0; // 3 Radians, about 1 Hz diff --git a/libraries/APO/AP_ArmingMechanism.cpp b/libraries/APO/AP_ArmingMechanism.cpp new file mode 100644 index 0000000000..0b80bc3895 --- /dev/null +++ b/libraries/APO/AP_ArmingMechanism.cpp @@ -0,0 +1,57 @@ +/* + * AP_ArmingMechanism.cpp + * + */ + + +#include "AP_ArmingMechanism.h" +#include "AP_RcChannel.h" +#include "../FastSerial/FastSerial.h" +#include "AP_HardwareAbstractionLayer.h" +#include "AP_CommLink.h" + +namespace apo { + +void AP_ArmingMechanism::update(const float dt) { + + // arming + if ( (_hal->getState() != MAV_STATE_ACTIVE) && + (_hal->rc[_ch1]->getRadioPosition() < _ch1Min) && + (_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) { + + // always start clock at 0 + if (_armingClock<0) _armingClock = 0; + + if (_armingClock++ >= 100) { + _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed")); + _hal->setState(MAV_STATE_ACTIVE); + } else { + _hal->gcs->sendText(SEVERITY_HIGH, PSTR("arming")); + } + } + // disarming + else if ( (_hal->getState() == MAV_STATE_ACTIVE) && + (_hal->rc[_ch1]->getRadioPosition() < _ch1Min) && + (_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) { + + // always start clock at 0 + if (_armingClock>0) _armingClock = 0; + + if (_armingClock-- <= -100) { + _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed")); + _hal->setState(MAV_STATE_STANDBY); + } else { + _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarming")); + } + } + // reset arming clock and report status + else if (_armingClock != 0) { + _armingClock = 0; + if (_hal->getState()==MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed")); + else if (_hal->getState()!=MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed")); + } +} + +} // apo + +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_ArmingMechanism.h b/libraries/APO/AP_ArmingMechanism.h new file mode 100644 index 0000000000..e172d822ee --- /dev/null +++ b/libraries/APO/AP_ArmingMechanism.h @@ -0,0 +1,67 @@ +/* + * AP_ArmingMechanism.h + * + */ + +#ifndef AP_ARMINGMECHANISM_H_ +#define AP_ARMINGMECHANISM_H_ + +#include +#include + +namespace apo { + +class AP_HardwareAbstractionLayer; + +class AP_ArmingMechanism { + +public: + /** + * Constructor + * + * @param ch1: typically throttle channel + * @param ch2: typically yaw channel + * @param ch1Min: disarms/arms belows this + * @param ch2Min: disarms below this + * @param ch2Max: arms above this + */ + AP_ArmingMechanism(AP_HardwareAbstractionLayer * hal, + uint8_t ch1, uint8_t ch2, float ch1Min, float ch2Min, + float ch2Max) : _armingClock(0), _hal(hal), _ch1(ch1), _ch2(ch2), + _ch1Min(ch1Min), _ch2Min(ch2Min), _ch2Max(ch2Max) { + } + + /** + * update + * + * arming: + * + * to arm: put stick to bottom right for 100 controller cycles + * (max yaw, min throttle) + * + * didn't use clock here in case of millis() roll over + * for long runs + * + * disarming: + * + * to disarm: put stick to bottom left for 100 controller cycles + * (min yaw, min throttle) + */ + void update(const float dt); + +private: + + AP_HardwareAbstractionLayer * _hal; + int8_t _armingClock; + uint8_t _ch1; /// typically throttle channel + uint8_t _ch2; /// typically yaw channel + float _ch1Min; /// arms/disarms below this on ch1 + float _ch2Min; /// disarms below this on ch2 + float _ch2Max; /// arms above this on ch2 +}; + +} // namespace apo + +#endif /* AP_ARMINGMECHANISM */ + +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp index 61a52e0ce7..1c600eef44 100644 --- a/libraries/APO/AP_Autopilot.cpp +++ b/libraries/APO/AP_Autopilot.cpp @@ -94,7 +94,7 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, AP_MavlinkCommand::home.getCommand()); /* - * Attach loops + * Attach loops, stacking for priority */ hal->debug->println_P(PSTR("attaching loops")); subLoops().push_back(new Loop(loop0Rate, callback0, this)); @@ -211,10 +211,10 @@ void AP_Autopilot::callback2(void * data) { if (apo->getHal()->gcs) { // send messages apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW); - //apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); + apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); + apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW); //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); + apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU); } /* diff --git a/libraries/APO/AP_BatteryMonitor.cpp b/libraries/APO/AP_BatteryMonitor.cpp index e5550d2798..835f3072d2 100644 --- a/libraries/APO/AP_BatteryMonitor.cpp +++ b/libraries/APO/AP_BatteryMonitor.cpp @@ -8,3 +8,5 @@ namespace apo { } // apo + +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_BatteryMonitor.h b/libraries/APO/AP_BatteryMonitor.h index 7f82db349d..96b94bc46d 100644 --- a/libraries/APO/AP_BatteryMonitor.h +++ b/libraries/APO/AP_BatteryMonitor.h @@ -13,39 +13,41 @@ namespace apo { class AP_BatteryMonitor { public: - AP_BatteryMonitor(uint8_t pin, float voltageDivRatio, float minVolt, float maxVolt) : - _pin(pin), _voltageDivRatio(voltageDivRatio), - _minVolt(minVolt), _maxVolt(maxVolt), _voltage(maxVolt) { - } + AP_BatteryMonitor(uint8_t pin, float voltageDivRatio, float minVolt, float maxVolt) : + _pin(pin), _voltageDivRatio(voltageDivRatio), + _minVolt(minVolt), _maxVolt(maxVolt), _voltage(maxVolt) { + } - void update() { - // low pass filter on voltage - _voltage = _voltage*.9 + (analogRead(_pin)/255)*_voltageDivRatio*0.1; - } + void update() { + // low pass filter on voltage + _voltage = _voltage*.9 + (analogRead(_pin)/255)*_voltageDivRatio*0.1; + } - /** - * Accessors - */ - float getVoltage() { - return _voltage; - } + /** + * Accessors + */ + float getVoltage() { + return _voltage; + } - uint8_t getPercentage() { - float norm = (_voltage-_minVolt)/(_maxVolt-_minVolt); - if (norm < 0) norm = 0; - else if (norm > 1) norm = 1; - return 100*norm; - } + uint8_t getPercentage() { + float norm = (_voltage-_minVolt)/(_maxVolt-_minVolt); + if (norm < 0) norm = 0; + else if (norm > 1) norm = 1; + return 100*norm; + } private: - uint8_t _pin; - float _voltageDivRatio; - float _voltage; - float _minVolt; - float _maxVolt; + uint8_t _pin; + float _voltageDivRatio; + float _voltage; + float _minVolt; + float _maxVolt; }; } // namespace apo #endif /* AP_BATTERYMONITOR_H_ */ + +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index 0944620247..15b1e1e40b 100644 --- a/libraries/APO/AP_Guide.h +++ b/libraries/APO/AP_Guide.h @@ -146,3 +146,4 @@ private: #endif // AP_Guide_H // vim:ts=4:sw=4:expandtab + diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_HardwareAbstractionLayer.h index 422bb55d36..a1562efd0a 100644 --- a/libraries/APO/AP_HardwareAbstractionLayer.h +++ b/libraries/APO/AP_HardwareAbstractionLayer.h @@ -80,6 +80,19 @@ public: pinMode(slideSwitchPin, INPUT); pinMode(pushButtonPin, INPUT); DDRL |= B00000100; // set port L, pint 2 to output for the relay + } else if (board == BOARD_ARDUPILOTMEGA_2) { + 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 } } diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp index 46e74d4d87..4260ef1461 100644 --- a/libraries/APO/AP_Navigator.cpp +++ b/libraries/APO/AP_Navigator.cpp @@ -69,13 +69,25 @@ DcmNavigator::DcmNavigator(AP_HardwareAbstractionLayer * hal) : } if (_hal->getMode() == MODE_LIVE) { - if (_hal->adc) + + if (_hal->adc) { _hal->imu = new AP_IMU_Oilpan(_hal->adc, k_sensorCalib); - if (_hal->imu) + } + + if (_hal->imu) { _dcm = new AP_DCM(_hal->imu, _hal->gps, _hal->compass); + + // tune down dcm + _dcm->kp_roll_pitch(0.030000); + _dcm->ki_roll_pitch(0.00001278), // 50 hz I term + + // tune down compass in dcm + _dcm->kp_yaw(0.08); + _dcm->ki_yaw(0); + } + if (_hal->compass) { _dcm->set_compass(_hal->compass); - } } } @@ -121,7 +133,7 @@ void DcmNavigator::updateFast(float dt) { // dcm class for attitude if (_dcm) { - _dcm->update_DCM(); + _dcm->update_DCM_fast(); setRoll(_dcm->roll); setPitch(_dcm->pitch); setYaw(_dcm->yaw);