From b5321816070a0b522d9d99d8ea1410abdfc7f04e Mon Sep 17 00:00:00 2001 From: jasonshort Date: Wed, 27 Apr 2011 05:10:02 +0000 Subject: [PATCH] This is how I right waypoints to EEPROM for now. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1923 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- WaypointWriterCopterMega/EEPROM.pde | 105 ++++++ .../WaypointWriterCopterMega.pde | 108 +++++++ WaypointWriterCopterMega/defines.h | 298 ++++++++++++++++++ WaypointWriterCopterMega/event_example.h | 15 + WaypointWriterCopterMega/mission_example.h | 35 ++ WaypointWriterCopterMega/sparkfun.h | 53 ++++ WaypointWriterCopterMega/takeoff.h | 12 + 7 files changed, 626 insertions(+) create mode 100644 WaypointWriterCopterMega/EEPROM.pde create mode 100644 WaypointWriterCopterMega/WaypointWriterCopterMega.pde create mode 100644 WaypointWriterCopterMega/defines.h create mode 100644 WaypointWriterCopterMega/event_example.h create mode 100644 WaypointWriterCopterMega/mission_example.h create mode 100644 WaypointWriterCopterMega/sparkfun.h create mode 100644 WaypointWriterCopterMega/takeoff.h diff --git a/WaypointWriterCopterMega/EEPROM.pde b/WaypointWriterCopterMega/EEPROM.pde new file mode 100644 index 0000000000..ee0fcc718a --- /dev/null +++ b/WaypointWriterCopterMega/EEPROM.pde @@ -0,0 +1,105 @@ + +void writePoints() +{ + int mem; + struct Location loc; + + + for (byte i = 0; i < waypoint_total; i++){ + + loc.id = (uint8_t) mission[i][0]; + loc.options = (uint8_t) mission[i][1]; + loc.p1 = (uint8_t) mission[i][2]; + loc.alt = (long)(mission[i][3] * 100); + loc.lat = (long)(mission[i][4] * t7); + loc.lng = (long)(mission[i][5] * t7); + + switch(loc.id){ + case MAV_CMD_NAV_WAYPOINT: + //loc.p1 = (byte)mission[i][4];// wp_radius + //loc.p1 = WP_RADIUS; + break; + + case MAV_CMD_CONDITION_YAW: + loc.alt = (long)mission[i][3]; // speed + loc.lat = (long)mission[i][4]; // rotation direction + loc.lng = (long)mission[i][5]; // target yaw in deg + break; + } + + set_wp_with_index(loc,(i+1)); + + /* + Serial.print((i+1),DEC); + Serial.print(": "); + Serial.print(loc.id,DEC); + Serial.print(", "); + Serial.print(loc.p1,DEC); + Serial.print(", "); + Serial.print(loc.alt,DEC); + Serial.print(", "); + Serial.print(loc.lat,DEC); + Serial.print(", "); + Serial.println(loc.lng,DEC); + */ + } +} + + +struct Location get_wp_with_index(int i) +{ + struct Location temp; + long mem; + + // Find out proper location in memory by using the start_byte position + the index + // -------------------------------------------------------------------------------- + if (i > waypoint_total) { + temp.id = CMD_BLANK; + }else{ + // read WP position + mem = (WP_START_BYTE) + (i * WP_SIZE); + temp.id = eeprom_read_byte((uint8_t*)mem); + + mem++; + temp.options = eeprom_read_byte((uint8_t*)mem); + + mem++; + temp.p1 = eeprom_read_byte((uint8_t*)mem); + + mem++; + temp.alt = (long)eeprom_read_dword((uint32_t*)mem); // alt is stored in CM! + + mem += 4; + temp.lat = (long)eeprom_read_dword((uint32_t*)mem); + + mem += 4; + temp.lng = (long)eeprom_read_dword((uint32_t*)mem); + } + return temp; +} + +void set_wp_with_index(struct Location temp, int i) +{ + i = constrain(i, 0, waypoint_total.get()); + uint32_t mem = WP_START_BYTE + (i * WP_SIZE); + + eeprom_write_byte((uint8_t *) mem, temp.id); + + mem++; + eeprom_write_byte((uint8_t *) mem, temp.options); + + mem++; + eeprom_write_byte((uint8_t *) mem, temp.p1); + + mem++; + eeprom_write_dword((uint32_t *) mem, temp.alt); // alt is stored in CM! + + mem += 4; + eeprom_write_dword((uint32_t *) mem, temp.lat); + + mem += 4; + eeprom_write_dword((uint32_t *) mem, temp.lng); +} + + + diff --git a/WaypointWriterCopterMega/WaypointWriterCopterMega.pde b/WaypointWriterCopterMega/WaypointWriterCopterMega.pde new file mode 100644 index 0000000000..aad38e8603 --- /dev/null +++ b/WaypointWriterCopterMega/WaypointWriterCopterMega.pde @@ -0,0 +1,108 @@ +/* +Ardupilot 1.0.1a MEGA Waypoint writer +Use this release to manually upload waypoints +*/ +#include "defines.h" +#include +#include +#include // MAVLink GCS definitions + +FastSerialPort0(Serial); // FTDI/console + +// EEPROM addresses +#define EEPROM_MAX_ADDR 4096 +// parameters get the first 1KiB of EEPROM, remainder is for waypoints +#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP +#define WP_SIZE 15 + +// you can keep your missions stored here, just uncommment the mission to load: (only 1 at a time.) +//#include "mission_example.h" +#include "sparkfun.h" +//#include "takeoff.h" + +//#include "SFO_T3.h" +//#include "SFO_landMe.h" +//#include "Basic_mission_example.h" + +const long t7 = 10000000; // used to scale GPS values for EEPROM storage + +byte yaw_tracking = TRACK_NONE; // no tracking, point at next wp, or at a target + + +#define k_param_RTL_altitude 163 +#define k_param_waypoint_total 221 +#define k_param_waypoint_radius 224 + +AP_Int16 RTL_altitude ((int8_t)9, k_param_RTL_altitude, NULL); +AP_Int8 waypoint_total ((int8_t)0, k_param_waypoint_total, NULL); +AP_Int8 waypoint_radius ((int8_t)WP_RADIUS, k_param_waypoint_radius, NULL); + + +// You DON'T need to edit below this line +// ---------------------------------- +#include +#include + +void setup() +{ + Serial.begin(38400); + delay(1000); + Serial.println("\nACM Waypoint writer 1.0.3 Public Alpha \n\n"); + + Serial.printf("Test: %d\n", (int)MAV_CMD_NAV_LAND); + + //* + // number of waypoints, add 1 for home + waypoint_total.set_and_save((sizeof(mission) / 24)); + waypoint_radius.set_and_save(WP_RADIUS); + RTL_altitude.set_and_save(ALT_TO_HOLD * 100); + + writePoints(); // saves Waypoint Array + + delay(1000); + + if(RTL_altitude < 0){ + Serial.print("Hold current altitude above home after RTL.\n"); + }else{ + Serial.printf("Hold this altitude over home: %ld meters\n", (long)RTL_altitude.get()); + } + + Serial.printf("WP Radius: %ld meters\n", (long)RTL_altitude.get()); + Serial.printf("WP Radius: %d meters\n", (int)waypoint_radius.get()); + Serial.printf("total # of commands: %d\n", (int)waypoint_total.get()); + + report_wp(255); + + //*/ +} + +void loop() +{ + +} + + +void report_wp(byte index) +{ + if(index == 255){ + for(byte i = 0; i <= waypoint_total; i++){ + struct Location temp = get_wp_with_index(i); + print_wp(&temp, i); + } + }else{ + struct Location temp = get_wp_with_index(index); + print_wp(&temp, index); + } +} + +void print_wp(struct Location *cmd, byte index) +{ + Serial.printf_P(PSTR("command #: %d \tid:%d\top:%d\tp1:%d\tp2:%ld\tp3:%ld\tp4:%ld \n"), + (int)index, + (int)cmd->id, + (int)cmd->options, + (int)cmd->p1, + cmd->alt, + cmd->lat, + cmd->lng); +} diff --git a/WaypointWriterCopterMega/defines.h b/WaypointWriterCopterMega/defines.h new file mode 100644 index 0000000000..85e5eaca5b --- /dev/null +++ b/WaypointWriterCopterMega/defines.h @@ -0,0 +1,298 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + + +// ACM: +// Motors +#define RIGHT CH_1 +#define LEFT CH_2 +#define FRONT CH_3 +#define BACK CH_4 +#define RIGHTFRONT CH_7 +#define LEFTBACK CH_8 +#define MAX_SERVO_OUTPUT 2700 + +// active altitude sensor +// ---------------------- +#define SONAR 0 +#define BARO 1 + +// Frame types +#define PLUS_FRAME 0 +#define X_FRAME 1 +#define TRI_FRAME 2 +#define HEXAX_FRAME 3 +#define Y6_FRAME 4 +#define HEXAP_FRAME 5 + +// Internal defines, don't edit and expect things to work +// ------------------------------------------------------- + +#define TRUE 1 +#define FALSE 0 +#define ToRad(x) (x*0.01745329252) // *pi/180 +#define ToDeg(x) (x*57.2957795131) // *180/pi + +#define DEBUG 0 +#define LOITER_RANGE 60 // for calculating power outside of loiter radius + +#define T6 1000000 +#define T7 10000000 + +// GPS type codes - use the names, not the numbers +#define GPS_PROTOCOL_NONE -1 +#define GPS_PROTOCOL_NMEA 0 +#define GPS_PROTOCOL_SIRF 1 +#define GPS_PROTOCOL_UBLOX 2 +#define GPS_PROTOCOL_IMU 3 +#define GPS_PROTOCOL_MTK 4 +#define GPS_PROTOCOL_HIL 5 +#define GPS_PROTOCOL_MTK16 6 + +// Radio channels +// Note channels are from 0! +// +// XXX these should be CH_n defines from RC.h at some point. +#define CH_1 0 +#define CH_2 1 +#define CH_3 2 +#define CH_4 3 +#define CH_5 4 +#define CH_6 5 +#define CH_7 6 +#define CH_8 7 + +#define CH_ROLL CH_1 +#define CH_PITCH CH_2 +#define CH_THROTTLE CH_3 +#define CH_RUDDER CH_4 +#define CH_YAW CH_4 + +// HIL enumerations +#define HIL_PROTOCOL_XPLANE 1 +#define HIL_PROTOCOL_MAVLINK 2 + +#define HIL_MODE_DISABLED 0 +#define HIL_MODE_ATTITUDE 1 +#define HIL_MODE_SENSORS 2 + +// GCS enumeration +#define GCS_PROTOCOL_STANDARD 0 // standard APM protocol +#define GCS_PROTOCOL_LEGACY 1 // legacy ArduPilot protocol +#define GCS_PROTOCOL_XPLANE 2 // X-Plane HIL simulation +#define GCS_PROTOCOL_DEBUGTERMINAL 3 //Human-readable debug interface for use with a dumb terminal +#define GCS_PROTOCOL_MAVLINK 4 // binary protocol for qgroundcontrol +#define GCS_PROTOCOL_NONE -1 // No GCS output + +// Auto Pilot modes +// ---------------- +#define STABILIZE 0 // hold level position +#define ACRO 1 // rate control +#define ALT_HOLD 2 // AUTO control +#define SIMPLE 3 // +#define AUTO 4 // AUTO control +#define GCS_AUTO 5 // AUTO control +#define LOITER 6 // Hold a single location +#define RTL 7 // AUTO control +#define NUM_MODES 8 + +// YAW debug +// --------- +#define YAW_HOLD 0 +#define YAW_BRAKE 1 +#define YAW_RATE 2 + +// CH_6 Tuning +// ----------- +#define CH6_NONE 0 +#define CH6_STABLIZE_KP 1 +#define CH6_STABLIZE_KD 2 +#define CH6_BARO_KP 3 +#define CH6_BARO_KD 4 +#define CH6_SONAR_KP 5 +#define CH6_SONAR_KD 6 +#define CH6_Y6_SCALING 7 + +// nav byte mask +// ------------- +#define NAV_LOCATION 1 +#define NAV_ALTITUDE 2 +#define NAV_DELAY 4 + + +// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library +#define CMD_BLANK 0 // there is no command stored in the mem location requested +#define NO_COMMAND 0 + +// Nav Yaw Tracking +#define TRACK_NONE 1 +#define TRACK_NEXT_WP 2 +#define TRACK_TARGET_WP 4 + +// Waypoint options +#define WP_OPTION_ALT_RELATIVE 1 +#define WP_OPTION_ALT_CHANGE 2 +#define WP_OPTION_YAW 4 +#define WP_OPTION_ALT_REQUIRED 8 +#define WP_OPTION_RELATIVE 16 +//#define WP_OPTION_ 32 +//#define WP_OPTION_ 64 +#define WP_OPTION_NEXT_CMD 128 + +//repeating events +#define NO_REPEAT 0 +#define CH_5_TOGGLE 1 +#define CH_6_TOGGLE 2 +#define CH_7_TOGGLE 3 +#define CH_8_TOGGLE 4 +#define RELAY_TOGGLE 5 +#define STOP_REPEAT 10 + +//#define MAV_CMD_CONDITION_YAW 23 + +// GCS Message ID's +#define MSG_ACKNOWLEDGE 0x00 +#define MSG_HEARTBEAT 0x01 +#define MSG_ATTITUDE 0x02 +#define MSG_LOCATION 0x03 +#define MSG_PRESSURE 0x04 +#define MSG_STATUS_TEXT 0x05 +#define MSG_PERF_REPORT 0x06 +#define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed +#define MSG_VERSION_REQUEST 0x08 +#define MSG_VERSION 0x09 +#define MSG_EXTENDED_STATUS 0x0a +#define MSG_CPU_LOAD 0x0b + +#define MSG_COMMAND_REQUEST 0x20 +#define MSG_COMMAND_UPLOAD 0x21 +#define MSG_COMMAND_LIST 0x22 +#define MSG_COMMAND_MODE_CHANGE 0x23 +#define MSG_CURRENT_WAYPOINT 0x24 + +#define MSG_VALUE_REQUEST 0x30 +#define MSG_VALUE_SET 0x31 +#define MSG_VALUE 0x32 + +#define MSG_PID_REQUEST 0x40 +#define MSG_PID_SET 0x41 +#define MSG_PID 0x42 +#define MSG_VFR_HUD 0x4a + +#define MSG_TRIM_STARTUP 0x50 +#define MSG_TRIM_MIN 0x51 +#define MSG_TRIM_MAX 0x52 +#define MSG_RADIO_OUT 0x53 +#define MSG_RADIO_IN 0x54 + +#define MSG_RAW_IMU 0x60 +#define MSG_GPS_STATUS 0x61 +#define MSG_GPS_RAW 0x62 + +#define MSG_SERVO_OUT 0x70 + +#define MSG_PIN_REQUEST 0x80 +#define MSG_PIN_SET 0x81 + +#define MSG_DATAFLASH_REQUEST 0x90 +#define MSG_DATAFLASH_SET 0x91 + +#define MSG_EEPROM_REQUEST 0xa0 +#define MSG_EEPROM_SET 0xa1 + +#define MSG_POSITION_CORRECT 0xb0 +#define MSG_ATTITUDE_CORRECT 0xb1 +#define MSG_POSITION_SET 0xb2 +#define MSG_ATTITUDE_SET 0xb3 +#define MSG_LOCAL_LOCATION 0xb4 + +#define SEVERITY_LOW 1 +#define SEVERITY_MEDIUM 2 +#define SEVERITY_HIGH 3 +#define SEVERITY_CRITICAL 4 + +// Logging parameters +#define LOG_INDEX_MSG 0xF0 +#define LOG_ATTITUDE_MSG 0x01 +#define LOG_GPS_MSG 0x02 +#define LOG_MODE_MSG 0X03 +#define LOG_CONTROL_TUNING_MSG 0X04 +#define LOG_NAV_TUNING_MSG 0X05 +#define LOG_PERFORMANCE_MSG 0X06 +#define LOG_RAW_MSG 0x07 +#define LOG_CMD_MSG 0x08 +#define LOG_CURRENT_MSG 0x09 +#define LOG_STARTUP_MSG 0x0A +#define TYPE_AIRSTART_MSG 0x00 +#define TYPE_GROUNDSTART_MSG 0x01 +#define MAX_NUM_LOGS 50 + +#define MASK_LOG_ATTITUDE_FAST (1<<0) +#define MASK_LOG_ATTITUDE_MED (1<<1) +#define MASK_LOG_GPS (1<<2) +#define MASK_LOG_PM (1<<3) +#define MASK_LOG_CTUN (1<<4) +#define MASK_LOG_NTUN (1<<5) +#define MASK_LOG_MODE (1<<6) +#define MASK_LOG_RAW (1<<7) +#define MASK_LOG_CMD (1<<8) +#define MASK_LOG_CURRENT (1<<9) +#define MASK_LOG_SET_DEFAULTS (1<<15) + +// Waypoint Modes +// ---------------- +#define ABS_WP 0 +#define REL_WP 1 + +// Command Queues +// --------------- +#define COMMAND_MUST 0 +#define COMMAND_MAY 1 +#define COMMAND_NOW 2 + +// Events +// ------ +#define EVENT_WILL_REACH_WAYPOINT 1 +#define EVENT_SET_NEW_WAYPOINT_INDEX 2 +#define EVENT_LOADED_WAYPOINT 3 +#define EVENT_LOOP 4 + +// Climb rate calculations +#define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to regress a climb rate from + + +#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO + +#define CURRENT_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*CURR_VOLT_DIV_RATIO +#define CURRENT_AMPS(x) (x*(INPUT_VOLTAGE/1024.0))*CURR_AMP_DIV_RATIO + +#define AIRSPEED_CH 7 // The external ADC channel for the airspeed sensor +#define BATTERY_PIN1 0 // These are the pins for the voltage dividers +#define BATTERY_PIN2 1 +#define BATTERY_PIN3 2 +#define BATTERY_PIN4 3 + +#define VOLTAGE_PIN_0 0 // These are the pins for current sensor: voltage +#define CURRENT_PIN_1 1 // and current + +#define RELAY_PIN 47 + + +// sonar +#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters + +// Hardware Parameters +#define SLIDE_SWITCH_PIN 40 +#define PUSHBUTTON_PIN 41 + +#define A_LED_PIN 37 //36 = B, 37 = A, 35 = C +#define B_LED_PIN 36 +#define C_LED_PIN 35 + + +// EEPROM addresses +#define EEPROM_MAX_ADDR 4096 +// parameters get the first 1KiB of EEPROM, remainder is for waypoints +#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP +#define WP_SIZE 15 +#define ONBOARD_PARAM_NAME_LENGTH 15 +#define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe diff --git a/WaypointWriterCopterMega/event_example.h b/WaypointWriterCopterMega/event_example.h new file mode 100644 index 0000000000..1ffe8b6c91 --- /dev/null +++ b/WaypointWriterCopterMega/event_example.h @@ -0,0 +1,15 @@ +// Mission example: + +#define WP_RADIUS 3 // What is the minimum distance to reach a waypoint? +#define ALT_TO_HOLD -1 // Altitude to hold above home in meters + // Enter -1 to maintain current altitude when returning to home + + +// The mission: +float mission[][5] = { +{MAV_CMD_NAV_TAKEOFF, 0, 6, 0, 0}, // pitch 20, Altitude meters +{MAV_CMD_NAV_WAYPOINT, 0, 6, 37.776849, -122.405752}, // go to waypoint +{MAV_CMD_CONDITION_YAW, 180, 5, 1, 1}, // 180°, 5 seconds, clockwise, relative +{MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0}, // no options for RTL +{MAV_CMD_NAV_LAND, 0, 0, 0, 0}, // +}; diff --git a/WaypointWriterCopterMega/mission_example.h b/WaypointWriterCopterMega/mission_example.h new file mode 100644 index 0000000000..f2968a9296 --- /dev/null +++ b/WaypointWriterCopterMega/mission_example.h @@ -0,0 +1,35 @@ +// Mission example: + +#define WP_RADIUS 3 // What is the minimum distance to reach a waypoint? +#define ALT_TO_HOLD -1 // Altitude to hold above home in meters + // Enter -1 to maintain current altitude when returning to home + +// The mission: +float mission[][6] = { + // CMD options P1 Alt Lat Long + {MAV_CMD_NAV_TAKEOFF, 0, 0, 3.2, 0, 0}, // 1 + {MAV_CMD_NAV_WAYPOINT, 0, 0, 3.2, 37.716899, -122.381898}, // 2 turn 1 + {MAV_CMD_NAV_WAYPOINT, 0, 0, 3.2, 37.717475, -122.381394}, // 3 turn 2 + {MAV_CMD_NAV_WAYPOINT, 0, 0, 3.2, 37.717149, -122.380819}, // 4 turn 3 + {MAV_CMD_NAV_WAYPOINT, 0, 2, 3.2, 37.716592, -122.381358}, // 5 turn 4 with delay + {MAV_CMD_NAV_WAYPOINT, 0, 5, 3.2, 37.716752, -122.381632}, // 6 Land WP with delay + {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}, // 7 LAND! +}; + + +/* +command #: 0 id:16 op:0 p1:0 p2:5007 p3:377659180 p4:-1224329500 +command #: 1 id:22 op:0 p1:0 p2:220 p3:0 p4:0 +command #: 2 id:16 op:0 p1:0 p2:220 p3:377168992 p4:-1223819008 +command #: 3 id:16 op:0 p1:0 p2:220 p3:377174752 p4:-1223813888 +command #: 4 id:16 op:0 p1:0 p2:220 p3:377171488 p4:-1223808256 +command #: 5 id:16 op:0 p1:2 p2:220 p3:377165920 p4:-1223813504 +command #: 6 id:16 op:0 p1:5 p2:220 p3:377167520 p4:-1223816320 +command #: 7 id:21 op:0 p1:0 p2:0 p3:0 p4:0 + +*/ + + +/* +Sparkfun +*/ \ No newline at end of file diff --git a/WaypointWriterCopterMega/sparkfun.h b/WaypointWriterCopterMega/sparkfun.h new file mode 100644 index 0000000000..9ad88fd22b --- /dev/null +++ b/WaypointWriterCopterMega/sparkfun.h @@ -0,0 +1,53 @@ +// Mission example: + +#define WP_RADIUS 5 // What is the minimum distance to reach a waypoint? +#define ALT_TO_HOLD -1 // Altitude to hold above home in meters + +/* // Enter -1 to maintain current altitude when returning to home +// The mission: +float mission[][6] = { + // CMD options P1 Alt Lat Long + {MAV_CMD_NAV_TAKEOFF, 0, 0, 3.0, 0, 0}, // 1 + {MAV_CMD_NAV_WAYPOINT, 0, 0, 3.0, 40.065219, -105.209760}, // 2 turn 1 + {MAV_CMD_NAV_WAYPOINT, 0, 0, 3.0, 40.064561, -105.209798}, // 3 turn 2 + {MAV_CMD_NAV_WAYPOINT, 0, 0, 3.0, 40.064511, -105.210402}, // 4 turn 3 + {MAV_CMD_NAV_WAYPOINT, 0, 0, 3.0, 40.065167, -105.210453}, // 5 turn 4 with delay + {MAV_CMD_NAV_WAYPOINT, 0, 5, 3.0, 40.065189, -105.210007}, // 6 Land WP with delay + {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}, // 7 LAND! +}; +*/ +///* + // Enter -1 to maintain current altitude when returning to home +// The mission: +float mission[][6] = { + // CMD options P1 Alt Lat Long + {MAV_CMD_NAV_TAKEOFF, 0, 0, 3.0, 0, 0}, // 1 + {MAV_CMD_NAV_WAYPOINT, 0, 0, 3.0, 40.065219, -105.209760}, // turn 1 + + {MAV_CMD_NAV_WAYPOINT, 0, 4, 3.0, 40.064561, -105.209798}, // turn 2 pause + {MAV_CMD_NAV_WAYPOINT, 0, 0, 3.0, 40.064508, -105.209808}, // turn 2 + + {MAV_CMD_NAV_WAYPOINT, 0, 4, 3.0, 40.064507, -105.210303}, // turn 3 pause + {MAV_CMD_NAV_WAYPOINT, 0, 0, 3.0, 40.064524, -105.210464}, // turn 3 + + {MAV_CMD_NAV_WAYPOINT, 0, 1, 3.0, 40.065092, -105.210483}, // turn 4 pause + {MAV_CMD_NAV_WAYPOINT, 0, 0, 3.0, 40.065191, -105.210349}, // turn 4 + + {MAV_CMD_NAV_WAYPOINT, 0, 4, 3.0, 40.065189, -105.210007}, // Land WP with delay + {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}, // LAND! +}; +//*/ + +/* +// The mission: +float mission[][6] = { + // CMD options P1 Alt Lat Long + {MAV_CMD_NAV_TAKEOFF, 0, 0, 6.0, 0, 0}, // 1 + {MAV_CMD_NAV_WAYPOINT, 0, 0, 6.0, 40.065219, -105.209760}, // 2 turn 1 + {MAV_CMD_NAV_WAYPOINT, 0, 0, 6.0, 40.064561, -105.209798}, // 3 turn 2 + {MAV_CMD_NAV_WAYPOINT, 0, 0, 6.0, 40.064511, -105.210402}, // 4 turn 3 + {MAV_CMD_NAV_WAYPOINT, 0, 0, 6.0, 40.065167, -105.210453}, // 5 turn 4 with delay + {MAV_CMD_NAV_WAYPOINT, 0, 5, 6.0, 40.065189, -105.210007}, // 6 Land WP with delay + {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}, // 7 LAND! +}; +//*/ \ No newline at end of file diff --git a/WaypointWriterCopterMega/takeoff.h b/WaypointWriterCopterMega/takeoff.h new file mode 100644 index 0000000000..c6e3e527a7 --- /dev/null +++ b/WaypointWriterCopterMega/takeoff.h @@ -0,0 +1,12 @@ +// Mission example: + +#define WP_RADIUS 5 // What is the minimum distance to reach a waypoint? +#define ALT_TO_HOLD -1 // Altitude to hold above home in meters + + // Enter -1 to maintain current altitude when returning to home +// The mission: +float mission[][6] = { + // CMD options P1 Alt Lat Long + {MAV_CMD_NAV_TAKEOFF, 0, 0, 3.0, 0, 0}, // 1 + {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}, // 7 LAND! +};