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-11-25 08:08:14 +08:00
parent 747e1187e7 22b1b64092
commit e59a910fb9
32 changed files with 1138 additions and 735 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduBoat/ControllerBoat.h
View File

@ -8,7 +8,7 @@
#ifndef CONTROLLERBOAT_H_
#define CONTROLLERBOAT_H_
#include "../APO/AP_Controller.h"
#include "../APO/APO.h"
namespace apo {
@ -16,7 +16,7 @@ class ControllerBoat: public AP_Controller {
public:
ControllerBoat(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode),
AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl),
pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
@ -49,7 +49,7 @@ private:
_guide->getGroundSpeedCommand()
- _nav->getGroundSpeed(), dt);
}
void setMotorsActive() {
void setMotors() {
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
@ -58,6 +58,10 @@ private:
_hal->rc[ch_str]->setPosition(_strCmd);
}
}
void handleFailsafe() {
// failsafe is to turn off
setMode(MAV_MODE_LOCKED);
}
// attributes
enum {

224
ArduPlane/Log.pde
View File

@ -53,8 +53,8 @@ print_log_menu(void)
int log_start;
int log_end;
int temp;
uint16_t num_logs = get_num_logs();
//Serial.print("num logs: "); Serial.println(num_logs, DEC);
Serial.printf_P(PSTR("logs enabled: "));
@ -82,7 +82,7 @@ print_log_menu(void)
Serial.println();
if (num_logs == 0) {
Serial.printf_P(PSTR("\nNo logs\nType 'dump 0'.\n\n"));
Serial.printf_P(PSTR("\nNo logs\n\n"));
}else{
Serial.printf_P(PSTR("\n%d logs\n"), num_logs);
@ -109,17 +109,18 @@ dump_log(uint8_t argc, const Menu::arg *argv)
last_log_num = g.log_last_filenumber;
if ((argc != 2) || (dump_log <= (last_log_num - get_num_logs())) || (dump_log > last_log_num)) {
Serial.printf_P(PSTR("bad log number\n"));
Log_Read(0, 4095);
return(-1);
}
get_log_boundaries(dump_log, dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"),
Serial.printf_P(PSTR("Dumping Log %d, start pg %d, end pg %d\n"),
dump_log,
dump_log_start,
dump_log_end);
Log_Read(dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Log read complete\n"));
Serial.printf_P(PSTR("Done\n"));
return 0;
}
@ -204,6 +205,7 @@ static byte get_num_logs(void)
if(g.log_last_filenumber < 1) return 0;
DataFlash.StartRead(1);
if(DataFlash.GetFileNumber() == 0XFFFF) return 0;
lastpage = find_last();
@ -211,7 +213,7 @@ static byte get_num_logs(void)
last = DataFlash.GetFileNumber();
DataFlash.StartRead(lastpage + 2);
first = DataFlash.GetFileNumber();
if(first == 0xFFFF) {
if(first > last) {
DataFlash.StartRead(1);
first = DataFlash.GetFileNumber();
}
@ -285,132 +287,120 @@ static int find_last(void)
// This function finds the last page of a particular log file
static int find_last_log_page(uint16_t log_number)
{
int16_t bottom_page;
int16_t top_page;
int16_t bottom_page_file;
int16_t bottom_page_filepage;
int16_t top_page_file;
int16_t top_page_filepage;
int16_t look_page;
int16_t look_page_file;
int16_t look_page_filepage;
int16_t check;
bool XLflag = false;
// First see if the logs are empty
DataFlash.StartRead(1);
if(DataFlash.GetFileNumber() == 0XFFFF) {
uint16_t bottom_page;
uint16_t bottom_page_file;
uint16_t bottom_page_filepage;
uint16_t top_page;
uint16_t top_page_file;
uint16_t top_page_filepage;
uint16_t look_page;
uint16_t look_page_file;
uint16_t look_page_filepage;
bottom_page = 1;
DataFlash.StartRead(bottom_page);
bottom_page_file = DataFlash.GetFileNumber();
bottom_page_filepage = DataFlash.GetFilePage();
// First see if the logs are empty. If so we will exit right away.
if(bottom_page_file == 0XFFFF) {
return 0;
}
// Next, see if logs wrap the top of the dataflash
DataFlash.StartRead(DF_LAST_PAGE);
if(DataFlash.GetFileNumber() == 0xFFFF)
{
// This case is that we have not wrapped the top of the dataflash
top_page = DF_LAST_PAGE;
bottom_page = 1;
while((top_page - bottom_page) > 1) {
look_page = (top_page + bottom_page) / 2;
DataFlash.StartRead(look_page);
if(DataFlash.GetFileNumber() > log_number)
top_page = look_page;
else
top_page = DF_LAST_PAGE;
DataFlash.StartRead(top_page);
top_page_file = DataFlash.GetFileNumber();
top_page_filepage = DataFlash.GetFilePage();
while((top_page - bottom_page) > 1) {
look_page = ((long)top_page + (long)bottom_page) / 2L;
DataFlash.StartRead(look_page);
look_page_file = DataFlash.GetFileNumber();
look_page_filepage = DataFlash.GetFilePage();
// We have a lot of different logic cases dependant on if the log space is overwritten
// and where log breaks might occur relative to binary search points.
// Someone could make work up a logic table and simplify this perhaps, or perhaps
// it is easier to interpret as is.
if (look_page_file == 0xFFFF) {
top_page = look_page;
top_page_file = look_page_file;
top_page_filepage = look_page_filepage;
} else if (look_page_file == log_number && bottom_page_file == log_number && top_page_file == log_number) {
// This case is typical if a single file fills the log and partially overwrites itself
if (bottom_page_filepage < top_page_filepage) {
bottom_page = look_page;
}
return bottom_page;
} else {
// The else case is that the logs do wrap the top of the dataflash
bottom_page = 1;
top_page = DF_LAST_PAGE;
DataFlash.StartRead(bottom_page);
bottom_page_file = DataFlash.GetFileNumber();
bottom_page_filepage = DataFlash.GetFilePage();
DataFlash.StartRead(top_page);
top_page_file = DataFlash.GetFileNumber();
top_page_filepage = DataFlash.GetFilePage();
// Check is we have exactly filled the dataflash but not wrapped. If so we can exit quickly.
if(top_page_file == log_number && bottom_page_file != log_number) {
return top_page_file;
}
// Check if the top is 1 file higher than we want. If so we can exit quickly.
if(top_page_file == log_number+1) {
return top_page - top_page_filepage;
}
// Check if the file has partially overwritten itself
if(top_page_filepage >= DF_LAST_PAGE) {
XLflag = true;
} else {
top_page = top_page - top_page_filepage;
DataFlash.StartRead(top_page);
top_page_file = DataFlash.GetFileNumber();
}
if(top_page_file == log_number) {
bottom_page = top_page;
top_page = DF_LAST_PAGE;
DataFlash.StartRead(top_page);
top_page_filepage = DataFlash.GetFilePage();
if(XLflag) bottom_page = 1;
while((top_page - bottom_page) > 1) {
look_page = (top_page + bottom_page) / 2;
DataFlash.StartRead(look_page);
if(DataFlash.GetFilePage() < top_page_filepage)
{
top_page = look_page;
top_page_filepage = DataFlash.GetFilePage();
} else {
bottom_page = look_page;
}
bottom_page_file = look_page_file;
bottom_page_filepage = look_page_filepage;
} else {
top_page = look_page;
top_page_file = look_page_file;
top_page_filepage = look_page_filepage;
}
return bottom_page;
}
// Step down through the files to find the one we want
bottom_page = top_page;
bottom_page_filepage = top_page_filepage;
do
{
int16_t last_bottom_page_file;
top_page = bottom_page;
bottom_page = bottom_page - bottom_page_filepage;
if(bottom_page < 1) bottom_page = 1;
DataFlash.StartRead(bottom_page);
last_bottom_page_file = bottom_page_file;
bottom_page_file = DataFlash.GetFileNumber();
bottom_page_filepage = DataFlash.GetFilePage();
if (bottom_page_file == last_bottom_page_file &&
bottom_page_filepage == 0) {
/* no progress can be made - give up. The log may be corrupt */
return -1;
}
} while (bottom_page_file != log_number && bottom_page != 1);
// Deal with stepping down too far due to overwriting a file
while((top_page - bottom_page) > 1) {
look_page = (top_page + bottom_page) / 2;
DataFlash.StartRead(look_page);
if(DataFlash.GetFileNumber() < log_number)
top_page = look_page;
else
} else if (look_page_file == log_number && look_page_file ==bottom_page_file) {
if (bottom_page_filepage < look_page_filepage) {
bottom_page = look_page;
bottom_page_file = look_page_file;
bottom_page_filepage = look_page_filepage;
} else {
top_page = look_page;
top_page_file = look_page_file;
top_page_filepage = look_page_filepage;
}
} else if (look_page_file == log_number) {
bottom_page = look_page;
bottom_page_file = look_page_file;
bottom_page_filepage = look_page_filepage;
} else if(look_page_file < log_number && bottom_page_file > look_page_file && bottom_page_file <= log_number) {
top_page = look_page;
top_page_file = look_page_file;
top_page_filepage = look_page_filepage;
} else if(look_page_file < log_number) {
bottom_page = look_page;
bottom_page_file = look_page_file;
bottom_page_filepage = look_page_filepage;
} else if(look_page_file > log_number && top_page_file < look_page_file && top_page_file >= log_number) {
bottom_page = look_page;
bottom_page_file = look_page_file;
bottom_page_filepage = look_page_filepage;
} else {
top_page = look_page;
top_page_file = look_page_file;
top_page_filepage = look_page_filepage;
}
// The -1 return is for the case where a very short power up increments the log
// number counter but no log file is actually created. This happens if power is
// removed before the first page is written to flash.
if(bottom_page ==1 && DataFlash.GetFileNumber() != log_number) return -1;
return bottom_page;
// End while
}
if (bottom_page_file == log_number && top_page_file == log_number) {
if( bottom_page_filepage < top_page_filepage)
return top_page;
else
return bottom_page;
} else if (bottom_page_file == log_number) {
return bottom_page;
} else if (top_page_file == log_number) {
return top_page;
} else {
return -1;
}
}
// Write an attitude packet. Total length : 10 bytes
static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw)
{

8
ArduRover/ControllerCar.h
View File

@ -16,7 +16,7 @@ class ControllerCar: public AP_Controller {
public:
ControllerCar(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode),
AP_Controller(nav, guide, hal,new AP_ArmingMechanism(hal,this,ch_thrust,ch_str,0.1,-0.9,0.9), ch_mode, k_cntrl),
pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
steeringI, steeringD, steeringIMax, steeringYMax,steeringDFCut),
pidThrust(new AP_Var_group(k_pidThrust, PSTR("THR_")), 1, throttleP,
@ -84,10 +84,14 @@ private:
_strCmd = steering;
_thrustCmd = thrust;
}
void setMotorsActive() {
void setMotors() {
_hal->rc[ch_str]->setPosition(_strCmd);
_hal->rc[ch_thrust]->setPosition(fabs(_thrustCmd) < 0.1 ? 0 : _thrustCmd);
}
void handleFailsafe() {
// turn off
setMode(MAV_MODE_LOCKED);
}
// attributes
enum {

2
ArduRover/ControllerTank.h
View File

@ -16,7 +16,7 @@ class ControllerTank: public AP_Controller {
public:
ControllerTank(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9),ch_mode),
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_str,0.1,-0.9,0.9),ch_mode,k_cntrl),
pidStr(new AP_Var_group(k_pidStr, PSTR("STR_")), 1, steeringP,
steeringI, steeringD, steeringIMax, steeringYMax, steeringDFCut),
pidThr(new AP_Var_group(k_pidThr, PSTR("THR_")), 1, throttleP,

12
CMakeLists.txt
View File

@ -86,8 +86,8 @@ macro(add_sketch SKETCH_NAME BOARD PORT)
# files
set(SKETCH_CPP ${CMAKE_CURRENT_BINARY_DIR}/${SKETCH_NAME}/${SKETCH_NAME}.cpp)
set(SKETCH_PDE ${CMAKE_CURRENT_SOURCE_DIR}/${SKETCH_NAME}/${SKETCH_NAME}.pde)
message(STATUS "SKETCH_PDE:\n${SKETCH_PDE}")
message(STATUS "SKETCH_CPP:\n${SKETCH_CPP}")
#message(STATUS "SKETCH_PDE:\n${SKETCH_PDE}")
#message(STATUS "SKETCH_CPP:\n${SKETCH_CPP}")
# settings
set(${SKETCH_NAME}_BOARD ${BOARD})
@ -109,12 +109,12 @@ macro(add_sketch SKETCH_NAME BOARD PORT)
string(FIND "${STR1}" "\n" POS2)
math(EXPR POS3 "${POS1}+${POS2}")
string(SUBSTRING "${FILE}" 0 ${POS3} FILE_HEAD)
message(STATUS "FILE_HEAD:\n${FILE_HEAD}")
#message(STATUS "FILE_HEAD:\n${FILE_HEAD}")
# find the body of the main pde
math(EXPR BODY_LENGTH "${FILE_LENGTH}-${POS3}-1")
string(SUBSTRING "${FILE}" "${POS3}+1" "${BODY_LENGTH}" FILE_BODY)
message(STATUS "BODY:\n${FILE_BODY}")
#message(STATUS "BODY:\n${FILE_BODY}")
# write the file head
file(APPEND ${SKETCH_CPP} "${FILE_HEAD}")
@ -122,11 +122,11 @@ macro(add_sketch SKETCH_NAME BOARD PORT)
# write prototypes
foreach(PDE ${PDE_SOURCES})
message(STATUS "pde: ${PDE}")
#message(STATUS "pde: ${PDE}")
file(READ ${PDE} FILE)
string(REGEX MATCHALL "[\n]([a-zA-Z]+[ ])*[_a-zA-Z0-9]+([ ]*[\n][\t]*|[ ])[_a-zA-Z0-9]+[ ]?[\n]?[\t]*[ ]*[(]([\t]*[ ]*[*]?[ ]?[a-zA-Z0-9_][,]?[ ]*[\n]?)*[)]" PROTOTYPES ${FILE})
foreach(PROTOTYPE ${PROTOTYPES})
message(STATUS "\tprototype: ${PROTOTYPE};")
#message(STATUS "\tprototype: ${PROTOTYPE};")
file(APPEND ${SKETCH_CPP} "${PROTOTYPE};")
endforeach()
endforeach()

18
README.txt
View File

@ -14,9 +14,17 @@ Building using cmake
-----------------------------------------------
- mkdir build
- cd build
- cmake ..
- cmake .. -DBOARD=mega -PORT=/dev/ttyUSB0
You can select from mega/mega2560.
If you have arduino installed in a non-standard location you by specify it by using:
-DARDUINO_SDK_PATH=/path/to/arduino ..
- make (will build every sketch)
- make ArduPlane (will build just ArduPlane etc.)
- make ArduPloat-upload (will upload the sketch)
If you have a sync error during upload reset the board/power cycle the board
before the upload starts.
Building using eclipse
-----------------------------------------------
@ -36,8 +44,12 @@ Building using eclipse
Note: Unix can be substituted for MinGW/ MSYS/ NMake (for windows)
(see http://www.vtk.org/Wiki/Eclipse_CDT4_Generator)
PORT is the port for uploading to the board, COM0 etc on windows.
BOARD is your board type, mega for the 1280 or mega2560 for the 2560 boards.
input options:
CMAKE_BUILD_TYPE choose from DEBUG, RELEASE etc.
PORT is the port for uploading to the board, COM0 etc on windows. /dev/ttyUSB0 etc. on linux
BOARD is your board type, mega for the 1280 or mega2560 for the 2560 boards.
ARDUINO_SDK_PATH if it is not in default path can specify as /path/to/arduino
Importing the Eclipse Build Project:

9
Tools/ArduPPM/ATMega328p/Encoder-PPM.c
View File

@ -1,5 +1,5 @@
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
// ArduPPM Version v0.9.86
// ArduPPM Version v0.9.87
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
// ARDUCOPTER 2 : PPM ENCODER for AT Mega 328p and APM v1.4 Boards
// By:John Arne Birkeland - 2011
@ -34,6 +34,7 @@
// 0.9.84 : Corrected pin and port names in Encoder-PPM.c according to #define for Mega32U2 compatibility
// 0.9.85 : Added brownout reset detection flag
// 0.9.86 : Added a #define to disable Radio Passthrough mode (hardware failsafe for Arduplane)
// 0.9.87 : #define correction for radio passthrough (was screwed up).
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
// PREPROCESSOR DIRECTIVES
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
@ -46,7 +47,7 @@
#define ERROR_DETECTION_WINDOW 3000 * LOOP_TIMER_10MS // Detection window for error detection (default to 30s)
#define ERROR_CONDITION_DELAY 500 * LOOP_TIMER_10MS // Servo error condition LED delay (LED blinking duration)
#define PASSTHROUGH_MODE ENABLED // Set it to "DISABLED" to remove radio passthrough mode (hardware failsafe for Arduplane)
#define PASSTHROUGH_MODE_ENABLED // Comment this line to remove CH8 radio passthrough mode support (hardware failsafe for Arduplane)
#define PASSTHROUGH_CHANNEL 8 * 2 // Channel for passthrough mode selection
#define PASSTHROUGH_CHANNEL_OFF_US ONE_US * 1600 - PPM_PRE_PULSE // Passthrough off threshold
#define PASSTHROUGH_CHANNEL_ON_US ONE_US * 1800 - PPM_PRE_PULSE // Passthrough on threshold
@ -97,7 +98,7 @@ int main(void)
static uint16_t servo_error_condition_timer=0; // Servo error condition timer
static uint16_t blink_led_timer = 0; // Blink led timer
#if PASSTHROUGH_MODE == ENABLED
#ifdef PASSTHROUGH_MODE_ENABLED
static uint8_t mux_timer = 0; // Mux timer
static uint8_t mux_counter = 0; // Mux counter
static int8_t mux_check = 0;
@ -314,7 +315,7 @@ int main(void)
PWM_LOOP: // SERVO_PWM_MODE
while( 1 )
{
#if PASSTHROUGH_MODE == ENABLED
#ifdef PASSTHROUGH_MODE_ENABLED
// ------------------------------------------------------------------------------
// Radio passthrough control (mux chip A/B control)
// ------------------------------------------------------------------------------

21
Tools/ArduPPM/ATMega328p/manual.txt
View File

@ -1,7 +1,7 @@
Arducoder is the new 8 channels ppm encoder code for ArduPilot Mega / Arducopter boards.
It is compatible with APM v1.x board (ATMEGA 328p), Phonedrone and futur boards using ATmega32u2
It is compatible with APM v1.x board (ATMEGA 328p), Phonedrone (using ATmega32u2) and futur boards.
Emphasis has been put on code simplicity and reliability.
@ -13,18 +13,19 @@ Emphasis has been put on code simplicity and reliability.
--------------------------------------------------------------------------------------------------
--------------------------------------------------
Warning - Warning - Warning - Warning
Warning
--------------------------------------------------
Code has not yet been extensively tested in the field.
Code has not yet been massively tested in the field.
Nevertheless extensive tests have been done in the lab with a limited set of different receivers, and a limited number of users did report very sucessfull results.
Nevertheless extensive tests have been done in the lab with a limited set of different receivers.
Carefully check that your radio is working perfectly before you fly.
If you see the blue status LED blinking blinking very fast, this is an indication that something is wrong in the decoding.
If you see the blue status LED blinking very fast very often, this is an indication that something is wrong in the decoding. Rare decoding errors do not hurt.
If you have problems, please report the problem and what brand/modell receiver you are using.
@ -44,10 +45,12 @@ Blue status LED is used for status reports :
------------------------------------------
Passthrough mode (mux)
Radio Passthrough mode (mux)
------------------------------------------
Passthrough mode is trigged by channel 8 > 1800 us.
This mode is described as hardware failsafe in Arduplane terminology.
Radio Passthrough mode is trigged by channel 8 > 1800 us.
Blue status LED has different behavior in passthrough mode :
@ -64,7 +67,9 @@ If all contact with the receiver is lost, an internal failsafe is trigged after
Default failsafe values are :
Throttle channel low (channel 3 = 1000 us)
Throttle channel low (channel 3 = 900 us)
Mode Channel set to flight mode 4 (channel 5 = 1555 us)
All other channels set to midstick (1500 us)

10
apo/ControllerPlane.h
View File

@ -16,7 +16,7 @@ class ControllerPlane: public AP_Controller {
public:
ControllerPlane(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode),
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,this,ch_thrust,ch_yaw,0.1,-0.9,0.9),ch_mode,k_cntrl),
_trimGroup(k_trim, PSTR("trim_")),
_rdrAilMix(&_group, 2, rdrAilMix, PSTR("rdrAilMix")),
_needsTrim(false),
@ -100,7 +100,7 @@ private:
_rudder += _rdrTrim;
_throttle += _thrTrim;
}
void setMotorsActive() {
void setMotors() {
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
@ -111,6 +111,12 @@ private:
_hal->rc[ch_thrust]->setPosition(_throttle);
}
}
void handleFailsafe() {
// note if testing and communication is lost the motors will not shut off,
// it will attempt to land
_guide->setMode(MAV_NAV_LANDING);
setMode(MAV_MODE_AUTO);
}
// attributes
enum {

53
apo/ControllerQuad.h
View File

@ -18,7 +18,7 @@ class ControllerQuad: public AP_Controller {
public:
ControllerQuad(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal) :
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode),
AP_Controller(nav, guide, hal, new AP_ArmingMechanism(hal,this,ch_thrust,ch_yaw,0.1,-0.9,0.9), ch_mode, k_cntrl),
pidRoll(new AP_Var_group(k_pidRoll, PSTR("ROLL_")), 1,
PID_ATT_P, PID_ATT_I, PID_ATT_D, PID_ATT_AWU,
PID_ATT_LIM, PID_ATT_DFCUT),
@ -31,12 +31,10 @@ public:
pidYawRate(new AP_Var_group(k_pidYawRate, PSTR("YAWRT_")), 1,
PID_YAWSPEED_P, PID_YAWSPEED_I, PID_YAWSPEED_D,
PID_YAWSPEED_AWU, PID_YAWSPEED_LIM, PID_YAWSPEED_DFCUT),
pidPN(new AP_Var_group(k_pidPN, PSTR("NORTH_")), 1, PID_POS_P,
PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_DFCUT),
pidPE(new AP_Var_group(k_pidPE, PSTR("EAST_")), 1, PID_POS_P,
PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM, PID_POS_DFCUT),
pidTilt(new AP_Var_group(k_pidTilt, PSTR("TILT_")), 1, PID_TILT_P,
PID_TILT_I, PID_TILT_D, PID_TILT_AWU, PID_TILT_LIM, PID_TILT_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),
PID_POS_Z_I, PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM, PID_POS_Z_DFCUT),
_thrustMix(0), _pitchMix(0), _rollMix(0), _yawMix(0),
_cmdRoll(0), _cmdPitch(0), _cmdYawRate(0) {
_hal->debug->println_P(PSTR("initializing quad controller"));
@ -88,27 +86,18 @@ private:
void autoLoop(const float dt) {
autoPositionLoop(dt);
autoAttitudeLoop(dt);
// XXX currently auto loop not tested, so
// put vehicle in standby
_hal->setState(MAV_STATE_STANDBY);
}
void autoPositionLoop(float dt) {
float cmdNorthTilt = pidPN.update(_nav->getPN(),_nav->getVN(),dt);
float cmdEastTilt = pidPE.update(_nav->getPE(),_nav->getVE(),dt);
float cmdDown = pidPD.update(_nav->getPD(),_nav->getVD(),dt);
// XXX need to add waypoint coordinates
//float cmdNorthTilt = pidPN.update(_guide->getPNError(),_nav->getVN(),dt);
//float cmdEastTilt = pidPE.update(_guide->getPEError(),_nav->getVE(),dt);
float cmdTilt = pidTilt.update(_guide->getDistanceToNextWaypoint(),dt);
float cmdDown = pidPD.update(_guide->getPDError(),_nav->getVD(),dt);
// "transform-to-body"
{
float trigSin = sin(-_nav->getYaw());
float trigCos = cos(-_nav->getYaw());
_cmdPitch = cmdEastTilt * trigCos - cmdNorthTilt * trigSin;
_cmdRoll = -cmdEastTilt * trigSin + cmdNorthTilt * trigCos;
// note that the north tilt is negative of the pitch
}
_cmdYawRate = 0;
_thrustMix = THRUST_HOVER_OFFSET + cmdDown;
_cmdPitch = -cmdTilt*cos(_guide->getHeadingError());
_cmdRoll = cmdTilt*sin(_guide->getHeadingError());
_cmdYawRate = pidYaw.update(_guide->getHeadingError(),_nav->getYawRate(),dt); // always points to next waypoint
_thrustMix = THRUST_HOVER_OFFSET - cmdDown;
// "thrust-trim-adjust"
if (fabs(_cmdRoll) > 0.5) _thrustMix *= 1.13949393;
@ -116,6 +105,9 @@ private:
if (fabs(_cmdPitch) > 0.5) _thrustMix *= 1.13949393;
else _thrustMix /= cos(_cmdPitch);
// debug for position loop
_hal->debug->printf_P(PSTR("cmd: tilt(%f), down(%f), pitch(%f), roll(%f)\n"),cmdTilt,cmdDown,_cmdPitch,_cmdRoll);
}
void autoAttitudeLoop(float dt) {
_rollMix = pidRoll.update(_cmdRoll - _nav->getRoll(),
@ -124,7 +116,7 @@ private:
_nav->getPitchRate(), dt);
_yawMix = pidYawRate.update(_cmdYawRate - _nav->getYawRate(), dt);
}
void setMotorsActive() {
void setMotors() {
// turn all motors off if below 0.1 throttle
if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
setAllRadioChannelsToNeutral();
@ -136,6 +128,11 @@ private:
}
}
void handleFailsafe() {
// turn off
setMode(MAV_MODE_LOCKED);
}
// attributes
/**
* note that these are not the controller radio channel numbers, they are just
@ -167,8 +164,7 @@ private:
enum {
k_pidGroundSpeed2Throttle = k_controllersStart,
k_pidStr,
k_pidPN,
k_pidPE,
k_pidTilt,
k_pidPD,
k_pidRoll,
k_pidPitch,
@ -177,7 +173,8 @@ private:
};
BlockPIDDfb pidRoll, pidPitch, pidYaw;
BlockPID pidYawRate;
BlockPIDDfb pidPN, pidPE, pidPD;
BlockPID pidTilt;
BlockPIDDfb pidPD;
float _thrustMix, _pitchMix, _rollMix, _yawMix;
float _cmdRoll, _cmdPitch, _cmdYawRate;
};

25
apo/QuadArducopter.h
View File

@ -10,9 +10,9 @@
// vehicle options
static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD;
static const apo::halMode_t halMode = apo::MODE_LIVE;
static const apo::halMode_t halMode = apo::MODE_HIL_CNTL;
static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
static const uint8_t heartBeatTimeout = 3;
static const uint8_t heartBeatTimeout = 0;
// algorithm selection
#define CONTROLLER_CLASS ControllerQuad
@ -30,7 +30,7 @@ static const uint8_t heartBeatTimeout = 3;
static const uint32_t debugBaud = 57600;
static const uint32_t telemBaud = 57600;
static const uint32_t gpsBaud = 38400;
static const uint32_t hilBaud = 57600;
static const uint32_t hilBaud = 115200;
// optional sensors
static const bool gpsEnabled = false;
@ -61,17 +61,18 @@ static const float loop2Rate = 1; // gcs slow
static const float loop3Rate = 0.1;
// position control loop
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_TILT_P = 0.1;
static const float PID_TILT_I = 0;
static const float PID_TILT_D = 0.1;
static const float PID_TILT_LIM = 0.04; // about 2 deg
static const float PID_TILT_AWU = 0.02; // about 1 deg
static const float PID_TILT_DFCUT = 10; // cut derivative feedback at 10 hz
static const float PID_POS_Z_P = 0.1;
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_D = 0.2;
static const float PID_POS_Z_LIM = 0.1;
static const float PID_POS_Z_AWU = 0;
static const float PID_POS_DFCUT = 10; // cut derivative feedback at 10 hz
static const float PID_POS_Z_DFCUT = 10; // cut derivative feedback at 10 hz
// attitude control loop
static const float PID_ATT_P = 0.17;

5
apo/apo.pde
View File

@ -8,7 +8,6 @@
#include <AP_ADC.h>
#include <AP_DCM.h>
#include <AP_Compass.h>
#include <Wire.h>
#include <AP_GPS.h>
#include <AP_IMU.h>
#include <APM_BMP085.h>
@ -16,8 +15,8 @@
#include <APO.h>
// Vehicle Configuration
//#include "QuadArducopter.h"
#include "PlaneEasystar.h"
#include "QuadArducopter.h"
//#include "PlaneEasystar.h"
// ArduPilotOne Default Setup
#include "APO_DefaultSetup.h"

201
cmake/modules/FindArduino.cmake
View File

@ -280,7 +280,10 @@ function(setup_arduino_core VAR_NAME BOARD_ID)
if(BOARD_CORE AND NOT TARGET ${CORE_LIB_NAME})
set(BOARD_CORE_PATH ${ARDUINO_CORES_PATH}/${BOARD_CORE})
find_sources(CORE_SRCS ${BOARD_CORE_PATH} True)
list(REMOVE_ITEM CORE_SRCS "${BOARD_CORE_PATH}/main.cxx")
# Debian/Ubuntu fix
list(REMOVE_ITEM CORE_SRCS "${BOARD_CORE_PATH}/main.cxx")
add_library(${CORE_LIB_NAME} ${CORE_SRCS})
set(${VAR_NAME} ${CORE_LIB_NAME} PARENT_SCOPE)
endif()
@ -317,7 +320,7 @@ function(find_arduino_libraries VAR_NAME SRCS)
get_property(LIBRARY_SEARCH_PATH
DIRECTORY # Property Scope
PROPERTY LINK_DIRECTORIES)
foreach(LIB_SEARCH_PATH ${LIBRARY_SEARCH_PATH} ${ARDUINO_LIBRARIES_PATH} ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/libraries)
foreach(LIB_SEARCH_PATH ${LIBRARY_SEARCH_PATH} ${ARDUINO_LIBRARIES_PATH} ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/libraries)
if(EXISTS ${LIB_SEARCH_PATH}/${INCLUDE_NAME}/${CMAKE_MATCH_1})
list(APPEND ARDUINO_LIBS ${LIB_SEARCH_PATH}/${INCLUDE_NAME})
break()
@ -340,23 +343,27 @@ endfunction()
#
# Creates an Arduino library, with all it's library dependencies.
#
# "LIB_NAME"_RECURSE controls if the library will recurse
# when looking for source files
# ${LIB_NAME}_RECURSE controls if the library will recurse
# when looking for source files.
#
# For known libraries can list recurse here
set(Wire_RECURSE True)
set(Ethernet_RECURSE True)
function(setup_arduino_library VAR_NAME BOARD_ID LIB_PATH)
set(LIB_TARGETS)
get_filename_component(LIB_NAME ${LIB_PATH} NAME)
set(TARGET_LIB_NAME ${BOARD_ID}_${LIB_NAME})
if(NOT TARGET ${TARGET_LIB_NAME})
string(REGEX REPLACE ".*/" "" LIB_SHORT_NAME ${LIB_NAME})
#message(STATUS "short name: ${LIB_SHORT_NAME} recures: ${${LIB_SHORT_NAME}_RECURSE}")
if (NOT DEFINED ${LIB_SHORT_NAME}_RECURSE)
set(${LIB_SHORT_NAME}_RECURSE False)
endif()
find_sources(LIB_SRCS ${LIB_PATH} ${${LIB_SHORT_NAME}_RECURSE})
string(REGEX REPLACE ".*/" "" LIB_SHORT_NAME ${LIB_NAME})
# Detect if recursion is needed
if (NOT DEFINED ${LIB_SHORT_NAME}_RECURSE)
set(${LIB_SHORT_NAME}_RECURSE False)
endif()
find_sources(LIB_SRCS ${LIB_PATH} ${${LIB_SHORT_NAME}_RECURSE})
if(LIB_SRCS)
message(STATUS "Generating Arduino ${LIB_NAME} library")
@ -393,7 +400,7 @@ endfunction()
#
function(setup_arduino_libraries VAR_NAME BOARD_ID SRCS)
set(LIB_TARGETS)
find_arduino_libraries(TARGET_LIBS ${SRCS})
find_arduino_libraries(TARGET_LIBS "${SRCS}")
foreach(TARGET_LIB ${TARGET_LIBS})
setup_arduino_library(LIB_DEPS ${BOARD_ID} ${TARGET_LIB}) # Create static library instead of returning sources
list(APPEND LIB_TARGETS ${LIB_DEPS})
@ -443,16 +450,19 @@ function(setup_arduino_upload BOARD_ID TARGET_NAME PORT)
if(DEFINED ${TARGET_NAME}_AFLAGS)
set(AVRDUDE_FLAGS ${${TARGET_NAME}_AFLAGS})
endif()
if (${${BOARD_ID}.upload.protocol} STREQUAL "stk500")
set(${BOARD_ID}.upload.protocol "stk500v1")
endif()
add_custom_target(${TARGET_NAME}-upload
${ARDUINO_AVRDUDE_PROGRAM}
-U flash:w:${TARGET_NAME}.hex
${AVRDUDE_FLAGS}
-C ${ARDUINO_AVRDUDE_CONFIG_PATH}
-p ${${BOARD_ID}.build.mcu}
-c ${${BOARD_ID}.upload.protocol}
-b ${${BOARD_ID}.upload.speed}
-P ${PORT}
DEPENDS ${TARGET_NAME})
${AVRDUDE_FLAGS}
-C${ARDUINO_AVRDUDE_CONFIG_PATH}
-p${${BOARD_ID}.build.mcu}
-c${${BOARD_ID}.upload.protocol}
-P${PORT} -b${${BOARD_ID}.upload.speed}
#-D
-Uflash:w:${CMAKE_BINARY_DIR}/${TARGET_NAME}.hex:i
DEPENDS ${TARGET_NAME})
if(NOT TARGET upload)
add_custom_target(upload)
endif()
@ -468,20 +478,21 @@ endfunction()
# Finds all C/C++ sources located at the specified path.
#
function(find_sources VAR_NAME LIB_PATH RECURSE)
set(FILE_SEARCH_LIST
${LIB_PATH}/*.cpp
${LIB_PATH}/*.c
${LIB_PATH}/*.cc
${LIB_PATH}/*.cxx
${LIB_PATH}/*.h
${LIB_PATH}/*.hh
${LIB_PATH}/*.hxx)
if (RECURSE)
file(GLOB_RECURSE LIB_FILES ${FILE_SEARCH_LIST})
else()
file(GLOB LIB_FILES ${FILE_SEARCH_LIST})
endif()
#message(STATUS "${LIB_PATH} recurse: ${RECURSE}")
set(FILE_SEARCH_LIST
${LIB_PATH}/*.cpp
${LIB_PATH}/*.c
${LIB_PATH}/*.cc
${LIB_PATH}/*.cxx
${LIB_PATH}/*.h
${LIB_PATH}/*.hh
${LIB_PATH}/*.hxx)
if(RECURSE)
file(GLOB_RECURSE LIB_FILES ${FILE_SEARCH_LIST})
else()
file(GLOB LIB_FILES ${FILE_SEARCH_LIST})
endif()
if(LIB_FILES)
set(${VAR_NAME} ${LIB_FILES} PARENT_SCOPE)
endif()
@ -522,75 +533,79 @@ endfunction()
# Setting up Arduino enviroment settings
if(NOT ARDUINO_FOUND)
find_file(ARDUINO_CORES_PATH
NAMES cores
PATHS ${ARDUINO_SDK_PATH}
PATH_SUFFIXES hardware/arduino)
find_file(ARDUINO_CORES_PATH
NAMES cores
PATHS ${ARDUINO_SDK_PATH}
PATH_SUFFIXES hardware/arduino
NO_DEFAULT_PATH)
find_file(ARDUINO_LIBRARIES_PATH
NAMES libraries
PATHS ${ARDUINO_SDK_PATH})
find_file(ARDUINO_LIBRARIES_PATH
NAMES libraries
PATHS ${ARDUINO_SDK_PATH}
NO_DEFAULT_PATH)
find_file(ARDUINO_BOARDS_PATH
NAMES boards.txt
PATHS ${ARDUINO_SDK_PATH}
PATH_SUFFIXES hardware/arduino)
find_file(ARDUINO_BOARDS_PATH
NAMES boards.txt
PATHS ${ARDUINO_SDK_PATH}
PATH_SUFFIXES hardware/arduino
NO_DEFAULT_PATH)
find_file(ARDUINO_PROGRAMMERS_PATH
NAMES programmers.txt
PATHS ${ARDUINO_SDK_PATH}
PATH_SUFFIXES hardware/arduino)
find_file(ARDUINO_PROGRAMMERS_PATH
NAMES programmers.txt
PATHS ${ARDUINO_SDK_PATH}
PATH_SUFFIXES hardware/arduino
NO_DEFAULT_PATH)
find_file(ARDUINO_REVISIONS_PATH
NAMES revisions.txt
PATHS ${ARDUINO_SDK_PATH})
find_file(ARDUINO_REVISIONS_PATH
NAMES revisions.txt
PATHS ${ARDUINO_SDK_PATH}
NO_DEFAULT_PATH)
find_file(ARDUINO_VERSION_PATH
NAMES lib/version.txt
PATHS ${ARDUINO_SDK_PATH})
find_file(ARDUINO_VERSION_PATH
NAMES lib/version.txt
PATHS ${ARDUINO_SDK_PATH}
NO_DEFAULT_PATH)
find_program(ARDUINO_AVRDUDE_PROGRAM
NAMES avrdude
PATHS ${ARDUINO_SDK_PATH}
PATH_SUFFIXES hardware/tools)
find_program(ARDUINO_AVRDUDE_PROGRAM
NAMES avrdude
PATHS ${ARDUINO_SDK_PATH}
PATH_SUFFIXES hardware/tools
NO_DEFAULT_PATH)
find_program(ARDUINO_AVRDUDE_CONFIG_PATH
NAMES avrdude.conf
PATHS ${ARDUINO_SDK_PATH} /etc/avrdude
PATH_SUFFIXES hardware/tools
hardware/tools/avr/etc)
find_file(ARDUINO_AVRDUDE_CONFIG_PATH
NAMES avrdude.conf
PATHS ${ARDUINO_SDK_PATH} /etc/avrdude
PATH_SUFFIXES hardware/tools
hardware/tools/avr/etc
NO_DEFAULT_PATH)
set(ARDUINO_OBJCOPY_EEP_FLAGS -O ihex -j .eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0
CACHE STRING "")
set(ARDUINO_OBJCOPY_HEX_FLAGS -O ihex -R .eeprom
CACHE STRING "")
set(ARDUINO_AVRDUDE_FLAGS -V -F
CACHE STRING "Arvdude global flag list.")
set(ARDUINO_OBJCOPY_EEP_FLAGS -O ihex -j .eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0
CACHE STRING "")
set(ARDUINO_OBJCOPY_HEX_FLAGS -O ihex -R .eeprom
CACHE STRING "")
set(ARDUINO_AVRDUDE_FLAGS -V -F
CACHE STRING "Arvdude global flag list.")
if(ARDUINO_SDK_PATH)
detect_arduino_version(ARDUINO_SDK_VERSION)
set(ARDUINO_SDK_VERSION ${ARDUINO_SDK_VERSION} CACHE STRING "Arduino SDK Version")
endif(ARDUINO_SDK_PATH)
if(ARDUINO_SDK_PATH)
detect_arduino_version(ARDUINO_SDK_VERSION)
set(ARDUINO_SDK_VERSION ${ARDUINO_SDK_VERSION} CACHE STRING "Arduino SDK Version")
endif(ARDUINO_SDK_PATH)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Arduino
REQUIRED_VARS ARDUINO_SDK_PATH
ARDUINO_SDK_VERSION
VERSION_VAR ARDUINO_SDK_VERSION)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Arduino
REQUIRED_VARS ARDUINO_SDK_PATH
ARDUINO_SDK_VERSION
VERSION_VAR ARDUINO_SDK_VERSION)
mark_as_advanced(ARDUINO_CORES_PATH
ARDUINO_SDK_VERSION
ARDUINO_LIBRARIES_PATH
ARDUINO_BOARDS_PATH
ARDUINO_PROGRAMMERS_PATH
ARDUINO_REVISIONS_PATH
ARDUINO_AVRDUDE_PROGRAM
ARDUINO_AVRDUDE_CONFIG_PATH
ARDUINO_OBJCOPY_EEP_FLAGS
ARDUINO_OBJCOPY_HEX_FLAGS)
load_board_settings()
endif()
mark_as_advanced(ARDUINO_CORES_PATH
ARDUINO_SDK_VERSION
ARDUINO_LIBRARIES_PATH
ARDUINO_BOARDS_PATH
ARDUINO_PROGRAMMERS_PATH
ARDUINO_REVISIONS_PATH
ARDUINO_AVRDUDE_PROGRAM
ARDUINO_AVRDUDE_CONFIG_PATH
ARDUINO_OBJCOPY_EEP_FLAGS
ARDUINO_OBJCOPY_HEX_FLAGS)
load_board_settings()

6
cmake/toolchains/Arduino.cmake
View File

@ -7,7 +7,7 @@ set(CMAKE_CXX_COMPILER avr-g++)
# C Flags #
#=============================================================================#
if (NOT DEFINED ARDUINO_C_FLAGS)
set(ARDUINO_C_FLAGS "-ffunction-sections -fdata-sections")
set(ARDUINO_C_FLAGS "-ffunction-sections -fdata-sections")
endif()
set(CMAKE_C_FLAGS "-g -Os ${ARDUINO_C_FLAGS}" CACHE STRING "")
set(CMAKE_C_FLAGS_DEBUG "-g ${ARDUINO_C_FLAGS}" CACHE STRING "")
@ -19,7 +19,7 @@ set(CMAKE_C_FLAGS_RELWITHDEBINFO "-0s -g -w ${ARDUINO_C_FLAGS}" CACHE STRI
# C++ Flags #
#=============================================================================#
if (NOT DEFINED ARDUINO_CXX_FLAGS)
set(ARDUINO_CXX_FLAGS "${ARDUINO_C_FLAGS} -fno-exceptions")
set(ARDUINO_CXX_FLAGS "${ARDUINO_C_FLAGS} -fno-exceptions")
endif()
set(CMAKE_CXX_FLAGS "-g -Os ${ARDUINO_CXX_FLAGS}" CACHE STRING "")
set(CMAKE_CXX_FLAGS_DEBUG "-g ${ARDUINO_CXX_FLAGS}" CACHE STRING "")
@ -31,7 +31,7 @@ set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-0s -g ${ARDUINO_CXX_FLAGS}" CACHE STR
# Executable Linker Flags #
#=============================================================================#
if (NOT DEFINED ARDUINO_LINKER_FLAGS)
set(ARDUINO_LINKER_FLAGS "-Wl,--gc-sections")
set(ARDUINO_LINKER_FLAGS "-Wl,--gc-sections")
endif()
set(CMAKE_EXE_LINKER_FLAGS "${ARDUINO_LINKER_FLAGS}" CACHE STRING "")
set(CMAKE_EXE_LINKER_FLAGS_DEBUG "${ARDUINO_LINKER_FLAGS}" CACHE STRING "")

2
cmake/updated-arduino-cmake.sh
View File

@ -1,5 +1,5 @@
#!/bin/bash
git clone git@github.com:jgoppert/arduino-cmake.git tmp
git clone git://github.com/jgoppert/arduino-cmake.git tmp
cp -rf tmp/cmake/modules/* modules
cp -rf tmp/cmake/toolchains/* toolchains
rm -rf tmp

1
libraries/APO/APO.h
View File

@ -11,6 +11,7 @@
#include "AP_Autopilot.h"
#include "AP_Guide.h"
#include "AP_Controller.h"
#include "AP_ControllerBlock.h"
#include "AP_HardwareAbstractionLayer.h"
#include "AP_MavlinkCommand.h"
#include "AP_Navigator.h"

16
libraries/APO/AP_ArmingMechanism.cpp
View File

@ -3,8 +3,8 @@
*
*/
#include "AP_ArmingMechanism.h"
#include "AP_Controller.h"
#include "AP_RcChannel.h"
#include "../FastSerial/FastSerial.h"
#include "AP_HardwareAbstractionLayer.h"
@ -15,7 +15,7 @@ namespace apo {
void AP_ArmingMechanism::update(const float dt) {
//_hal->debug->printf_P(PSTR("ch1: %f\tch2: %f\n"),_hal->rc[_ch1]->getRadioPosition(), _hal->rc[_ch2]->getRadioPosition());
// arming
if ( (_hal->getState() != MAV_STATE_ACTIVE) &&
if ( (_controller->getState() != MAV_STATE_ACTIVE) &&
(fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
(_hal->rc[_ch2]->getRadioPosition() < _ch2Min) ) {
@ -23,14 +23,13 @@ void AP_ArmingMechanism::update(const float dt) {
if (_armingClock<0) _armingClock = 0;
if (_armingClock++ >= 100) {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
_hal->setState(MAV_STATE_ACTIVE);
_controller->setMode(MAV_MODE_READY);
} else {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("arming"));
}
}
// disarming
else if ( (_hal->getState() == MAV_STATE_ACTIVE) &&
else if ( (_controller->getState() == MAV_STATE_ACTIVE) &&
(fabs(_hal->rc[_ch1]->getRadioPosition()) < _ch1Min) &&
(_hal->rc[_ch2]->getRadioPosition() > _ch2Max) ) {
@ -38,8 +37,7 @@ void AP_ArmingMechanism::update(const float dt) {
if (_armingClock>0) _armingClock = 0;
if (_armingClock-- <= -100) {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
_hal->setState(MAV_STATE_STANDBY);
_controller->setMode(MAV_MODE_LOCKED);
} else {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarming"));
}
@ -47,8 +45,8 @@ void AP_ArmingMechanism::update(const float dt) {
// 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"));
if (_controller->getState()==MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
else if (_controller->getState()!=MAV_STATE_ACTIVE) _hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
}
}

8
libraries/APO/AP_ArmingMechanism.h
View File

@ -12,6 +12,7 @@
namespace apo {
class AP_HardwareAbstractionLayer;
class AP_Controller;
class AP_ArmingMechanism {
@ -25,10 +26,10 @@ public:
* @param ch2Min: disarms below this
* @param ch2Max: arms above this
*/
AP_ArmingMechanism(AP_HardwareAbstractionLayer * hal,
AP_ArmingMechanism(AP_HardwareAbstractionLayer * hal, AP_Controller * controller,
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) {
float ch2Max) : _armingClock(0), _hal(hal), _controller(controller),
_ch1(ch1), _ch2(ch2), _ch1Min(ch1Min), _ch2Min(ch2Min), _ch2Max(ch2Max) {
}
/**
@ -52,6 +53,7 @@ public:
private:
AP_HardwareAbstractionLayer * _hal;
AP_Controller * _controller;
int8_t _armingClock;
uint8_t _ch1; /// typically throttle channel
uint8_t _ch2; /// typically yaw channel

7
libraries/APO/AP_Autopilot.cpp
View File

@ -37,7 +37,6 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
_controller(controller), _hal(hal),
callbackCalls(0) {
hal->setState(MAV_STATE_BOOT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
@ -50,7 +49,7 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
/*
* Calibration
*/
hal->setState(MAV_STATE_CALIBRATING);
controller->setState(MAV_STATE_CALIBRATING);
hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
@ -110,6 +109,8 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
AP_MavlinkCommand::home.getLon()*rad2Deg,
AP_MavlinkCommand::home.getCommand());
guide->setCurrentIndex(0);
controller->setMode(MAV_MODE_LOCKED);
controller->setState(MAV_STATE_STANDBY);
/*
* Attach loops, stacking for priority
@ -122,7 +123,6 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
hal->debug->println_P(PSTR("running"));
hal->gcs->sendText(SEVERITY_LOW, PSTR("running"));
hal->setState(MAV_STATE_STANDBY);
}
void AP_Autopilot::callback(void * data) {
@ -232,6 +232,7 @@ void AP_Autopilot::callback2(void * data) {
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_LOCAL_POSITION);
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU);
}

143
libraries/APO/AP_CommLink.cpp
View File

@ -105,11 +105,18 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION: {
mavlink_msg_local_position_send(_channel, timeStamp,
_navigator->getPN(),_navigator->getPE(), _navigator->getPD(),
_navigator->getVN(), _navigator->getVE(), _navigator->getVD());
break;
}
case MAVLINK_MSG_ID_GPS_RAW: {
mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(),
_navigator->getLat() * rad2Deg,
_navigator->getLon() * rad2Deg, _navigator->getAlt(), 0, 0,
_navigator->getGroundSpeed(),
_navigator->getGroundSpeed()*1000.0,
_navigator->getYaw() * rad2Deg);
break;
}
@ -124,21 +131,23 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
*/
case MAVLINK_MSG_ID_SCALED_IMU: {
/*
* accel/gyro debug
*/
/*
Vector3f accel = _hal->imu->get_accel();
Vector3f gyro = _hal->imu->get_gyro();
Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"),
accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z);
*/
Vector3f accel = _hal->imu->get_accel();
Vector3f gyro = _hal->imu->get_gyro();
mavlink_msg_raw_imu_send(_channel, timeStamp, 1000 * accel.x,
1000 * accel.y, 1000 * accel.z, 1000 * gyro.x,
1000 * gyro.y, 1000 * gyro.z, _hal->compass->mag_x,
_hal->compass->mag_y, _hal->compass->mag_z); // XXX THIS IS NOT SCALED FOR MAG
int16_t xmag, ymag, zmag;
xmag = ymag = zmag = 0;
if (_hal->compass) {
// XXX THIS IS NOT SCALED
xmag = _hal->compass->mag_x;
ymag = _hal->compass->mag_y;
zmag = _hal->compass->mag_z;
}
mavlink_msg_scaled_imu_send(_channel, timeStamp,
_navigator->getXAccel()*1e3,
_navigator->getYAccel()*1e3,
_navigator->getZAccel()*1e3,
_navigator->getRollRate()*1e3,
_navigator->getPitchRate()*1e3,
_navigator->getYawRate()*1e3,
xmag, ymag, zmag);
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
@ -174,7 +183,7 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
batteryVoltage = _hal->batteryMonitor->getVoltage()*1000;
}
mavlink_msg_sys_status_send(_channel, _controller->getMode(),
_guide->getMode(), _hal->getState(), _hal->load * 10,
_guide->getMode(), _controller->getState(), _hal->load * 10,
batteryVoltage, batteryPercentage, _packetDrops);
break;
}
@ -328,6 +337,29 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
break;
}
case MAVLINK_MSG_ID_HIL_STATE: {
// decode
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(msg, &packet);
_navigator->setTimeStamp(timeStamp);
_navigator->setRoll(packet.roll);
_navigator->setPitch(packet.pitch);
_navigator->setYaw(packet.yaw);
_navigator->setRollRate(packet.rollspeed);
_navigator->setPitchRate(packet.pitchspeed);
_navigator->setYawRate(packet.yawspeed);
_navigator->setVN(packet.vx/ 1e2);
_navigator->setVE(packet.vy/ 1e2);
_navigator->setVD(packet.vz/ 1e2);
_navigator->setLat_degInt(packet.lat);
_navigator->setLon_degInt(packet.lon);
_navigator->setAlt(packet.alt / 1e3);
_navigator->setXAccel(packet.xacc/ 1e3);
_navigator->setYAccel(packet.xacc/ 1e3);
_navigator->setZAccel(packet.xacc/ 1e3);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
// decode
mavlink_attitude_t packet;
@ -364,31 +396,66 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
AP_Var::save_all();
break;
case MAV_ACTION_CALIBRATE_RC:
case MAV_ACTION_MOTORS_START:
_controller->setMode(MAV_MODE_READY);
break;
case MAV_ACTION_CALIBRATE_GYRO:
case MAV_ACTION_CALIBRATE_MAG:
case MAV_ACTION_CALIBRATE_ACC:
case MAV_ACTION_CALIBRATE_PRESSURE:
_controller->setMode(MAV_MODE_LOCKED);
_navigator->calibrate();
break;
case MAV_ACTION_EMCY_KILL:
case MAV_ACTION_CONFIRM_KILL:
case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN:
_controller->setMode(MAV_MODE_LOCKED);
break;
case MAV_ACTION_LAUNCH:
case MAV_ACTION_TAKEOFF:
_guide->setMode(MAV_NAV_LIFTOFF);
break;
case MAV_ACTION_LAND:
_guide->setCurrentIndex(0);
_guide->setMode(MAV_NAV_LANDING);
break;
case MAV_ACTION_EMCY_LAND:
_guide->setMode(MAV_NAV_LANDING);
break;
case MAV_ACTION_LOITER:
case MAV_ACTION_HALT:
_guide->setMode(MAV_NAV_LOITER);
break;
case MAV_ACTION_SET_AUTO:
_controller->setMode(MAV_MODE_AUTO);
break;
case MAV_ACTION_SET_MANUAL:
_controller->setMode(MAV_MODE_MANUAL);
break;
case MAV_ACTION_RETURN:
_guide->setMode(MAV_NAV_RETURNING);
break;
case MAV_ACTION_NAVIGATE:
case MAV_ACTION_CONTINUE:
_guide->setMode(MAV_NAV_WAYPOINT);
break;
case MAV_ACTION_CALIBRATE_RC:
case MAV_ACTION_REBOOT:
case MAV_ACTION_REC_START:
case MAV_ACTION_REC_PAUSE:
case MAV_ACTION_REC_STOP:
case MAV_ACTION_TAKEOFF:
case MAV_ACTION_LAND:
case MAV_ACTION_NAVIGATE:
case MAV_ACTION_LOITER:
case MAV_ACTION_MOTORS_START:
case MAV_ACTION_CONFIRM_KILL:
case MAV_ACTION_EMCY_KILL:
case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN:
case MAV_ACTION_CONTINUE:
case MAV_ACTION_SET_MANUAL:
case MAV_ACTION_SET_AUTO:
case MAV_ACTION_LAUNCH:
case MAV_ACTION_RETURN:
case MAV_ACTION_EMCY_LAND:
case MAV_ACTION_HALT:
sendText(SEVERITY_LOW, PSTR("action not implemented"));
break;
default:
@ -582,6 +649,14 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK);
_receivingCmds = false;
_guide->setNumberOfCommands(_cmdNumberRequested);
// make sure curernt waypoint still exists
if (_cmdNumberRequested > _guide->getCurrentIndex()) {
_guide->setCurrentIndex(0);
mavlink_msg_waypoint_current_send(_channel,
_guide->getCurrentIndex());
}
//sendText(SEVERITY_LOW, PSTR("waypoint ack sent"));
} else if (_cmdRequestIndex > _cmdNumberRequested) {
_receivingCmds = false;

100
libraries/APO/AP_Controller.cpp
View File

@ -5,16 +5,15 @@
* Author: jgoppert
*/
#include "AP_Controller.h"
#include "../FastSerial/FastSerial.h"
#include "AP_ArmingMechanism.h"
#include "AP_BatteryMonitor.h"
#include "AP_HardwareAbstractionLayer.h"
#include "../AP_Common/include/menu.h"
#include "AP_RcChannel.h"
#include "../GCS_MAVLink/include/mavlink_types.h"
#include "constants.h"
#include "AP_CommLink.h"
#include "AP_Controller.h"
namespace apo {
@ -24,7 +23,7 @@ AP_Controller::AP_Controller(AP_Navigator * nav, AP_Guide * guide,
_nav(nav), _guide(guide), _hal(hal), _armingMechanism(armingMechanism),
_group(key, name ? : PSTR("CNTRL_")),
_chMode(chMode),
_mode(&_group, 1, MAV_MODE_LOCKED, PSTR("MODE")) {
_mode(MAV_MODE_LOCKED), _state(MAV_STATE_BOOT) {
setAllRadioChannelsToNeutral();
}
@ -45,82 +44,61 @@ void AP_Controller::setAllRadioChannelsManually() {
void AP_Controller::update(const float dt) {
if (_armingMechanism) _armingMechanism->update(dt);
// determine flight mode
// handle emergencies
//
// check for heartbeat from gcs, if not found go to failsafe
if (_hal->heartBeatLost()) {
_mode = MAV_MODE_FAILSAFE;
setMode(MAV_MODE_FAILSAFE);
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat"));
// if battery less than 5%, go to failsafe
// if battery less than 5%, go to failsafe
} else if (_hal->batteryMonitor && _hal->batteryMonitor->getPercentage() < 5) {
_mode = MAV_MODE_FAILSAFE;
setMode(MAV_MODE_FAILSAFE);
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("recharge battery"));
// manual/auto switch
} else {
// if all emergencies cleared, fall back to standby
if (_hal->getState()==MAV_STATE_EMERGENCY) _hal->setState(MAV_STATE_STANDBY);
if (_hal->rc[_chMode]->getRadioPosition() > 0) _mode = MAV_MODE_MANUAL;
else _mode = MAV_MODE_AUTO;
}
// handle flight modes
switch(_mode) {
case MAV_MODE_LOCKED: {
_hal->setState(MAV_STATE_STANDBY);
break;
// handle modes
//
// if in locked mode
if (getMode() == MAV_MODE_LOCKED) {
// if state is not stanby then set it to standby and alert gcs
if (getState()!=MAV_STATE_STANDBY) {
setState(MAV_STATE_STANDBY);
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("disarmed"));
}
}
// if not locked
else {
// if state is not active, set it to active and alert gcs
if (getState()!=MAV_STATE_ACTIVE) {
setState(MAV_STATE_ACTIVE);
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("armed"));
}
case MAV_MODE_FAILSAFE: {
_hal->setState(MAV_STATE_EMERGENCY);
break;
}
// if in auto mode and manual switch set, change to manual
if (_hal->rc[_chMode]->getRadioPosition() > 0) setMode(MAV_MODE_MANUAL);
else setMode(MAV_MODE_AUTO);
case MAV_MODE_MANUAL: {
manualLoop(dt);
break;
}
case MAV_MODE_AUTO: {
autoLoop(dt);
break;
}
default: {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode"));
_hal->setState(MAV_STATE_EMERGENCY);
}
// handle all possible modes
if (getMode()==MAV_MODE_MANUAL) {
manualLoop(dt);
} else if (getMode()==MAV_MODE_AUTO) {
autoLoop(dt);
} else {
_hal->gcs->sendText(SEVERITY_HIGH, PSTR("unknown mode"));
setMode(MAV_MODE_FAILSAFE);
}
}
// this sends commands to motors
setMotors();
}
void AP_Controller::setMotors() {
switch (_hal->getState()) {
case MAV_STATE_ACTIVE: {
if(getState()==MAV_STATE_ACTIVE) {
digitalWrite(_hal->aLedPin, HIGH);
setMotorsActive();
break;
}
case MAV_STATE_EMERGENCY: {
setMotors();
} else {
digitalWrite(_hal->aLedPin, LOW);
setMotorsEmergency();
break;
}
case MAV_STATE_STANDBY: {
digitalWrite(_hal->aLedPin,LOW);
setMotorsStandby();
break;
}
default: {
setMotorsStandby();
}
setAllRadioChannelsToNeutral();
}
}
} // namespace apo
// vim:ts=4:sw=4:expandtab

378
libraries/APO/AP_Controller.h
View File

@ -19,307 +19,127 @@
#ifndef AP_Controller_H
#define AP_Controller_H
#include <inttypes.h>
#include "../GCS_MAVLink/GCS_MAVLink.h"
#include <math.h>
// inclusions
#include "../AP_Common/AP_Common.h"
#include "../AP_Common/AP_Var.h"
#include "AP_Var_keys.h"
class AP_Var_group;
#include <inttypes.h>
#include <math.h>
#include "../GCS_MAVLink/GCS_MAVLink.h"
namespace apo {
// forward declarations within apo
class AP_HardwareAbstractionLayer;
class AP_Guide;
class AP_Navigator;
class Menu;
class AP_ArmingMechanism;
/// Controller class
///
// The control system class.
// Given where the vehicle wants to go and where it is,
// this class is responsible for sending commands to the
// motors. It is also responsible for monitoring manual
// input.
class AP_Controller {
public:
///
// The controller constructor.
// Creates the control system.
// @nav the navigation system
// @guide the guidance system
// @hal the hardware abstraction layer
// @armingMechanism the device that controls arming/ disarming
// @chMode the channel that the mode switch is on
// @key the unique key for the control system saved AP_Var variables
// @name the name of the control system
AP_Controller(AP_Navigator * nav, AP_Guide * guide,
AP_HardwareAbstractionLayer * hal,
AP_ArmingMechanism * armingMechanism,
const uint8_t _chMode,
const uint16_t key = k_cntrl,
const uint16_t key,
const prog_char_t * name = NULL);
virtual void update(const float dt);
///
// The loop callback function.
// The callback function for the controller loop.
// This is inherited from loop.
// This function cannot be overriden.
// @dt The loop update interval.
void update(const float dt);
///
// This sets all radio outputs to neutral.
// This function cannot be overriden.
void setAllRadioChannelsToNeutral();
///
// This sets all radio outputs using the radio input.
// This function cannot be overriden.
void setAllRadioChannelsManually();
virtual void setMotors();
virtual void setMotorsActive() = 0;
virtual void setMotorsEmergency() {
setAllRadioChannelsToNeutral();
};
virtual void setMotorsStandby() {
setAllRadioChannelsToNeutral();
};
virtual void manualLoop(const float dt) {
setAllRadioChannelsToNeutral();
};
virtual void autoLoop(const float dt) {
setAllRadioChannelsToNeutral();
};
AP_Uint8 getMode() {
///
// Sets the motor pwm outputs.
// This function sets the motors given the control system outputs.
// This function must be defined. There is no default implementation.
virtual void setMotors() = 0;
///
// The manual control loop function.
// This uses radio to control the aircraft.
// This function must be defined. There is no default implementation.
// @dt The loop update interval.
virtual void manualLoop(const float dt) = 0;
///
// The automatic control update function.
// This loop is responsible for taking the
// vehicle to a waypoint.
// This function must be defined. There is no default implementation.
// @dt The loop update interval.
virtual void autoLoop(const float dt) = 0;
///
// Handles failsafe events.
// This function is responsible for setting the mode of the vehicle during
// a failsafe event (low battery, loss of gcs comms, ...).
// This function must be defined. There is no default implementation.
virtual void handleFailsafe() = 0;
///
// The mode accessor.
// @return The current vehicle mode.
MAV_MODE getMode() {
return _mode;
}
///
// The mode setter.
// @mode The mode to set the vehicle to.
void setMode(MAV_MODE mode) {
_mode = mode;
}
///
// The state acessor.
// @return The current state of the vehicle.
MAV_STATE getState() const {
return _state;
}
///
// state setter
// @sate The state to set the vehicle to.
void setState(MAV_STATE state) {
_state = state;
}
protected:
AP_Navigator * _nav;
AP_Guide * _guide;
AP_HardwareAbstractionLayer * _hal;
AP_ArmingMechanism * _armingMechanism;
uint8_t _chMode;
AP_Var_group _group;
AP_Uint8 _mode;
};
class AP_ControllerBlock {
public:
AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart,
uint8_t groupLength) :
_group(group), _groupStart(groupStart),
_groupEnd(groupStart + groupLength) {
}
uint8_t getGroupEnd() {
return _groupEnd;
}
protected:
AP_Var_group * _group; /// helps with parameter management
uint8_t _groupStart;
uint8_t _groupEnd;
};
class BlockLowPass: public AP_ControllerBlock {
public:
BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut,
const prog_char_t * fCutLabel = NULL) :
AP_ControllerBlock(group, groupStart, 1),
_fCut(group, groupStart, fCut, fCutLabel ? : PSTR("fCut")),
_y(0) {
}
float update(const float & input, const float & dt) {
float RC = 1 / (2 * M_PI * _fCut); // low pass filter
_y = _y + (input - _y) * (dt / (dt + RC));
return _y;
}
protected:
AP_Float _fCut;
float _y;
};
class BlockSaturation: public AP_ControllerBlock {
public:
BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax,
const prog_char_t * yMaxLabel = NULL) :
AP_ControllerBlock(group, groupStart, 1),
_yMax(group, groupStart, yMax, yMaxLabel ? : PSTR("yMax")) {
}
float update(const float & input) {
// pid sum
float y = input;
// saturation
if (y > _yMax)
y = _yMax;
if (y < -_yMax)
y = -_yMax;
return y;
}
protected:
AP_Float _yMax; /// output saturation
};
class BlockDerivative {
public:
BlockDerivative() :
_lastInput(0), firstRun(true) {
}
float update(const float & input, const float & dt) {
float derivative = (input - _lastInput) / dt;
_lastInput = input;
if (firstRun) {
firstRun = false;
return 0;
} else
return derivative;
}
protected:
float _lastInput; /// last input
bool firstRun;
};
class BlockIntegral {
public:
BlockIntegral() :
_i(0) {
}
float update(const float & input, const float & dt) {
_i += input * dt;
return _i;
}
protected:
float _i; /// integral
};
class BlockP: public AP_ControllerBlock {
public:
BlockP(AP_Var_group * group, uint8_t groupStart, float kP,
const prog_char_t * kPLabel = NULL) :
AP_ControllerBlock(group, groupStart, 1),
_kP(group, groupStart, kP, kPLabel ? : PSTR("p")) {
}
float update(const float & input) {
return _kP * input;
}
protected:
AP_Float _kP; /// proportional gain
};
class BlockI: public AP_ControllerBlock {
public:
BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax,
const prog_char_t * kILabel = NULL,
const prog_char_t * iMaxLabel = NULL) :
AP_ControllerBlock(group, groupStart, 2),
_kI(group, groupStart, kI, kILabel ? : PSTR("i")),
_blockSaturation(group, groupStart + 1, iMax, iMaxLabel ? : PSTR("iMax")),
_eI(0) {
}
float update(const float & input, const float & dt) {
// integral
_eI += input * dt;
_eI = _blockSaturation.update(_eI);
return _kI * _eI;
}
protected:
AP_Float _kI; /// integral gain
BlockSaturation _blockSaturation; /// integrator saturation
float _eI; /// integral of input
};
class BlockD: public AP_ControllerBlock {
public:
BlockD(AP_Var_group * group, uint8_t groupStart, float kD, uint8_t dFCut,
const prog_char_t * kDLabel = NULL,
const prog_char_t * dFCutLabel = NULL) :
AP_ControllerBlock(group, groupStart, 2),
_blockLowPass(group, groupStart, dFCut,
dFCutLabel ? : PSTR("dFCut")),
_kD(group, _blockLowPass.getGroupEnd(), kD,
kDLabel ? : PSTR("d")), _eP(0) {
}
float update(const float & input, const float & dt) {
// derivative with low pass
float _eD = _blockLowPass.update((_eP - input) / dt, dt);
// proportional, note must come after derivative
// because derivatve uses _eP as previous value
_eP = input;
return _kD * _eD;
}
protected:
BlockLowPass _blockLowPass;
AP_Float _kD; /// derivative gain
float _eP; /// input
};
class BlockPID: public AP_ControllerBlock {
public:
BlockPID(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float kD, float iMax, float yMax, uint8_t dFcut) :
AP_ControllerBlock(group, groupStart, 6),
_blockP(group, groupStart, kP),
_blockI(group, _blockP.getGroupEnd(), kI, iMax),
_blockD(group, _blockI.getGroupEnd(), kD, dFcut),
_blockSaturation(group, _blockD.getGroupEnd(), yMax) {
}
float update(const float & input, const float & dt) {
return _blockSaturation.update(
_blockP.update(input) + _blockI.update(input, dt)
+ _blockD.update(input, dt));
}
protected:
BlockP _blockP;
BlockI _blockI;
BlockD _blockD;
BlockSaturation _blockSaturation;
};
class BlockPI: public AP_ControllerBlock {
public:
BlockPI(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float iMax, float yMax) :
AP_ControllerBlock(group, groupStart, 4),
_blockP(group, groupStart, kP),
_blockI(group, _blockP.getGroupEnd(), kI, iMax),
_blockSaturation(group, _blockI.getGroupEnd(), yMax) {
}
float update(const float & input, const float & dt) {
float y = _blockP.update(input) + _blockI.update(input, dt);
return _blockSaturation.update(y);
}
protected:
BlockP _blockP;
BlockI _blockI;
BlockSaturation _blockSaturation;
};
class BlockPD: public AP_ControllerBlock {
public:
BlockPD(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float kD, float iMax, float yMax, uint8_t dFcut) :
AP_ControllerBlock(group, groupStart, 4),
_blockP(group, groupStart, kP),
_blockD(group, _blockP.getGroupEnd(), kD, dFcut),
_blockSaturation(group, _blockD.getGroupEnd(), yMax) {
}
float update(const float & input, const float & dt) {
float y = _blockP.update(input) + _blockD.update(input, dt);
return _blockSaturation.update(y);
}
protected:
BlockP _blockP;
BlockD _blockD;
BlockSaturation _blockSaturation;
};
/// PID with derivative feedback (ignores reference derivative)
class BlockPIDDfb: public AP_ControllerBlock {
public:
BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float kD, float iMax, float yMax, float dFCut,
const prog_char_t * dFCutLabel = NULL,
const prog_char_t * dLabel = NULL) :
AP_ControllerBlock(group, groupStart, 5),
_blockP(group, groupStart, kP),
_blockI(group, _blockP.getGroupEnd(), kI, iMax),
_blockSaturation(group, _blockI.getGroupEnd(), yMax),
_blockLowPass(group, _blockSaturation.getGroupEnd(), dFCut,
dFCutLabel ? : PSTR("dFCut")),
_kD(group, _blockLowPass.getGroupEnd(), kD, dLabel ? : PSTR("d"))
{
}
float update(const float & input, const float & derivative,
const float & dt) {
float y = _blockP.update(input) + _blockI.update(input, dt) - _kD
* _blockLowPass.update(derivative,dt);
return _blockSaturation.update(y);
}
protected:
BlockP _blockP;
BlockI _blockI;
BlockSaturation _blockSaturation;
BlockLowPass _blockLowPass;
AP_Float _kD; /// derivative gain
AP_Navigator * _nav; /// navigator
AP_Guide * _guide; /// guide
AP_HardwareAbstractionLayer * _hal; /// hardware abstraction layer
AP_ArmingMechanism * _armingMechanism; /// controls arming/ disarming
uint8_t _chMode; /// the channel the mode switch is on
AP_Var_group _group; /// holds controller parameters
MAV_MODE _mode; /// vehicle mode (auto, guided, manual, failsafe, ...)
MAV_STATE _state; /// vehicle state (active, standby, boot, calibrating ...)
};
} // apo

179
libraries/APO/AP_ControllerBlock.cpp Normal file
View File

@ -0,0 +1,179 @@
/*
* AP_ControllerBlock.cpp
*
* Created on: Apr 30, 2011
* Author: jgoppert
*/
#include "AP_ControllerBlock.h"
#include <math.h>
namespace apo {
AP_ControllerBlock::AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart,
uint8_t groupLength) :
_group(group), _groupStart(groupStart),
_groupEnd(groupStart + groupLength) {
}
BlockLowPass::BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut,
const prog_char_t * fCutLabel) :
AP_ControllerBlock(group, groupStart, 1),
_fCut(group, groupStart, fCut, fCutLabel ? : PSTR("fCut")),
_y(0) {
}
float BlockLowPass::update(const float & input, const float & dt) {
float RC = 1 / (2 * M_PI * _fCut); // low pass filter
_y = _y + (input - _y) * (dt / (dt + RC));
return _y;
}
BlockSaturation::BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax,
const prog_char_t * yMaxLabel) :
AP_ControllerBlock(group, groupStart, 1),
_yMax(group, groupStart, yMax, yMaxLabel ? : PSTR("yMax")) {
}
float BlockSaturation::update(const float & input) {
// pid sum
float y = input;
// saturation
if (y > _yMax)
y = _yMax;
if (y < -_yMax)
y = -_yMax;
return y;
}
BlockDerivative::BlockDerivative() :
_lastInput(0), firstRun(true) {
}
float BlockDerivative::update(const float & input, const float & dt) {
float derivative = (input - _lastInput) / dt;
_lastInput = input;
if (firstRun) {
firstRun = false;
return 0;
} else
return derivative;
}
BlockIntegral::BlockIntegral() :
_i(0) {
}
float BlockIntegral::update(const float & input, const float & dt) {
_i += input * dt;
return _i;
}
BlockP::BlockP(AP_Var_group * group, uint8_t groupStart, float kP,
const prog_char_t * kPLabel) :
AP_ControllerBlock(group, groupStart, 1),
_kP(group, groupStart, kP, kPLabel ? : PSTR("p")) {
}
float BlockP::update(const float & input) {
return _kP * input;
}
BlockI::BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax,
const prog_char_t * kILabel,
const prog_char_t * iMaxLabel) :
AP_ControllerBlock(group, groupStart, 2),
_kI(group, groupStart, kI, kILabel ? : PSTR("i")),
_blockSaturation(group, groupStart + 1, iMax, iMaxLabel ? : PSTR("iMax")),
_eI(0) {
}
float BlockI::update(const float & input, const float & dt) {
// integral
_eI += input * dt;
_eI = _blockSaturation.update(_eI);
return _kI * _eI;
}
BlockD::BlockD(AP_Var_group * group, uint8_t groupStart, float kD, uint8_t dFCut,
const prog_char_t * kDLabel,
const prog_char_t * dFCutLabel) :
AP_ControllerBlock(group, groupStart, 2),
_blockLowPass(group, groupStart, dFCut,
dFCutLabel ? : PSTR("dFCut")),
_kD(group, _blockLowPass.getGroupEnd(), kD,
kDLabel ? : PSTR("d")), _eP(0) {
}
float BlockD::update(const float & input, const float & dt) {
// derivative with low pass
float _eD = _blockLowPass.update((input - _eP) / dt, dt);
// proportional, note must come after derivative
// because derivatve uses _eP as previous value
_eP = input;
return _kD * _eD;
}
BlockPID::BlockPID(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float kD, float iMax, float yMax, uint8_t dFcut) :
AP_ControllerBlock(group, groupStart, 6),
_blockP(group, groupStart, kP),
_blockI(group, _blockP.getGroupEnd(), kI, iMax),
_blockD(group, _blockI.getGroupEnd(), kD, dFcut),
_blockSaturation(group, _blockD.getGroupEnd(), yMax) {
}
float BlockPID::update(const float & input, const float & dt) {
return _blockSaturation.update(
_blockP.update(input) + _blockI.update(input, dt)
+ _blockD.update(input, dt));
}
BlockPI::BlockPI(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float iMax, float yMax) :
AP_ControllerBlock(group, groupStart, 4),
_blockP(group, groupStart, kP),
_blockI(group, _blockP.getGroupEnd(), kI, iMax),
_blockSaturation(group, _blockI.getGroupEnd(), yMax) {
}
float BlockPI::update(const float & input, const float & dt) {
float y = _blockP.update(input) + _blockI.update(input, dt);
return _blockSaturation.update(y);
}
BlockPD::BlockPD(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float kD, float iMax, float yMax, uint8_t dFcut) :
AP_ControllerBlock(group, groupStart, 4),
_blockP(group, groupStart, kP),
_blockD(group, _blockP.getGroupEnd(), kD, dFcut),
_blockSaturation(group, _blockD.getGroupEnd(), yMax) {
}
float BlockPD::update(const float & input, const float & dt) {
float y = _blockP.update(input) + _blockD.update(input, dt);
return _blockSaturation.update(y);
}
BlockPIDDfb::BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float kD, float iMax, float yMax, float dFCut,
const prog_char_t * dFCutLabel,
const prog_char_t * dLabel) :
AP_ControllerBlock(group, groupStart, 5),
_blockP(group, groupStart, kP),
_blockI(group, _blockP.getGroupEnd(), kI, iMax),
_blockSaturation(group, _blockI.getGroupEnd(), yMax),
_blockLowPass(group, _blockSaturation.getGroupEnd(), dFCut,
dFCutLabel ? : PSTR("dFCut")),
_kD(group, _blockLowPass.getGroupEnd(), kD, dLabel ? : PSTR("d"))
{
}
float BlockPIDDfb::update(const float & input, const float & derivative,
const float & dt) {
float y = _blockP.update(input) + _blockI.update(input, dt) - _kD
* _blockLowPass.update(derivative,dt);
return _blockSaturation.update(y);
}
} // namespace apo
// vim:ts=4:sw=4:expandtab

237
libraries/APO/AP_ControllerBlock.h Normal file
View File

@ -0,0 +1,237 @@
/*
* AP_ControllerBlock.h
* Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef AP_ControllerBlock_H
#define AP_ControllerBlock_H
// inclusions
#include "../AP_Common/AP_Common.h"
#include "../AP_Common/AP_Var.h"
// ArduPilotOne namespace
namespace apo {
///
// The abstract class defining a controller block.
class AP_ControllerBlock {
public:
///
// Controller block constructor.
// This creates a controller block.
// @group The group containing the class parameters.
// @groupStart The start of the group. Used for chaining parameters.
// @groupEnd The end of the group.
AP_ControllerBlock(AP_Var_group * group, uint8_t groupStart,
uint8_t groupLength);
///
// Get the end of the AP_Var group.
// This is used for chaining multiple AP_Var groups into one.
uint8_t getGroupEnd() {
return _groupEnd;
}
protected:
AP_Var_group * _group; /// Contains all the parameters of the class.
uint8_t _groupStart; /// The start of the AP_Var group. Used for chaining parameters.
uint8_t _groupEnd; /// The end of the AP_Var group.
};
///
// A low pass filter block.
// This takes a signal and smooths it out. It
// cuts all frequencies higher than the frequency provided.
class BlockLowPass: public AP_ControllerBlock {
public:
///
// The constructor.
// @group The group containing the class parameters.
// @groupStart The start of the group. Used for chaining parameters.
// @fCut The cut-off frequency in Hz for smoothing.
BlockLowPass(AP_Var_group * group, uint8_t groupStart, float fCut,
const prog_char_t * fCutLabel = NULL);
///
// The update function.
// @input The input signal.
// @dt The timer interval.
// @return The output (smoothed) signal.
float update(const float & input, const float & dt);
protected:
AP_Float _fCut; /// The cut-off frequency in Hz.
float _y; /// The internal state of the low pass filter.
};
///
// This block saturates a signal.
// If the signal is above max it is set to max.
// If it is below -max it is set to -max.
class BlockSaturation: public AP_ControllerBlock {
public:
///
// Controller block constructor.
// This creates a controller block.
// @group The group containing the class parameters.
// @groupStart The start of the group. Used for chaining parameters.
// @yMax The maximum threshold.
BlockSaturation(AP_Var_group * group, uint8_t groupStart, float yMax,
const prog_char_t * yMaxLabel = NULL);
///
// The update function.
// @input The input signal.
// @return The output (saturated) signal.
float update(const float & input);
protected:
AP_Float _yMax; /// output saturation
};
///
// This block calculates a derivative.
class BlockDerivative {
public:
/// The constructor.
BlockDerivative();
///
// The update function.
// @input The input signal.
// @return The derivative.
float update(const float & input, const float & dt);
protected:
float _lastInput; /// The last input to the block.
bool firstRun; /// Keeps track of first run inorder to decide if _lastInput should be used.
};
/// This block calculates an integral.
class BlockIntegral {
public:
/// The constructor.
BlockIntegral();
///
// The update function.
// @input The input signal.
// @dt The timer interval.
// @return The output (integrated) signal.
float update(const float & input, const float & dt);
protected:
float _i; /// integral
};
///
// This is a proportional block with built-in gain.
class BlockP: public AP_ControllerBlock {
public:
BlockP(AP_Var_group * group, uint8_t groupStart, float kP,
const prog_char_t * kPLabel = NULL);
///
// The update function.
// @input The input signal.
// @return The output signal (kP*input).
float update(const float & input);
protected:
AP_Float _kP; /// proportional gain
};
///
// This is a integral block with built-in gain.
class BlockI: public AP_ControllerBlock {
public:
BlockI(AP_Var_group * group, uint8_t groupStart, float kI, float iMax,
const prog_char_t * kILabel = NULL,
const prog_char_t * iMaxLabel = NULL);
float update(const float & input, const float & dt);
protected:
AP_Float _kI; /// integral gain
BlockSaturation _blockSaturation; /// integrator saturation
float _eI; /// internal integrator state
};
///
// This is a derivative block with built-in gain.
class BlockD: public AP_ControllerBlock {
public:
BlockD(AP_Var_group * group, uint8_t groupStart, float kD, uint8_t dFCut,
const prog_char_t * kDLabel = NULL,
const prog_char_t * dFCutLabel = NULL);
float update(const float & input, const float & dt);
protected:
BlockLowPass _blockLowPass; /// The low-pass filter block
AP_Float _kD; /// The derivative gain
float _eP; /// The previous state
};
///
// This is a proportional, integral, derivative block with built-in gains.
class BlockPID: public AP_ControllerBlock {
public:
BlockPID(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float kD, float iMax, float yMax, uint8_t dFcut);
float update(const float & input, const float & dt);
protected:
BlockP _blockP; /// The proportional block.
BlockI _blockI; /// The integral block.
BlockD _blockD; /// The derivative block.
BlockSaturation _blockSaturation; /// The saturation block.
};
///
// This is a proportional, integral block with built-in gains.
class BlockPI: public AP_ControllerBlock {
public:
BlockPI(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float iMax, float yMax);
float update(const float & input, const float & dt);
protected:
BlockP _blockP; /// The proportional block.
BlockI _blockI; /// The derivative block.
BlockSaturation _blockSaturation; /// The saturation block.
};
///
// This is a proportional, derivative block with built-in gains.
class BlockPD: public AP_ControllerBlock {
public:
BlockPD(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float kD, float iMax, float yMax, uint8_t dFcut);
float update(const float & input, const float & dt);
protected:
BlockP _blockP; /// The proportional block.
BlockD _blockD; /// The derivative block.
BlockSaturation _blockSaturation; /// The saturation block.
};
/// PID with derivative feedback (ignores reference derivative)
class BlockPIDDfb: public AP_ControllerBlock {
public:
BlockPIDDfb(AP_Var_group * group, uint8_t groupStart, float kP, float kI,
float kD, float iMax, float yMax, float dFCut,
const prog_char_t * dFCutLabel = NULL,
const prog_char_t * dLabel = NULL);
float update(const float & input, const float & derivative,
const float & dt);
protected:
BlockP _blockP; /// The proportional block.
BlockI _blockI; /// The integral block.
BlockSaturation _blockSaturation; /// The saturation block.
BlockLowPass _blockLowPass; /// The low-pass filter block.
AP_Float _kD; /// derivative gain
};
} // apo
#endif // AP_ControllerBlock_H
// vim:ts=4:sw=4:expandtab

37
libraries/APO/AP_Guide.cpp
View File

@ -18,8 +18,8 @@ AP_Guide::AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal)
_navigator(navigator), _hal(hal), _command(AP_MavlinkCommand::home),
_previousCommand(AP_MavlinkCommand::home),
_headingCommand(0), _airSpeedCommand(0),
_groundSpeedCommand(0), _altitudeCommand(0), _pNCmd(0),
_pECmd(0), _pDCmd(0), _mode(MAV_NAV_LOST),
_groundSpeedCommand(0), _altitudeCommand(0),
_mode(MAV_NAV_LOST),
_numberOfCommands(1), _cmdIndex(0), _nextCommandCalls(0),
_nextCommandTimer(0) {
}
@ -42,6 +42,11 @@ float AP_Guide::getHeadingError() {
return headingError;
}
float AP_Guide::getDistanceToNextWaypoint() {
return _command.distanceTo(_navigator->getLat_degInt(),
_navigator->getLon_degInt());
}
MavlinkGuide::MavlinkGuide(AP_Navigator * navigator,
AP_HardwareAbstractionLayer * hal, float velCmd, float xt, float xtLim) :
AP_Guide(navigator, hal),
@ -56,6 +61,16 @@ void MavlinkGuide::update() {
handleCommand();
}
float MavlinkGuide::getPNError() {
return -_command.getPN(_navigator->getLat_degInt(), _navigator->getLon_degInt());
}
float MavlinkGuide::getPEError() {
return -_command.getPE(_navigator->getLat_degInt(), _navigator->getLon_degInt());
}
float MavlinkGuide::getPDError() {
return -_command.getPD(_navigator->getAlt_intM());
}
void MavlinkGuide::nextCommand() {
// within 1 seconds, check if more than 5 calls to next command occur
// if they do, go to home waypoint
@ -141,12 +156,9 @@ void MavlinkGuide::handleCommand() {
return;
}
float distanceToNext = _command.distanceTo(
_navigator->getLat_degInt(), _navigator->getLon_degInt());
// check if we are at waypoint or if command
// radius is zero within tolerance
if (distanceToNext < _command.getRadius() | distanceToNext < 1e-5) {
if ( (getDistanceToNextWaypoint() < _command.getRadius()) | (getDistanceToNextWaypoint() < 1e-5) ) {
_hal->gcs->sendText(SEVERITY_LOW,PSTR("waypoint reached (distance)"));
_hal->debug->printf_P(PSTR("waypoint reached (distance)"));
nextCommand();
@ -209,17 +221,10 @@ void MavlinkGuide::handleCommand() {
_groundSpeedCommand = _velocityCommand;
// calculate pN,pE,pD from home and gps coordinates
_pNCmd = _command.getPN(_navigator->getLat_degInt(),
_navigator->getLon_degInt());
_pECmd = _command.getPE(_navigator->getLat_degInt(),
_navigator->getLon_degInt());
_pDCmd = _command.getPD(_navigator->getAlt_intM());
// debug
_hal->debug->printf_P(
PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"),
getNumberOfCommands(), getCurrentIndex(), getPreviousIndex());
//_hal->debug->printf_P(
//PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"),
//getNumberOfCommands(), getCurrentIndex(), getPreviousIndex());
}
} // namespace apo

26
libraries/APO/AP_Guide.h
View File

@ -48,6 +48,11 @@ public:
MAV_NAV getMode() const {
return _mode;
}
void setMode(MAV_NAV mode) {
_mode = mode;
}
uint8_t getCurrentIndex() {
return _cmdIndex;
}
@ -92,15 +97,12 @@ public:
float getAltitudeCommand() {
return _altitudeCommand;
}
float getPNCmd() {
return _pNCmd;
}
float getPECmd() {
return _pECmd;
}
float getPDCmd() {
return _pDCmd;
}
float getDistanceToNextWaypoint();
virtual float getPNError() = 0;
virtual float getPEError() = 0;
virtual float getPDError() = 0;
MAV_NAV getMode() {
return _mode;
}
@ -116,9 +118,6 @@ protected:
float _airSpeedCommand;
float _groundSpeedCommand;
float _altitudeCommand;
float _pNCmd;
float _pECmd;
float _pDCmd;
MAV_NAV _mode;
AP_Uint8 _numberOfCommands;
AP_Uint8 _cmdIndex;
@ -134,6 +133,9 @@ public:
void nextCommand();
void handleCommand();
void updateCommand();
virtual float getPNError();
virtual float getPEError();
virtual float getPDError();
private:
AP_Var_group _group;

10
libraries/APO/AP_HardwareAbstractionLayer.h
View File

@ -50,7 +50,7 @@ public:
adc(), gps(), baro(), compass(), rangeFinders(), imu(), batteryMonitor(), rc(), gcs(),
hil(), debug(), load(), lastHeartBeat(),
_heartBeatTimeout(heartBeatTimeout), _mode(mode),
_board(board), _vehicle(vehicle), _state(MAV_STATE_UNINIT) {
_board(board), _vehicle(vehicle) {
/*
* Board specific hardware initialization
@ -146,13 +146,6 @@ public:
vehicle_t getVehicle() {
return _vehicle;
}
MAV_STATE getState() {
return _state;
}
void setState(MAV_STATE state) {
_state = state;
}
bool heartBeatLost() {
if (_heartBeatTimeout == 0)
return false;
@ -167,7 +160,6 @@ private:
halMode_t _mode;
board_t _board;
vehicle_t _vehicle;
MAV_STATE _state;
};
} // namespace apo

4
libraries/APO/AP_MavlinkCommand.cpp
View File

@ -62,8 +62,8 @@ AP_MavlinkCommand::AP_MavlinkCommand(const mavlink_waypoint_t & cmd) :
setZ(cmd.z);
save();
// reload home if sent
if (cmd.seq == 0) home.load();
// reload home if sent, home must be a global waypoint
if ( (cmd.seq == 0) && (cmd.frame == MAV_FRAME_GLOBAL)) home.load();
Serial.println("============================================================");
Serial.println("storing new command from mavlink_waypoint_t");

5
libraries/APO/AP_Navigator.cpp
View File

@ -22,8 +22,9 @@ namespace apo {
AP_Navigator::AP_Navigator(AP_HardwareAbstractionLayer * hal) :
_hal(hal), _timeStamp(0), _roll(0), _rollRate(0), _pitch(0),
_pitchRate(0), _yaw(0), _yawRate(0), _airSpeed(0),
_groundSpeed(0), _vD(0), _lat_degInt(0),
_pitchRate(0), _yaw(0), _yawRate(0),
_windSpeed(0), _windDirection(0),
_vN(0), _vE(0), _vD(0), _lat_degInt(0),
_lon_degInt(0), _alt_intM(0) {
}
void AP_Navigator::calibrate() {

103
libraries/APO/AP_Navigator.h
View File

@ -45,7 +45,18 @@ public:
void setPN(float _pN);
float getAirSpeed() const {
return _airSpeed;
// neglects vertical wind
float vWN = getVN() + getWindSpeed()*cos(getWindDirection());
float vWE = getVE() + getWindSpeed()*sin(getWindDirection());
return sqrt(vWN*vWN+vWE+vWE+getVD()*getVD());
}
float getGroundSpeed() const {
return sqrt(getVN()*getVN()+getVE()*getVE());
}
float getWindSpeed() const {
return _windSpeed;
}
int32_t getAlt_intM() const {
@ -80,16 +91,16 @@ public:
this->_lon_degInt = _lon * rad2DegInt;
}
float getVD() const {
return _vD;
float getVN() const {
return _vN;
}
float getVE() const {
return sin(getYaw()) * getGroundSpeed();
return _vE;
}
float getGroundSpeed() const {
return _groundSpeed;
float getVD() const {
return _vD;
}
int32_t getLat_degInt() const {
@ -103,10 +114,6 @@ public:
return _lon_degInt;
}
float getVN() const {
return cos(getYaw()) * getGroundSpeed();
}
float getPitch() const {
return _pitch;
}
@ -131,20 +138,71 @@ public:
return _yawRate;
}
float getWindDirection() const {
return _windDirection;
}
float getCourseOverGround() const {
return atan2(getVE(),getVN());
}
float getSpeedOverGround() const {
return sqrt(getVN()*getVN()+getVE()*getVE());
}
float getXAccel() const {
return _xAccel;
}
float getYAccel() const {
return _yAccel;
}
float getZAccel() const {
return _zAccel;
}
void setAirSpeed(float airSpeed) {
_airSpeed = airSpeed;
// assumes wind constant and rescale navigation speed
float vScale = (1 + airSpeed/getAirSpeed());
float vNorm = sqrt(getVN()*getVN()+getVE()*getVE()+getVD()*getVD());
_vN *= vScale/vNorm;
_vE *= vScale/vNorm;
_vD *= vScale/vNorm;
}
void setAlt_intM(int32_t alt_intM) {
_alt_intM = alt_intM;
}
void setVN(float vN) {
_vN = vN;
}
void setVE(float vE) {
_vE = vE;
}
void setVD(float vD) {
_vD = vD;
}
void setXAccel(float xAccel) {
_xAccel = xAccel;
}
void setYAccel(float yAccel) {
_yAccel = yAccel;
}
void setZAccel(float zAccel) {
_zAccel = zAccel;
}
void setGroundSpeed(float groundSpeed) {
_groundSpeed = groundSpeed;
float cog = getCourseOverGround();
_vN = cos(cog)*groundSpeed;
_vE = sin(cog)*groundSpeed;
}
void setLat_degInt(int32_t lat_degInt) {
@ -180,13 +238,24 @@ public:
void setYawRate(float yawRate) {
_yawRate = yawRate;
}
void setTimeStamp(int32_t timeStamp) {
_timeStamp = timeStamp;
}
int32_t getTimeStamp() const {
return _timeStamp;
}
void setWindDirection(float windDirection) {
_windDirection = windDirection;
}
void setWindSpeed(float windSpeed) {
_windSpeed = windSpeed;
}
protected:
AP_HardwareAbstractionLayer * _hal;
private:
@ -197,9 +266,15 @@ private:
float _pitchRate; // rad/s
float _yaw; // rad
float _yawRate; // rad/s
float _airSpeed; // m/s
float _groundSpeed; // m/s
// wind assumed to be N-E plane
float _windSpeed; // m/s
float _windDirection; // m/s
float _vN;
float _vE;
float _vD; // m/s
float _xAccel;
float _yAccel;
float _zAccel;
int32_t _lat_degInt; // deg / 1e7
int32_t _lon_degInt; // deg / 1e7
int32_t _alt_intM; // meters / 1e3

1
libraries/AP_Common/AP_Vector.h
View File

@ -19,6 +19,7 @@
#ifndef Vector_H
#define Vector_H
#include "../FastSerial/BetterStream.h"
#include <stdlib.h>
#include <inttypes.h>
#include <WProgram.h>

2
libraries/AP_Common/include/menu.h
View File

@ -16,6 +16,8 @@
#ifndef __AP_COMMON_MENU_H
#define __AP_COMMON_MENU_H
#include <inttypes.h>
#define MENU_COMMANDLINE_MAX 32 ///< maximum input line length
#define MENU_ARGS_MAX 4 ///< maximum number of arguments
#define MENU_COMMAND_MAX 14 ///< maximum size of a command name