From aa598b575a44fe80e56d78be29bdf0a0a7821140 Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Sun, 1 May 2011 02:05:17 +0000 Subject: [PATCH] APO merge. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1935 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/APO/APO.cpp | 8 + libraries/APO/APO.h | 21 + libraries/APO/APO_Config.h | 50 ++ libraries/APO/AP_Autopilot.cpp | 216 +++++ libraries/APO/AP_Autopilot.h | 153 ++++ libraries/APO/AP_CommLink.cpp | 15 + libraries/APO/AP_CommLink.h | 775 ++++++++++++++++++ libraries/APO/AP_Controller.cpp | 8 + libraries/APO/AP_Controller.h | 167 ++++ libraries/APO/AP_Guide.cpp | 8 + libraries/APO/AP_Guide.h | 298 +++++++ libraries/APO/AP_HardwareAbstractionLayer.cpp | 8 + libraries/APO/AP_HardwareAbstractionLayer.h | 99 +++ libraries/APO/AP_MavlinkCommand.cpp | 8 + libraries/APO/AP_MavlinkCommand.h | 456 +++++++++++ libraries/APO/AP_Navigator.cpp | 8 + libraries/APO/AP_Navigator.h | 445 ++++++++++ libraries/APO/AP_RcChannel.cpp | 109 +++ .../{AP_RcChannel => APO}/AP_RcChannel.h | 51 +- libraries/APO/AP_Var_keys.h | 41 + libraries/APO/constants.h | 23 + libraries/APO/controllers.h | 277 +++++++ .../examples/Manual/Makefile | 0 .../examples/Manual/Manual.pde | 19 +- .../examples/ServoSweep/Makefile | 0 .../examples/ServoSweep/ServoSweep.pde | 21 +- libraries/APO/mikrokopter.h | 56 ++ libraries/APO/template.h | 32 + libraries/AP_Compass/AP_Compass_HMC5843.h | 4 +- libraries/AP_Compass/Compass.h | 4 +- libraries/AP_DCM/AP_DCM.h | 12 +- libraries/AP_DCM/AP_DCM_HIL.h | 12 +- libraries/AP_GPS/AP_GPS_Auto.h | 4 +- libraries/AP_GPS/AP_GPS_HIL.h | 2 +- libraries/AP_GPS/AP_GPS_IMU.h | 4 +- libraries/AP_GPS/AP_GPS_MTK.h | 2 +- libraries/AP_GPS/AP_GPS_MTK16.h | 2 +- libraries/AP_GPS/AP_GPS_NMEA.h | 2 +- libraries/AP_GPS/AP_GPS_None.h | 2 +- libraries/AP_GPS/AP_GPS_SIRF.h | 2 +- libraries/AP_GPS/AP_GPS_Shim.h | 2 +- libraries/AP_GPS/AP_GPS_UBLOX.h | 2 +- libraries/AP_IMU/AP_IMU_Oilpan.h | 6 +- libraries/AP_IMU/IMU.h | 2 +- libraries/AP_RcChannel/AP_RcChannel.cpp | 128 --- libraries/FastSerial/BetterStream.h | 2 +- 46 files changed, 3364 insertions(+), 202 deletions(-) create mode 100644 libraries/APO/APO.cpp create mode 100644 libraries/APO/APO.h create mode 100644 libraries/APO/APO_Config.h create mode 100644 libraries/APO/AP_Autopilot.cpp create mode 100644 libraries/APO/AP_Autopilot.h create mode 100644 libraries/APO/AP_CommLink.cpp create mode 100644 libraries/APO/AP_CommLink.h create mode 100644 libraries/APO/AP_Controller.cpp create mode 100644 libraries/APO/AP_Controller.h create mode 100644 libraries/APO/AP_Guide.cpp create mode 100644 libraries/APO/AP_Guide.h create mode 100644 libraries/APO/AP_HardwareAbstractionLayer.cpp create mode 100644 libraries/APO/AP_HardwareAbstractionLayer.h create mode 100644 libraries/APO/AP_MavlinkCommand.cpp create mode 100644 libraries/APO/AP_MavlinkCommand.h create mode 100644 libraries/APO/AP_Navigator.cpp create mode 100644 libraries/APO/AP_Navigator.h create mode 100644 libraries/APO/AP_RcChannel.cpp rename libraries/{AP_RcChannel => APO}/AP_RcChannel.h (57%) create mode 100644 libraries/APO/AP_Var_keys.h create mode 100644 libraries/APO/constants.h create mode 100644 libraries/APO/controllers.h rename libraries/{AP_RcChannel => APO}/examples/Manual/Makefile (100%) rename libraries/{AP_RcChannel => APO}/examples/Manual/Manual.pde (71%) rename libraries/{AP_RcChannel => APO}/examples/ServoSweep/Makefile (100%) rename libraries/{AP_RcChannel => APO}/examples/ServoSweep/ServoSweep.pde (90%) create mode 100644 libraries/APO/mikrokopter.h create mode 100644 libraries/APO/template.h delete mode 100644 libraries/AP_RcChannel/AP_RcChannel.cpp diff --git a/libraries/APO/APO.cpp b/libraries/APO/APO.cpp new file mode 100644 index 0000000000..2592d1140c --- /dev/null +++ b/libraries/APO/APO.cpp @@ -0,0 +1,8 @@ +/* + * APO.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "APO.h" diff --git a/libraries/APO/APO.h b/libraries/APO/APO.h new file mode 100644 index 0000000000..e6842df84c --- /dev/null +++ b/libraries/APO/APO.h @@ -0,0 +1,21 @@ +/* + * APO.h + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#ifndef APO_H_ +#define APO_H_ + +#include "AP_Autopilot.h" +#include "AP_Guide.h" +#include "AP_Controller.h" +#include "AP_HardwareAbstractionLayer.h" +#include "AP_MavlinkCommand.h" +#include "AP_Navigator.h" +#include "AP_RcChannel.h" +//#include "AP_Var_keys.h" +//#include "APO_Config.h" + +#endif /* APO_H_ */ diff --git a/libraries/APO/APO_Config.h b/libraries/APO/APO_Config.h new file mode 100644 index 0000000000..1f01178b25 --- /dev/null +++ b/libraries/APO/APO_Config.h @@ -0,0 +1,50 @@ +#ifndef APO_Config_H +#define APO_Config_H + +#include "AP_HardwareAbstractionLayer.h" +#include "mikrokopter.h" +#include "constants.h" + +// Serial 0: debug /dev/ttyUSB0 +// Serial 1: gps/hil /dev/ttyUSB1 +// Serial 2: gcs /dev/ttyUSB2 + +// select hardware absraction mode from +// MODE_LIVE, actual flight +// TODO: IMPLEMENT --> MODE_HIL_NAV, hardware in the loop with sensors running, tests navigation system and control +// MODE_HIL_CNTL, hardware in the loop with only controller running, just tests controller +extern apo::halMode_t halMode; + +// select from, BOARD_ARDUPILOTMEGA +extern apo::board_t board; + +// select from, VEHICLE_CAR, VEHICLE_QUAD, VEHICLE_PLANE +extern apo::vehicle_t vehicle; + +//---------ADVANCED SECTION ----------------// + +// loop rates +extern const float loop0Rate; +extern const float loop1Rate; +extern const float loop2Rate; +extern const float loop3Rate; +extern const float loop4Rate; + +// max time in seconds to allow flight without ground station comms +// zero will ignore timeout +extern const uint8_t heartbeatTimeout; + +//---------HARDWARE CONFIG ----------------// + +//Hardware Parameters +#define SLIDE_SWITCH_PIN 40 +#define PUSHBUTTON_PIN 41 +#define A_LED_PIN 37 //36 = B,3637 = A,363735 = C +#define B_LED_PIN 36 +#define C_LED_PIN 35 +#define EEPROM_MAX_ADDR 2048 +#define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarLV + + +#endif +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp new file mode 100644 index 0000000000..8be1adfbff --- /dev/null +++ b/libraries/APO/AP_Autopilot.cpp @@ -0,0 +1,216 @@ +/* + * AP_Autopilot.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_Autopilot.h" + +namespace apo { + +class AP_HardwareAbstractionLayer; + +AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, AP_Controller * controller, + AP_HardwareAbstractionLayer * hal) : + Loop(loop0Rate, callback0, this), + _navigator(navigator), _guide(guide), _controller(controller), _hal(hal) { + + /* + * Calibration + */ + navigator->calibrate(); + + // start clock + uint32_t timeStart = millis(); + uint16_t gpsWaitTime = 5000; // 5 second wait for gps + + /* + * Look for valid initial state + */ + while (1) { + // letc gcs known we are alive + hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + hal->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + delay(1000); + if (hal->mode() == MODE_LIVE) { + _navigator->updateSlow(0); + if (_hal->gps) { + if (hal->gps->fix) { + break; + } else { + hal->gcs->sendText(SEVERITY_LOW,PSTR("waiting for gps lock\n")); + hal->debug->printf_P(PSTR("waiting for gps lock\n")); + } + } else { // no gps, can skip + break; + } + } else if(hal->mode() == MODE_HIL_CNTL){ // hil + _hal->hil->receive(); + Serial.println("HIL Recieve Called"); + if (_navigator->getTimeStamp() != 0) { + // give hil a chance to send some packets + for (int i=0;i<5;i++) { + hal->debug->println_P(PSTR("reading initial hil packets")); + hal->gcs->sendText(SEVERITY_LOW,PSTR("reading initial hil packets")); + delay(1000); + } + break; + } + hal->debug->println_P(PSTR("waiting for hil packet")); + } + } + AP_MavlinkCommand home(0); + home.setAlt(_navigator->getAlt()); + home.setLat(_navigator->getLat()); + home.setLon(_navigator->getLon()); + home.save(); + _hal->debug->printf_P(PSTR("home before load lat: %f deg, lon: %f deg\n"), home.getLat()*rad2Deg,home.getLon()*rad2Deg); + home.load(); + _hal->debug->printf_P(PSTR("home after load lat: %f deg, lon: %f deg\n"), home.getLat()*rad2Deg,home.getLon()*rad2Deg); + + /* + * Attach loops + */ + hal->debug->println_P(PSTR("attaching loops")); + subLoops().push_back(new Loop(loop1Rate, callback1, this)); + subLoops().push_back(new Loop(loop2Rate, callback2, this)); + subLoops().push_back(new Loop(loop3Rate, callback3, this)); + subLoops().push_back(new Loop(loop4Rate, callback4, this)); + + hal->debug->println_P(PSTR("running")); + hal->gcs->sendText(SEVERITY_LOW,PSTR("running")); +} + +void AP_Autopilot::callback0(void * data) { + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->hal()->debug->println_P(PSTR("callback 0")); + + /* + * ahrs update + */ + if (apo->navigator()) + apo->navigator()->updateFast(1.0/loop0Rate); +} + +void AP_Autopilot::callback1(void * data) { + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->hal()->debug->println_P(PSTR("callback 1")); + + /* + * hardware in the loop + */ + if (apo->hal()->hil && apo->hal()->mode()!=MODE_LIVE) + { + apo->hal()->hil->receive(); + apo->hal()->hil->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); + } + + /* + * update control laws + */ + if (apo->guide()) + apo->guide()->update(); + + /* + * update control laws + */ + if (apo->controller()) + { + //apo->hal()->debug->println_P(PSTR("updating controller")); + apo->controller()->update(1./loop1Rate); + } + /* + char msg[50]; + sprintf(msg, "c_hdg: %f, c_thr: %f", apo->guide()->headingCommand, apo->guide()->groundSpeedCommand); + apo->hal()->gcs->sendText(AP_CommLink::SEVERITY_LOW, msg); + */ +} + +void AP_Autopilot::callback2(void * data) { + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->hal()->debug->println_P(PSTR("callback 2")); + + /* + * send telemetry + */ + if (apo->hal()->gcs) { + // send messages + apo->hal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW); + apo->hal()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE); + //apo->hal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED); + //apo->hal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION); + apo->hal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW); + //apo->hal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU); + } + + /* + * slow navigation loop update + */ + if (apo->navigator()) { + apo->navigator()->updateSlow(1.0/loop2Rate); + } + + /* + * handle ground control station communication + */ + if (apo->hal()->gcs) { + // send messages + apo->hal()->gcs->requestCmds(); + apo->hal()->gcs->sendParameters(); + + // receive messages + apo->hal()->gcs->receive(); + } + + /* + * navigator debug + */ + /* + if (apo->navigator()) { + apo->hal()->debug->printf_P(PSTR("roll: %f deg\tpitch: %f deg\tyaw: %f deg\n"), + apo->navigator()->getRoll()*rad2Deg, + apo->navigator()->getPitch()*rad2Deg, + apo->navigator()->getYaw()*rad2Deg); + apo->hal()->debug->printf_P(PSTR("lat: %f deg\tlon: %f deg\talt: %f m\n"), + apo->navigator()->getLat()*rad2Deg, + apo->navigator()->getLon()*rad2Deg, + apo->navigator()->getAlt()); + } + */ +} + +void AP_Autopilot::callback3(void * data) { + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->hal()->debug->println_P(PSTR("callback 3")); + + /* + * send heartbeat + */ + apo->hal()->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT); + + /* + * load/loop rate/ram debug + */ + + apo->hal()->load = apo->load(); + apo->hal()->debug->printf_P(PSTR("load: %d%%\trate: %f Hz\tfree ram: %d bytes\n"), + apo->load(),1.0/apo->dt(),freeMemory()); + + apo->hal()->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS); + + /* + * adc debug + */ + //apo->getDebug().printf_P(PSTR("adc: %d %d %d %d %d %d %d %d\n"), + //apo->adc()->Ch(0), apo->adc()->Ch(1), apo->adc()->Ch(2), + //apo->adc()->Ch(3), apo->adc()->Ch(4), apo->adc()->Ch(5), + //apo->adc()->Ch(6), apo->adc()->Ch(7), apo->adc()->Ch(8)); +} + +void AP_Autopilot::callback4(void * data) { + AP_Autopilot * apo = (AP_Autopilot *) data; + //apo->hal()->debug->println_P(PSTR("callback 4")); +} + +} // apo diff --git a/libraries/APO/AP_Autopilot.h b/libraries/APO/AP_Autopilot.h new file mode 100644 index 0000000000..93054be8b5 --- /dev/null +++ b/libraries/APO/AP_Autopilot.h @@ -0,0 +1,153 @@ +/* + * AP_Autopilot.h + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#ifndef AP_AUTOPILOT_H_ +#define AP_AUTOPILOT_H_ + +/* + * AVR runtime + */ +#include +#include +#include +#include +/* + * Libraries + */ +#include "../AP_Common/AP_Common.h" +#include "../FastSerial/FastSerial.h" +#include "../AP_GPS/GPS.h" +#include "../APM_RC/APM_RC.h" +#include "../AP_ADC/AP_ADC.h" +#include "../APM_BMP085/APM_BMP085.h" +#include "../AP_Compass/AP_Compass.h" +#include "../AP_Math/AP_Math.h" +#include "../AP_IMU/AP_IMU.h" +#include "../AP_DCM/AP_DCM.h" +#include "../AP_Common/AP_Loop.h" +#include "../GCS_MAVLink/GCS_MAVLink.h" +#include "../AP_RangeFinder/AP_RangeFinder.h" +/* + * Local Modules + */ +#include "AP_HardwareAbstractionLayer.h" +#include "AP_RcChannel.h" +#include "AP_Controller.h" +#include "AP_Navigator.h" +#include "AP_Guide.h" +#include "AP_CommLink.h" +#include "controllers.h" + +/** + * ArduPilotOne namespace to protect varibles + * from overlap with avr and libraries etc. + * ArduPilotOne does not use any global + * variables. + */ +namespace apo { + +// forward declarations +class AP_CommLink; + +/** + * This class encapsulates the entire autopilot system + * The constructor takes the serial streams available + * and attaches them to the appropriate devices. + * Also, since APM_RC is globally instantiated this + * is also passed to the constructor so that we + * can avoid and global instance calls for maximum + * clarity. + * + * It inherits from loop to manage + * the subloops and sets the overal + * frequency for the autopilot. + * + + */ +class AP_Autopilot: public Loop { +public: + /** + * Default constructor + */ + AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide, AP_Controller * controller, + AP_HardwareAbstractionLayer * hal); + + /** + * Accessors + */ + AP_Navigator * navigator() { + return _navigator; + } + AP_Guide * guide() { + return _guide; + } + AP_Controller * controller() { + return _controller; + } + AP_HardwareAbstractionLayer * hal() { + return _hal; + } + +private: + + /** + * Loop 0 Callbacks (fastest) + * - inertial navigation + * @param data A void pointer used to pass the apo class + * so that the apo public interface may be accessed. + */ + static void callback0(void * data); + + /** + * Loop 1 Callbacks + * - control + * - compass reading + * @see callback0 + */ + static void callback1(void * data); + + /** + * Loop 2 Callbacks + * - gps sensor fusion + * - compass sensor fusion + * @see callback0 + */ + static void callback2(void * data); + + /** + * Loop 3 Callbacks + * - slow messages + * @see callback0 + */ + static void callback3(void * data); + + /** + * Loop 4 Callbacks + * - super slow mesages + * - log writing + * @see callback0 + */ + static void callback4(void * data); + + /** + * Components + */ + AP_Navigator * _navigator; + AP_Guide * _guide; + AP_Controller * _controller; + AP_HardwareAbstractionLayer * _hal; + + /** + * Constants + */ + static const float deg2rad = M_PI / 180; + static const float rad2deg = 180 / M_PI; +}; + +} // namespace apo + +#endif /* AP_AUTOPILOT_H_ */ diff --git a/libraries/APO/AP_CommLink.cpp b/libraries/APO/AP_CommLink.cpp new file mode 100644 index 0000000000..92b9d0fdde --- /dev/null +++ b/libraries/APO/AP_CommLink.cpp @@ -0,0 +1,15 @@ +/* + * AP_CommLink.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_CommLink.h" + +namespace apo { + +uint8_t MavlinkComm::_nChannels = 0; +uint8_t MavlinkComm::_paramNameLengthMax = 13; + +} // apo diff --git a/libraries/APO/AP_CommLink.h b/libraries/APO/AP_CommLink.h new file mode 100644 index 0000000000..3c0cb5da80 --- /dev/null +++ b/libraries/APO/AP_CommLink.h @@ -0,0 +1,775 @@ +/* + * AP_CommLink.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_CommLink_H +#define AP_CommLink_H + +#include "AP_HardwareAbstractionLayer.h" +#include "AP_MavlinkCommand.h" +#include "AP_Controller.h" + +namespace apo { + +class AP_Controller; +class AP_Navigator; +class AP_Guide; +class AP_HardwareAbstractionLayer; + +enum { + SEVERITY_LOW, SEVERITY_MED, SEVERITY_HIGH +}; + +// forward declarations +//class ArduPilotOne; +//class AP_Controller; + +/// CommLink class +class AP_CommLink { +public: + + AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide, AP_Controller * controller, AP_HardwareAbstractionLayer * hal) : + _link(link), _navigator(navigator), _guide(guide), _controller(controller), _hal(hal) { + } + virtual void send() = 0; + virtual void receive() = 0; + virtual void sendMessage(uint8_t id, uint32_t param = 0) = 0; + virtual void sendText(uint8_t severity, const char *str) = 0; + virtual void sendText(uint8_t severity, const prog_char_t *str) = 0; + virtual void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) = 0; + virtual void sendParameters() = 0; + virtual void requestCmds() = 0; + +protected: + FastSerial * _link; + AP_Navigator * _navigator; + AP_Guide * _guide; + AP_Controller * _controller; + AP_HardwareAbstractionLayer * _hal; +}; + +class MavlinkComm: public AP_CommLink { +public: + MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide, + AP_Controller * controller, AP_HardwareAbstractionLayer * hal) : + AP_CommLink(link, nav, guide, controller, hal), + + // options + _useRelativeAlt(true), + + // commands + _sendingCmds(false), _receivingCmds(false), + _cmdTimeLastSent(millis()), _cmdTimeLastReceived(millis()), + _cmdDestSysId(0), _cmdDestCompId(0), _cmdRequestIndex(0), + _cmdMax(30), + + // parameters + _parameterCount(0), _queuedParameter(NULL), + _queuedParameterIndex(0) { + + switch (_nChannels) { + case 0: + mavlink_comm_0_port = link; + _channel = MAVLINK_COMM_0; + _nChannels++; + break; + case 1: + mavlink_comm_1_port = link; + _channel = MAVLINK_COMM_1; + _nChannels++; + break; + default: + // signal that number of channels exceeded + _channel = MAVLINK_COMM_NB_HIGH; + break; + } + } + + virtual void send() { + // if number of channels exceeded return + if (_channel == MAVLINK_COMM_NB_HIGH) + return; + } + + void sendMessage(uint8_t id, uint32_t param = 0) { + //_hal->debug->printf_P(PSTR("send message\n")); + + // if number of channels exceeded return + if (_channel == MAVLINK_COMM_NB_HIGH) + return; + + uint64_t timeStamp = micros(); + + switch (id) { + + case MAVLINK_MSG_ID_HEARTBEAT: { + mavlink_msg_heartbeat_send(_channel, mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); + break; + } + + case MAVLINK_MSG_ID_ATTITUDE: { + mavlink_msg_attitude_send(_channel, timeStamp, + _navigator->getRoll(), _navigator->getPitch(), + _navigator->getYaw(), _navigator->getRollRate(), + _navigator->getPitchRate(), _navigator->getYawRate()); + break; + } + + case MAVLINK_MSG_ID_GLOBAL_POSITION: { + mavlink_msg_global_position_send(_channel, timeStamp, + _navigator->getLat()*rad2Deg, _navigator->getLon()*rad2Deg, + _navigator->getAlt(), _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->getYaw()*rad2Deg); + break; + } + + /* + case MAVLINK_MSG_ID_GPS_RAW_INT: { + mavlink_msg_gps_raw_int_send(_channel,timeStamp,_hal->gps->status(), + _navigator->getLat_degInt(), _navigator->getLon_degInt(),_navigator->getAlt_intM(), 0,0, + _navigator->getGroundSpeed(),_navigator->getYaw()*rad2Deg); + break; + } + */ + + 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 + } + + case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: { + int16_t ch[8]; + for (int i = 0; i < 8; i++) + ch[i] = 0; + for (int i = 0; i < 8 && i < _hal->rc.getSize(); i++) + { + ch[i] = 10000 * _hal->rc[i]->getPosition(); + //_hal->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]); + } + mavlink_msg_rc_channels_scaled_send(_channel, ch[0], ch[1], ch[2], + ch[3], ch[4], ch[5], ch[6], ch[7], 255); + break; + } + + case MAVLINK_MSG_ID_RC_CHANNELS_RAW: { + int16_t ch[8]; + for (int i = 0; i < 8; i++) + ch[i] = 0; + for (int i = 0; i < 8 && i < _hal->rc.getSize(); i++) + ch[i] = _hal->rc[i]->getPwm(); + mavlink_msg_rc_channels_raw_send(_channel, ch[0], ch[1], ch[2], + ch[3], ch[4], ch[5], ch[6], ch[7], 255); + break; + } + + case MAVLINK_MSG_ID_SYS_STATUS: { + + float batteryVoltage, prev_cell, temp; + temp = analogRead(0); + batteryVoltage = ((temp*5/1023)/0.28); + + mavlink_msg_sys_status_send(_channel, _controller->getMode(), + _guide->getMode(), _hal->state(), _hal->load * 10, + batteryVoltage*1000,(batteryVoltage - 3.3)/(4.2-3.3)*1000, + _packetDrops); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: { + sendText(SEVERITY_LOW, PSTR("waypoint ack")); + mavlink_waypoint_ack_t packet; + uint8_t type = 0; // ok (0), error(1) + mavlink_msg_waypoint_ack_send(_channel, _cmdDestSysId, + _cmdDestCompId, type); + + // turn off waypoint send + _receivingCmds = false; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CURRENT: { + mavlink_msg_waypoint_current_send(_channel,_guide->getCurrentIndex()); + break; + } + + default: { + char msg[50]; + sprintf(msg, "autopilot sending unknown command with id: %d", id); + sendText(SEVERITY_HIGH, msg); + } + + } // switch + } // send message + + virtual void receive() { + //_hal->debug->printf_P(PSTR("receive\n")); + // if number of channels exceeded return + // + if (_channel == MAVLINK_COMM_NB_HIGH) + return; + + // receive new packets + mavlink_message_t msg; + mavlink_status_t status; + + // process received bytes + while (comm_get_available(_channel)) { + uint8_t c = comm_receive_ch(_channel); + + // Try to get a new message + if (mavlink_parse_char(_channel, c, &msg, &status)) + _handleMessage(&msg); + } + + // Update packet drops counter + _packetDrops += status.packet_rx_drop_count; + } + + void sendText(uint8_t severity, const char *str) { + mavlink_msg_statustext_send(_channel, severity, (const int8_t*) str); + } + + void sendText(uint8_t severity, const prog_char_t *str) { + mavlink_statustext_t m; + uint8_t i; + for (i = 0; i < sizeof(m.text); i++) { + m.text[i] = pgm_read_byte((const prog_char *) (str++)); + } + if (i < sizeof(m.text)) + m.text[i] = 0; + sendText(severity, (const char *) m.text); + } + + void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) { + } + + /** + * sends parameters one at a time + */ + void sendParameters() { + //_hal->debug->printf_P(PSTR("send parameters\n")); + // Check to see if we are sending parameters + while (NULL != _queuedParameter) { + AP_Var *vp; + float value; + + // copy the current parameter and prepare to move to the next + vp = _queuedParameter; + _queuedParameter = _queuedParameter->next(); + + // if the parameter can be cast to float, report it here and break out of the loop + value = vp->cast_to_float(); + if (!isnan(value)) { + + char paramName[_paramNameLengthMax]; + vp->copy_name(paramName, sizeof(paramName)); + + mavlink_msg_param_value_send(_channel, (int8_t*) paramName, + value, _countParameters(), _queuedParameterIndex); + + _queuedParameterIndex++; + break; + } + } + + } + + /** + * request commands one at a time + */ + void requestCmds() { + //_hal->debug->printf_P(PSTR("requesting commands\n")); + // request cmds one by one + if (_receivingCmds && _cmdRequestIndex <= _guide->getNumberOfCommands()) { + mavlink_msg_waypoint_request_send(_channel, _cmdDestSysId, + _cmdDestCompId, _cmdRequestIndex); + } + } + +private: + + // options + bool _useRelativeAlt; + + // commands + bool _sendingCmds; + bool _receivingCmds; + uint16_t _cmdTimeLastSent; + uint16_t _cmdTimeLastReceived; + uint16_t _cmdDestSysId; + uint16_t _cmdDestCompId; + uint16_t _cmdRequestIndex; + Vector _cmdList; + + // parameters + static uint8_t _paramNameLengthMax; + uint16_t _parameterCount; + AP_Var * _queuedParameter; + uint16_t _queuedParameterIndex; + uint16_t _cmdMax; + + // channel + mavlink_channel_t _channel; + uint16_t _packetDrops; + static uint8_t _nChannels; + + void _handleMessage(mavlink_message_t * msg) { + + uint32_t timeStamp = micros(); + + switch (msg->msgid) { + //_hal->debug->printf_P(PSTR("message received: %d"), msg->msgid); + + case MAVLINK_MSG_ID_HEARTBEAT: { + mavlink_heartbeat_t packet; + mavlink_msg_heartbeat_decode(msg, &packet); + _hal->lastHeartBeat = micros(); + break; + } + + case MAVLINK_MSG_ID_GPS_RAW: { + // decode + mavlink_gps_raw_t packet; + mavlink_msg_gps_raw_decode(msg, &packet); + + _navigator->setTimeStamp(timeStamp); + _navigator->setLat(packet.lat*deg2Rad); + _navigator->setLon(packet.lon*deg2Rad); + _navigator->setAlt(packet.alt); + _navigator->setHeading(packet.hdg*deg2Rad); + _navigator->setGroundSpeed(packet.v); + _navigator->setAirSpeed(packet.v); + //_hal->debug->printf_P(PSTR("received hil gps raw packet\n")); + /* + _hal->debug->printf_P(PSTR("received lat: %f deg\tlon: %f deg\talt: %f m\n"), + packet.lat, + packet.lon, + packet.alt); + */ + break; + } + + case MAVLINK_MSG_ID_ATTITUDE: { + // decode + mavlink_attitude_t packet; + mavlink_msg_attitude_decode(msg, &packet); + + // set dcm hil sensor + _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); + //_hal->debug->printf_P(PSTR("received hil attitude packet\n")); + break; + } + + case MAVLINK_MSG_ID_ACTION: { + // decode + mavlink_action_t packet; + mavlink_msg_action_decode(msg, &packet); + if (_checkTarget(packet.target, packet.target_component)) + break; + + // do action + sendText(SEVERITY_LOW, PSTR("action received")); + switch (packet.action) { + + case MAV_ACTION_STORAGE_READ: + AP_Var::load_all(); + break; + + case MAV_ACTION_STORAGE_WRITE: + AP_Var::save_all(); + break; + + case MAV_ACTION_CALIBRATE_RC: + case MAV_ACTION_CALIBRATE_GYRO: + case MAV_ACTION_CALIBRATE_MAG: + case MAV_ACTION_CALIBRATE_ACC: + case MAV_ACTION_CALIBRATE_PRESSURE: + 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: + sendText(SEVERITY_LOW, PSTR("unknown action")); + break; + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: { + sendText(SEVERITY_LOW, PSTR("waypoint request list")); + + // decode + mavlink_waypoint_request_list_t packet; + mavlink_msg_waypoint_request_list_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // Start sending waypoints + mavlink_msg_waypoint_count_send(_channel, msg->sysid, msg->compid, + _guide->getNumberOfCommands()); + + _cmdTimeLastSent = millis(); + _cmdTimeLastReceived = millis(); + _sendingCmds = true; + _receivingCmds = false; + _cmdDestSysId = msg->sysid; + _cmdDestCompId = msg->compid; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: { + sendText(SEVERITY_LOW, PSTR("waypoint request")); + + // Check if sending waypiont + if (!_sendingCmds) + break; + + // decode + mavlink_waypoint_request_t packet; + mavlink_msg_waypoint_request_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + //_hal->debug->printf_P(PSTR("sequence: %d\n"),packet.seq); + AP_MavlinkCommand cmd(packet.seq); + + mavlink_waypoint_t msg = cmd.convert(_guide->getCurrentIndex()); + mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId, + msg.seq, msg.frame, msg.command, msg.current, msg.autocontinue, + msg.param1, msg.param2, msg.param3, msg.param4, msg.x, msg.y, + msg.z); + + // update last waypoint comm stamp + _cmdTimeLastSent = millis(); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: { + sendText(SEVERITY_LOW, PSTR("waypoint ack")); + + // decode + mavlink_waypoint_ack_t packet; + mavlink_msg_waypoint_ack_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // check for error + uint8_t type = packet.type; // ok (0), error(1) + + // turn off waypoint send + _sendingCmds = false; + break; + } + + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + sendText(SEVERITY_LOW, PSTR("param request list")); + + // decode + mavlink_param_request_list_t packet; + mavlink_msg_param_request_list_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // Start sending parameters - next call to ::update will kick the first one out + + _queuedParameter = AP_Var::first(); + _queuedParameterIndex = 0; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: { + sendText(SEVERITY_LOW, PSTR("waypoint clear all")); + + // decode + mavlink_waypoint_clear_all_t packet; + mavlink_msg_waypoint_clear_all_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // clear all waypoints + uint8_t type = 0; // ok (0), error(1) + _guide->setNumberOfCommands(1); + _guide->setCurrentIndex(0); + + // send acknowledgement 3 times to makes sure it is received + for (int i = 0; i < 3; i++) + mavlink_msg_waypoint_ack_send(_channel, msg->sysid, msg->compid, + type); + + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: { + sendText(SEVERITY_LOW, PSTR("waypoint set current")); + + // decode + mavlink_waypoint_set_current_t packet; + mavlink_msg_waypoint_set_current_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // set current waypoint + _guide->setCurrentIndex(packet.seq); + mavlink_msg_waypoint_current_send(_channel, _guide->getCurrentIndex()); + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_COUNT: { + sendText(SEVERITY_LOW, PSTR("waypoint count")); + + // decode + mavlink_waypoint_count_t packet; + mavlink_msg_waypoint_count_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // start waypoint receiving + if (packet.count > _cmdMax) { + packet.count = _cmdMax; + } + _guide->setNumberOfCommands(packet.count); + _cmdTimeLastReceived = millis(); + _receivingCmds = true; + _sendingCmds = false; + _cmdRequestIndex = 0; + break; + } + + case MAVLINK_MSG_ID_WAYPOINT: { + sendText(SEVERITY_LOW, PSTR("waypoint")); + + // Check if receiving waypiont + if (!_receivingCmds) { + //sendText(SEVERITY_HIGH, PSTR("not receiving commands")); + break; + } + + // decode + mavlink_waypoint_t packet; + mavlink_msg_waypoint_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // check if this is the requested waypoint + if (packet.seq != _cmdRequestIndex) { + /* + char msg[50]; + sprintf(msg, + "waypoint request out of sequence: (packet) %d / %d (ap)", + packet.seq, _cmdRequestIndex); + sendText(SEVERITY_HIGH, msg); + */ + break; + } + + _hal->debug->printf_P(PSTR("received waypoint x: %f\ty: %f\tz: %f\n"), + packet.x, + packet.y, + packet.z); + + // store waypoint + AP_MavlinkCommand command(packet); + //sendText(SEVERITY_HIGH, PSTR("waypoint stored")); + _cmdRequestIndex++; + if (_cmdRequestIndex >= _guide->getNumberOfCommands()) { + sendMessage( MAVLINK_MSG_ID_WAYPOINT_ACK); + //sendText(SEVERITY_LOW, PSTR("waypoint ack sent")); + } + _cmdTimeLastReceived = millis(); + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: { + sendText(SEVERITY_LOW, PSTR("param set")); + AP_Var *vp; + AP_Meta_class::Type_id var_type; + + // decode + mavlink_param_set_t packet; + mavlink_msg_param_set_decode(msg, &packet); + if (_checkTarget(packet.target_system, packet.target_component)) + break; + + // set parameter + + char key[_paramNameLengthMax + 1]; + strncpy(key, (char *) packet.param_id, _paramNameLengthMax); + key[_paramNameLengthMax] = 0; + + // find the requested parameter + vp = AP_Var::find(key); + if ((NULL != vp) && // exists + !isnan(packet.param_value) && // not nan + !isinf(packet.param_value)) { // not inf + + // add a small amount before casting parameter values + // from float to integer to avoid truncating to the + // next lower integer value. + const float rounding_addition = 0.01; + + // fetch the variable type ID + var_type = vp->meta_type_id(); + + // handle variables with standard type IDs + if (var_type == AP_Var::k_typeid_float) { + ((AP_Float *) vp)->set_and_save(packet.param_value); + + } else if (var_type == AP_Var::k_typeid_float16) { + ((AP_Float16 *) vp)->set_and_save(packet.param_value); + + } else if (var_type == AP_Var::k_typeid_int32) { + ((AP_Int32 *) vp)->set_and_save( + packet.param_value + rounding_addition); + + } else if (var_type == AP_Var::k_typeid_int16) { + ((AP_Int16 *) vp)->set_and_save( + packet.param_value + rounding_addition); + + } else if (var_type == AP_Var::k_typeid_int8) { + ((AP_Int8 *) vp)->set_and_save( + packet.param_value + rounding_addition); + } else { + // we don't support mavlink set on this parameter + break; + } + + // Report back the new value if we accepted the change + // we send the value we actually set, which could be + // different from the value sent, in case someone sent + // a fractional value to an integer type + mavlink_msg_param_value_send(_channel, (int8_t *) key, + vp->cast_to_float(), _countParameters(), -1); // XXX we don't actually know what its index is... + } + + break; + } // end case + + + } + } + + uint16_t _countParameters() { + // if we haven't cached the parameter count yet... + if (0 == _parameterCount) { + AP_Var *vp; + + vp = AP_Var::first(); + do { + // if a parameter responds to cast_to_float then we are going to be able to report it + if (!isnan(vp->cast_to_float())) { + _parameterCount++; + } + } while (NULL != (vp = vp->next())); + } + return _parameterCount; + } + + AP_Var * _findParameter(uint16_t index) { + AP_Var *vp; + + vp = AP_Var::first(); + while (NULL != vp) { + + // if the parameter is reportable + if (!(isnan(vp->cast_to_float()))) { + // if we have counted down to the index we want + if (0 == index) { + // return the parameter + return vp; + } + // count off this parameter, as it is reportable but not + // the one we want + index--; + } + // and move to the next parameter + vp = vp->next(); + } + return NULL; + } + + // check the target + uint8_t _checkTarget(uint8_t sysid, uint8_t compid) { + /* + char msg[50]; + sprintf(msg, "target = %d / %d\tcomp = %d / %d", sysid, + mavlink_system.sysid, compid, mavlink_system.compid); + sendText(SEVERITY_LOW, msg); + */ + if (sysid != mavlink_system.sysid) { + //sendText(SEVERITY_LOW, PSTR("system id mismatch")); + return 1; + + } else if (compid != mavlink_system.compid) { + //sendText(SEVERITY_LOW, PSTR("component id mismatch")); + return 0; // XXX currently not receiving correct compid from gcs + + } else { + return 0; // no error + } + } + +}; + + +} // namespace apo + +#endif // AP_CommLink_H +// vim:ts=4:sw=4:tw=78:expandtab diff --git a/libraries/APO/AP_Controller.cpp b/libraries/APO/AP_Controller.cpp new file mode 100644 index 0000000000..ebbd5ed524 --- /dev/null +++ b/libraries/APO/AP_Controller.cpp @@ -0,0 +1,8 @@ +/* + * AP_Controller.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_Controller.h" diff --git a/libraries/APO/AP_Controller.h b/libraries/APO/AP_Controller.h new file mode 100644 index 0000000000..def541c041 --- /dev/null +++ b/libraries/APO/AP_Controller.h @@ -0,0 +1,167 @@ +/* + * AP_Controller.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_Controller_H +#define AP_Controller_H + +#include "AP_Navigator.h" +#include "AP_Guide.h" +#include "AP_HardwareAbstractionLayer.h" +#include "../AP_Common/AP_Vector.h" +#include "../AP_Common/AP_Var.h" + +namespace apo { + +/// Controller class +class AP_Controller { +public: + AP_Controller(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + _nav(nav), _guide(guide), _hal(hal) { + } + + bool commLost() { + if (_hal->getTimeSinceLastHeartBeat() > heartbeatTimeout) return true; + else return false; + } + + virtual void update(const float & dt) = 0; + + MAV_MODE getMode() { return _mode; } + +protected: + MAV_MODE _mode; + AP_Navigator * _nav; + AP_Guide * _guide; + AP_HardwareAbstractionLayer * _hal; +}; + +class Pid2 { +public: + Pid2(AP_Var::Key key, const prog_char_t * name, float kP = 0.0, + float kI = 0.0, float kD = 0.0, float iMax = 0.0, float yMax = 0.0, + uint8_t dFcut = 20.0) : + _group(key, name), _eP(0), _eI(0), _eD(0), + _kP(&_group, 1, kP, PSTR("P")), + _kI(&_group, 2, kI, PSTR("I")), + _kD(&_group, 3, kD, PSTR("D")), + _iMax(&_group, 4, iMax, PSTR("IMAX")), + _yMax(&_group, 5, yMax, PSTR("YMAX")), + _fCut(&_group, 6, dFcut, PSTR("FCUT")) { + } + + float update(const float & input, const float & dt) { + + // derivative with low pass + float RC = 1 / (2 * M_PI * _fCut); // low pass filter + _eD = _eD + ((_eP - input) / dt - _eD) * (dt / (dt + RC)); + + // proportional, note must come after derivative + // because derivatve uses _eP as previous value + _eP = input; + + // integral + _eI += _eP * dt; + + // wind up guard + if (_eI > _iMax) + _eI = _iMax; + if (_eI < -_iMax) + _eI = -_iMax; + + // pid sum + float y = _kP * _eP + _kI * _eI + _kD * _eD; + + // saturation + if (y > _yMax) + y = _yMax; + if (y < -_yMax) + y = -_yMax; + + return y; + } +protected: + AP_Var_group _group; /// helps with parameter management + float _eP; /// input + float _eI; /// integral of input + float _eD; /// derivative of input + AP_Float _kP; /// proportional gain + AP_Float _kI; /// integral gain + AP_Float _kD; /// derivative gain + AP_Float _iMax; /// integrator saturation + AP_Float _yMax; /// output saturation + AP_Uint8 _fCut; /// derivative low-pass cut freq (Hz) +}; + + +/// PID(DFB) block +class PidDFB2 { +public: + PidDFB2(AP_Var::Key key, const prog_char_t * name, + float kP = 0.0, float kI = 0.0, float kD = 0.0, float iMax = 0.0, + float yMax = 0.0) : + _group(key, name), _eP(0), _eI(0), _eD(0), + _kP(&_group, 1, kP, PSTR("P")), + _kI(&_group, 2, kI, PSTR("I")), + _kD(&_group, 3, kD, PSTR("D")), + _iMax(&_group, 4, iMax, PSTR("IMAX")), + _yMax(&_group, 5, yMax, PSTR("YMAX")) { + } + + float update(const float & input, const float & derivative, const float & dt) { + + // proportional, note must come after derivative + // because derivative uses _eP as previous value + _eP = input; + + // integral + _eI += _eP * dt; + + // wind up guard + if (_eI > _iMax) + _eI = _iMax; + if (_eI < -_iMax) + _eI = -_iMax; + + // pid sum + float y = _kP * _eP + _kI * _eI - _kD * derivative; + + // saturation + if (y > _yMax) + y = _yMax; + if (y < -_yMax) + y = -_yMax; + + return y; + } +protected: + AP_Var_group _group; /// helps with parameter management + float _eP; /// input + float _eI; /// integral of input + float _eD; /// derivative of input + AP_Float _kP; /// proportional gain + AP_Float _kI; /// integral gain + AP_Float _kD; /// derivative gain + AP_Float _iMax; /// integrator saturation + AP_Float _yMax; /// integrator saturation +}; + +} // apo + +#endif // AP_Controller_H +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp new file mode 100644 index 0000000000..488b746610 --- /dev/null +++ b/libraries/APO/AP_Guide.cpp @@ -0,0 +1,8 @@ +/* + * AP_Guide.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_Guide.h" diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h new file mode 100644 index 0000000000..9cdc7b6a51 --- /dev/null +++ b/libraries/APO/AP_Guide.h @@ -0,0 +1,298 @@ +/* + * AP_Guide.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_Guide_H +#define AP_Guide_H + +#include "../GCS_MAVLink/GCS_MAVLink.h" +#include "AP_HardwareAbstractionLayer.h" +#include "AP_Navigator.h" +#include "../AP_Common/AP_Common.h" +#include "../AP_Common/AP_Vector.h" +#include "AP_MavlinkCommand.h" +#include "constants.h" + +//#include "AP_CommLink.h" + +namespace apo { + + +/// Guide class +class AP_Guide { +public: + + /** + * This is the constructor, which requires a link to the navigator. + * @param navigator This is the navigator pointer. + */ + AP_Guide(AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal) : + _navigator(navigator), headingCommand(0), airSpeedCommand(0), + groundSpeedCommand(0), altitudeCommand(0), pNCmd(0), pECmd(0), + pDCmd(0), _hal(hal), _home(0), _command(0), _previousCommand(0), + _mode(MAV_NAV_LOST), _numberOfCommands(1), _cmdIndex(0) + { + } + + virtual void update() = 0; + virtual void nextCommand() = 0; + float headingCommand; + float airSpeedCommand; + float groundSpeedCommand; + float altitudeCommand; + float pNCmd; + float pECmd; + float pDCmd; + MAV_NAV getMode() const + { + return _mode; + } + uint8_t getCurrentIndex() + { + return _cmdIndex; + } + + void setCurrentIndex(uint8_t val) + { + _cmdIndex.set_and_save(val); + _command = AP_MavlinkCommand(getCurrentIndex()); + _previousCommand = AP_MavlinkCommand(getPreviousIndex()); + //_hal->gcs->sendMessage(MAVLINK_MSG_ID_WAYPOINT_CURRENT); + } + + uint8_t getNumberOfCommands() + { + return _numberOfCommands; + } + + void setNumberOfCommands(uint8_t val) + { + _numberOfCommands.set_and_save(val); + } + + uint8_t getPreviousIndex() { + // find previous waypoint, TODO, handle non-nav commands + int16_t prevIndex = int16_t(getCurrentIndex()) -1 ; + if (prevIndex < 0) prevIndex = getNumberOfCommands()-1; + return (uint8_t)prevIndex; + } + + uint8_t getNextIndex() { + // find previous waypoint, TODO, handle non-nav commands + int16_t nextIndex = int16_t(getCurrentIndex()) + 1 ; + if (nextIndex > (getNumberOfCommands() -1)) nextIndex = 0; + return nextIndex; + } + +protected: + + AP_MavlinkCommand _home, _command, _previousCommand; + MAV_NAV _mode; + AP_Uint8 _numberOfCommands; + AP_Uint8 _cmdIndex; + AP_Navigator * _navigator; + AP_HardwareAbstractionLayer * _hal; +}; + +class MavlinkGuide: public AP_Guide { +public: + MavlinkGuide(AP_Var::Key key, AP_Navigator * navigator, AP_HardwareAbstractionLayer * hal) : + AP_Guide(navigator, hal), _rangeFinderFront(), _rangeFinderBack(), + _rangeFinderLeft(), _rangeFinderRight(), + _group(key,PSTR("GUIDE_")), + _velocityCommand(&_group, 1, 1, PSTR("VELCMD")), + _crossTrackGain(&_group, 2, 2, PSTR("XTK")), + _crossTrackLim(&_group, 3, 10, PSTR("XTLIM")) + { + + for (int i = 0; i < _hal->rangeFinders.getSize(); i++) { + RangeFinder * rF = _hal->rangeFinders[i]; + if (rF == NULL) + continue; + if (rF->orientation_x == 1 && rF->orientation_y == 0 + && rF->orientation_z == 0) + _rangeFinderFront = rF; + else if (rF->orientation_x == -1 && rF->orientation_y == 0 + && rF->orientation_z == 0) + _rangeFinderBack = rF; + else if (rF->orientation_x == 0 && rF->orientation_y == 1 + && rF->orientation_z == 0) + _rangeFinderRight = rF; + else if (rF->orientation_x == 0 && rF->orientation_y == -1 + && rF->orientation_z == 0) + _rangeFinderLeft = rF; + + } + } + + virtual void update() { + /*_hal->debug->printf_P( + PSTR("guide loop, number: %d, current index: %d, previous index: %d\n"), + getNumberOfCommands(), + getCurrentIndex(), + getPreviousIndex());*/ + + + + // if we don't have enough waypoint for cross track calcs + // go home + if (_numberOfCommands == 1) { + headingCommand = _home.bearingTo(_navigator->getLat_degInt(), + _navigator->getLon_degInt()) + 180*deg2Rad; + if (headingCommand > 360*deg2Rad) headingCommand -= 360*deg2Rad; + /* + _hal->debug->printf_P(PSTR("going home: bearing: %f distance: %f\n"), + headingCommand,home.distanceTo(_navigator->getLat_degInt(),_navigator->getLon_degInt())); + */ + } else { + // TODO wrong behavior if 0 selected as waypoint, says previous 0 + float dXt = AP_MavlinkCommand::crossTrack(_previousCommand, + _command, _navigator->getLat_degInt(), + _navigator->getLon_degInt()); + float temp = dXt*_crossTrackGain*deg2Rad; // crosstrack gain, rad/m + if (temp > _crossTrackLim * deg2Rad) + temp = _crossTrackLim * deg2Rad; + if (temp < -_crossTrackLim * deg2Rad) + temp = -_crossTrackLim * deg2Rad; + float bearing = _previousCommand.bearingTo(_command); + headingCommand = bearing - temp; + float alongTrack = AP_MavlinkCommand::alongTrack(_previousCommand,_command, + _navigator->getLat_degInt(),_navigator->getLon_degInt()); + float distanceToNext = _command.distanceTo(_navigator->getLat_degInt(),_navigator->getLon_degInt()); + float segmentLength = _previousCommand.distanceTo(_command); + if (distanceToNext < _command.getRadius() || alongTrack > segmentLength) nextCommand(); + /* + _hal->debug->printf_P( + PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\n"), + bearing * rad2Deg, dXt, headingCommand * rad2Deg, distanceToNext, alongTrack); + */ + } + + groundSpeedCommand = _velocityCommand; + + // TODO : calculate pN,pE,pD from home and gps coordinates + pNCmd = 0; + pECmd = 0; + pDCmd = 0; + + // process mavlink commands + //handleCommand(); + + // obstacle avoidance overrides + // stop if your going to drive into something in front of you + for(int i=0;i < _hal->rangeFinders.getSize(); i++) _hal->rangeFinders[i]->read(); + float frontDistance = _rangeFinderFront->distance/200.0; //convert for other adc + if (_rangeFinderFront && frontDistance < 2) { + //airSpeedCommand = 0; + //groundSpeedCommand = 0; + headingCommand -= 45*deg2Rad; + _hal->debug->print("Obstacle Distance (m): "); + _hal->debug->println(frontDistance); + _hal->debug->print("Obstacle avoidance Heading Command: "); + _hal->debug->println(headingCommand); + _hal->debug->printf_P( + PSTR("Front Distance, %f\n"), + frontDistance); + } + if (_rangeFinderBack && _rangeFinderBack->distance < 5) { + airSpeedCommand = 0; + groundSpeedCommand = 0; + + } + + if (_rangeFinderLeft && _rangeFinderLeft->distance < 5) { + airSpeedCommand = 0; + groundSpeedCommand = 0; + } + + if (_rangeFinderRight && _rangeFinderRight->distance < 5) { + airSpeedCommand = 0; + groundSpeedCommand = 0; + } + } + + void nextCommand() { + _cmdIndex = getNextIndex(); + _command = AP_MavlinkCommand(getCurrentIndex()); + _previousCommand = AP_MavlinkCommand(getPreviousIndex()); + //_hal->gcs->sendMessage(MAVLINK_MSG_ID_WAYPOINT_CURRENT); + } + + void handleCommand(AP_MavlinkCommand command, + AP_MavlinkCommand previousCommand) { + // TODO handle more commands + switch (command.getCommand()) { + case MAV_CMD_NAV_WAYPOINT: { + // if within radius, increment + float d = previousCommand.distanceTo(_navigator->getLat_degInt(), + _navigator->getLon_degInt()); + if (d < command.getRadius()) { + nextCommand(); + } + break; + } + /* + case MAV_CMD_CONDITION_CHANGE_ALT: + case MAV_CMD_CONDITION_DELAY: + case MAV_CMD_CONDITION_DISTANCE: + case MAV_CMD_CONDITION_LAST: + case MAV_CMD_CONDITION_YAW: + case MAV_CMD_DO_CHANGE_SPEED: + case MAV_CMD_DO_CONTROL_VIDEO: + case MAV_CMD_DO_JUMP: + case MAV_CMD_DO_LAST: + case MAV_CMD_DO_LAST: + case MAV_CMD_DO_REPEAT_RELAY: + case MAV_CMD_DO_REPEAT_SERVO: + case MAV_CMD_DO_SET_HOME: + case MAV_CMD_DO_SET_MODE: + case MAV_CMD_DO_SET_PARAMETER: + case MAV_CMD_DO_SET_RELAY: + case MAV_CMD_DO_SET_SERVO: + case MAV_CMD_PREFLIGHT_CALIBRATION: + case MAV_CMD_PREFLIGHT_STORAGE: + case MAV_CMD_NAV_LAND: + case MAV_CMD_NAV_LAST: + case MAV_CMD_NAV_LOITER_TIME: + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_LOITER_UNLIM: + case MAV_CMD_NAV_ORIENTATION_TARGET: + case MAV_CMD_NAV_PATHPLANNING: + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + case MAV_CMD_NAV_TAKEOFF: + */ + default: + // unhandled command, skip + nextCommand(); + break; + } + } +private: + RangeFinder * _rangeFinderFront; + RangeFinder * _rangeFinderBack; + RangeFinder * _rangeFinderLeft; + RangeFinder * _rangeFinderRight; + AP_Var_group _group; + AP_Float _velocityCommand; + AP_Float _crossTrackGain; + AP_Float _crossTrackLim; +}; + +} // namespace apo + +#endif // AP_Guide_H +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_HardwareAbstractionLayer.cpp b/libraries/APO/AP_HardwareAbstractionLayer.cpp new file mode 100644 index 0000000000..afa06769ab --- /dev/null +++ b/libraries/APO/AP_HardwareAbstractionLayer.cpp @@ -0,0 +1,8 @@ +/* + * AP_HardwareAbstractionLayer.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_HardwareAbstractionLayer.h" diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_HardwareAbstractionLayer.h new file mode 100644 index 0000000000..0fdeee2aae --- /dev/null +++ b/libraries/APO/AP_HardwareAbstractionLayer.h @@ -0,0 +1,99 @@ +/* + * AP_HardwareAbstractionLayer.h + * + * Created on: Apr 4, 2011 + * + */ + +#ifndef AP_HARDWAREABSTRACTIONLAYER_H_ +#define AP_HARDWAREABSTRACTIONLAYER_H_ + +#include "../AP_Common/AP_Common.h" +#include "../FastSerial/FastSerial.h" +#include "../AP_Common/AP_Vector.h" +#include "../GCS_MAVLink/GCS_MAVLink.h" + +#include "../AP_ADC/AP_ADC.h" +#include "../AP_IMU/AP_IMU.h" +#include "../AP_GPS/GPS.h" +#include "../APM_BMP085/APM_BMP085.h" +#include "../AP_Compass/AP_Compass.h" +#include "AP_RcChannel.h" +#include "../AP_RangeFinder/AP_RangeFinder.h" +#include "../GCS_MAVLink/GCS_MAVLink.h" + +class AP_ADC; +class IMU; +class GPS; +class APM_BMP085_Class; +class Compass; +class BetterStream; +class RangeFinder; + +namespace apo { + +class AP_RcChannel; +class AP_CommLink; + +// enumerations +enum halMode_t {MODE_LIVE, MODE_HIL_CNTL, /*MODE_HIL_NAV*/}; +enum board_t {BOARD_ARDUPILOTMEGA}; +enum vehicle_t {VEHICLE_CAR, VEHICLE_QUAD, VEHICLE_PLANE}; + +class AP_HardwareAbstractionLayer { + +public: + + AP_HardwareAbstractionLayer(halMode_t mode, board_t board, vehicle_t vehicle) : + _mode(mode), _board(board), _vehicle(vehicle), adc(), + gps(), baro(), compass(), rangeFinders(), + imu(), rc(), gcs(), hil(), debug(), load() + { + } + /** + * Sensors + */ + AP_ADC * adc; + GPS * gps; + APM_BMP085_Class * baro; + Compass * compass; + Vector rangeFinders; + IMU * imu; + + /** + * Radio Channels + */ + Vector rc; + + /** + * Communication Channels + */ + AP_CommLink * gcs; + AP_CommLink * hil; + FastSerial * debug; + + // accessors + halMode_t mode() { return _mode; } + board_t board() { return _board; } + vehicle_t vehicle() { return _vehicle; } + MAV_STATE state(){ return _state; } + + float getTimeSinceLastHeartBeat() { + return (micros() - lastHeartBeat)/1e6; + } + + uint8_t load; + uint32_t lastHeartBeat; + +private: + + // enumerations + halMode_t _mode; + board_t _board; + vehicle_t _vehicle; + MAV_STATE _state; +}; + +} + +#endif /* AP_HARDWAREABSTRACTIONLAYER_H_ */ diff --git a/libraries/APO/AP_MavlinkCommand.cpp b/libraries/APO/AP_MavlinkCommand.cpp new file mode 100644 index 0000000000..a33eb9ed45 --- /dev/null +++ b/libraries/APO/AP_MavlinkCommand.cpp @@ -0,0 +1,8 @@ +/* + * AP_MavlinkCommand.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_MavlinkCommand.h" diff --git a/libraries/APO/AP_MavlinkCommand.h b/libraries/APO/AP_MavlinkCommand.h new file mode 100644 index 0000000000..483dc24a4b --- /dev/null +++ b/libraries/APO/AP_MavlinkCommand.h @@ -0,0 +1,456 @@ +/* + * AP_MavlinkCommand.h + * + * Created on: Apr 4, 2011 + * Author: jgoppert + */ + +#ifndef AP_MAVLINKCOMMAND_H_ +#define AP_MAVLINKCOMMAND_H_ + +#include "../GCS_MAVLink/GCS_MAVLink.h" +#include "../AP_Common/AP_Common.h" +#include "AP_Var_keys.h" +#include "constants.h" + +class AP_MavlinkCommand { +private: + struct CommandStorage { + MAV_CMD command; + bool autocontinue; + MAV_FRAME frame; + float param1; + float param2; + float param3; + float param4; + float x; + float y; + float z; + }; + AP_VarS _data; + uint16_t _seq; + //static AP_Var_group _group; +public: + /** + * Constructor for loading from memory. + * @param index Start at zero. + */ + AP_MavlinkCommand(uint16_t index) : + _seq(index), _data(k_commands+index) { + _data.load(); + //Serial.print("x: "); Serial.println(_data.get().x); + } + + /** + * Constructor for saving from command a mavlink waypoint. + * @param cmd The mavlink_waopint_t structure for the command. + */ + AP_MavlinkCommand(mavlink_waypoint_t cmd) : + _data(k_commands+cmd.seq), _seq(cmd.seq) { + setCommand(MAV_CMD(cmd.command)); + setAutocontinue(cmd.autocontinue); + setFrame((MAV_FRAME) cmd.frame); + setParam1(cmd.param1); + setParam2(cmd.param2); + setParam3(cmd.param3); + setParam4(cmd.param4); + setX(cmd.x); + setY(cmd.y); + setZ(cmd.z); + _data.save(); + /* + Serial.println("============================================================"); + Serial.println("storing new command from mavlink_waypoint_t"); + Serial.print("key: "); Serial.println(_data.key(),DEC); + Serial.print("number: "); Serial.println(cmd.seq,DEC); + Serial.print("command: "); Serial.println(getCommand()); + Serial.print("autocontinue: "); Serial.println(getAutocontinue(),DEC); + Serial.print("frame: "); Serial.println(getFrame(),DEC); + Serial.print("1000*param1: "); Serial.println(int(1000*getParam1()),DEC); + Serial.print("1000*param2: "); Serial.println(int(1000*getParam2()),DEC); + Serial.print("1000*param3: "); Serial.println(int(1000*getParam3()),DEC); + Serial.print("1000*param4: "); Serial.println(int(1000*getParam4()),DEC); + Serial.print("1000*x0: "); Serial.println(int(1000*cmd.x),DEC); + Serial.print("1000*y0: "); Serial.println(int(1000*cmd.y),DEC); + Serial.print("1000*z0: "); Serial.println(int(1000*cmd.z),DEC); + Serial.print("1000*x: "); Serial.println(int(1000*getX()),DEC); + Serial.print("1000*y: "); Serial.println(int(1000*getY()),DEC); + Serial.print("1000*z: "); Serial.println(int(1000*getZ()),DEC); + */ + _data.load(); + /* + Serial.print("1000*x1: "); Serial.println(int(1000*getX()),DEC); + Serial.print("1000*y1: "); Serial.println(int(1000*getY()),DEC); + Serial.print("1000*z1: "); Serial.println(int(1000*getZ()),DEC); + */ + } + bool save() { + return _data.save(); + } + bool load() { + return _data.load(); + } + uint8_t getSeq() { + return _seq; + } + bool getAutocontinue() { + return _data.get().autocontinue; + } + void setAutocontinue(bool val) { + _data.get().autocontinue = val; + } + void setSeq(uint8_t val) { + _seq = val; + } + MAV_CMD getCommand() { + return _data.get().command; + } + void setCommand(MAV_CMD val) { + _data.get().command = val; + } + MAV_FRAME getFrame() { + return _data.get().frame; + } + void setFrame(MAV_FRAME val) { + _data.get().frame = val; + } + float getParam1() { + return _data.get().param1; + } + void setParam1(float val) { + _data.get().param1 = val; + } + float getParam2() { + return _data.get().param2; + } + void setParam2(float val) { + _data.get().param2 = val; + } + float getParam3() { + return _data.get().param3; + } + void setParam3(float val) { + _data.get().param3 = val; + } + float getParam4() { + return _data.get().param4; + } + void setParam4(float val) { + _data.get().param4 = val; + } + float getX() { + return _data.get().x; + } + void setX(float val) { + _data.get().x = val; + } + float getY() { + return _data.get().y; + } + void setY(float val) { + _data.get().y = val; + } + float getZ() { + return _data.get().z; + } + void setZ(float val) { + _data.get().z = val; + } + float getLatDeg() { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + return getX(); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + void setLatDeg(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + setX(val); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_MISSION: + default: + break; + } + } + float getLonDeg() { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + return getY(); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + void setLonDeg(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + setY(val); + break; + case MAV_FRAME_LOCAL: + case MAV_FRAME_MISSION: + default: + break; + } + } + void setLon(float val) { + setLonDeg(val*rad2Deg); + } + void setLon_degInt(int32_t val) { + setLonDeg(val/1.0e7); + } + void setLat_degInt(int32_t val) { + setLatDeg(val/1.0e7); + } + int32_t getLon_degInt() { + return getLonDeg()*1e7; + } + int32_t getLat_degInt() { + return getLatDeg()*1e7; + } + float getLon() { + return getLonDeg()*deg2Rad; + } + float getLat() { + return getLatDeg()*deg2Rad; + } + void setLat(float val) { + setLatDeg(val*rad2Deg); + } + float getAlt() { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + return getZ(); + break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_LOCAL: + return getZ() + AP_MavlinkCommand(0).getAlt(); + break; + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + /** + * set the altitude in meters + */ + void setAlt(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + setZ(val); + break; + case MAV_FRAME_LOCAL: + setZ(val - AP_MavlinkCommand(0).getLonDeg()); + break; + case MAV_FRAME_MISSION: + default: + break; + } + } + /** + * Get the relative altitud to home + * @return relative altitude in meters + */ + float getRelAlt() { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + return getZ() - AP_MavlinkCommand(0).getAlt(); + break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_LOCAL: + return getZ(); + break; + case MAV_FRAME_MISSION: + default: + return 0; + break; + } + } + /** + * set the relative altitude in meters from home + */ + void setRelAlt(float val) { + switch (getFrame()) { + case MAV_FRAME_GLOBAL: + setZ(val + AP_MavlinkCommand(0).getAlt()); + break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_LOCAL: + setZ(val); + break; + case MAV_FRAME_MISSION: + break; + } + } + + float getRadius() { + return getParam2(); + } + + void setRadius(float val) { + setParam2(val); + } + + /** + * conversion for outbound packets to ground station + * @return output the mavlink_waypoint_t packet + */ + mavlink_waypoint_t convert(uint8_t current) { + mavlink_waypoint_t mavCmd; + mavCmd.seq = getSeq(); + mavCmd.command = getCommand(); + mavCmd.frame = getFrame(); + mavCmd.param1 = getParam1(); + mavCmd.param2 = getParam2(); + mavCmd.param3 = getParam3(); + mavCmd.param4 = getParam4(); + mavCmd.x = getX(); + mavCmd.y = getY(); + mavCmd.z = getZ(); + mavCmd.autocontinue = getAutocontinue(); + mavCmd.current = (getSeq() == current); + mavCmd.target_component = mavlink_system.compid; + mavCmd.target_system = mavlink_system.sysid; + return mavCmd; + } + /** + * Calculate the bearing from this command to the next command + * @param next The command to calculate the bearing to. + * @return the bearing + */ + float bearingTo(AP_MavlinkCommand next) { + float deltaLon = next.getLon() - getLon(); + /* + Serial.print("Lon: "); Serial.println(getLon()); + Serial.print("nextLon: "); Serial.println(next.getLon()); + Serial.print("deltaLonDeg * 1e7: "); Serial.println(deltaLon*rad2DegInt); + */ + float bearing = atan2(sin(deltaLon)*cos(next.getLat()), + cos(getLat())*sin(next.getLat()) - + sin(getLat())*cos(next.getLat())*cos(deltaLon)); + return bearing; + } + + /** + * Bearing form this command to a gps coordinate in integer units + * @param latDegInt latitude in degrees E-7 + * @param lonDegInt longitude in degrees E-7 + * @return + */ + float bearingTo(int32_t latDegInt, int32_t lonDegInt) { + // have to be careful to maintain the precision of the gps coordinate + float deltaLon = (lonDegInt - getLon_degInt())*degInt2Rad; + float nextLat = latDegInt*degInt2Rad; + float bearing = atan2(sin(deltaLon)*cos(nextLat), cos(getLat())*sin(nextLat) - + sin(getLat())*cos(nextLat)*cos(deltaLon)); + if (bearing < 0) bearing += 2*M_PI; + return bearing; + } + + /** + * Distance to another command + * @param next The command to measure to. + * @return The distance in meters. + */ + float distanceTo(AP_MavlinkCommand next) { + float sinDeltaLat2 = sin((getLat()-next.getLat())/2); + float sinDeltaLon2 = sin((getLon()-next.getLon())/2); + float a = sinDeltaLat2*sinDeltaLat2 + cos(getLat())*cos(next.getLat())* + sinDeltaLon2*sinDeltaLon2; + float c = 2*atan2(sqrt(a),sqrt(1-a)); + return rEarth*c; + } + + /** + * Distance to a gps coordinate in integer units + * @param latDegInt latitude in degrees E-7 + * @param lonDegInt longitude in degrees E-7 + * @return The distance in meters. + */ + float distanceTo(int32_t lat_degInt, int32_t lon_degInt) { + float sinDeltaLat2 = sin((lat_degInt - getLat_degInt())*degInt2Rad/2); + float sinDeltaLon2 = sin((lon_degInt - getLon_degInt())*degInt2Rad/2); + float a = sinDeltaLat2*sinDeltaLat2 + + cos(getLat())*cos(lat_degInt*degInt2Rad)*sinDeltaLon2*sinDeltaLon2; + float c = 2*atan2(sqrt(a),sqrt(1-a)); + /* + Serial.print("wp lat_degInt: "); Serial.println(getLat_degInt()); + Serial.print("wp lon_degInt: "); Serial.println(getLon_degInt()); + Serial.print("lat_degInt: "); Serial.println(lat_degInt); + Serial.print("lon_degInt: "); Serial.println(lon_degInt); + Serial.print("sinDeltaLat2: "); Serial.println(sinDeltaLat2); + Serial.print("sinDeltaLon2: "); Serial.println(sinDeltaLon2); + */ + return rEarth*c; + } + + float getPN(int32_t lat_degInt, int32_t lon_degInt) { + // local tangent approximation at this waypoint + float deltaLat = (lat_degInt - getLat_degInt())*degInt2Rad; + return deltaLat*rEarth; + } + + float getPE(int32_t lat_degInt, int32_t lon_degInt) { + // local tangent approximation at this waypoint + float deltaLon = (lon_degInt - getLon_degInt())*degInt2Rad; + return cos(getLat())*deltaLon*rEarth; + } + + float getPD(int32_t alt_intM) { + return -(alt_intM/scale_m - getAlt()); + } + + float getLat(float pN) { + + return pN/rEarth + getLat(); + } + + float getLon(float pE) { + + return pE/rEarth/cos(getLat()) + getLon(); + } + + /** + * Gets altitude in meters + * @param pD alt in meters + * @return + */ + float getAlt(float pD) { + + return getAlt() - pD; + } + + //calculates cross track of a current location + static float crossTrack(AP_MavlinkCommand previous, AP_MavlinkCommand current, int32_t lat_degInt, int32_t lon_degInt) { + float d = previous.distanceTo(lat_degInt,lon_degInt); + float bCurrent = previous.bearingTo(lat_degInt,lon_degInt); + float bNext = previous.bearingTo(current); + return asin(sin(d/rEarth) * sin(bCurrent - bNext)) * rEarth; + } + + // calculates along track distance of a current location + static float alongTrack(AP_MavlinkCommand previous, AP_MavlinkCommand current, int32_t lat_degInt, int32_t lon_degInt) { + // ignores lat/lon since single prec. + float dXt = crossTrack(previous,current,lat_degInt,lon_degInt); + float d = previous.distanceTo(lat_degInt,lon_degInt); + return dXt/tan(asin(dXt/d)); + } +}; + + +#endif /* AP_MAVLINKCOMMAND_H_ */ diff --git a/libraries/APO/AP_Navigator.cpp b/libraries/APO/AP_Navigator.cpp new file mode 100644 index 0000000000..ff33e8ed2c --- /dev/null +++ b/libraries/APO/AP_Navigator.cpp @@ -0,0 +1,8 @@ +/* + * AP_Navigator.cpp + * + * Created on: Apr 30, 2011 + * Author: jgoppert + */ + +#include "AP_Navigator.h" diff --git a/libraries/APO/AP_Navigator.h b/libraries/APO/AP_Navigator.h new file mode 100644 index 0000000000..d760bc09de --- /dev/null +++ b/libraries/APO/AP_Navigator.h @@ -0,0 +1,445 @@ +/* + * AP_Navigator.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_Navigator_H +#define AP_Navigator_H + +#include "AP_HardwareAbstractionLayer.h" +#include "../AP_DCM/AP_DCM.h" +#include "../AP_Math/AP_Math.h" +#include "../AP_Compass/AP_Compass.h" +#include "AP_MavlinkCommand.h" +#include "constants.h" +#include "APO_Config.h" +#include "AP_Var_keys.h" +#include "../AP_RangeFinder/AP_RangeFinder.h" +#include "../AP_IMU/AP_IMU.h" + +namespace apo { + +/// Navigator class +class AP_Navigator { +public: + 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), _heading(0), + _lon_degInt(0), _alt_intM(0) + { + } + virtual void calibrate() = 0; + virtual void updateFast(float dt) = 0; + virtual void updateSlow(float dt) = 0; + float getPD() const + { + AP_MavlinkCommand home = AP_MavlinkCommand(0); + return home.getPD(getAlt_intM()); + } + + float getPE() const + { + AP_MavlinkCommand home = AP_MavlinkCommand(0); + return home.getPE(getLat_degInt(),getLon_degInt()); + } + + float getPN() const + { + AP_MavlinkCommand home = AP_MavlinkCommand(0); + return home.getPN(getLat_degInt(),getLon_degInt()); + } + + void setPD(float _pD) + { + AP_MavlinkCommand home = AP_MavlinkCommand(0); + setAlt(home.getAlt(_pD)); + } + + void setPE(float _pE) + { + AP_MavlinkCommand home = AP_MavlinkCommand(0); + setLat(home.getLat(_pE)); + } + + void setPN(float _pN) + { + AP_MavlinkCommand home = AP_MavlinkCommand(0); + setLon(home.getLon(_pN)); + } + + float getAirSpeed() const + { + return _airSpeed; + } + + int32_t getAlt_intM() const + { + return _alt_intM; + } + + float getAlt() const + { + return _alt_intM / scale_m; + } + + void setAlt(float _alt) + { + this->_alt_intM = _alt * scale_m; + } + + float getLat() const + { + return _lat_degInt * degInt2Rad; + } + + void setLat(float _lat) + { + this->_lat_degInt = _lat * rad2DegInt; + } + + float getLon() const + { + return _lon_degInt * degInt2Rad; + } + + void setLon(float _lon) + { + this->_lon_degInt = _lon * rad2DegInt; + } + + float getVD() const + { + return _vD; + } + + float getHeading() const + { + return _heading; + } + + float getVE() const + { + return sin(getHeading())*getGroundSpeed(); + } + + float getGroundSpeed() const + { + return _groundSpeed; + } + + int32_t getLat_degInt() const + { + return _lat_degInt; + } + + int32_t getLon_degInt() const + { + return _lon_degInt; + } + + float getVN() const + { + return cos(getHeading())*getGroundSpeed(); + } + + float getPitch() const + { + return _pitch; + } + + float getPitchRate() const + { + return _pitchRate; + } + + float getRoll() const + { + return _roll; + } + + float getRollRate() const + { + return _rollRate; + } + + float getYaw() const + { + return _yaw; + } + + float getYawRate() const + { + return _yawRate; + } + + void setAirSpeed(float _airSpeed) + { + this->_airSpeed = _airSpeed; + } + + void setHeading(float _heading) + { + this->_heading = _heading; + } + + void setAlt_intM(int32_t _alt_intM) + { + this->_alt_intM = _alt_intM; + } + + void setVD(float _vD) + { + this->_vD = _vD; + } + + void setGroundSpeed(float _groundSpeed) + { + this->_groundSpeed = _groundSpeed; + } + + void setLat_degInt(int32_t _lat_degInt) + { + this->_lat_degInt = _lat_degInt; + } + + void setLon_degInt(int32_t _lon_degInt) + { + this->_lon_degInt = _lon_degInt; + } + + void setPitch(float _pitch) + { + this->_pitch = _pitch; + } + + void setPitchRate(float _pitchRate) + { + this->_pitchRate = _pitchRate; + } + + void setRoll(float _roll) + { + this->_roll = _roll; + } + + void setRollRate(float _rollRate) + { + this->_rollRate = _rollRate; + } + + void setYaw(float _yaw) + { + this->_yaw = _yaw; + } + + void setYawRate(float _yawRate) + { + this->_yawRate = _yawRate; + } + void setTimeStamp(int32_t _timeStamp) + { + this->_timeStamp = _timeStamp; + } + int32_t getTimeStamp() const + { + return _timeStamp; + } + +protected: + AP_HardwareAbstractionLayer * _hal; +private: + int32_t _timeStamp; // micros clock + float _roll; // rad + float _rollRate; //rad/s + float _pitch; // rad + float _pitchRate; // rad/s + float _yaw; // rad + float _yawRate; // rad/s + float _airSpeed; // m/s + float _groundSpeed; // m/s + float _heading; // rad + float _vD; // m/s + int32_t _lat_degInt; // deg / 1e7 + int32_t _lon_degInt; // deg / 1e7 + int32_t _alt_intM; // meters / 1e3 +}; + +class DcmNavigator: public AP_Navigator { +private: + /** + * Sensors + */ + + RangeFinder * _rangeFinderDown; + AP_DCM * _dcm; + IMU * _imu; + uint16_t _imuOffsetAddress; + +public: + DcmNavigator(AP_HardwareAbstractionLayer * hal) : + AP_Navigator(hal), _dcm(), _imuOffsetAddress(0) { + + // if orientation equal to front, store as front + /** + * rangeFinder is assigned values based on orientation which + * is specified in ArduPilotOne.pde. + */ + for (int i = 0; i < _hal-> rangeFinders.getSize(); i++) { + if (_hal->rangeFinders[i] == NULL) + continue; + if (_hal->rangeFinders[i]->orientation_x == 0 + && _hal->rangeFinders[i]->orientation_y == 0 + && _hal->rangeFinders[i]->orientation_z == 1) + _rangeFinderDown = _hal->rangeFinders[i]; + } + + if (_hal->mode() == MODE_LIVE) { + if (_hal->adc) + _hal->imu = new AP_IMU_Oilpan(_hal->adc, k_sensorCalib); + if (_hal->imu) + _dcm = new AP_DCM(_hal->imu, _hal->gps, _hal->compass); + if (_hal->compass) { + _dcm->set_compass(_hal->compass); + + } + } + } + virtual void calibrate() { + // TODO: handle cold restart + if (_hal->imu) { + /* + * Gyro has built in warm up cycle and should + * run first */ + _hal->imu->init_gyro(); + _hal->imu->init_accel(); + } + } + virtual void updateFast(float dt) { + if (_hal->mode() != MODE_LIVE) + return; + + setTimeStamp(micros()); // if running in live mode, record new time stamp + + + //_hal->debug->println_P(PSTR("nav loop")); + + /** + * The altitued is read off the barometer by implementing the following formula: + * altitude (in m) = 44330*(1-(p/po)^(1/5.255)), + * where, po is pressure in Pa at sea level (101325 Pa). + * See http://www.sparkfun.com/tutorials/253 or type this formula + * in a search engine for more information. + * altInt contains the altitude in meters. + */ + if (_hal->baro) { + + if (_rangeFinderDown != NULL && _rangeFinderDown->distance <= 695) + setAlt(_rangeFinderDown->distance); + + else { + float tmp = (_hal->baro->Press/ 101325.0); + tmp = pow(tmp, 0.190295); + //setAlt(44330 * (1.0 - tmp)); //sets the altitude in meters XXX wrong, baro reads 0 press + setAlt(0.0); + } + } + + // dcm class for attitude + if (_dcm) { + _dcm->update_DCM(dt); + setRoll(_dcm->roll); + setPitch(_dcm->pitch); + setYaw(_dcm->yaw); + setRollRate(_dcm->get_gyro().x); + setPitchRate(_dcm->get_gyro().y); + setYawRate(_dcm->get_gyro().z); + + /* + * 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); + */ + } + } + virtual void updateSlow(float dt) { + if (_hal->mode() != MODE_LIVE) + return; + + setTimeStamp(micros()); // if running in live mode, record new time stamp + + if (_hal->gps) { + _hal->gps->update(); + updateGpsLight(); + if (_hal->gps->fix && _hal->gps->new_data) { + setLat_degInt(_hal->gps->latitude); + setLon_degInt(_hal->gps->longitude); + setAlt_intM(_hal->gps->altitude*10); // gps in cm, intM in mm + setGroundSpeed(_hal->gps->ground_speed/100.0); // gps is in cm/s + } + } + + if (_hal->compass) { + _hal->compass->read(); + _hal->compass->calculate(getRoll(),getPitch()); + _hal->compass->null_offsets(_dcm->get_dcm_matrix()); + } + + // need to use lat/lon and convert + // TODO make a local copy of home location, EEPROM takes way too long to read +// AP_MavlinkCommand home(0); +// setPN((getLat() - home.getLat())/rEarth); +// setPE((getLon() - home.getLon())*cos(home.getLat())/rEarth); +// setPD(-(getAlt() - home.getAlt())); + } + void updateGpsLight(void) { + // GPS LED on if we have a fix or Blink GPS LED if we are receiving data + // --------------------------------------------------------------------- + static bool GPS_light = false; + switch (_hal->gps->status()) { + case (2): + digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. + break; + + case (1): + if (_hal->gps->valid_read == true) { + GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock + if (GPS_light) { + digitalWrite(C_LED_PIN, LOW); + } else { + digitalWrite(C_LED_PIN, HIGH); + } + _hal->gps->valid_read = false; + } + break; + + default: + digitalWrite(C_LED_PIN, LOW); + break; + } + } + + +}; + +} // namespace apo + +#endif // AP_Navigator_H +// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/AP_RcChannel.cpp b/libraries/APO/AP_RcChannel.cpp new file mode 100644 index 0000000000..d00a7954c2 --- /dev/null +++ b/libraries/APO/AP_RcChannel.cpp @@ -0,0 +1,109 @@ +/* + AP_RcChannel.cpp - Radio library for Arduino + Code by Jason Short, James Goppert. DIYDrones.com + + This library is free software; you can redistribute it and / or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + +*/ + +#include +#include +#include "AP_RcChannel.h" +#include "../AP_Common/AP_Common.h" +#include + + +namespace apo { + +AP_RcChannel::AP_RcChannel(AP_Var::Key key, const prog_char_t * name, APM_RC_Class & rc, const uint8_t & ch, + const uint16_t & pwmMin, + const uint16_t & pwmNeutral, const uint16_t & pwmMax, + const rcMode_t & rcMode, const bool & reverse) : + AP_Var_group(key,name), + _rc(rc), + _ch(this,0,ch,PSTR("CH")), + _pwmMin(this,1,pwmMin,PSTR("PMIN")), + _pwmMax(this,2,pwmMax,PSTR("PMAX")), + _pwmNeutral(this,3,pwmNeutral,PSTR("PNTRL")), + _rcMode(rcMode), + _reverse(this,4,reverse,PSTR("REV")), + _pwm(pwmNeutral) + { + //Serial.print("pwm after ctor: "); Serial.println(pwmNeutral); + if (rcMode == RC_MODE_IN) return; + //Serial.print("pwm set for ch: "); Serial.println(int(ch)); + rc.OutputCh(ch,pwmNeutral); + + } + + +uint16_t AP_RcChannel::readRadio() { + if (_rcMode == RC_MODE_OUT) { + Serial.print("tryng to read from output channel: "); Serial.println(int(_ch)); + return _pwmNeutral; // if this happens give a safe value of neutral + } + return _rc.InputCh(_ch); +} + +void +AP_RcChannel::setPwm(uint16_t pwm) +{ + //Serial.printf("pwm in setPwm: %d\n", pwm); + //Serial.printf("reverse: %s\n", (reverse)?"true":"false"); + + // apply reverse + if(_reverse) pwm = int16_t(_pwmNeutral-pwm) + _pwmNeutral; + //Serial.printf("pwm after reverse: %d\n", pwm); + + // apply saturation + if (_pwm > _pwmMax) _pwm = _pwmMax; + if (_pwm < _pwmMin) _pwm = _pwmMin; + _pwm = pwm; + + //Serial.print("ch: "); Serial.print(ch); Serial.print(" pwm: "); Serial.println(pwm); + if (_rcMode == RC_MODE_IN) return; + _rc.OutputCh(_ch,_pwm); +} + +void +AP_RcChannel::setPosition(float position) +{ + if (position > 1.0) position = 1.0; + else if (position < -1.0) position = -1.0; + setPwm(_positionToPwm(position)); +} + +uint16_t +AP_RcChannel::_positionToPwm(const float & position) +{ + uint16_t pwm; + //Serial.printf("position: %f\n", position); + if(position < 0) + pwm = position * int16_t(_pwmNeutral - _pwmMin) + _pwmNeutral; + else + pwm = position * int16_t(_pwmMax - _pwmNeutral) + _pwmNeutral; + + if (pwm > _pwmMax) pwm = _pwmMax; + if (pwm < _pwmMin) pwm = _pwmMin; + return pwm; +} + +float +AP_RcChannel::_pwmToPosition(const uint16_t & pwm) +{ + float position; + if(pwm < _pwmNeutral) + position = 1.0 * int16_t(pwm - _pwmNeutral)/ + int16_t(_pwmNeutral - _pwmMin); + else + position = 1.0 * int16_t(pwm - _pwmNeutral)/ + int16_t(_pwmMax - _pwmNeutral) ; + if (position > 1) position = 1; + if (position < -1) position = -1; + return position; +} + +} // namespace apo diff --git a/libraries/AP_RcChannel/AP_RcChannel.h b/libraries/APO/AP_RcChannel.h similarity index 57% rename from libraries/AP_RcChannel/AP_RcChannel.h rename to libraries/APO/AP_RcChannel.h index 5729c91cfd..99b236c1c3 100644 --- a/libraries/AP_RcChannel/AP_RcChannel.h +++ b/libraries/APO/AP_RcChannel.h @@ -3,13 +3,21 @@ /// @file AP_RcChannel.h /// @brief AP_RcChannel manager -#ifndef AP_RcChannel_h -#define AP_RcChannel_h +#ifndef AP_RCCHANNEL_H +#define AP_RCCHANNEL_H #include -#include -#include -#include +#include "../APM_RC/APM_RC.h" +#include "../AP_Common/AP_Common.h" +#include "../AP_Common/AP_Var.h" + +namespace apo { + +enum rcMode_t { + RC_MODE_IN, + RC_MODE_OUT, + RC_MODE_INOUT +}; /// @class AP_RcChannel /// @brief Object managing one RC channel @@ -17,39 +25,32 @@ class AP_RcChannel : public AP_Var_group { public: + /// Constructor AP_RcChannel(AP_Var::Key key, const prog_char_t * name, APM_RC_Class & rc, const uint8_t & ch, - const float & scale=45.0, const float & center=0.0, - const uint16_t & pwmMin=1200, - const uint16_t & pwmNeutral=1500, const uint16_t & pwmMax=1800, - const uint16_t & pwmDeadZone=10, - const bool & filter=false, const bool & reverse=false); + const uint16_t & pwmMin,const uint16_t & pwmNeutral, const uint16_t & pwmMax, + const rcMode_t & rcMode=RC_MODE_INOUT, const bool & reverse=false); // configuration - AP_Uint8 ch; - AP_Float scale; - AP_Float center; - AP_Uint16 pwmMin; - AP_Uint16 pwmNeutral; - AP_Uint16 pwmMax; - AP_Uint16 pwmDeadZone; - AP_Bool filter; - AP_Bool reverse; + AP_Uint8 _ch; + AP_Uint16 _pwmMin; + AP_Uint16 _pwmNeutral; + AP_Uint16 _pwmMax; + rcMode_t _rcMode; + AP_Bool _reverse; + // set uint16_t readRadio(); void setPwm(uint16_t pwm); void setPosition(float position); - void setNormalized(float normPosition); - void mixRadio(uint16_t infStart); // get uint16_t getPwm() { return _pwm; } float getPosition() { return _pwmToPosition(_pwm); } - float getNormalized() { return getPosition()/scale; } // did our read come in 50µs below the min? - bool failSafe() { _pwm < (pwmMin - 50); } + bool failSafe() { _pwm < (_pwmMin - 50); } private: @@ -65,4 +66,6 @@ private: float _pwmToPosition(const uint16_t & pwm); }; -#endif +} // apo + +#endif // AP_RCCHANNEL_H diff --git a/libraries/APO/AP_Var_keys.h b/libraries/APO/AP_Var_keys.h new file mode 100644 index 0000000000..7daea2d1ff --- /dev/null +++ b/libraries/APO/AP_Var_keys.h @@ -0,0 +1,41 @@ +#ifndef AP_Var_keys_H +#define AP_Var_keys_H + +enum keys { + + // general + k_config = 0, + k_cntrl, + k_guide, + k_sensorCalib, + + // radio channels + k_chMode = 20, + k_chLeft, + k_chRight, + k_chFront, + k_chBack, + k_chRoll, + k_chPitch, + k_chYaw, + k_chThr, + k_chStr, + + // pids + k_pidPN = 40, + k_pidPE, + k_pidPD, + k_pidThr, + k_pidStr, + k_pidRoll, + k_pidPitch, + k_pidYawRate, + k_pidYaw, + + // 200-256 reserved for commands + k_commands = 200 +}; + +// max 256 keys + +#endif diff --git a/libraries/APO/constants.h b/libraries/APO/constants.h new file mode 100644 index 0000000000..4bc1b29262 --- /dev/null +++ b/libraries/APO/constants.h @@ -0,0 +1,23 @@ +/* + * constants.h + * + * Created on: Apr 7, 2011 + * Author: nkgentry + */ + +#ifndef CONSTANTS_H_ +#define CONSTANTS_H_ + +#include "math.h" + +const float scale_deg = 1e7; // scale of integer degrees/ degree +const float scale_m = 1e3; // scale of integer meters/ meter +const float rEarth = 6371000; // radius of earth, meters +const float rad2Deg = 180/M_PI; // radians to degrees +const float deg2Rad = M_PI/180; // degrees to radians +const float rad2DegInt = rad2Deg*scale_deg; // radians to degrees x 1e7 +const float degInt2Rad = deg2Rad/scale_deg; // degrees x 1e7 to radians + +#define MAV_MODE_FAILSAFE MAV_MODE_TEST3 + +#endif /* CONSTANTS_H_ */ diff --git a/libraries/APO/controllers.h b/libraries/APO/controllers.h new file mode 100644 index 0000000000..408dc1532c --- /dev/null +++ b/libraries/APO/controllers.h @@ -0,0 +1,277 @@ +#ifndef defaultControllers_H +#define defaultControllers_H + +#include "AP_Controller.h" +#include "AP_HardwareAbstractionLayer.h" +#include "../AP_Common/AP_Var.h" +#include +#include "AP_Navigator.h" + +namespace apo { + +class CarController: public AP_Controller { +private: + // control mode + AP_Var_group _group;AP_Uint8 _mode; + enum { + CH_MODE = 0, CH_STR, CH_THR + }; + PidDFB2 pidStr; + Pid2 pidThr; +public: + CarController(AP_Var::Key cntrlKey, AP_Var::Key pidStrKey, + AP_Var::Key pidThrKey, AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + AP_Controller(nav, guide, hal), _group(cntrlKey, PSTR("CNTRL_")), + _mode(&_group, 1, 0, PSTR("MODE")), + pidStr(pidStrKey, PSTR("STR_"), 1.0, 0, 0, 0, 3), + pidThr(pidThrKey, PSTR("THR_"), 0.6, 0.5, 0, 1, 3) { + _hal->debug->println_P(PSTR("initializing car controller")); + + _hal->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7,1100,1500,1900)); + _hal->rc.push_back( + new AP_RcChannel(k_chStr, PSTR("STR_"), APM_RC, 0,1100,1540,1900)); + _hal->rc.push_back( + new AP_RcChannel(k_chThr, PSTR("THR_"), APM_RC, 1,1100,1500,1900)); + } + virtual void update(const float & dt) { + // read mode switch + _hal->rc[CH_MODE]->setPwm(_hal->rc[CH_MODE]->readRadio()); + // manual + if (_hal->rc[CH_MODE]->getPosition() > 0) { + _hal->rc[CH_STR]->setPwm(_hal->rc[CH_STR]->readRadio()); + _hal->rc[CH_THR]->setPwm(_hal->rc[CH_THR]->readRadio()); + //_hal->debug->println("manual"); + + } else { // auto + float headingError = _guide->headingCommand - _nav->getHeading(); + if (headingError > 180 * deg2Rad) + headingError -= 360 * deg2Rad; + if (headingError < -180 * deg2Rad) + headingError += 360 * deg2Rad; + _hal->rc[CH_STR]->setPosition(pidStr.update(headingError,_nav->getYawRate(),dt)); + _hal->rc[CH_THR]->setPosition(pidThr.update(_guide->groundSpeedCommand - _nav->getGroundSpeed(),dt)); + //_hal->debug->println("automode"); + } + } +}; + +#if CUSTOM_INCLUDES == CUSTOM_MIKROKOPTER +#include "mikrokopter.h" +#endif + +class QuadController: public AP_Controller { +public: + + /** + * note that these are not the controller radio channel numbers, they are just + * unique keys so they can be reaccessed from the hal rc vector + */ + enum autoChannel_t { + CH_MODE = 0, // note scicoslab channels set mode, left, right, front, back order + CH_LEFT, // this enum must match this + CH_RIGHT, + CH_FRONT, + CH_BACK, + CH_ROLL, + CH_PITCH, + CH_YAW, + CH_THRUST + }; + + QuadController(AP_Navigator * nav, AP_Guide * guide, + AP_HardwareAbstractionLayer * hal) : + AP_Controller(nav, guide, hal), + pidRoll(k_pidRoll, PSTR("ROLL_"), PID_ATT_P, PID_ATT_I, + PID_ATT_D, PID_ATT_AWU, PID_ATT_LIM), + pidPitch(k_pidPitch, PSTR("PITCH_"), PID_ATT_P, PID_ATT_I, + PID_ATT_D, PID_ATT_AWU, PID_ATT_LIM), + pidYaw(k_pidYaw, PSTR("YAW_"), PID_YAWPOS_P, PID_YAWPOS_I, + PID_YAWPOS_D, PID_YAWPOS_AWU, PID_YAWPOS_LIM), + pidYawRate(k_pidYawRate, PSTR("YAWRATE_"), PID_YAWSPEED_P, + PID_YAWSPEED_I, PID_YAWSPEED_D, PID_YAWSPEED_AWU, + PID_YAWSPEED_LIM), + pidPN(k_pidPN, PSTR("NORTH_"), PID_POS_P, + PID_POS_I, PID_POS_D, PID_POS_AWU, PID_POS_LIM), + pidPE(k_pidPE, PSTR("EAST_"), PID_POS_P, PID_POS_I, + PID_POS_D, PID_POS_AWU, PID_POS_LIM), + pidPD(k_pidPD, PSTR("DOWN_"), PID_POS_Z_P, PID_POS_Z_I, + PID_POS_Z_D, PID_POS_Z_AWU, PID_POS_Z_LIM) + { + /* + * allocate radio channels + * the order of the channels has to match the enumeration above + */ + _hal->rc.push_back( + new AP_RcChannel(k_chMode, PSTR("MODE_"), APM_RC, 7, 1100, 1500, 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chLeft, PSTR("LEFT_"), APM_RC, 0, 1100, 1100, 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chRight, PSTR("RIGHT_"), APM_RC, 1, 1100, 1100, 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chFront, PSTR("FRONT_"), APM_RC, 2, 1100, 1100, 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chBack, PSTR("BACK_"), APM_RC, 3, 1100, 1100, 1900, RC_MODE_OUT)); + _hal->rc.push_back( + new AP_RcChannel(k_chRoll, PSTR("ROLL_"), APM_RC, 0, 1100, 1500, 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chPitch, PSTR("PITCH_"), APM_RC, 1, 1100, 1500, 1900, RC_MODE_IN)); + _hal->rc.push_back( + new AP_RcChannel(k_chYaw, PSTR("YAW_"), APM_RC, 2, 1100, 1500, 1900, RC_MODE_IN)); + _hal->rc.push_back( // -1 -> 0 maps to 1200, linear 0-1 -> 1200-1800 + new AP_RcChannel(k_chThr, PSTR("THRUST_"), APM_RC, 3, 1100, 1100, 1900, RC_MODE_IN)); + } + + virtual void update(const float & dt) { + + if (commLost()) { + _mode = MAV_MODE_FAILSAFE; + _hal->rc[CH_LEFT]->setPosition(0); + _hal->rc[CH_RIGHT]->setPosition(0); + _hal->rc[CH_FRONT]->setPosition(0); + _hal->rc[CH_BACK]->setPosition(0); + } + + // read and set pwm so they can be read as positions later + _hal->rc[CH_MODE]->setPwm(_hal->rc[CH_MODE]->readRadio()); + _hal->rc[CH_ROLL]->setPwm(_hal->rc[CH_ROLL]->readRadio()); + _hal->rc[CH_PITCH]->setPwm(_hal->rc[CH_PITCH]->readRadio()); + _hal->rc[CH_YAW]->setPwm(_hal->rc[CH_YAW]->readRadio()); + _hal->rc[CH_THRUST]->setPwm(_hal->rc[CH_THRUST]->readRadio()); + + // manual mode + float mixRemoteWeight = 0; + if (_hal->rc[CH_MODE]->getPwm() > 1350) mixRemoteWeight = 1; + + // "mix manual" + float cmdRoll = _hal->rc[CH_ROLL]->getPosition() * mixRemoteWeight; + float cmdPitch = _hal->rc[CH_PITCH]->getPosition() * mixRemoteWeight; + float cmdYawRate = _hal->rc[CH_YAW]->getPosition() * mixRemoteWeight; + float thrustMix = _hal->rc[CH_THRUST]->getPosition() * mixRemoteWeight; + + // position loop + /* + 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); + + // "transform-to-body" + { + float trigSin = sin(-yaw); + float trigCos = cos(-yaw); + _cmdPitch = _cmdEastTilt * trigCos + - _cmdNorthTilt * trigSin; + _cmdRoll = -_cmdEastTilt * trigSin + + _cmdNorthTilt * trigCos; + // note that the north tilt is negative of the pitch + } + + //thrustMix += THRUST_HOVER_OFFSET; + + // "thrust-trim-adjust" + if (fabs(_cmdRoll) > 0.5) { + _thrustMix *= 1.13949393; + } else { + _thrustMix /= cos(_cmdRoll); + } + if (fabs(_cmdPitch) > 0.5) { + _thrustMix *= 1.13949393; + } else { + _thrustMix /= cos(_cmdPitch); + } + */ + + // attitude loop + float rollMix = pidRoll.update(cmdRoll - _nav->getRoll(),_nav->getRollRate(),dt); + float pitchMix = pidPitch.update(cmdPitch - _nav->getPitch(),_nav->getPitchRate(),dt); + float yawMix = pidYawRate.update(cmdYawRate - _nav->getYawRate(),dt); + + _hal->rc[CH_LEFT]->setPosition(thrustMix + rollMix + yawMix); + _hal->rc[CH_RIGHT]->setPosition(thrustMix - rollMix + yawMix); + _hal->rc[CH_FRONT]->setPosition(thrustMix + pitchMix - yawMix); + _hal->rc[CH_BACK]->setPosition(thrustMix - pitchMix - yawMix); + + _hal->debug->printf("L: %f\t R: %f\t F: %f\t B: %f\n", + _hal->rc[CH_LEFT]->getPosition(), + _hal->rc[CH_RIGHT]->getPosition(), + _hal->rc[CH_FRONT]->getPosition(), + _hal->rc[CH_BACK]->getPosition()); + + _hal->debug->printf("rollMix: %f\t pitchMix: %f\t yawMix: %f\t thrustMix: %f\n", + rollMix, + pitchMix, + yawMix, + thrustMix); + +// _hal->debug->printf("thrust pwm: %d\n",_hal->rc[CH_THRUST]->readRadio()); + } + +private: + PidDFB2 pidRoll, pidPitch, pidYaw, pidPN, pidPE, pidPD; + Pid2 pidYawRate; +}; + +/* + class PlaneController : public AP_Controller + { + private: + // state + AP_Float roll; + AP_Float airspeed; + AP_Float velocity; + AP_Float heading; + + // servo positions + AP_Float steering; + AP_Float throttle; + + // control variables + AP_Float headingCommand; + AP_Float airspeedCommand; + AP_Float rollCommand; + + // channels + static const uint8_t chRoll = 0; + static const uint8_t chPitch = 1; + static const uint8_t chYaw = 2; + + public: + PlaneController(AP_Var::Key chRollKey, AP_Var::Key chPitchKey, AP_Var::Key chYawKey, + AP_Var::Key pidRollKey, AP_Var::Key pidPitchKey, AP_Var::Key pidYawKey) : { + // rc channels + addCh(new AP_RcChannelSimple(chRollKey,PSTR("ROLL"),APM_RC,chRoll,45)); + addCh(new AP_RcChannelSimple(chPitchKey,PSTR("PTCH"),APM_RC,chPitch,45)); + addCh(new AP_RcChannelSimple(chYawKey,PSTR("YAW"),APM_RC,chYaw,45)); + + // pitch control loop + #if AIRSPEED_SENSOR == ENABLED + // pitch control loop w/ airspeed + addBlock(new SumGain(airspeedCommand,AP_Float_unity,airspeed,AP_Float_negative_unity)); + #else + // cross feed variables + addBlock(new SumGain(roll,kffPitchCompk,throttleServo,kffT2P)); + #endif + addBlock(new Pid(pidPitchKey,PSTR("PTCH"),0.1,0,0,1,20)); + addBlock(new ToServo(getRc(chPitch))); + + // roll control loop + addBlock(new SumGain(headingCommand,one,heading,negOne)); + addBlock(new Pid(headingkP,headingKI,headingKD)); + addBlock(new Sink(rollCommand)); + addBlock(new SumGain(rollCommand,one,roll,negOne)); + addBlock(new Pid(rollKP,rollKI,rollKD)); + addBlock(new ToServo(getRc(chRoll))); + + // throttle control loop + addBlock(new SumGain(airspeedCommand,one,airspeed,negOne)); + addBlock(new Pid(throttleKP,throttleKI,throttleKD)); + addBlock(new ToServo(getRc(chThr))); + } + }; + */ + +} // namespace apo + +#endif // defaultControllers_H +// vim:ts=4:sw=4:expandtab diff --git a/libraries/AP_RcChannel/examples/Manual/Makefile b/libraries/APO/examples/Manual/Makefile similarity index 100% rename from libraries/AP_RcChannel/examples/Manual/Makefile rename to libraries/APO/examples/Manual/Makefile diff --git a/libraries/AP_RcChannel/examples/Manual/Manual.pde b/libraries/APO/examples/Manual/Manual.pde similarity index 71% rename from libraries/AP_RcChannel/examples/Manual/Manual.pde rename to libraries/APO/examples/Manual/Manual.pde index 0649b1f643..df32ead302 100644 --- a/libraries/AP_RcChannel/examples/Manual/Manual.pde +++ b/libraries/APO/examples/Manual/Manual.pde @@ -7,6 +7,7 @@ #include #include +#include #include // ArduPilot Mega RC Library #include #include @@ -15,6 +16,8 @@ FastSerialPort0(Serial); // make sure this proceeds variable declarations // test settings +using namespace apo; + class RadioTest { private: @@ -36,14 +39,14 @@ private: public: RadioTest() : testPosition(2), testSign(1) { - ch.push_back(new AP_RcChannel(rollKey,PSTR("ROLL"),APM_RC,1,45)); - ch.push_back(new AP_RcChannel(pitchKey,PSTR("PITCH"),APM_RC,2,45)); - ch.push_back(new AP_RcChannel(thrKey,PSTR("THR"),APM_RC,3,100)); - ch.push_back(new AP_RcChannel(yawKey,PSTR("YAW"),APM_RC,4,45)); - ch.push_back(new AP_RcChannel(ch5Key,PSTR("CH5"),APM_RC,5,1)); - ch.push_back(new AP_RcChannel(ch6Key,PSTR("CH6"),APM_RC,6,1)); - ch.push_back(new AP_RcChannel(ch7Key,PSTR("CH7"),APM_RC,7,1)); - ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1)); + ch.push_back(new AP_RcChannel(rollKey,PSTR("ROLL"),APM_RC,1,1100,1500,1900)); + ch.push_back(new AP_RcChannel(pitchKey,PSTR("PITCH"),APM_RC,2,1100,1500,1900)); + ch.push_back(new AP_RcChannel(thrKey,PSTR("THR"),APM_RC,3,1100,1500,1900)); + ch.push_back(new AP_RcChannel(yawKey,PSTR("YAW"),APM_RC,4,1100,1500,1900)); + ch.push_back(new AP_RcChannel(ch5Key,PSTR("CH5"),APM_RC,5,1100,1500,1900)); + ch.push_back(new AP_RcChannel(ch6Key,PSTR("CH6"),APM_RC,6,1100,1500,1900)); + ch.push_back(new AP_RcChannel(ch7Key,PSTR("CH7"),APM_RC,7,1100,1500,1900)); + ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1100,1500,1900)); Serial.begin(115200); delay(2000); diff --git a/libraries/AP_RcChannel/examples/ServoSweep/Makefile b/libraries/APO/examples/ServoSweep/Makefile similarity index 100% rename from libraries/AP_RcChannel/examples/ServoSweep/Makefile rename to libraries/APO/examples/ServoSweep/Makefile diff --git a/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde b/libraries/APO/examples/ServoSweep/ServoSweep.pde similarity index 90% rename from libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde rename to libraries/APO/examples/ServoSweep/ServoSweep.pde index 2d5de8215d..345da1769c 100644 --- a/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde +++ b/libraries/APO/examples/ServoSweep/ServoSweep.pde @@ -7,6 +7,7 @@ #include #include +#include #include // ArduPilot Mega RC Library #include #include @@ -15,6 +16,8 @@ FastSerialPort0(Serial); // make sure this proceeds variable declarations // test settings +using namespace apo; + class RadioTest { private: @@ -36,14 +39,14 @@ private: public: RadioTest() : testPosition(2), testSign(1) { - ch.push_back(new AP_RcChannel(rollKey,PSTR("ROLL"),APM_RC,1,45)); - ch.push_back(new AP_RcChannel(pitchKey,PSTR("PITCH"),APM_RC,2,45)); - ch.push_back(new AP_RcChannel(thrKey,PSTR("THR"),APM_RC,3,100)); - ch.push_back(new AP_RcChannel(yawKey,PSTR("YAW"),APM_RC,4,45)); - ch.push_back(new AP_RcChannel(ch5Key,PSTR("CH5"),APM_RC,5,1)); - ch.push_back(new AP_RcChannel(ch6Key,PSTR("CH6"),APM_RC,6,1)); - ch.push_back(new AP_RcChannel(ch7Key,PSTR("CH7"),APM_RC,7,1)); - ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1)); + ch.push_back(new AP_RcChannel(rollKey,PSTR("ROLL"),APM_RC,1,1100,1500,1900)); + ch.push_back(new AP_RcChannel(pitchKey,PSTR("PITCH"),APM_RC,2,1100,1500,1900)); + ch.push_back(new AP_RcChannel(thrKey,PSTR("THR"),APM_RC,3,1100,1500,1900)); + ch.push_back(new AP_RcChannel(yawKey,PSTR("YAW"),APM_RC,4,1100,1500,1900)); + ch.push_back(new AP_RcChannel(ch5Key,PSTR("CH5"),APM_RC,5,1100,1500,1900)); + ch.push_back(new AP_RcChannel(ch6Key,PSTR("CH6"),APM_RC,6,1100,1500,1900)); + ch.push_back(new AP_RcChannel(ch7Key,PSTR("CH7"),APM_RC,7,1100,1500,1900)); + ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1100,1500,1900)); Serial.begin(115200); delay(2000); @@ -69,7 +72,7 @@ public: } // set channel positions - for (int i=0;isetNormalized(testPosition); + for (int i=0;isetPosition(testPosition); // print test position Serial.printf("\nnormalized position (%f)\n",testPosition); diff --git a/libraries/APO/mikrokopter.h b/libraries/APO/mikrokopter.h new file mode 100644 index 0000000000..c3d8b50def --- /dev/null +++ b/libraries/APO/mikrokopter.h @@ -0,0 +1,56 @@ +/* + * mikrokopter.h + * + * Created on: Apr 6, 2011 + * Author: nkgentry + */ + +#ifndef MIKROKOPTER_H_ +#define MIKROKOPTER_H_ + +#define VEHICLE_MIKROKOPTER + +//motor parameters +#define MOTOR_MAX 1 +#define MOTOR_MIN 0.1 + +// position control loop +#define PID_POS_INTERVAL 1/5 // 5 hz +#define PID_POS_P 0.02 +#define PID_POS_I 0 +#define PID_POS_D 0.1 +#define PID_POS_LIM 0.1 // about 5 deg +#define PID_POS_AWU 0.0 // about 5 deg +#define PID_POS_Z_P 0.5 +#define PID_POS_Z_I 0.2 +#define PID_POS_Z_D 0.5 +#define PID_POS_Z_LIM 0.5 +#define PID_POS_Z_AWU 0.1 + +// attitude control loop +#define PID_ATT_INTERVAL 1/20 // 20 hz +#define PID_ATT_P 0.03 // 0.1 +#define PID_ATT_I 0.03 // 0.0 +#define PID_ATT_D 0.03 // 0.1 +#define PID_ATT_LIM 0.1 // 0.01 // 10 % #define MOTORs +#define PID_ATT_AWU 0.1 // 0.0 +#define PID_YAWPOS_P 1 +#define PID_YAWPOS_I 0.1 +#define PID_YAWPOS_D 0 +#define PID_YAWPOS_LIM 1 // 1 rad/s +#define PID_YAWPOS_AWU 1 // 1 rad/s +#define PID_YAWSPEED_P 1 +#define PID_YAWSPEED_I 0 +#define PID_YAWSPEED_D 0.5 +#define PID_YAWSPEED_LIM 0.1 // 0.01 // 10 % #define MOTORs +#define PID_YAWSPEED_AWU 0.0 + +// mixing +#define MIX_REMOTE_WEIGHT 1 +#define MIX_POSITION_WEIGHT 1 +#define MIX_POSITION_Z_WEIGHT 1 +#define MIX_POSITION_YAW_WEIGHT 1 + +#define THRUST_HOVER_OFFSET 0.475 + +#endif /* MIKROKOPTER_H_ */ diff --git a/libraries/APO/template.h b/libraries/APO/template.h new file mode 100644 index 0000000000..4bed52d485 --- /dev/null +++ b/libraries/APO/template.h @@ -0,0 +1,32 @@ +/* + * Class.h + * Copyright (C) Author 2011 + * + * 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 Class_H +#define Class_H + +namespace apo { + +/// Class desciprtion +class Class { +public: +} + +} // namespace apo + +#endif // Class_H +// vim:ts=4:sw=4:expandtab diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index c0b6e894d7..08b7fadab7 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/libraries/AP_Compass/AP_Compass_HMC5843.h @@ -1,8 +1,8 @@ #ifndef AP_Compass_HMC5843_H #define AP_Compass_HMC5843_H -#include -#include +#include "../AP_Common/AP_Common.h" +#include "../AP_Math/AP_Math.h" #include "Compass.h" diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index af0414576a..da1904fa8d 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -2,8 +2,8 @@ #define Compass_h #include -#include -#include +#include "../AP_Common/AP_Common.h" +#include "../AP_Math/AP_Math.h" // standard rotation matrices #define ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1) diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index c41e82429e..ece657cd8b 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -7,14 +7,14 @@ // header file #include "AP_DCM_HIL.h" -#include -#include +#include "../FastSerial/FastSerial.h" +#include "../AP_Math/AP_Math.h" #include #include "WProgram.h" -#include -#include -#include -#include +#include "../AP_Compass/AP_Compass.h" +#include "../AP_ADC/AP_ADC.h" +#include "../AP_GPS/AP_GPS.h" +#include "../AP_IMU/AP_IMU.h" class AP_DCM diff --git a/libraries/AP_DCM/AP_DCM_HIL.h b/libraries/AP_DCM/AP_DCM_HIL.h index 1fce1c1f4f..0a3049f811 100644 --- a/libraries/AP_DCM/AP_DCM_HIL.h +++ b/libraries/AP_DCM/AP_DCM_HIL.h @@ -1,14 +1,14 @@ #ifndef AP_DCM_HIL_H #define AP_DCM_HIL_H -#include -#include +#include "../FastSerial/FastSerial.h" +#include "../AP_Math/AP_Math.h" #include #include "WProgram.h" -#include -#include -#include -#include +#include "../AP_Compass/AP_Compass.h" +#include "../AP_ADC/AP_ADC.h" +#include "../AP_GPS/AP_GPS.h" +#include "../AP_IMU/AP_IMU.h" class AP_DCM_HIL diff --git a/libraries/AP_GPS/AP_GPS_Auto.h b/libraries/AP_GPS/AP_GPS_Auto.h index 2c03cd1a13..61be2de201 100644 --- a/libraries/AP_GPS/AP_GPS_Auto.h +++ b/libraries/AP_GPS/AP_GPS_Auto.h @@ -6,8 +6,8 @@ #ifndef AP_GPS_Auto_h #define AP_GPS_Auto_h -#include -#include +#include "../FastSerial/FastSerial.h" +#include "GPS.h" class AP_GPS_Auto : public GPS { diff --git a/libraries/AP_GPS/AP_GPS_HIL.h b/libraries/AP_GPS/AP_GPS_HIL.h index 6c96c69c8b..9fa644efe8 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.h +++ b/libraries/AP_GPS/AP_GPS_HIL.h @@ -12,7 +12,7 @@ #ifndef AP_GPS_HIL_h #define AP_GPS_HIL_h -#include +#include "GPS.h" class AP_GPS_HIL : public GPS { public: diff --git a/libraries/AP_GPS/AP_GPS_IMU.h b/libraries/AP_GPS/AP_GPS_IMU.h index dd74463a19..1a31c615f0 100644 --- a/libraries/AP_GPS/AP_GPS_IMU.h +++ b/libraries/AP_GPS/AP_GPS_IMU.h @@ -3,7 +3,7 @@ #ifndef AP_GPS_IMU_h #define AP_GPS_IMU_h -#include +#include "GPS.h" #define MAXPAYLOAD 32 class AP_GPS_IMU : public GPS { @@ -45,4 +45,4 @@ class AP_GPS_IMU : public GPS { void checksum(unsigned char data); }; -#endif \ No newline at end of file +#endif diff --git a/libraries/AP_GPS/AP_GPS_MTK.h b/libraries/AP_GPS/AP_GPS_MTK.h index 9132ceafbe..bae99f1c41 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.h +++ b/libraries/AP_GPS/AP_GPS_MTK.h @@ -15,7 +15,7 @@ #ifndef AP_GPS_MTK_h #define AP_GPS_MTK_h -#include +#include "GPS.h" #include "AP_GPS_MTK_Common.h" class AP_GPS_MTK : public GPS { diff --git a/libraries/AP_GPS/AP_GPS_MTK16.h b/libraries/AP_GPS/AP_GPS_MTK16.h index a220d4c50b..78c29ebb21 100644 --- a/libraries/AP_GPS/AP_GPS_MTK16.h +++ b/libraries/AP_GPS/AP_GPS_MTK16.h @@ -13,7 +13,7 @@ #ifndef AP_GPS_MTK16_h #define AP_GPS_MTK16_h -#include +#include "GPS.h" #include "AP_GPS_MTK_Common.h" class AP_GPS_MTK16 : public GPS { diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index 3f0df041e7..765c1e82b7 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -42,7 +42,7 @@ #ifndef AP_GPS_NMEA_h #define AP_GPS_NMEA_h -#include +#include "GPS.h" #include diff --git a/libraries/AP_GPS/AP_GPS_None.h b/libraries/AP_GPS/AP_GPS_None.h index f7c5d9daad..6b93cd83e8 100644 --- a/libraries/AP_GPS/AP_GPS_None.h +++ b/libraries/AP_GPS/AP_GPS_None.h @@ -3,7 +3,7 @@ #ifndef AP_GPS_None_h #define AP_GPS_None_h -#include +#include "GPS.h" class AP_GPS_None : public GPS { diff --git a/libraries/AP_GPS/AP_GPS_SIRF.h b/libraries/AP_GPS/AP_GPS_SIRF.h index 167d05eb55..dd20ea726d 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.h +++ b/libraries/AP_GPS/AP_GPS_SIRF.h @@ -11,7 +11,7 @@ #ifndef AP_GPS_SIRF_h #define AP_GPS_SIRF_h -#include +#include "GPS.h" #define SIRF_SET_BINARY "$PSRF100,0,38400,8,1,0*3C" diff --git a/libraries/AP_GPS/AP_GPS_Shim.h b/libraries/AP_GPS/AP_GPS_Shim.h index a99ab6e9ab..ce7828d588 100644 --- a/libraries/AP_GPS/AP_GPS_Shim.h +++ b/libraries/AP_GPS/AP_GPS_Shim.h @@ -12,7 +12,7 @@ #ifndef AP_GPS_Shim_h #define AP_GPS_Shim_h -#include +#include "GPS.h" class AP_GPS_Shim : public GPS { diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 73b5cf7cc7..cbb7b29df8 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -11,7 +11,7 @@ #ifndef AP_GPS_UBLOX_h #define AP_GPS_UBLOX_h -#include +#include "GPS.h" #define UBLOX_SET_BINARY "$PUBX,41,1,0003,0001,38400,0*26" diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.h b/libraries/AP_IMU/AP_IMU_Oilpan.h index d37bc69015..a849b1e349 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.h +++ b/libraries/AP_IMU/AP_IMU_Oilpan.h @@ -7,9 +7,9 @@ #define AP_IMU_Oilpan_h -#include -#include -#include +#include "../AP_Common/AP_Common.h" +#include "../AP_Math/AP_Math.h" +#include "../AP_ADC/AP_ADC.h" #include #include "IMU.h" diff --git a/libraries/AP_IMU/IMU.h b/libraries/AP_IMU/IMU.h index 11e7d748a6..bea43fddd0 100644 --- a/libraries/AP_IMU/IMU.h +++ b/libraries/AP_IMU/IMU.h @@ -7,7 +7,7 @@ #ifndef IMU_h #define IMU_h -#include +#include "../AP_Math/AP_Math.h" #include class IMU diff --git a/libraries/AP_RcChannel/AP_RcChannel.cpp b/libraries/AP_RcChannel/AP_RcChannel.cpp deleted file mode 100644 index 689d8ab230..0000000000 --- a/libraries/AP_RcChannel/AP_RcChannel.cpp +++ /dev/null @@ -1,128 +0,0 @@ -/* - AP_RcChannel.cpp - Radio library for Arduino - Code by Jason Short, James Goppert. DIYDrones.com - - This library is free software; you can redistribute it and / or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - -*/ - -#include -#include -#include "AP_RcChannel.h" -#include - -AP_RcChannel::AP_RcChannel(AP_Var::Key key, const prog_char_t * name, APM_RC_Class & rc, const uint8_t & ch, - const float & scale, const float & center, - const uint16_t & pwmMin, - const uint16_t & pwmNeutral, const uint16_t & pwmMax, - const uint16_t & pwmDeadZone, - const bool & filter, const bool & reverse) : - AP_Var_group(key,name), - _rc(rc), - ch(this,0,ch,PSTR("CH")), - scale(this,1,scale,PSTR("SCALE")), - center(this,2,center,PSTR("CENTER")), - pwmMin(this,3,pwmMin,PSTR("PMIN")), - pwmMax(this,4,pwmMax,PSTR("PMAX")), - pwmNeutral(this,5,pwmNeutral,PSTR("PNTRL")), - pwmDeadZone(this,6,pwmDeadZone,PSTR("PDEAD")), - filter(this,7,filter,PSTR("FLTR")), - reverse(this,8,reverse,PSTR("REV")), - _pwm(0) - { - setNormalized(0.0); - } - - -uint16_t AP_RcChannel::readRadio() { - return _rc.InputCh(ch); -} - -void -AP_RcChannel::setPwm(uint16_t pwm) -{ - //Serial.printf("pwm in setPwm: %d\n", pwm); - //Serial.printf("reverse: %s\n", (reverse)?"true":"false"); - - // apply reverse - if(reverse) pwm = int16_t(pwmNeutral-pwm) + pwmNeutral; - - //Serial.printf("pwm after reverse: %d\n", pwm); - - // apply filter - if(filter){ - if(_pwm == 0) - _pwm = pwm; - else - _pwm = ((pwm + _pwm) >> 1); // Small filtering - }else{ - _pwm = pwm; - } - - //Serial.printf("pwm after filter: %d\n", _pwm); - - // apply deadzone - _pwm = (abs(_pwm - pwmNeutral) < pwmDeadZone) ? uint16_t(pwmNeutral) : _pwm; - - //Serial.printf("pwm after deadzone: %d\n", _pwm); - _rc.OutputCh(ch,_pwm); -} - -void -AP_RcChannel::setPosition(float position) -{ - if (position > scale) position = scale; - else if (position < -scale) position = -scale; - setPwm(_positionToPwm(position)); -} - -void -AP_RcChannel::setNormalized(float normPosition) -{ - setPosition(normPosition*scale); -} - -void -AP_RcChannel::mixRadio(uint16_t infStart) -{ - uint16_t pwmRadio = _rc.InputCh(ch); - float inf = abs( int16_t(pwmRadio - pwmNeutral) ); - inf = min(inf, infStart); - inf = ((infStart - inf) /infStart); - setPwm(_pwm*inf + pwmRadio); -} - -uint16_t -AP_RcChannel::_positionToPwm(const float & position) -{ - uint16_t pwm; - //Serial.printf("position: %f\n", position); - float p = position - center; - if(p < 0) - pwm = p * int16_t(pwmNeutral - pwmMin) / - scale + pwmNeutral; - else - pwm = p * int16_t(pwmMax - pwmNeutral) / - scale + pwmNeutral; - constrain(pwm,uint16_t(pwmMin),uint16_t(pwmMax)); - return pwm; -} - -float -AP_RcChannel::_pwmToPosition(const uint16_t & pwm) -{ - float position; - if(pwm < pwmNeutral) - position = scale * int16_t(pwm - pwmNeutral)/ - int16_t(pwmNeutral - pwmMin) + center; - else - position = scale * int16_t(pwm - pwmNeutral)/ - int16_t(pwmMax - pwmNeutral) + center; - constrain(position,center-scale,center+scale); - return position; -} - -// ------------------------------------------ diff --git a/libraries/FastSerial/BetterStream.h b/libraries/FastSerial/BetterStream.h index 648f3160c2..e9b16a00d0 100644 --- a/libraries/FastSerial/BetterStream.h +++ b/libraries/FastSerial/BetterStream.h @@ -13,7 +13,7 @@ #include #include -#include +#include "../AP_Common/AP_Common.h" class BetterStream : public Stream { public: