ardupilot/archive/WaypointWriterMega/WaypointWriterCopterMega.pde

109 lines
2.7 KiB
Plaintext

/*
Arducopter 2 Waypoint writer
Use this release to manually upload waypoints
*/
#include "defines.h"
#include <FastSerial.h>
#include <AP_Common.h>
#include <GCS_MAVLink.h> // 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 <avr/io.h>
#include <avr/eeprom.h>
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);
}