mirror of https://github.com/ArduPilot/ardupilot
Merge branch 'master' of https://code.google.com/p/ardupilot-mega
This commit is contained in:
commit
cc88eb2b9d
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -199,6 +199,10 @@ static void init_ardupilot()
|
|||
}
|
||||
#endif
|
||||
|
||||
// agmatthews USERHOOKS
|
||||
#ifdef USERHOOK_INIT
|
||||
USERHOOK_INIT
|
||||
#endif
|
||||
// Logging:
|
||||
// --------
|
||||
// DataFlash log initialization
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -99,6 +99,7 @@
|
|||
<desc>
|
||||
#define AUTO_RESET_LOITER 0
|
||||
#define FRAME_CONFIG HELI_FRAME
|
||||
#define INSTANT_PWM ENABLED
|
||||
|
||||
// DEFAULT PIDS
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
|
@ -0,0 +1,67 @@
|
|||
/*
|
||||
* AP_ArmingMechanism.h
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef AP_ARMINGMECHANISM_H_
|
||||
#define AP_ARMINGMECHANISM_H_
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <wiring.h>
|
||||
|
||||
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
|
|
@ -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);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
@ -8,3 +8,5 @@
|
|||
namespace apo {
|
||||
|
||||
} // apo
|
||||
|
||||
// vim:ts=4:sw=4:expandtab
|
||||
|
|
|
@ -49,3 +49,5 @@ private:
|
|||
} // namespace apo
|
||||
|
||||
#endif /* AP_BATTERYMONITOR_H_ */
|
||||
|
||||
// vim:ts=4:sw=4:expandtab
|
||||
|
|
|
@ -146,3 +146,4 @@ private:
|
|||
#endif // AP_Guide_H
|
||||
|
||||
// vim:ts=4:sw=4:expandtab
|
||||
|
||||
|
|
|
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue