From 6d13081fcf47295dfd1097022667635e62c5e44e Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Sun, 1 May 2011 17:38:12 +0000 Subject: [PATCH] Removed APO lib. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1957 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/APO/APO.cpp | 8 - libraries/APO/APO.h | 21 - libraries/APO/APO_Config.h | 49 -- libraries/APO/AP_Autopilot.cpp | 216 ----- libraries/APO/AP_Autopilot.h | 148 ---- 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 --- libraries/APO/AP_RcChannel.h | 71 -- libraries/APO/AP_Var_keys.h | 41 - libraries/APO/constants.h | 23 - libraries/APO/examples/MavlinkTest/Makefile | 2 - .../APO/examples/MavlinkTest/MavlinkTest.pde | 46 -- libraries/APO/examples/ServoManual/Makefile | 2 - .../APO/examples/ServoManual/ServoManual.pde | 97 --- libraries/APO/examples/ServoSweep/Makefile | 2 - .../APO/examples/ServoSweep/ServoSweep.pde | 116 --- libraries/APO/template.h | 32 - 28 files changed, 3278 deletions(-) delete mode 100644 libraries/APO/APO.cpp delete mode 100644 libraries/APO/APO.h delete mode 100644 libraries/APO/APO_Config.h delete mode 100644 libraries/APO/AP_Autopilot.cpp delete mode 100644 libraries/APO/AP_Autopilot.h delete mode 100644 libraries/APO/AP_CommLink.cpp delete mode 100644 libraries/APO/AP_CommLink.h delete mode 100644 libraries/APO/AP_Controller.cpp delete mode 100644 libraries/APO/AP_Controller.h delete mode 100644 libraries/APO/AP_Guide.cpp delete mode 100644 libraries/APO/AP_Guide.h delete mode 100644 libraries/APO/AP_HardwareAbstractionLayer.cpp delete mode 100644 libraries/APO/AP_HardwareAbstractionLayer.h delete mode 100644 libraries/APO/AP_MavlinkCommand.cpp delete mode 100644 libraries/APO/AP_MavlinkCommand.h delete mode 100644 libraries/APO/AP_Navigator.cpp delete mode 100644 libraries/APO/AP_Navigator.h delete mode 100644 libraries/APO/AP_RcChannel.cpp delete mode 100644 libraries/APO/AP_RcChannel.h delete mode 100644 libraries/APO/AP_Var_keys.h delete mode 100644 libraries/APO/constants.h delete mode 100644 libraries/APO/examples/MavlinkTest/Makefile delete mode 100644 libraries/APO/examples/MavlinkTest/MavlinkTest.pde delete mode 100644 libraries/APO/examples/ServoManual/Makefile delete mode 100644 libraries/APO/examples/ServoManual/ServoManual.pde delete mode 100644 libraries/APO/examples/ServoSweep/Makefile delete mode 100644 libraries/APO/examples/ServoSweep/ServoSweep.pde delete mode 100644 libraries/APO/template.h diff --git a/libraries/APO/APO.cpp b/libraries/APO/APO.cpp deleted file mode 100644 index 2592d1140c..0000000000 --- a/libraries/APO/APO.cpp +++ /dev/null @@ -1,8 +0,0 @@ -/* - * APO.cpp - * - * Created on: Apr 30, 2011 - * Author: jgoppert - */ - -#include "APO.h" diff --git a/libraries/APO/APO.h b/libraries/APO/APO.h deleted file mode 100644 index e6842df84c..0000000000 --- a/libraries/APO/APO.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * 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 deleted file mode 100644 index 5b00a701b6..0000000000 --- a/libraries/APO/APO_Config.h +++ /dev/null @@ -1,49 +0,0 @@ -#ifndef APO_Config_H -#define APO_Config_H - -#include "AP_HardwareAbstractionLayer.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 deleted file mode 100644 index 8be1adfbff..0000000000 --- a/libraries/APO/AP_Autopilot.cpp +++ /dev/null @@ -1,216 +0,0 @@ -/* - * 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 deleted file mode 100644 index d0ce425593..0000000000 --- a/libraries/APO/AP_Autopilot.h +++ /dev/null @@ -1,148 +0,0 @@ -/* - * 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" - -/** - * 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 guide, navigator, and controller - * as well as the hardware abstraction layer. - * - * It inherits from loop to manage - * the subloops and sets the overall - * 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 deleted file mode 100644 index 92b9d0fdde..0000000000 --- a/libraries/APO/AP_CommLink.cpp +++ /dev/null @@ -1,15 +0,0 @@ -/* - * 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 deleted file mode 100644 index 3c0cb5da80..0000000000 --- a/libraries/APO/AP_CommLink.h +++ /dev/null @@ -1,775 +0,0 @@ -/* - * 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 deleted file mode 100644 index ebbd5ed524..0000000000 --- a/libraries/APO/AP_Controller.cpp +++ /dev/null @@ -1,8 +0,0 @@ -/* - * 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 deleted file mode 100644 index def541c041..0000000000 --- a/libraries/APO/AP_Controller.h +++ /dev/null @@ -1,167 +0,0 @@ -/* - * 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 deleted file mode 100644 index 488b746610..0000000000 --- a/libraries/APO/AP_Guide.cpp +++ /dev/null @@ -1,8 +0,0 @@ -/* - * 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 deleted file mode 100644 index c96d54a583..0000000000 --- a/libraries/APO/AP_Guide.h +++ /dev/null @@ -1,298 +0,0 @@ -/* - * 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 deleted file mode 100644 index afa06769ab..0000000000 --- a/libraries/APO/AP_HardwareAbstractionLayer.cpp +++ /dev/null @@ -1,8 +0,0 @@ -/* - * 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 deleted file mode 100644 index c24e5f2c14..0000000000 --- a/libraries/APO/AP_HardwareAbstractionLayer.h +++ /dev/null @@ -1,99 +0,0 @@ -/* - * 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(), lastHeartBeat() - { - } - /** - * 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 deleted file mode 100644 index a33eb9ed45..0000000000 --- a/libraries/APO/AP_MavlinkCommand.cpp +++ /dev/null @@ -1,8 +0,0 @@ -/* - * 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 deleted file mode 100644 index 483dc24a4b..0000000000 --- a/libraries/APO/AP_MavlinkCommand.h +++ /dev/null @@ -1,456 +0,0 @@ -/* - * 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 deleted file mode 100644 index ff33e8ed2c..0000000000 --- a/libraries/APO/AP_Navigator.cpp +++ /dev/null @@ -1,8 +0,0 @@ -/* - * 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 deleted file mode 100644 index d760bc09de..0000000000 --- a/libraries/APO/AP_Navigator.h +++ /dev/null @@ -1,445 +0,0 @@ -/* - * 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 deleted file mode 100644 index d00a7954c2..0000000000 --- a/libraries/APO/AP_RcChannel.cpp +++ /dev/null @@ -1,109 +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_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/APO/AP_RcChannel.h b/libraries/APO/AP_RcChannel.h deleted file mode 100644 index 99b236c1c3..0000000000 --- a/libraries/APO/AP_RcChannel.h +++ /dev/null @@ -1,71 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/// @file AP_RcChannel.h -/// @brief AP_RcChannel manager - -#ifndef AP_RCCHANNEL_H -#define AP_RCCHANNEL_H - -#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 -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 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_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); - - // get - uint16_t getPwm() { return _pwm; } - float getPosition() { return _pwmToPosition(_pwm); } - - // did our read come in 50µs below the min? - bool failSafe() { _pwm < (_pwmMin - 50); } - -private: - - // configuration - const char * _name; - APM_RC_Class & _rc; - - // internal states - uint16_t _pwm; // this is the internal state, position is just created when needed - - // private methods - uint16_t _positionToPwm(const float & position); - float _pwmToPosition(const uint16_t & pwm); -}; - -} // apo - -#endif // AP_RCCHANNEL_H diff --git a/libraries/APO/AP_Var_keys.h b/libraries/APO/AP_Var_keys.h deleted file mode 100644 index 7daea2d1ff..0000000000 --- a/libraries/APO/AP_Var_keys.h +++ /dev/null @@ -1,41 +0,0 @@ -#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 deleted file mode 100644 index 4bc1b29262..0000000000 --- a/libraries/APO/constants.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * 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/examples/MavlinkTest/Makefile b/libraries/APO/examples/MavlinkTest/Makefile deleted file mode 100644 index 85b4d8856b..0000000000 --- a/libraries/APO/examples/MavlinkTest/Makefile +++ /dev/null @@ -1,2 +0,0 @@ -BOARD = mega -include ../../../AP_Common/Arduino.mk diff --git a/libraries/APO/examples/MavlinkTest/MavlinkTest.pde b/libraries/APO/examples/MavlinkTest/MavlinkTest.pde deleted file mode 100644 index e417746317..0000000000 --- a/libraries/APO/examples/MavlinkTest/MavlinkTest.pde +++ /dev/null @@ -1,46 +0,0 @@ -/* - AnalogReadSerial - Reads an analog input on pin 0, prints the result to the serial monitor - - This example code is in the public domain. - */ - -#include - -int packetDrops = 0; - -void handleMessage(mavlink_message_t * msg) { - Serial.print(", received mavlink message: "); - Serial.print(msg->msgid,DEC); -} - -void setup() { - Serial.begin(57600); - Serial3.begin(57600); - mavlink_comm_0_port = &Serial3; - packetDrops = 0; -} - -void loop() { - mavlink_msg_heartbeat_send(MAVLINK_COMM_0,mavlink_system.type,MAV_AUTOPILOT_ARDUPILOTMEGA); - Serial.print("heartbeat sent"); - - // receive new packets - mavlink_message_t msg; - mavlink_status_t status; - - Serial.print(", bytes available: "); Serial.print(comm_get_available(MAVLINK_COMM_0)); - while(comm_get_available(MAVLINK_COMM_0)) { - uint8_t c = comm_receive_ch(MAVLINK_COMM_0); - - // Try to get a new message - if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) handleMessage(&msg); - } - - // Update packet drops counter - packetDrops += status.packet_rx_drop_count; - - Serial.print(", dropped packets: "); - Serial.println(packetDrops); - delay(1000); -} diff --git a/libraries/APO/examples/ServoManual/Makefile b/libraries/APO/examples/ServoManual/Makefile deleted file mode 100644 index 85b4d8856b..0000000000 --- a/libraries/APO/examples/ServoManual/Makefile +++ /dev/null @@ -1,2 +0,0 @@ -BOARD = mega -include ../../../AP_Common/Arduino.mk diff --git a/libraries/APO/examples/ServoManual/ServoManual.pde b/libraries/APO/examples/ServoManual/ServoManual.pde deleted file mode 100644 index df32ead302..0000000000 --- a/libraries/APO/examples/ServoManual/ServoManual.pde +++ /dev/null @@ -1,97 +0,0 @@ -/* - Example of RC_Channel library. - Code by James Goppert/ Jason Short. 2010 - DIYDrones.com - -*/ - -#include -#include -#include -#include // ArduPilot Mega RC Library -#include -#include - -FastSerialPort0(Serial); // make sure this proceeds variable declarations - -// test settings - -using namespace apo; - -class RadioTest -{ -private: - float testPosition; - int8_t testSign; - enum - { - version, - rollKey, - pitchKey, - thrKey, - yawKey, - ch5Key, - ch6Key, - ch7Key, - ch8Key - }; - Vector ch; -public: - RadioTest() : testPosition(2), testSign(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); - Serial.println("ArduPilot RC Channel test"); - APM_RC.Init(); // APM Radio initialization - delay(2000); - } - - void update() - { - // read the radio - for (int i=0;ireadRadio(); - - // print channel names - Serial.print("\t\t"); - static char name[7]; - for (int i=0;icopy_name(name,7); - Serial.printf("%7s\t",name); - } - Serial.println(); - - // print pwm - Serial.printf("pwm :\t"); - for (int i=0;igetPwm()); - Serial.println(); - - // print position - Serial.printf("position :\t"); - for (int i=0;igetPosition()); - Serial.println(); - - delay(500); - } -}; - -RadioTest * test; - -void setup() -{ - test = new RadioTest; -} - -void loop() -{ - test->update(); -} diff --git a/libraries/APO/examples/ServoSweep/Makefile b/libraries/APO/examples/ServoSweep/Makefile deleted file mode 100644 index 85b4d8856b..0000000000 --- a/libraries/APO/examples/ServoSweep/Makefile +++ /dev/null @@ -1,2 +0,0 @@ -BOARD = mega -include ../../../AP_Common/Arduino.mk diff --git a/libraries/APO/examples/ServoSweep/ServoSweep.pde b/libraries/APO/examples/ServoSweep/ServoSweep.pde deleted file mode 100644 index 345da1769c..0000000000 --- a/libraries/APO/examples/ServoSweep/ServoSweep.pde +++ /dev/null @@ -1,116 +0,0 @@ -/* - Example of RC_Channel library. - Code by James Goppert/ Jason Short. 2010 - DIYDrones.com - -*/ - -#include -#include -#include -#include // ArduPilot Mega RC Library -#include -#include - -FastSerialPort0(Serial); // make sure this proceeds variable declarations - -// test settings - -using namespace apo; - -class RadioTest -{ -private: - float testPosition; - int8_t testSign; - enum - { - version, - rollKey, - pitchKey, - thrKey, - yawKey, - ch5Key, - ch6Key, - ch7Key, - ch8Key - }; - Vector ch; -public: - RadioTest() : testPosition(2), testSign(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); - Serial.println("ArduPilot RC Channel test"); - APM_RC.Init(); // APM Radio initialization - delay(2000); - } - - void update() - { - // update test value - testPosition += testSign*.1; - if (testPosition > 1) - { - //eepromRegistry.print(Serial); // show eeprom map - testPosition = 1; - testSign = -1; - } - else if (testPosition < -1) - { - testPosition = -1; - testSign = 1; - } - - // set channel positions - for (int i=0;isetPosition(testPosition); - - // print test position - Serial.printf("\nnormalized position (%f)\n",testPosition); - - // print channel names - Serial.print("\t\t"); - static char name[7]; - for (int i=0;icopy_name(name,7); - Serial.printf("%7s\t",name); - } - Serial.println(); - - // print pwm - Serial.printf("pwm :\t"); - for (int i=0;igetPwm()); - Serial.println(); - - // print position - Serial.printf("position :\t"); - for (int i=0;igetPosition()); - Serial.println(); - - delay(500); - } -}; - -RadioTest * test; - -void setup() -{ - test = new RadioTest; -} - -void loop() -{ - test->update(); -} - -// vim:ts=4:sw=4:expandtab diff --git a/libraries/APO/template.h b/libraries/APO/template.h deleted file mode 100644 index 4bed52d485..0000000000 --- a/libraries/APO/template.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * 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