static bool
test_null()
{
GlidePolar glide_polar(fixed_two);
Waypoints waypoints;
setup_waypoints(waypoints);
if (verbose)
distance_counts();
TaskEventsPrint default_events(verbose);
TaskManager task_manager(waypoints);
task_manager.SetTaskEvents(default_events);
TaskBehaviour task_behaviour = task_manager.GetTaskBehaviour();
task_behaviour.DisableAll();
task_behaviour.enable_trace = false;
task_manager.SetTaskBehaviour(task_behaviour);
task_manager.SetGlidePolar(glide_polar);
task_report(task_manager, "null");
waypoints.Clear(); // clear waypoints so abort wont do anything
autopilot_parms.goto_target = true;
return run_flight(task_manager, autopilot_parms, 0);
}
static bool
test_goto(int n_wind, unsigned id, bool auto_mc)
{
GlidePolar glide_polar(fixed_two);
Waypoints waypoints;
setup_waypoints(waypoints);
if (verbose)
distance_counts();
TaskEventsPrint default_events(verbose);
TaskManager task_manager(waypoints);
task_manager.SetTaskEvents(default_events);
TaskBehaviour task_behaviour = task_manager.GetTaskBehaviour();
task_behaviour.DisableAll();
task_behaviour.auto_mc = auto_mc;
task_behaviour.enable_trace = false;
task_manager.SetTaskBehaviour(task_behaviour);
task_manager.SetGlidePolar(glide_polar);
test_task(task_manager, waypoints, 1);
task_manager.DoGoto(*waypoints.LookupId(id));
task_report(task_manager, "goto");
waypoints.Clear(); // clear waypoints so abort wont do anything
autopilot_parms.goto_target = true;
return run_flight(task_manager, autopilot_parms, n_wind);
}
static bool
test_abort(int n_wind)
{
GlidePolar glide_polar(fixed_two);
Waypoints waypoints;
setup_waypoints(waypoints);
if (verbose)
distance_counts();
TaskEventsPrint default_events(verbose);
TaskManager task_manager(waypoints);
task_manager.SetTaskEvents(default_events);
TaskBehaviour task_behaviour = task_manager.GetTaskBehaviour();
task_behaviour.DisableAll();
task_behaviour.enable_trace = false;
task_manager.SetTaskBehaviour(task_behaviour);
task_manager.SetGlidePolar(glide_polar);
test_task(task_manager, waypoints, 1);
task_manager.Abort();
task_report(task_manager, "abort");
autopilot_parms.goto_target = true;
return run_flight(task_manager, autopilot_parms, n_wind);
}
void setup()
{
init_ardupilot();
Init_servo();//Initalizing servo, see "Servo_Control" tab.
//Testing servos (max and mins), we are aware of the propeller and won't spin that without warning... =)
//test_throttle();
test_yaw();
setup_waypoints();//See tab "Mission_Setup"
init_startup_parameters(); //
}
int main(int argc, char** argv)
{
if (!parse_args(argc,argv)) {
return 0;
}
plan_tests(16);
Waypoints waypoints;
ok(setup_waypoints(waypoints),"waypoint setup",0);
unsigned size = waypoints.size();
ok(test_lookup(waypoints,3),"waypoint lookup",0);
ok(!test_lookup(waypoints,5000),"waypoint bad lookup",0);
ok(test_nearest(waypoints),"waypoint nearest",0);
ok(test_nearest_landable(waypoints),"waypoint nearest landable",0);
ok(test_location(waypoints,true),"waypoint location good",0);
ok(test_location(waypoints,false),"waypoint location bad",0);
ok(test_range(waypoints,100)==1,"waypoint visit range 100m",0);
ok(test_radius(waypoints,100)==1,"waypoint radius 100m",0);
ok(test_range(waypoints,500000)== waypoints.size(),"waypoint range 500000m",0);
ok(test_radius(waypoints,25000)<= test_range(waypoints,25000),"waypoint radius<range",0);
// test clear
waypoints.clear();
ok(waypoints.size()==0,"waypoint clear",0);
setup_waypoints(waypoints);
ok(size == waypoints.size(),"waypoint setup after clear",0);
ok(test_copy(waypoints),"waypoint copy",0);
ok(test_erase(waypoints,3),"waypoint erase",0);
ok(test_replace(waypoints,4),"waypoint replace",0);
return exit_status();
}
bool
test_flight(int test_num, int n_wind, const double speed_factor,
const bool auto_mc)
{
// multipurpose flight test
GlidePolar glide_polar(fixed_two);
Waypoints waypoints;
setup_waypoints(waypoints);
if (verbose)
distance_counts();
TaskEventsPrint default_events(verbose);
TaskManager task_manager(default_events, waypoints);
task_manager.set_glide_polar(glide_polar);
task_manager.get_ordered_task_behaviour().aat_min_time = aat_min_time(test_num);
TaskBehaviour task_behaviour = task_manager.get_task_behaviour();
task_behaviour.enable_trace = false;
task_behaviour.auto_mc = auto_mc;
task_behaviour.calc_glide_required = false;
if ((test_num == 0) || (test_num == 2))
task_behaviour.optimise_targets_bearing = false;
task_manager.set_task_behaviour(task_behaviour);
bool goto_target = false;
switch (test_num) {
case 0:
case 2:
case 7:
goto_target = true;
break;
};
autopilot_parms.goto_target = goto_target;
test_task(task_manager, waypoints, test_num);
waypoints.clear(); // clear waypoints so abort wont do anything
return run_flight(task_manager, autopilot_parms, n_wind,
speed_factor);
}
int main(int argc, char** argv)
{
if (!parse_args(argc,argv)) {
return 0;
}
#define NUM_RANDOM 50
#define NUM_TYPE_MANIPS 50
plan_tests(NUM_TASKS+2+NUM_RANDOM+8+NUM_TYPE_MANIPS);
GlidePolar glide_polar(fixed_two);
Waypoints waypoints;
setup_waypoints(waypoints);
{
TaskManager task_manager(waypoints);
task_manager.SetGlidePolar(glide_polar);
test_task_bad(task_manager,waypoints);
}
{
for (unsigned i = 0; i < NUM_TYPE_MANIPS; i++) {
TaskManager task_manager(waypoints);
ok(test_task_type_manip(task_manager,waypoints, i+2), "construction: task type manip", 0);
}
}
for (int i=0; i<NUM_TASKS+2; i++) {
TaskManager task_manager(waypoints);
task_manager.SetGlidePolar(glide_polar);
ok(test_task(task_manager, waypoints, i),test_name("construction",i,0),0);
}
for (int i=0; i<NUM_RANDOM; i++) {
TaskManager task_manager(waypoints);
task_manager.SetGlidePolar(glide_polar);
ok(test_task(task_manager, waypoints, 7),test_name("construction",7,0),0);
}
return exit_status();
}
int main(int argc, char** argv) {
// default arguments
verbose=1;
if (!parse_args(argc,argv)) {
return 0;
}
TaskBehaviour task_behaviour;
TaskEventsPrint default_events(verbose);
GlidePolar glide_polar(fixed_two);
Waypoints waypoints;
setup_waypoints(waypoints);
TaskManager task_manager(default_events,
task_behaviour,
waypoints);
task_manager.set_glide_polar(glide_polar);
test_task(task_manager, waypoints, 0);
plan_tests(1);
ok(test_edit(task_manager, task_behaviour),
"edit task", 0);
/*
plan_tests(task_manager.task_size());
// here goes, example, edit all task points
SafeTaskEdit ste(task_manager, waypoints);
for (unsigned i=0; i<task_manager.task_size(); i++) {
ok(ste.edit(i),"edit tp",0);
task_report(task_manager, "edit tp\n");
}
*/
return exit_status();
}
int main() {
//host.baud(115200);
xbee.baud(115200);
Ticker heartbeat_tkr;
heartbeat_tkr.attach_us(&beat, HEARTBEAT_UPDATE_RATE * 1000);
autopilotRemote.rise(&autopilot_ISR);
//i2c.frequency(400);
// Initialise PIDs
speedOverGroundPid.setInputLimits(0.0, SPEED_IN_KNOTS_LIMIT); // Assuming speed is in knots -- check this!
speedOverGroundPid.setOutputLimits(1, THROTTLE_LIMIT);
speedOverGroundPid.setMode(AUTO_MODE);
headingPid.setInputLimits(-180,180);
headingPid.setOutputLimits(0.0, THROTTLE_LIMIT);
headingPid.setMode(AUTO_MODE);
speedOverGroundPid.setSetPoint(2.5);
headingPid.setSetPoint(0);
DEBUG_OUTPUT(" _ _ _ _ \r\n"
" | | | | | | | | \r\n"
" ___| |__ ___ __| | | |__ ___ __ _| |_ \r\n"
"/ __| '_ \\ / _ \\/ _` | | '_ \\ / _ \\ / _` | __|\r\n"
"\\__ \\ | | | __/ (_| | | |_) | (_) | (_| | |_ \r\n"
"|___/_| |_|\\___|\\__,_| |_.__/ \\___/ \\__,_|\\__|\r\n"
" ______ \r\n"
" |______|\r\n"
"\r\n\n");
NMEA::init();
compass.init();
// Parkwood: 51.298997, 1.056683
// Chestfield: 51.349215, 1.066184
// Initialise Motors (Arming Sequence) -- If a value is not sent periodicially, then the motors WILL disarm!
leftMotor.set(0);
rightMotor.set(0);
wait(1);
setup_waypoints();
// Initialise Scheduler
Ticker gpsTelemetryUpdateTkr;
Ticker systemTelemetryUpdateTkr;
Ticker debugTelemetryUpdateTkr;
Ticker trackandSpeedUpdateTkr;
Ticker motorUpdateTkr;
gpsTelemetryUpdateTkr.attach_us(&gps_telemetry_update_ISR, GPS_TELEMETRY_UPDATE_RATE * 1000);
systemTelemetryUpdateTkr.attach_us(&system_telemetry_update_ISR, SYSTEM_TELEMETRY_UPDATE_RATE * 1000);
debugTelemetryUpdateTkr.attach_us(&debug_telemetry_update_ISR, DEBUG_TELEMETRY_UPDATE_RATE * 1000);
trackandSpeedUpdateTkr.attach_us(&track_and_speed_update_ISR, TRACK_UPDATE_RATE * 1000);
motorUpdateTkr.attach_us(&motor_update_ISR, MOTOR_UPDATE_RATE * 1000);
while(1) {
if(updateTrackAndSpeed && autopilotEngaged){
update_speed_and_heading();
updateTrackAndSpeed = false;
}
if(updateMotor && autopilotEngaged){
update_motors();
updateMotor = false;
}
if(updateGPSTelemetry){
gps_satellite_telemetry();
updateGPSTelemetry = false;
}
if(updateSystemTelemetry){
send_system_telemetry();
updateSystemTelemetry = false;
}
if(updateDebugTelemetry){
send_debug_telemetry();
updateDebugTelemetry = false;
}
}
}
