From 6c51157b2d60ceac519cfd5520750c74bdfa023d Mon Sep 17 00:00:00 2001 From: Wenyao Xie Date: Tue, 15 Nov 2011 17:15:54 -0500 Subject: [PATCH 01/11] Working on new hil message for mavlink. --- apo/ControllerQuad.h | 58 ++++++++++++++++-- apo/QuadArducopter.h | 2 +- apo/apo.pde | 5 +- libraries/APO/AP_Autopilot.cpp | 1 + libraries/APO/AP_CommLink.cpp | 29 ++++++++- libraries/APO/AP_Navigator.cpp | 5 +- libraries/APO/AP_Navigator.h | 103 +++++++++++++++++++++++++++----- libraries/AP_Common/AP_Vector.h | 1 + 8 files changed, 179 insertions(+), 25 deletions(-) diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 920c878abe..7912efbb98 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -88,10 +88,6 @@ 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); @@ -105,6 +101,11 @@ private: _cmdPitch = cmdEastTilt * trigCos - cmdNorthTilt * trigSin; _cmdRoll = -cmdEastTilt * trigSin + cmdNorthTilt * trigCos; // note that the north tilt is negative of the pitch + + Serial.print(" trigSin: "); + Serial.print(trigSin); + Serial.print(" trigCos: "); + Serial.print(trigCos); } _cmdYawRate = 0; @@ -116,6 +117,34 @@ private: if (fabs(_cmdPitch) > 0.5) _thrustMix *= 1.13949393; else _thrustMix /= cos(_cmdPitch); + + //debug statements + Serial.print(" getPN: "); + Serial.print(_nav->getPN()); + Serial.print(" getPE: "); + Serial.print(_nav->getPE()); + Serial.print(" getPD: "); + Serial.print(_nav->getPD()); + Serial.print(" getVN: "); + Serial.print(_nav->getVN()); + Serial.print(" getVE: "); + Serial.print(_nav->getVE()); + Serial.print(" getVD: "); + Serial.println(_nav->getVD()); + //Serial.print("Roll: "); + //Serial.print(_cmdRoll); + //Serial.print(" Pitch"); + //Serial.print(_cmdPitch); + //Serial.print(" YawRate"); + //Serial.print(_cmdYawRate); + //Serial.print(" ThrustMix"); + //Serial.print(_thrustMix); + //Serial.print(" North Tilt"); + //Serial.print(cmdNorthTilt); + //Serial.print(" East Tilt"); + //Serial.print(cmdEastTilt); + //Serial.print(" Down"); + //Serial.println(cmdDown); } void autoAttitudeLoop(float dt) { _rollMix = pidRoll.update(_cmdRoll - _nav->getRoll(), @@ -123,8 +152,29 @@ private: _pitchMix = pidPitch.update(_cmdPitch - _nav->getPitch(), _nav->getPitchRate(), dt); _yawMix = pidYawRate.update(_cmdYawRate - _nav->getYawRate(), dt); + + //debug statements + //Serial.print("Roll Cmd: "); + //Serial.print(_cmdRoll*1000); + //Serial.print(" Nav_GetRoll: "); + //Serial.print(_nav->getRoll()*1000); + //Serial.print(" Roll Error: "); + //Serial.print((_cmdRoll - _nav->getRoll())*1000); + //Serial.print(" Pitch Cmd: "); + //Serial.print(_cmdPitch*1000); + //Serial.print(" Nav_GetPitch: "); + //Serial.print(_nav->getPitch()*1000); + //Serial.print(" Pitch Error: "); + //Serial.println((_cmdPitch - _nav->getPitch())*1000); + //Serial.print(" YawRate Cmd: "); + //Serial.print(_cmdYawRate); + //Serial.print(" Nav_GetYawRate: "); + //Serial.print( _nav->getYawRate()); + //Serial.print(" YawRate Error: "); + //Serial.println(_cmdYawRate - _nav->getYawRate()); } void setMotorsActive() { + // turn all motors off if below 0.1 throttle if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) { setAllRadioChannelsToNeutral(); diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index 670aa18e05..547c27cf31 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -10,7 +10,7 @@ // 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; diff --git a/apo/apo.pde b/apo/apo.pde index a39d6e9f7f..4abd88c96e 100644 --- a/apo/apo.pde +++ b/apo/apo.pde @@ -8,7 +8,6 @@ #include #include #include -#include #include #include #include @@ -16,8 +15,8 @@ #include // Vehicle Configuration -//#include "QuadArducopter.h" -#include "PlaneEasystar.h" +#include "QuadArducopter.h" +//#include "PlaneEasystar.h" // ArduPilotOne Default Setup #include "APO_DefaultSetup.h" diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp index 03f13aaa86..a2f79fe1f6 100644 --- a/libraries/APO/AP_Autopilot.cpp +++ b/libraries/APO/AP_Autopilot.cpp @@ -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); } diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp index a18b91594a..1df492bde2 100644 --- a/libraries/APO/AP_CommLink.cpp +++ b/libraries/APO/AP_CommLink.cpp @@ -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; } @@ -328,6 +335,26 @@ 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); + break; + } + case MAVLINK_MSG_ID_ATTITUDE: { // decode mavlink_attitude_t packet; diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp index 70a2f3289c..8cd442e337 100644 --- a/libraries/APO/AP_Navigator.cpp +++ b/libraries/APO/AP_Navigator.cpp @@ -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() { diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h index 49db8c5982..ea772950b1 100644 --- a/libraries/APO/AP_Navigator.h +++ b/libraries/APO/AP_Navigator.h @@ -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 setXAcc(float xAccel) { + _xAccel = xAccel; + } + + void setYAcc(float yAccel) { + _yAccel = yAccel; + } + + void setZAcc(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 diff --git a/libraries/AP_Common/AP_Vector.h b/libraries/AP_Common/AP_Vector.h index 287056b2a1..a82018c388 100644 --- a/libraries/AP_Common/AP_Vector.h +++ b/libraries/AP_Common/AP_Vector.h @@ -19,6 +19,7 @@ #ifndef Vector_H #define Vector_H +#include "../FastSerial/BetterStream.h" #include #include #include From 715a7cb748525fae76f6461f0efb75946a30a89b Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 19 Nov 2011 01:08:17 -0500 Subject: [PATCH 02/11] Fixed some arduino cmake upload issues. May still be necessary to reset some board before upload. --- CMakeLists.txt | 12 +- README.txt | 10 +- cmake/modules/FindArduino.cmake | 200 +++++++++++++++++--------------- cmake/toolchains/Arduino.cmake | 6 +- cmake/updated-arduino-cmake.sh | 2 +- 5 files changed, 128 insertions(+), 102 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4c54fc7836..171b8b0e9f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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() diff --git a/README.txt b/README.txt index 50d0d38478..0e1ad4a89b 100644 --- a/README.txt +++ b/README.txt @@ -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 ----------------------------------------------- diff --git a/cmake/modules/FindArduino.cmake b/cmake/modules/FindArduino.cmake index e53873e9ad..dc20e07cd6 100644 --- a/cmake/modules/FindArduino.cmake +++ b/cmake/modules/FindArduino.cmake @@ -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}) + ${ARDUINO_AVRDUDE_PROGRAM} + ${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,82 @@ 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() +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() diff --git a/cmake/toolchains/Arduino.cmake b/cmake/toolchains/Arduino.cmake index 68c58a72e9..897ea4fa0c 100644 --- a/cmake/toolchains/Arduino.cmake +++ b/cmake/toolchains/Arduino.cmake @@ -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 "") diff --git a/cmake/updated-arduino-cmake.sh b/cmake/updated-arduino-cmake.sh index 3c86d4b728..c8c26b6781 100755 --- a/cmake/updated-arduino-cmake.sh +++ b/cmake/updated-arduino-cmake.sh @@ -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 From 1c5db5e963d544f9192c52936956159b82400c70 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 19 Nov 2011 21:39:14 -0500 Subject: [PATCH 03/11] Bug located in quad position loop, improved hil. --- apo/ControllerQuad.h | 57 ++--------------- apo/QuadArducopter.h | 10 +-- cmake/modules/FindArduino.cmake | 3 - libraries/APO/AP_CommLink.cpp | 106 ++++++++++++++++++++++---------- libraries/APO/AP_Controller.cpp | 12 ++-- libraries/APO/AP_Controller.h | 4 ++ libraries/APO/AP_Guide.cpp | 8 +-- libraries/APO/AP_Guide.h | 5 ++ libraries/APO/AP_Navigator.h | 6 +- 9 files changed, 107 insertions(+), 104 deletions(-) diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 7912efbb98..f1ba1b30c3 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -90,6 +90,7 @@ private: autoAttitudeLoop(dt); } void autoPositionLoop(float dt) { + // XXX need to add waypoint coordinates 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); @@ -101,11 +102,6 @@ private: _cmdPitch = cmdEastTilt * trigCos - cmdNorthTilt * trigSin; _cmdRoll = -cmdEastTilt * trigSin + cmdNorthTilt * trigCos; // note that the north tilt is negative of the pitch - - Serial.print(" trigSin: "); - Serial.print(trigSin); - Serial.print(" trigCos: "); - Serial.print(trigCos); } _cmdYawRate = 0; @@ -117,34 +113,9 @@ private: if (fabs(_cmdPitch) > 0.5) _thrustMix *= 1.13949393; else _thrustMix /= cos(_cmdPitch); - - //debug statements - Serial.print(" getPN: "); - Serial.print(_nav->getPN()); - Serial.print(" getPE: "); - Serial.print(_nav->getPE()); - Serial.print(" getPD: "); - Serial.print(_nav->getPD()); - Serial.print(" getVN: "); - Serial.print(_nav->getVN()); - Serial.print(" getVE: "); - Serial.print(_nav->getVE()); - Serial.print(" getVD: "); - Serial.println(_nav->getVD()); - //Serial.print("Roll: "); - //Serial.print(_cmdRoll); - //Serial.print(" Pitch"); - //Serial.print(_cmdPitch); - //Serial.print(" YawRate"); - //Serial.print(_cmdYawRate); - //Serial.print(" ThrustMix"); - //Serial.print(_thrustMix); - //Serial.print(" North Tilt"); - //Serial.print(cmdNorthTilt); - //Serial.print(" East Tilt"); - //Serial.print(cmdEastTilt); - //Serial.print(" Down"); - //Serial.println(cmdDown); + + // debug for position loop + _hal->debug->printf_P(PSTR("cmd: north tilt(%f), east tilt(%f), down(%f), pitch(%f), roll(%f)\n"),cmdNorthTilt,cmdEastTilt,cmdDown,_cmdPitch,_cmdRoll); } void autoAttitudeLoop(float dt) { _rollMix = pidRoll.update(_cmdRoll - _nav->getRoll(), @@ -152,26 +123,6 @@ private: _pitchMix = pidPitch.update(_cmdPitch - _nav->getPitch(), _nav->getPitchRate(), dt); _yawMix = pidYawRate.update(_cmdYawRate - _nav->getYawRate(), dt); - - //debug statements - //Serial.print("Roll Cmd: "); - //Serial.print(_cmdRoll*1000); - //Serial.print(" Nav_GetRoll: "); - //Serial.print(_nav->getRoll()*1000); - //Serial.print(" Roll Error: "); - //Serial.print((_cmdRoll - _nav->getRoll())*1000); - //Serial.print(" Pitch Cmd: "); - //Serial.print(_cmdPitch*1000); - //Serial.print(" Nav_GetPitch: "); - //Serial.print(_nav->getPitch()*1000); - //Serial.print(" Pitch Error: "); - //Serial.println((_cmdPitch - _nav->getPitch())*1000); - //Serial.print(" YawRate Cmd: "); - //Serial.print(_cmdYawRate); - //Serial.print(" Nav_GetYawRate: "); - //Serial.print( _nav->getYawRate()); - //Serial.print(" YawRate Error: "); - //Serial.println(_cmdYawRate - _nav->getYawRate()); } void setMotorsActive() { diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index 547c27cf31..d49cd5e1b7 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -12,7 +12,7 @@ static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD; 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; @@ -66,10 +66,10 @@ static const float PID_POS_I = 0; static const float PID_POS_D = 0; static const float PID_POS_LIM = 0; // about 5 deg static const float PID_POS_AWU = 0; // about 5 deg -static const float PID_POS_Z_P = 0; +static const float PID_POS_Z_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 diff --git a/cmake/modules/FindArduino.cmake b/cmake/modules/FindArduino.cmake index dc20e07cd6..d78194cfb5 100644 --- a/cmake/modules/FindArduino.cmake +++ b/cmake/modules/FindArduino.cmake @@ -609,6 +609,3 @@ mark_as_advanced(ARDUINO_CORES_PATH ARDUINO_OBJCOPY_EEP_FLAGS ARDUINO_OBJCOPY_HEX_FLAGS) load_board_settings() - -endif() - diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp index 1df492bde2..6be0bf1842 100644 --- a/libraries/APO/AP_CommLink.cpp +++ b/libraries/APO/AP_CommLink.cpp @@ -131,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: { @@ -352,6 +354,9 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) { _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; } @@ -391,31 +396,68 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) { AP_Var::save_all(); break; - case MAV_ACTION_CALIBRATE_RC: + case MAV_ACTION_MOTORS_START: + _hal->setState(MAV_STATE_ACTIVE); + break; + case MAV_ACTION_CALIBRATE_GYRO: case MAV_ACTION_CALIBRATE_MAG: case MAV_ACTION_CALIBRATE_ACC: case MAV_ACTION_CALIBRATE_PRESSURE: + _hal->setState(MAV_STATE_STANDBY); + _navigator->calibrate(); + break; + + case MAV_ACTION_EMCY_KILL: + case MAV_ACTION_CONFIRM_KILL: + case MAV_ACTION_MOTORS_STOP: + case MAV_ACTION_SHUTDOWN: + _hal->setState(MAV_STATE_STANDBY); + _controller->setMode(MAV_MODE_LOCKED); + break; + + case MAV_ACTION_LAUNCH: + case MAV_ACTION_TAKEOFF: + _controller->setMode(MAV_MODE_AUTO); + _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: diff --git a/libraries/APO/AP_Controller.cpp b/libraries/APO/AP_Controller.cpp index 48c6f6e475..73acf7c066 100644 --- a/libraries/APO/AP_Controller.cpp +++ b/libraries/APO/AP_Controller.cpp @@ -49,18 +49,22 @@ void AP_Controller::update(const float dt) { // // 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 } 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; + + // if in auto mode and manual switch set, change to manual + if (getMode()==MAV_MODE_AUTO && (_hal->rc[_chMode]->getRadioPosition() > 0)) setMode(MAV_MODE_MANUAL); + + // if in manual mode and auto switch set, switch to auto + if (getMode()==MAV_MODE_MANUAL && (_hal->rc[_chMode]->getRadioPosition() < 0)) setMode(MAV_MODE_AUTO); } // handle flight modes diff --git a/libraries/APO/AP_Controller.h b/libraries/APO/AP_Controller.h index 4c5fe8a260..d16fe48052 100644 --- a/libraries/APO/AP_Controller.h +++ b/libraries/APO/AP_Controller.h @@ -64,6 +64,10 @@ public: AP_Uint8 getMode() { return _mode; } + void setMode(MAV_MODE mode) { + _mode = mode; + } + protected: AP_Navigator * _nav; AP_Guide * _guide; diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp index 49eaf263a8..81f3bf0fd7 100644 --- a/libraries/APO/AP_Guide.cpp +++ b/libraries/APO/AP_Guide.cpp @@ -146,7 +146,7 @@ void MavlinkGuide::handleCommand() { // check if we are at waypoint or if command // radius is zero within tolerance - if (distanceToNext < _command.getRadius() | distanceToNext < 1e-5) { + if ( (distanceToNext < _command.getRadius()) | (distanceToNext < 1e-5) ) { _hal->gcs->sendText(SEVERITY_LOW,PSTR("waypoint reached (distance)")); _hal->debug->printf_P(PSTR("waypoint reached (distance)")); nextCommand(); @@ -217,9 +217,9 @@ void MavlinkGuide::handleCommand() { _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 diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index 033e27ed45..95a43b33f7 100644 --- a/libraries/APO/AP_Guide.h +++ b/libraries/APO/AP_Guide.h @@ -48,6 +48,11 @@ public: MAV_NAV getMode() const { return _mode; } + + void setMode(MAV_NAV mode) { + _mode = mode; + } + uint8_t getCurrentIndex() { return _cmdIndex; } diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h index ea772950b1..437386c54b 100644 --- a/libraries/APO/AP_Navigator.h +++ b/libraries/APO/AP_Navigator.h @@ -187,15 +187,15 @@ public: _vD = vD; } - void setXAcc(float xAccel) { + void setXAccel(float xAccel) { _xAccel = xAccel; } - void setYAcc(float yAccel) { + void setYAccel(float yAccel) { _yAccel = yAccel; } - void setZAcc(float zAccel) { + void setZAccel(float zAccel) { _zAccel = zAccel; } From 7a742c4a2a42be4436a5a7d20e87b96cf9c4b0c1 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 19 Nov 2011 22:20:06 -0500 Subject: [PATCH 04/11] Working on memory problem. --- apo/ControllerQuad.h | 12 ++++-------- cmake/modules/FindArduino.cmake | 2 +- libraries/APO/AP_MavlinkCommand.cpp | 4 ++-- 3 files changed, 7 insertions(+), 11 deletions(-) diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index f1ba1b30c3..77c8d517d7 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -96,15 +96,11 @@ private: float cmdDown = pidPD.update(_nav->getPD(),_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; + _cmdPitch = cmdNorthTilt * cos(_nav->getYaw()) + cmdEastTilt * sin(_nav->getYaw()); + _cmdRoll = -cmdNorthTilt * sin(_nav->getYaw()) + cmdEastTilt * cos(_nav->getYaw()); + _cmdPitch *= -1; // note that the north tilt is negative of the pitch + _cmdYawRate = 0; _thrustMix = THRUST_HOVER_OFFSET + cmdDown; // "thrust-trim-adjust" diff --git a/cmake/modules/FindArduino.cmake b/cmake/modules/FindArduino.cmake index d78194cfb5..dc2b18659a 100644 --- a/cmake/modules/FindArduino.cmake +++ b/cmake/modules/FindArduino.cmake @@ -460,7 +460,7 @@ function(setup_arduino_upload BOARD_ID TARGET_NAME PORT) -p${${BOARD_ID}.build.mcu} -c${${BOARD_ID}.upload.protocol} -P${PORT} -b${${BOARD_ID}.upload.speed} - -D + #-D -Uflash:w:${CMAKE_BINARY_DIR}/${TARGET_NAME}.hex:i DEPENDS ${TARGET_NAME}) if(NOT TARGET upload) diff --git a/libraries/APO/AP_MavlinkCommand.cpp b/libraries/APO/AP_MavlinkCommand.cpp index 9fd6cd5627..da2fdbf7b8 100644 --- a/libraries/APO/AP_MavlinkCommand.cpp +++ b/libraries/APO/AP_MavlinkCommand.cpp @@ -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"); From 50e8de999f887313e59ccc9f6d85eea52b7aa415 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 19 Nov 2011 23:34:14 -0500 Subject: [PATCH 05/11] Fixed most of quad guidance issues. --- apo/ControllerQuad.h | 9 ++++----- apo/QuadArducopter.h | 8 ++++---- libraries/APO/AP_Guide.cpp | 21 ++++++++++++--------- libraries/APO/AP_Guide.h | 19 +++++++------------ 4 files changed, 27 insertions(+), 30 deletions(-) diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 77c8d517d7..8f728f08d2 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -91,16 +91,15 @@ private: } void autoPositionLoop(float dt) { // XXX need to add waypoint coordinates - 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); + float cmdNorthTilt = pidPN.update(_guide->getPNError(),_nav->getVN(),dt); + float cmdEastTilt = pidPE.update(_guide->getPEError(),_nav->getVE(),dt); + float cmdDown = pidPD.update(_guide->getPDError(),_nav->getVD(),dt); // "transform-to-body" _cmdPitch = cmdNorthTilt * cos(_nav->getYaw()) + cmdEastTilt * sin(_nav->getYaw()); _cmdRoll = -cmdNorthTilt * sin(_nav->getYaw()) + cmdEastTilt * cos(_nav->getYaw()); _cmdPitch *= -1; // note that the north tilt is negative of the pitch - - _cmdYawRate = 0; + _cmdYawRate = pidYaw.update(_guide->getHeadingError(),_nav->getYawRate(),dt); // always points to next waypoint _thrustMix = THRUST_HOVER_OFFSET + cmdDown; // "thrust-trim-adjust" diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index d49cd5e1b7..785fee689e 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -61,11 +61,11 @@ 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_P = 0.1; 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_D = 0.1; +static const float PID_POS_LIM = 0.04; // about 2 deg +static const float PID_POS_AWU = 0.02; // about 1 deg 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.2; diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp index 81f3bf0fd7..c63cad4048 100644 --- a/libraries/APO/AP_Guide.cpp +++ b/libraries/APO/AP_Guide.cpp @@ -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) { } @@ -56,6 +56,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 @@ -209,13 +219,6 @@ 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"), diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index 95a43b33f7..2f2d80ec89 100644 --- a/libraries/APO/AP_Guide.h +++ b/libraries/APO/AP_Guide.h @@ -97,15 +97,10 @@ public: float getAltitudeCommand() { return _altitudeCommand; } - float getPNCmd() { - return _pNCmd; - } - float getPECmd() { - return _pECmd; - } - float getPDCmd() { - return _pDCmd; - } + virtual float getPNError() = 0; + virtual float getPEError() = 0; + virtual float getPDError() = 0; + MAV_NAV getMode() { return _mode; } @@ -121,9 +116,6 @@ protected: float _airSpeedCommand; float _groundSpeedCommand; float _altitudeCommand; - float _pNCmd; - float _pECmd; - float _pDCmd; MAV_NAV _mode; AP_Uint8 _numberOfCommands; AP_Uint8 _cmdIndex; @@ -139,6 +131,9 @@ public: void nextCommand(); void handleCommand(); void updateCommand(); + virtual float getPNError(); + virtual float getPEError(); + virtual float getPDError(); private: AP_Var_group _group; From 3c217d4186e5b5e770da49f8a38dcc5a07fc74f0 Mon Sep 17 00:00:00 2001 From: Wenyao Xie Date: Tue, 22 Nov 2011 16:42:51 -0500 Subject: [PATCH 06/11] Worked on autopilot state/ mode mapping. Corrected PIDBlock sign error. --- ArduBoat/ControllerBoat.h | 8 +- ArduRover/ControllerCar.h | 8 +- README.txt | 10 +- apo/ControllerPlane.h | 10 +- apo/ControllerQuad.h | 39 ++++---- apo/QuadArducopter.h | 13 +-- libraries/APO/AP_ArmingMechanism.cpp | 15 ++- libraries/APO/AP_ArmingMechanism.h | 8 +- libraries/APO/AP_Autopilot.cpp | 6 +- libraries/APO/AP_CommLink.cpp | 16 +++- libraries/APO/AP_Controller.cpp | 100 ++++++++------------ libraries/APO/AP_Controller.h | 40 ++++---- libraries/APO/AP_Guide.cpp | 10 +- libraries/APO/AP_Guide.h | 2 + libraries/APO/AP_HardwareAbstractionLayer.h | 7 -- 15 files changed, 150 insertions(+), 142 deletions(-) diff --git a/ArduBoat/ControllerBoat.h b/ArduBoat/ControllerBoat.h index 2b222a7ee0..2e32a371e7 100644 --- a/ArduBoat/ControllerBoat.h +++ b/ArduBoat/ControllerBoat.h @@ -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), 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 { diff --git a/ArduRover/ControllerCar.h b/ArduRover/ControllerCar.h index 75a2b76a73..2a244f1cfd 100644 --- a/ArduRover/ControllerCar.h +++ b/ArduRover/ControllerCar.h @@ -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), 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 { diff --git a/README.txt b/README.txt index 0e1ad4a89b..c72fe9aa77 100644 --- a/README.txt +++ b/README.txt @@ -44,9 +44,13 @@ 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: Import project using Menu File->Import diff --git a/apo/ControllerPlane.h b/apo/ControllerPlane.h index 368a323020..3c9d6e56c6 100644 --- a/apo/ControllerPlane.h +++ b/apo/ControllerPlane.h @@ -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), _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 { diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 8f728f08d2..0923f2a691 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -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), 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")); @@ -91,16 +89,15 @@ private: } void autoPositionLoop(float 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 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" - _cmdPitch = cmdNorthTilt * cos(_nav->getYaw()) + cmdEastTilt * sin(_nav->getYaw()); - _cmdRoll = -cmdNorthTilt * sin(_nav->getYaw()) + cmdEastTilt * cos(_nav->getYaw()); - _cmdPitch *= -1; // note that the north tilt is negative of the pitch + _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; + _thrustMix = THRUST_HOVER_OFFSET - cmdDown; // "thrust-trim-adjust" if (fabs(_cmdRoll) > 0.5) _thrustMix *= 1.13949393; @@ -110,7 +107,7 @@ private: else _thrustMix /= cos(_cmdPitch); // debug for position loop - _hal->debug->printf_P(PSTR("cmd: north tilt(%f), east tilt(%f), down(%f), pitch(%f), roll(%f)\n"),cmdNorthTilt,cmdEastTilt,cmdDown,_cmdPitch,_cmdRoll); + _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(), @@ -119,8 +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(); @@ -132,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 @@ -163,8 +164,7 @@ private: enum { k_pidGroundSpeed2Throttle = k_controllersStart, k_pidStr, - k_pidPN, - k_pidPE, + k_pidTilt, k_pidPD, k_pidRoll, k_pidPitch, @@ -173,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; }; diff --git a/apo/QuadArducopter.h b/apo/QuadArducopter.h index 785fee689e..cda3666c84 100644 --- a/apo/QuadArducopter.h +++ b/apo/QuadArducopter.h @@ -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.1; -static const float PID_POS_I = 0; -static const float PID_POS_D = 0.1; -static const float PID_POS_LIM = 0.04; // about 2 deg -static const float PID_POS_AWU = 0.02; // about 1 deg +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.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; diff --git a/libraries/APO/AP_ArmingMechanism.cpp b/libraries/APO/AP_ArmingMechanism.cpp index f319a6d91e..bb6bf22a53 100644 --- a/libraries/APO/AP_ArmingMechanism.cpp +++ b/libraries/APO/AP_ArmingMechanism.cpp @@ -5,6 +5,7 @@ #include "AP_ArmingMechanism.h" +#include "AP_Controller.h" #include "AP_RcChannel.h" #include "../FastSerial/FastSerial.h" #include "AP_HardwareAbstractionLayer.h" @@ -15,7 +16,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 +24,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 +38,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 +46,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")); } } diff --git a/libraries/APO/AP_ArmingMechanism.h b/libraries/APO/AP_ArmingMechanism.h index e747a7831e..8ff3e56105 100644 --- a/libraries/APO/AP_ArmingMechanism.h +++ b/libraries/APO/AP_ArmingMechanism.h @@ -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 diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp index a2f79fe1f6..858695ad3d 100644 --- a/libraries/APO/AP_Autopilot.cpp +++ b/libraries/APO/AP_Autopilot.cpp @@ -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) { diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp index 6be0bf1842..9ccd483ee2 100644 --- a/libraries/APO/AP_CommLink.cpp +++ b/libraries/APO/AP_CommLink.cpp @@ -183,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; } @@ -397,14 +397,14 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) { break; case MAV_ACTION_MOTORS_START: - _hal->setState(MAV_STATE_ACTIVE); + _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: - _hal->setState(MAV_STATE_STANDBY); + _controller->setMode(MAV_MODE_LOCKED); _navigator->calibrate(); break; @@ -412,13 +412,11 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) { case MAV_ACTION_CONFIRM_KILL: case MAV_ACTION_MOTORS_STOP: case MAV_ACTION_SHUTDOWN: - _hal->setState(MAV_STATE_STANDBY); _controller->setMode(MAV_MODE_LOCKED); break; case MAV_ACTION_LAUNCH: case MAV_ACTION_TAKEOFF: - _controller->setMode(MAV_MODE_AUTO); _guide->setMode(MAV_NAV_LIFTOFF); break; @@ -651,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; diff --git a/libraries/APO/AP_Controller.cpp b/libraries/APO/AP_Controller.cpp index 73acf7c066..9925923f59 100644 --- a/libraries/APO/AP_Controller.cpp +++ b/libraries/APO/AP_Controller.cpp @@ -24,7 +24,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,83 +45,63 @@ 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()) { 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) { 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); + } + + // 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")); + } // if in auto mode and manual switch set, change to manual - if (getMode()==MAV_MODE_AUTO && (_hal->rc[_chMode]->getRadioPosition() > 0)) setMode(MAV_MODE_MANUAL); + if (_hal->rc[_chMode]->getRadioPosition() > 0) setMode(MAV_MODE_MANUAL); + else setMode(MAV_MODE_AUTO); - // if in manual mode and auto switch set, switch to auto - if (getMode()==MAV_MODE_MANUAL && (_hal->rc[_chMode]->getRadioPosition() < 0)) setMode(MAV_MODE_AUTO); - } - - // handle flight modes - switch(_mode) { - - case MAV_MODE_LOCKED: { - _hal->setState(MAV_STATE_STANDBY); - break; - } - - case MAV_MODE_FAILSAFE: { - _hal->setState(MAV_STATE_EMERGENCY); - break; - } - - 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(); + handleMotors(); } -void AP_Controller::setMotors() { - switch (_hal->getState()) { - - case MAV_STATE_ACTIVE: { +void AP_Controller::handleMotors() { + 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(); } } diff --git a/libraries/APO/AP_Controller.h b/libraries/APO/AP_Controller.h index d16fe48052..639ec22b71 100644 --- a/libraries/APO/AP_Controller.h +++ b/libraries/APO/AP_Controller.h @@ -44,29 +44,32 @@ public: const uint8_t _chMode, const uint16_t key = k_cntrl, const prog_char_t * name = NULL); - virtual void update(const float dt); + + // derived class cannot override + void update(const float dt); void setAllRadioChannelsToNeutral(); 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() { + void handleMotors(); + + // derived class must define + virtual void setMotors() = 0; + virtual void manualLoop(const float dt) = 0; + virtual void autoLoop(const float dt) = 0; + virtual void handleFailsafe() = 0; + + // access + MAV_MODE getMode() { return _mode; } void setMode(MAV_MODE mode) { _mode = mode; } + MAV_STATE getState() const { + return _state; + } + void setState(MAV_STATE state) { + _state = state; + } protected: AP_Navigator * _nav; @@ -75,7 +78,8 @@ protected: AP_ArmingMechanism * _armingMechanism; uint8_t _chMode; AP_Var_group _group; - AP_Uint8 _mode; + MAV_MODE _mode; + MAV_STATE _state; }; class AP_ControllerBlock { @@ -218,7 +222,7 @@ public: } float update(const float & input, const float & dt) { // derivative with low pass - float _eD = _blockLowPass.update((_eP - input) / dt, dt); + float _eD = _blockLowPass.update((input - _eP) / dt, dt); // proportional, note must come after derivative // because derivatve uses _eP as previous value _eP = input; diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp index c63cad4048..9b365d8384 100644 --- a/libraries/APO/AP_Guide.cpp +++ b/libraries/APO/AP_Guide.cpp @@ -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), @@ -151,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(); diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index 2f2d80ec89..fbb6b055f2 100644 --- a/libraries/APO/AP_Guide.h +++ b/libraries/APO/AP_Guide.h @@ -97,6 +97,8 @@ public: float getAltitudeCommand() { return _altitudeCommand; } + float getDistanceToNextWaypoint(); + virtual float getPNError() = 0; virtual float getPEError() = 0; virtual float getPDError() = 0; diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_HardwareAbstractionLayer.h index 9b3b9e2810..14c9b8ad1c 100644 --- a/libraries/APO/AP_HardwareAbstractionLayer.h +++ b/libraries/APO/AP_HardwareAbstractionLayer.h @@ -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; From e02d615c8a2f777fd611cc0379f6642513e67a19 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Tue, 22 Nov 2011 20:17:28 -0700 Subject: [PATCH 07/11] Rework logging file system to be more robust --- ArduPlane/Log.pde | 230 ++++++++++++++++++++++------------------------ 1 file changed, 110 insertions(+), 120 deletions(-) diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index af858b6dfd..0e2eef8c2f 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -52,9 +52,9 @@ print_log_menu(void) { int log_start; int log_end; - int temp; + 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,130 +287,118 @@ 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; } + + top_page = DF_LAST_PAGE; + DataFlash.StartRead(top_page); + top_page_file = DataFlash.GetFileNumber(); + top_page_filepage = DataFlash.GetFilePage(); - // 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 + + 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 From f4df5f81f600ff46f2c9d2e67489362b688e8270 Mon Sep 17 00:00:00 2001 From: Olivier ADLER Date: Wed, 23 Nov 2011 23:36:23 +0100 Subject: [PATCH 08/11] * ArduPPM 0.9.87 update : #define to allow Radio Passthrough mode was not working. Corrected. Removed older hex file --- Tools/ArduPPM/ATMega328p/Encoder-PPM.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/Tools/ArduPPM/ATMega328p/Encoder-PPM.c b/Tools/ArduPPM/ATMega328p/Encoder-PPM.c index 1d8159e57e..630052061f 100644 --- a/Tools/ArduPPM/ATMega328p/Encoder-PPM.c +++ b/Tools/ArduPPM/ATMega328p/Encoder-PPM.c @@ -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) // ------------------------------------------------------------------------------ From aaca5094b60fe8431181149204fe85d60cbab645 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 24 Nov 2011 02:08:27 -0500 Subject: [PATCH 09/11] Removed old variable from hal. --- libraries/APO/AP_HardwareAbstractionLayer.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_HardwareAbstractionLayer.h index 14c9b8ad1c..acea7e6529 100644 --- a/libraries/APO/AP_HardwareAbstractionLayer.h +++ b/libraries/APO/AP_HardwareAbstractionLayer.h @@ -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 @@ -160,7 +160,6 @@ private: halMode_t _mode; board_t _board; vehicle_t _vehicle; - MAV_STATE _state; }; } // namespace apo From fc84c15426ae2b21343ba03346cfb179ee685462 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 24 Nov 2011 13:28:14 -0500 Subject: [PATCH 10/11] Separated controller block class. Also improved documentation of AP_ControllerBlock. --- ArduBoat/ControllerBoat.h | 4 +- ArduRover/ControllerCar.h | 2 +- ArduRover/ControllerTank.h | 2 +- apo/ControllerPlane.h | 2 +- apo/ControllerQuad.h | 2 +- libraries/APO/APO.h | 1 + libraries/APO/AP_ArmingMechanism.cpp | 1 - libraries/APO/AP_Controller.cpp | 8 +- libraries/APO/AP_Controller.h | 352 +++++++-------------------- libraries/APO/AP_ControllerBlock.cpp | 179 ++++++++++++++ libraries/APO/AP_ControllerBlock.h | 237 ++++++++++++++++++ libraries/AP_Common/include/menu.h | 2 + 12 files changed, 508 insertions(+), 284 deletions(-) create mode 100644 libraries/APO/AP_ControllerBlock.cpp create mode 100644 libraries/APO/AP_ControllerBlock.h diff --git a/ArduBoat/ControllerBoat.h b/ArduBoat/ControllerBoat.h index 2e32a371e7..9db7354b0f 100644 --- a/ArduBoat/ControllerBoat.h +++ b/ArduBoat/ControllerBoat.h @@ -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,this,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, diff --git a/ArduRover/ControllerCar.h b/ArduRover/ControllerCar.h index 2a244f1cfd..1989f8de8e 100644 --- a/ArduRover/ControllerCar.h +++ b/ArduRover/ControllerCar.h @@ -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,this,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, diff --git a/ArduRover/ControllerTank.h b/ArduRover/ControllerTank.h index 0c091d6d70..a72edad6c7 100644 --- a/ArduRover/ControllerTank.h +++ b/ArduRover/ControllerTank.h @@ -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, diff --git a/apo/ControllerPlane.h b/apo/ControllerPlane.h index 3c9d6e56c6..f6e17a46b0 100644 --- a/apo/ControllerPlane.h +++ b/apo/ControllerPlane.h @@ -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,this,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), diff --git a/apo/ControllerQuad.h b/apo/ControllerQuad.h index 0923f2a691..3a920464a3 100644 --- a/apo/ControllerQuad.h +++ b/apo/ControllerQuad.h @@ -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,this,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), diff --git a/libraries/APO/APO.h b/libraries/APO/APO.h index 360b93dada..3fe10bb203 100644 --- a/libraries/APO/APO.h +++ b/libraries/APO/APO.h @@ -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" diff --git a/libraries/APO/AP_ArmingMechanism.cpp b/libraries/APO/AP_ArmingMechanism.cpp index bb6bf22a53..6236e590c4 100644 --- a/libraries/APO/AP_ArmingMechanism.cpp +++ b/libraries/APO/AP_ArmingMechanism.cpp @@ -3,7 +3,6 @@ * */ - #include "AP_ArmingMechanism.h" #include "AP_Controller.h" #include "AP_RcChannel.h" diff --git a/libraries/APO/AP_Controller.cpp b/libraries/APO/AP_Controller.cpp index 9925923f59..54ae93ddbd 100644 --- a/libraries/APO/AP_Controller.cpp +++ b/libraries/APO/AP_Controller.cpp @@ -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 { @@ -92,10 +91,6 @@ void AP_Controller::update(const float dt) { } // this sends commands to motors - handleMotors(); -} - -void AP_Controller::handleMotors() { if(getState()==MAV_STATE_ACTIVE) { digitalWrite(_hal->aLedPin, HIGH); setMotors(); @@ -105,6 +100,5 @@ void AP_Controller::handleMotors() { } } - } // namespace apo // vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Controller.h b/libraries/APO/AP_Controller.h index 639ec22b71..cd3c10a81e 100644 --- a/libraries/APO/AP_Controller.h +++ b/libraries/APO/AP_Controller.h @@ -19,315 +19,127 @@ #ifndef AP_Controller_H #define AP_Controller_H -#include -#include "../GCS_MAVLink/GCS_MAVLink.h" -#include +// inclusions +#include "../AP_Common/AP_Common.h" #include "../AP_Common/AP_Var.h" -#include "AP_Var_keys.h" - -class AP_Var_group; +#include +#include +#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); - // derived class cannot override + /// + // 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); - void setAllRadioChannelsToNeutral(); - void setAllRadioChannelsManually(); - void handleMotors(); - // derived class must define + /// + // 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(); + + /// + // 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; - // access + /// + // 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; - MAV_MODE _mode; - MAV_STATE _state; -}; - -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((input - _eP) / 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 diff --git a/libraries/APO/AP_ControllerBlock.cpp b/libraries/APO/AP_ControllerBlock.cpp new file mode 100644 index 0000000000..526c3ae4d9 --- /dev/null +++ b/libraries/APO/AP_ControllerBlock.cpp @@ -0,0 +1,179 @@ +/* + * AP_ControllerBlock.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_ControllerBlock.h" +#include + +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 diff --git a/libraries/APO/AP_ControllerBlock.h b/libraries/APO/AP_ControllerBlock.h new file mode 100644 index 0000000000..012a34a374 --- /dev/null +++ b/libraries/APO/AP_ControllerBlock.h @@ -0,0 +1,237 @@ +/* + * AP_ControllerBlock.h + * Copyright (C) James Goppert 2010 + * + * 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 . + */ + +#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 diff --git a/libraries/AP_Common/include/menu.h b/libraries/AP_Common/include/menu.h index 1e4e3a0d89..2cff1b9aca 100644 --- a/libraries/AP_Common/include/menu.h +++ b/libraries/AP_Common/include/menu.h @@ -16,6 +16,8 @@ #ifndef __AP_COMMON_MENU_H #define __AP_COMMON_MENU_H +#include + #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 From 22b1b6409232f47e85778f44cc8ad483e48c0f60 Mon Sep 17 00:00:00 2001 From: Olivier ADLER Date: Thu, 24 Nov 2011 21:48:50 +0100 Subject: [PATCH 11/11] ArduPPM 0.9.87 manual update Corrected failsafe values Minor modifications --- Tools/ArduPPM/ATMega328p/manual.txt | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/Tools/ArduPPM/ATMega328p/manual.txt b/Tools/ArduPPM/ATMega328p/manual.txt index b4fb772833..0e7d851b89 100644 --- a/Tools/ArduPPM/ATMega328p/manual.txt +++ b/Tools/ArduPPM/ATMega328p/manual.txt @@ -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)