Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

This commit is contained in:
Michael Oborne 2011-10-23 17:48:55 +08:00
parent b5e6445b15 a503d9a7a7
commit cc88eb2b9d
23 changed files with 281 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/APM_Config.h
View File

@ -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

13
ArduCopter/config.h
View File

@ -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

16
ArduCopter/control_modes.pde
View File

@ -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;

1
ArduCopter/defines.h
View File

@ -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

4
ArduCopter/system.pde
View File

@ -199,6 +199,10 @@ static void init_ardupilot()
}
#endif
// agmatthews USERHOOKS
#ifdef USERHOOK_INIT
USERHOOK_INIT
#endif
// Logging:
// --------
// DataFlash log initialization

2
Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.build.log
View File

@ -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

2
Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.build.log
View File

@ -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

6
Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.build.log
View File

@ -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

4
Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.size.txt
View File

@ -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()

6
Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.build.log
View File

@ -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

4
Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.size.txt
View File

@ -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()

1
Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml
View File

@ -99,6 +99,7 @@
<desc>
#define AUTO_RESET_LOITER 0
#define FRAME_CONFIG HELI_FRAME
#define INSTANT_PWM ENABLED
// DEFAULT PIDS

37
Tools/ArdupilotMegaPlanner/Firmware/git.log
View File

@ -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

53
apo/ControllerQuad.h
View File

@ -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;

10
apo/QuadArducopter.h
View File

@ -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

57
libraries/APO/AP_ArmingMechanism.cpp Normal file
View File

@ -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

67
libraries/APO/AP_ArmingMechanism.h Normal file
View File

@ -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

8
libraries/APO/AP_Autopilot.cpp
View File

@ -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);
}
/*

2
libraries/APO/AP_BatteryMonitor.cpp
View File

@ -8,3 +8,5 @@
namespace apo {
} // apo
// vim:ts=4:sw=4:expandtab

52
libraries/APO/AP_BatteryMonitor.h
View File

@ -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

1
libraries/APO/AP_Guide.h
View File

@ -146,3 +146,4 @@ private:
#endif // AP_Guide_H
// vim:ts=4:sw=4:expandtab

13
libraries/APO/AP_HardwareAbstractionLayer.h
View File

@ -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
}
}

20
libraries/APO/AP_Navigator.cpp
View File

@ -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);