• Main Page
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

/home/jgoppert/Projects/ap/libraries/Waypoints/Waypoints.cpp

Go to the documentation of this file.
00001 /*
00002         AP_Radio.cpp - Radio library for Arduino
00003         Code by Jason Short. DIYDrones.com
00004         
00005         This library is free software; you can redistribute it and / or
00006                 modify it under the terms of the GNU Lesser General Public
00007                 License as published by the Free Software Foundation; either
00008                 version 2.1 of the License, or (at your option) any later version.
00009 
00010 */
00011 
00012 #include "Waypoints.h"
00013 
00014 Waypoints::Waypoints()
00015 {
00016 }
00017 
00018 void 
00019 Waypoints::set_waypoint_with_index(Waypoints::WP wp, uint8_t i)
00020 {
00021         i = constrain(i, 0, _total);
00022         uint32_t mem = _start_byte + (i * _wp_size);
00023 
00024         eeprom_busy_wait();
00025         eeprom_write_byte((uint8_t *) mem, wp.id);
00026 
00027         mem++;
00028         eeprom_busy_wait();
00029         eeprom_write_byte((uint8_t *) mem, wp.p1);
00030 
00031         mem++;
00032         eeprom_busy_wait();
00033         eeprom_write_dword((uint32_t *) mem, wp.alt);
00034 
00035         mem += 4;
00036         eeprom_busy_wait();
00037         eeprom_write_dword((uint32_t *) mem, wp.lat);
00038 
00039         mem += 4;
00040         eeprom_busy_wait();
00041         eeprom_write_dword((uint32_t *) mem, wp.lng);
00042 }
00043 
00044 Waypoints::WP
00045 Waypoints::get_waypoint_with_index(uint8_t i)
00046 {
00047         Waypoints::WP wp;
00048 
00049         i = constrain(i, 0, _total);
00050         uint32_t mem = _start_byte + (i * _wp_size);
00051         
00052         eeprom_busy_wait();
00053         wp.id = eeprom_read_byte((uint8_t *)mem);
00054 
00055         mem++;
00056         eeprom_busy_wait();
00057         wp.p1 = eeprom_read_byte((uint8_t *)mem);
00058 
00059         mem++;
00060         eeprom_busy_wait();
00061         wp.alt = (long)eeprom_read_dword((uint32_t *)mem);
00062 
00063         mem += 4;
00064         eeprom_busy_wait();
00065         wp.lat = (long)eeprom_read_dword((uint32_t *)mem);
00066 
00067         mem += 4;
00068         eeprom_busy_wait();
00069         wp.lng = (long)eeprom_read_dword((uint32_t *)mem);
00070 }
00071 
00072 
00073 Waypoints::WP
00074 Waypoints::get_current_waypoint(void)
00075 {
00076         return get_waypoint_with_index(_index);
00077 }
00078 
00079 uint8_t
00080 Waypoints::get_index(void)
00081 {
00082         return _index;
00083 }
00084 
00085 void
00086 Waypoints::next_index(void)
00087 {
00088         _index++;
00089         if (_index >= _total)
00090                 _index == 0;
00091 }
00092 
00093 void
00094 Waypoints::set_index(uint8_t i)
00095 {
00096         i = constrain(i, 0, _total);
00097 }
00098 
00099 uint8_t
00100 Waypoints::get_total(void)
00101 {
00102         return _total;
00103 }
00104 void
00105 Waypoints::set_total(uint8_t total)
00106 {
00107         _total = total;
00108 }
00109 
00110 void
00111 Waypoints::set_start_byte(uint16_t start_byte)
00112 {
00113         _start_byte = start_byte;
00114 }
00115 
00116 void
00117 Waypoints::set_wp_size(uint8_t wp_size)
00118 {
00119         _wp_size = wp_size;
00120 }

Generated for ArduPilot Libraries by doxygen