diff --git a/ArduCopterMega/.cproject b/ArduCopterMega/.cproject
new file mode 100644
index 0000000000..b27b2109a4
--- /dev/null
+++ b/ArduCopterMega/.cproject
@@ -0,0 +1,970 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ make
+
+
+ true
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ make
+
+
+ true
+ true
+ true
+
+
+
+
+
+
+
+
+
diff --git a/ArduCopterMega/.project b/ArduCopterMega/.project
new file mode 100644
index 0000000000..a70da34626
--- /dev/null
+++ b/ArduCopterMega/.project
@@ -0,0 +1,84 @@
+
+
+ ArduCopterMega
+
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.genmakebuilder
+ clean,full,incremental,
+
+
+ ?name?
+
+
+
+ org.eclipse.cdt.make.core.append_environment
+ true
+
+
+ org.eclipse.cdt.make.core.autoBuildTarget
+ all
+
+
+ org.eclipse.cdt.make.core.buildArguments
+
+
+
+ org.eclipse.cdt.make.core.buildCommand
+ make
+
+
+ org.eclipse.cdt.make.core.buildLocation
+ ${workspace_loc:/ArduCopterMega/Debug}
+
+
+ org.eclipse.cdt.make.core.cleanBuildTarget
+ clean
+
+
+ org.eclipse.cdt.make.core.contents
+ org.eclipse.cdt.make.core.activeConfigSettings
+
+
+ org.eclipse.cdt.make.core.enableAutoBuild
+ false
+
+
+ org.eclipse.cdt.make.core.enableCleanBuild
+ true
+
+
+ org.eclipse.cdt.make.core.enableFullBuild
+ true
+
+
+ org.eclipse.cdt.make.core.fullBuildTarget
+ all
+
+
+ org.eclipse.cdt.make.core.stopOnError
+ true
+
+
+ org.eclipse.cdt.make.core.useDefaultBuildCmd
+ true
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder
+ full,incremental,
+
+
+
+
+
+ org.eclipse.cdt.core.cnature
+ org.eclipse.cdt.core.ccnature
+ org.eclipse.cdt.managedbuilder.core.managedBuildNature
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigNature
+ de.innot.avreclipse.core.avrnature
+
+
diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde
index ea201a4d38..ca34f2a8b5 100644
--- a/ArduCopterMega/ArduCopterMega.pde
+++ b/ArduCopterMega/ArduCopterMega.pde
@@ -27,17 +27,17 @@ version 2.1 of the License, or (at your option) any later version.
#include
#include // ArduPilot Mega RC Library
#include // RC Channel Library
-#include // ArduPilot Mega Analog to Digital Converter Library
+#include // ArduPilot Mega Analog to Digital Converter Library
#include // ArduPilot GPS library
#include // Arduino I2C lib
-#include // ArduPilot Mega BMP085 Library
+#include // ArduPilot Mega BMP085 Library
#include // ArduPilot Mega Flash Memory Library
-#include // ArduPilot Mega Magnetometer Library
+#include // ArduPilot Mega Magnetometer Library
#include // ArduPilot Mega Vector/Matrix math Library
#include // ArduPilot Mega IMU Library
#include // ArduPilot Mega DCM Library
#include // ArduPilot Mega RC Library
-//#include // MAVLink GCS definitions
+#include // MAVLink GCS definitions
// Configuration
@@ -47,6 +47,7 @@ version 2.1 of the License, or (at your option) any later version.
#include "defines.h"
#include "Parameters.h"
#include "global_data.h"
+#include "GCS.h"
////////////////////////////////////////////////////////////////////////////////
// Serial ports
@@ -107,17 +108,18 @@ AP_GPS_None GPS(NULL);
# error Must define GPS_PROTOCOL in your configuration file.
#endif
*/
+
+GPS *g_gps;
#if GPS_PROTOCOL == GPS_PROTOCOL_NONE
-AP_GPS_None gps(NULL);
+AP_GPS_None GPS(NULL);
#else
-GPS *gps;
-AP_GPS_Auto GPS(&Serial1, &gps);
+AP_GPS_Auto GPS(&Serial1, &g_gps);
#endif // GPS PROTOCOL
AP_IMU_Oilpan imu(&adc, EE_IMU_OFFSET);
-AP_DCM dcm(&imu, &GPS);
+AP_DCM dcm(&imu, g_gps);
////////////////////////////////////////////////////////////////////////////////
// GCS selection
@@ -155,7 +157,7 @@ const char* flight_mode_strings[] = {
"LAND"};
/* Radio values
- Channel assignments
+ Channel assignments
1 Ailerons (rudder if no ailerons)
2 Elevator
3 Throttle
@@ -199,7 +201,7 @@ float scaleLongDown = 1; // used to reverse longtitude scaling
// ----------------------
Vector3f mag_offsets;
-// Location & Navigation
+// Location & Navigation
// ---------------------
const float radius_of_earth = 6378100; // meters
const float gravity = 9.81; // meters/ sec^2
@@ -229,7 +231,7 @@ float cos_yaw_x;
// Airspeed
// --------
int airspeed; // m/s * 100
-float airspeed_error; // m / s * 100
+float airspeed_error; // m / s * 100
// Throttle Failsafe
// ------------------
@@ -243,7 +245,7 @@ boolean motor_auto_safe = false;
// Location Errors
// ---------------
-long bearing_error; // deg * 100 : 0 to 36000
+long bearing_error; // deg * 100 : 0 to 36000
long altitude_error; // meters * 100 we are off in altitude
float crosstrack_error; // meters we are off trackline
long distance_error; // distance to the WP
@@ -291,10 +293,10 @@ byte altitude_sensor = BARO; // used to know whic sensor is active, BARO or
// --------------------
boolean takeoff_complete = false; // Flag for using take-off controls
boolean land_complete = false;
-int takeoff_altitude;
+int takeoff_altitude;
int landing_distance; // meters;
long old_alt; // used for managing altitude rates
-int velocity_land;
+int velocity_land;
// Loiter management
// -----------------
@@ -340,7 +342,7 @@ int event_delay; // how long to delay the next firing of event in millis
int event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice
int event_value; // per command value, such as PWM for servos
int event_undo_value; // the value used to undo commands
-byte repeat_forever;
+byte repeat_forever;
byte undo_event; // counter for timing the undo
// delay command
@@ -356,9 +358,9 @@ struct Location current_loc; // current location
struct Location next_WP; // next waypoint
struct Location tell_command; // command for telemetry
struct Location next_command; // command preloaded
-long target_altitude; // used for
-long offset_altitude; // used for
-boolean home_is_set; // Flag for if we have gps lock and have set the home location
+long target_altitude; // used for
+long offset_altitude; // used for
+boolean home_is_set; // Flag for if we have g_gps lock and have set the home location
// IMU variables
@@ -419,7 +421,7 @@ void setup() {
void loop()
{
- // We want this to execute at 100Hz
+ // We want this to execute at 100Hz
// --------------------------------
if (millis() - fast_loopTimer > 9) {
delta_ms_fast_loop = millis() - fast_loopTimer;
@@ -433,12 +435,12 @@ void loop()
fast_loop();
fast_loopTimeStamp = millis();
}
-
+
if (millis() - medium_loopTimer > 19) {
delta_ms_medium_loop = millis() - medium_loopTimer;
- medium_loopTimer = millis();
+ medium_loopTimer = millis();
medium_loop();
-
+
if (millis() - perf_mon_timer > 20000) {
if (mainLoop_count != 0) {
gcs.send_message(MSG_PERF_REPORT);
@@ -459,9 +461,9 @@ void fast_loop()
// This is the fast loop - we want it to execute at >= 100Hz
// ---------------------------------------------------------
- if (delta_ms_fast_loop > G_Dt_max)
+ if (delta_ms_fast_loop > G_Dt_max)
G_Dt_max = delta_ms_fast_loop;
-
+
// custom code/exceptions for flight modes
// ---------------------------------------
update_current_flight_mode();
@@ -477,27 +479,27 @@ void medium_loop()
// ----------
read_radio(); // read the radio first
-
+
// reads all of the necessary trig functions for cameras, throttle, etc.
update_trig();
-
+
// This is the start of the medium (10 Hz) loop pieces
// -----------------------------------------
switch(medium_loopCounter) {
-
+
// This case deals with the GPS
//-------------------------------
case 0:
medium_loopCounter++;
update_GPS();
//readCommands();
-
+
if(g.compass_enabled){
compass.read(); // Read magnetometer
compass.calculate(dcm.roll, dcm.pitch); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
}
-
+
break;
// This case performs some navigation computations
@@ -505,16 +507,16 @@ void medium_loop()
case 1:
medium_loopCounter++;
- if(gps->new_data){
- gps->new_data = false;
+ if(g_gps->new_data){
+ g_gps->new_data = false;
dTnav = millis() - nav_loopTimer;
nav_loopTimer = millis();
// calculate the plane's desired bearing
- // -------------------------------------
+ // -------------------------------------
navigate();
}
-
+
// calc pitch and roll to target
// -----------------------------
dTnav2 = millis() - nav2_loopTimer;
@@ -527,7 +529,7 @@ void medium_loop()
//-------------------
case 2:
medium_loopCounter++;
-
+
// Read Baro pressure
// ------------------
read_barometer();
@@ -545,7 +547,7 @@ void medium_loop()
//-------------------------------------------------
case 3:
medium_loopCounter++;
-
+
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED && (g.log_bitmask & MASK_LOG_ATTITUDE_FAST == 0))
Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
@@ -557,41 +559,41 @@ void medium_loop()
if (g.log_bitmask & MASK_LOG_GPS){
if(home_is_set)
- Log_Write_GPS(gps->time, current_loc.lat, current_loc.lng, gps->altitude, current_loc.alt, (long) gps->ground_speed, gps->ground_course, gps->fix, gps->num_sats);
+ Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats);
}
gcs.send_message(MSG_ATTITUDE); // Sends attitude data
break;
-
+
// This case controls the slow loop
//---------------------------------
case 4:
if (g.current_enabled){
read_current();
}
-
+
// shall we trim the copter?
// ------------------------
read_trim_switch();
-
+
// shall we check for engine start?
// --------------------------------
arm_motors();
-
+
medium_loopCounter = 0;
slow_loop();
break;
-
+
default:
medium_loopCounter = 0;
break;
}
-
+
// stuff that happens at 50 hz
// ---------------------------
-
+
// use Yaw to find our bearing error
calc_bearing_error();
-
+
// guess how close we are - fixed observer calc
calc_distance_error();
@@ -600,15 +602,15 @@ void medium_loop()
if (g.log_bitmask & MASK_LOG_RAW)
Log_Write_Raw();
-
+
#if GCS_PROTOCOL == 6 // This is here for Benjamin Pelletier. Please do not remove without checking with me. Doug W
readgcsinput();
#endif
-
+
#if ENABLE_CAM
camera_stabilization();
#endif
-
+
// kick the GCS to process uplink data
gcs.update();
}
@@ -621,40 +623,40 @@ void slow_loop()
switch (slow_loopCounter){
case 0:
slow_loopCounter++;
-
+
superslow_loopCounter++;
if(superslow_loopCounter == 30) {
-
+
// save current data to the flash
if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current();
-
+
}else if(superslow_loopCounter >= 400) {
compass.save_offsets();
//eeprom_write_word((uint16_t *) EE_LAST_LOG_PAGE, DataFlash.GetWritePage());
superslow_loopCounter = 0;
}
break;
-
+
case 1:
slow_loopCounter++;
// Read 3-position switch on radio
// -------------------------------
- read_control_switch();
-
+ read_control_switch();
+
// Read main battery voltage if hooked up - does not read the 5v from radio
// ------------------------------------------------------------------------
#if BATTERY_EVENT == 1
read_battery();
#endif
-
+
break;
-
+
case 2:
slow_loopCounter = 0;
update_events();
-
+
// XXX this should be a "GCS slow loop" interface
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(1,5);
@@ -662,9 +664,9 @@ void slow_loop()
// between 1 and 5 Hz
#else
gcs.send_message(MSG_LOCATION);
- gcs.send_message(MSG_CPU_LOAD, load*100);
+// XXX gcs.send_message(MSG_CPU_LOAD, load*100);
#endif
-
+
gcs.send_message(MSG_HEARTBEAT); // XXX This is running at 3 1/3 Hz instead of 1 Hz
break;
@@ -678,94 +680,94 @@ void slow_loop()
void update_GPS(void)
{
- gps->update();
+ g_gps->update();
update_GPS_light();
-
+
// !!! comment out after testing
//fake_out_gps();
-
- //if (gps->new_data && gps->fix) {
- if (gps->new_data){
+
+ //if (g_gps->new_data && g_gps->fix) {
+ if (g_gps->new_data){
send_message(MSG_LOCATION);
// for performance
// ---------------
gps_fix_count++;
-
+
//Serial.printf("gs: %d\n", (int)ground_start_count);
if(ground_start_count > 1){
ground_start_count--;
-
+
} else if (ground_start_count == 1) {
-
+
// We countdown N number of good GPS fixes
// so that the altitude is more accurate
// -------------------------------------
if (current_loc.lat == 0) {
SendDebugln("!! bad loc");
ground_start_count = 5;
-
+
} else {
//Serial.printf("init Home!");
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
-
+
// reset our nav loop timer
nav_loopTimer = millis();
init_home();
// init altitude
- current_loc.alt = gps->altitude;
+ current_loc.alt = g_gps->altitude;
ground_start_count = 0;
}
}
/* disabled for now
- // baro_offset is an integrator for the gps altitude error
- baro_offset += altitude_gain * (float)(gps->altitude - current_loc.alt);
+ // baro_offset is an integrator for the g_gps altitude error
+ baro_offset += altitude_gain * (float)(g_gps->altitude - current_loc.alt);
*/
-
- current_loc.lng = gps->longitude; // Lon * 10 * *7
- current_loc.lat = gps->latitude; // Lat * 10 * *7
+
+ current_loc.lng = g_gps->longitude; // Lon * 10 * *7
+ current_loc.lat = g_gps->latitude; // Lat * 10 * *7
/*Serial.printf_P(PSTR("Lat: %.7f, Lon: %.7f, Alt: %dm, GSP: %d COG: %d, dist: %d, #sats: %d\n"),
- ((float)gps->latitude / T7),
- ((float)gps->longitude / T7),
- (int)gps->altitude / 100,
- (int)gps->ground_speed / 100,
- (int)gps->ground_course / 100,
+ ((float)g_gps->latitude / T7),
+ ((float)g_gps->longitude / T7),
+ (int)g_gps->altitude / 100,
+ (int)g_gps->ground_speed / 100,
+ (int)g_gps->ground_course / 100,
(int)wp_distance,
- (int)gps->num_sats);
+ (int)g_gps->num_sats);
*/
}else{
//Serial.println("No fix");
}
}
-
+
void update_current_flight_mode(void)
{
if(control_mode == AUTO){
-
+
switch(command_must_ID){
//case MAV_CMD_NAV_TAKEOFF:
// break;
-
+
//case MAV_CMD_NAV_LAND:
// break;
-
+
default:
-
+
// based on altitude error
// -----------------------
calc_nav_throttle();
// Output Pitch, Roll, Yaw and Throttle
- // ------------------------------------
+ // ------------------------------------
auto_yaw();
-
+
// mix in user control
control_nav_mixer();
-
+
// perform stabilzation
output_stabilize_roll();
output_stabilize_pitch();
@@ -774,9 +776,9 @@ void update_current_flight_mode(void)
output_auto_throttle();
break;
}
-
+
}else{
-
+
switch(control_mode){
case ACRO:
// clear any AP naviagtion values
@@ -788,16 +790,16 @@ void update_current_flight_mode(void)
// Yaw control
output_manual_yaw();
-
+
// apply throttle control
output_manual_throttle();
-
+
// mix in user control
control_nav_mixer();
// perform rate or stabilzation
// ----------------------------
-
+
// Roll control
if(abs(g.rc_1.control_in) >= ACRO_RATE_TRIGGER){
output_rate_roll(); // rate control yaw
@@ -812,7 +814,7 @@ void update_current_flight_mode(void)
output_stabilize_pitch(); // hold yaw
}
break;
-
+
case STABILIZE:
// clear any AP naviagtion values
nav_pitch = 0;
@@ -823,10 +825,10 @@ void update_current_flight_mode(void)
// Yaw control
output_manual_yaw();
-
+
// apply throttle control
output_manual_throttle();
-
+
// mix in user control
control_nav_mixer();
@@ -834,11 +836,11 @@ void update_current_flight_mode(void)
output_stabilize_roll();
output_stabilize_pitch();
break;
-
+
case FBW:
// we are currently using manual throttle during alpha testing.
fbw_timer++;
-
+
//call at 5 hz
if(fbw_timer > 20){
fbw_timer = 0;
@@ -853,27 +855,27 @@ void update_current_flight_mode(void)
// set dTnav manually
dTnav = 200;
}
-
- next_WP.lng = home.lng + g.rc_1.control_in / 2; // X: 4500 / 2 = 2250 = 25 meteres
+
+ next_WP.lng = home.lng + g.rc_1.control_in / 2; // X: 4500 / 2 = 2250 = 25 meteres
// forward is negative so we reverse it to get a positive North translation
- next_WP.lat = home.lat - g.rc_2.control_in / 2; // Y: 4500 / 2 = 2250 = 25 meteres
+ next_WP.lat = home.lat - g.rc_2.control_in / 2; // Y: 4500 / 2 = 2250 = 25 meteres
}
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
-
+
// REMOVE AFTER TESTING !!!
//nav_yaw = dcm.yaw_sensor;
-
+
// Yaw control
output_manual_yaw();
-
+
// apply throttle control
output_manual_throttle();
// apply nav_pitch and nav_roll to output
fbw_nav_mixer();
-
+
// perform stabilzation
output_stabilize_roll();
output_stabilize_pitch();
@@ -883,27 +885,27 @@ void update_current_flight_mode(void)
// clear any AP naviagtion values
nav_pitch = 0;
nav_roll = 0;
-
+
//if(g.rc_3.control_in)
// get desired height from the throttle
next_WP.alt = home.alt + (g.rc_3.control_in * 4); // 0 - 1000 (40 meters)
-
+
// !!! testing
//next_WP.alt -= 500;
-
+
// Yaw control
// -----------
output_manual_yaw();
-
+
// based on altitude error
// -----------------------
calc_nav_throttle();
-
+
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
// apply throttle control
output_auto_throttle();
-
+
// mix in user control
control_nav_mixer();
@@ -911,8 +913,8 @@ void update_current_flight_mode(void)
output_stabilize_roll();
output_stabilize_pitch();
break;
-
- case RTL:
+
+ case RTL:
// based on altitude error
// -----------------------
calc_nav_throttle();
@@ -920,7 +922,7 @@ void update_current_flight_mode(void)
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
auto_yaw();
-
+
// apply throttle control
output_auto_throttle();
@@ -931,21 +933,21 @@ void update_current_flight_mode(void)
output_stabilize_roll();
output_stabilize_pitch();
break;
-
+
case POSITION_HOLD:
// Yaw control
// -----------
output_manual_yaw();
-
+
// based on altitude error
// -----------------------
calc_nav_throttle();
-
-
+
+
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
-
+
// apply throttle control
output_auto_throttle();
@@ -960,7 +962,7 @@ void update_current_flight_mode(void)
default:
//Serial.print("$");
break;
-
+
}
}
}
@@ -976,10 +978,10 @@ void update_navigation()
verify_must();
verify_may();
}else{
- switch(control_mode){
+ switch(control_mode){
case RTL:
update_crosstrack();
- break;
+ break;
}
}
}
@@ -991,7 +993,7 @@ void read_AHRS(void)
//-----------------------------------------------
dcm.update_DCM(G_Dt);
omega = dcm.get_gyro();
-
+
// Testing remove !!!
//dcm.pitch_sensor = 0;
//dcm.roll_sensor = 0;
@@ -1000,17 +1002,17 @@ void read_AHRS(void)
void update_trig(void){
Vector2f yawvector;
Matrix3f temp = dcm.get_dcm_matrix();
-
+
yawvector.x = temp.a.x; // sin
yawvector.y = temp.b.x; // cos
yawvector.normalize();
cos_yaw_x = yawvector.y; // 0 x = north
- sin_yaw_y = yawvector.x; // 1 y
-
+ sin_yaw_y = yawvector.x; // 1 y
+
sin_pitch_y = -temp.c.x;
cos_pitch_x = sqrt(1 - (temp.c.x * temp.c.x));
-
+
cos_roll_x = temp.c.z / cos_pitch_x;
sin_roll_y = temp.c.y / cos_pitch_x;
-}
\ No newline at end of file
+}
diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde
index 72e5852203..51092f5dc6 100644
--- a/ArduCopterMega/Attitude.pde
+++ b/ArduCopterMega/Attitude.pde
@@ -3,8 +3,8 @@ void init_pids()
{
// create limits to how much dampening we'll allow
// this creates symmetry with the P gain value preventing oscillations
-
- max_stabilize_dampener = g.pid_stabilize_roll.kP() * 2500; // = 0.6 * 2500 = 1500 or 15°
+
+ max_stabilize_dampener = g.pid_stabilize_roll.kP() * 2500; // = 0.6 * 2500 = 1500 or 15°
max_yaw_dampener = g.pid_yaw.kP() * 6000; // = .5 * 6000 = 3000
}
@@ -29,14 +29,14 @@ void output_stabilize_roll()
{
float error, rate;
int dampener;
-
- error = g.rc_1.servo_out - dcm.roll_sensor;
-
+
+ error = g.rc_1.servo_out - dcm.roll_sensor;
+
// limit the error we're feeding to the PID
error = constrain(error, -2500, 2500);
// write out angles back to servo out - this will be converted to PWM by RC_Channel
- g.rc_1.servo_out = g.g.pid_stabilize_roll.get_pid(error, delta_ms_fast_loop, 1.0);
+ g.rc_1.servo_out = g.pid_stabilize_roll.get_pid(error, delta_ms_fast_loop, 1.0);
// We adjust the output by the rate of rotation:
// Rate control through bias corrected gyro rates
@@ -52,9 +52,9 @@ void output_stabilize_pitch()
{
float error, rate;
int dampener;
-
+
error = g.rc_2.servo_out - dcm.pitch_sensor;
-
+
// limit the error we're feeding to the PID
error = constrain(error, -2500, 2500);
@@ -94,16 +94,16 @@ void output_yaw_with_hold(boolean hold)
hold = false;
}
}
-
+
}else{
// rate control
-
- // this indicates we are under rate control, when we enter Yaw Hold and
- // return to 0° per second, we exit rate control and hold the current Yaw
+
+ // this indicates we are under rate control, when we enter Yaw Hold and
+ // return to 0° per second, we exit rate control and hold the current Yaw
rate_yaw_flag = true;
yaw_error = 0;
}
-
+
if(hold){
// try and hold the current nav_yaw setting
yaw_error = nav_yaw - dcm.yaw_sensor; // +- 60°
@@ -114,15 +114,15 @@ void output_yaw_with_hold(boolean hold)
// Apply PID and save the new angle back to RC_Channel
g.rc_4.servo_out = g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); // .5 * 6000 = 3000
-
+
// We adjust the output by the rate of rotation
long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377
int dampener = ((float)rate * g.hold_yaw_dampener); // 18000 * .17 = 3000
-
+
// Limit dampening to be equal to propotional term for symmetry
g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000
-
- }else{
+
+ }else{
// rate control
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun!
@@ -162,7 +162,7 @@ void output_rate_pitch()
g.rc_1.servo_out = g.rc_2.control_in;
g.rc_2.servo_out = g.rc_2.control_in;
-
+
// Rate control through bias corrected gyro rates
// omega is the raw gyro reading plus Omega_I, so it´s bias corrected
g.rc_1.servo_out -= (omega.x * 5729.57795 * acro_dampener);
diff --git a/ArduCopterMega/Camera.pde b/ArduCopterMega/Camera.pde
index e467e92de4..69f92546b4 100644
--- a/ArduCopterMega/Camera.pde
+++ b/ArduCopterMega/Camera.pde
@@ -4,7 +4,7 @@ void init_camera()
g.rc_camera_pitch.radio_min = g.rc_6.radio_min;
g.rc_camera_pitch.radio_trim = g.rc_6.radio_trim;
g.rc_camera_pitch.radio_max = g.rc_6.radio_max;
-
+
g.rc_camera_roll.set_angle(4500);
g.rc_camera_roll.radio_min = 1000;
g.rc_camera_roll.radio_trim = 1500;
@@ -17,7 +17,7 @@ camera_stabilization()
g.rc_camera_pitch.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here.
// allow control mixing
- g.rc_camera_pitch.servo_out = rc_camera_pitch.control_mix(dcm.pitch_sensor / 2);
+ g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(dcm.pitch_sensor / 2);
// dont allow control mixing
//rc_camera_pitch.servo_out = dcm.pitch_sensor / 2;
@@ -34,6 +34,6 @@ camera_stabilization()
//If you want to do control mixing use this function.
// set servo_out to the control input from radio
//rc_camera_roll = g.rc_2.control_mix(dcm.pitch_sensor);
- //rc_camera_roll.calc_pwm();
+ //rc_camera_roll.calc_pwm();
}
diff --git a/ArduCopterMega/GCS.h b/ArduCopterMega/GCS.h
new file mode 100644
index 0000000000..5677e47fbc
--- /dev/null
+++ b/ArduCopterMega/GCS.h
@@ -0,0 +1,173 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file GCS.h
+/// @brief Interface definition for the various Ground Control System protocols.
+
+#ifndef __GCS_H
+#define __GCS_H
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+///
+/// @class GCS
+/// @brief Class describing the interface between the APM code
+/// proper and the GCS implementation.
+///
+/// GCS' are currently implemented inside the sketch and as such have
+/// access to all global state. The sketch should not, however, call GCS
+/// internal functions - all calls to the GCS should be routed through
+/// this interface (or functions explicitly exposed by a subclass).
+///
+class GCS_Class
+{
+public:
+
+ /// Startup initialisation.
+ ///
+ /// This routine performs any one-off initialisation required before
+ /// GCS messages are exchanged.
+ ///
+ /// @note The stream is expected to be set up and configured for the
+ /// correct bitrate before ::init is called.
+ ///
+ /// @note The stream is currently BetterStream so that we can use the _P
+ /// methods; this may change if Arduino adds them to Print.
+ ///
+ /// @param port The stream over which messages are exchanged.
+ ///
+ void init(BetterStream *port) { _port = port; }
+
+ /// Update GCS state.
+ ///
+ /// This may involve checking for received bytes on the stream,
+ /// or sending additional periodic messages.
+ void update(void) {}
+
+ /// Send a message with a single numeric parameter.
+ ///
+ /// This may be a standalone message, or the GCS driver may
+ /// have its own way of locating additional parameters to send.
+ ///
+ /// @param id ID of the message to send.
+ /// @param param Explicit message parameter.
+ ///
+ void send_message(uint8_t id, int32_t param = 0) {}
+
+ /// Send a text message.
+ ///
+ /// @param severity A value describing the importance of the message.
+ /// @param str The text to be sent.
+ ///
+ void send_text(uint8_t severity, const char *str) {}
+
+ /// Send acknowledgement for a message.
+ ///
+ /// @param id The message ID being acknowledged.
+ /// @param sum1 Checksum byte 1 from the message being acked.
+ /// @param sum2 Checksum byte 2 from the message being acked.
+ ///
+ void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) {}
+
+ /// Emit an update of the "current" waypoints, often previous, current and
+ /// next.
+ ///
+ void print_current_waypoints() {}
+
+ //
+ // The following interfaces are not currently implemented as their counterparts
+ // are not called in the mainline code. XXX ripe for re-specification.
+ //
+
+ /// Send a text message with printf-style formatting.
+ ///
+ /// @param severity A value describing the importance of the message.
+ /// @param fmt The format string to send.
+ /// @param ... Additional arguments to the format string.
+ ///
+ // void send_message(uint8_t severity, const char *fmt, ...) {}
+
+ /// Log a waypoint
+ ///
+ /// @param wp The waypoint to log.
+ /// @param index The index of the waypoint.
+ // void print_waypoint(struct Location *wp, uint8_t index) {}
+
+ // test if frequency within range requested for loop
+ // used by data_stream_send
+ static bool freqLoopMatch(uint16_t freq, uint16_t freqMin, uint16_t freqMax)
+ {
+ if (freq != 0 && freq >= freqMin && freq < freqMax) return true;
+ else return false;
+ }
+
+ // send streams which match frequency range
+ void data_stream_send(uint16_t freqMin, uint16_t freqMax);
+
+protected:
+ /// The stream we are communicating over
+ BetterStream *_port;
+};
+
+//
+// GCS class definitions.
+//
+// These are here so that we can declare the GCS object early in the sketch
+// and then reference it statically rather than via a pointer.
+//
+
+///
+/// @class GCS_MAVLINK
+/// @brief The mavlink protocol for qgroundcontrol
+///
+#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
+class GCS_MAVLINK : public GCS_Class
+{
+public:
+ GCS_MAVLINK();
+ void update(void);
+ void init(BetterStream *port);
+ void send_message(uint8_t id, uint32_t param = 0);
+ void send_text(uint8_t severity, const char *str);
+ void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2);
+ void data_stream_send(uint16_t freqMin, uint16_t freqMax);
+private:
+ void handleMessage(mavlink_message_t * msg);
+
+ /// Perform queued sending operations
+ ///
+ void _queued_send();
+
+ AP_Var *_queued_parameter; ///< next parameter to be sent in queue
+ uint16_t _queued_parameter_index; ///< next queued parameter's index
+
+ /// Count the number of reportable parameters.
+ ///
+ /// Not all parameters can be reported via MAVlink. We count the number that are
+ /// so that we can report to a GCS the number of parameters it should expect when it
+ /// requests the full set.
+ ///
+ /// @return The number of reportable parameters.
+ ///
+ uint16_t _count_parameters(); ///< count reportable parameters
+
+ uint16_t _parameter_count; ///< cache of reportable parameters
+ AP_Var *_find_parameter(uint16_t index); ///< find a reportable parameter by index
+
+
+ mavlink_channel_t chan;
+ uint16_t packet_drops;
+ uint16_t rawSensorStreamRate;
+ uint16_t attitudeStreamRate;
+ uint16_t positionStreamRate;
+ uint16_t rawControllerStreamRate;
+ uint16_t rcStreamRate;
+ uint16_t extraStreamRate[3];
+};
+#endif // GCS_PROTOCOL_MAVLINK
+
+#endif // __GCS_H
diff --git a/ArduCopterMega/GCS_Ardupilot.pde b/ArduCopterMega/GCS_Ardupilot.pde
index 9ddb309d55..d858a65d46 100644
--- a/ArduCopterMega/GCS_Ardupilot.pde
+++ b/ArduCopterMega/GCS_Ardupilot.pde
@@ -99,7 +99,7 @@ void print_position(void)
SendSer(",LON:");
SendSer(current_loc.lng/10,DEC); //wp_current_lat
SendSer(",SPD:");
- SendSer(gps->ground_speed/100,DEC);
+ SendSer(g_gps->ground_speed/100,DEC);
SendSer(",CRT:");
SendSer(climb_rate,DEC);
SendSer(",ALT:");
@@ -119,7 +119,7 @@ void print_position(void)
SendSer(",RSP:");
SendSer(g.rc_1.servo_out/100,DEC);
SendSer(",TOW:");
- SendSer(gps->time);
+ SendSer(g_gps->time);
SendSerln(",***");
}
diff --git a/ArduCopterMega/GCS_IMU_ouput.pde b/ArduCopterMega/GCS_IMU_ouput.pde
index 26ea49d4ed..00f4751119 100644
--- a/ArduCopterMega/GCS_IMU_ouput.pde
+++ b/ArduCopterMega/GCS_IMU_ouput.pde
@@ -150,16 +150,16 @@ void print_location(void)
Serial.print(",ALT:");
Serial.print(current_loc.alt/100); // meters
Serial.print(",COG:");
- Serial.print(gps->ground_course);
+ Serial.print(g_gps->ground_course);
Serial.print(",SOG:");
- Serial.print(gps->ground_speed);
+ Serial.print(g_gps->ground_speed);
Serial.print(",FIX:");
- Serial.print((int)gps->fix);
+ Serial.print((int)g_gps->fix);
Serial.print(",SAT:");
- Serial.print((int)gps->num_sats);
+ Serial.print((int)g_gps->num_sats);
Serial.print (",");
Serial.print("TOW:");
- Serial.print(gps->time);
+ Serial.print(g_gps->time);
Serial.println("***");
}
diff --git a/ArduCopterMega/GCS_Standard.pde b/ArduCopterMega/GCS_Standard.pde
index 5a96acc68e..54cc9c55ab 100644
--- a/ArduCopterMega/GCS_Standard.pde
+++ b/ArduCopterMega/GCS_Standard.pde
@@ -97,11 +97,11 @@ void send_message(byte id, long param) {
mess_buffer[9] = (templong >> 16) & 0xff;
mess_buffer[10] = (templong >> 24) & 0xff;
- tempint = gps->altitude / 100; // Altitude MSL in meters * 10 in 2 bytes
+ tempint = g_gps->altitude / 100; // Altitude MSL in meters * 10 in 2 bytes
mess_buffer[11] = tempint & 0xff;
mess_buffer[12] = (tempint >> 8) & 0xff;
- tempint = gps->ground_speed; // Speed in M / S * 100 in 2 bytes
+ tempint = g_gps->ground_speed; // Speed in M / S * 100 in 2 bytes
mess_buffer[13] = tempint & 0xff;
mess_buffer[14] = (tempint >> 8) & 0xff;
@@ -109,7 +109,7 @@ void send_message(byte id, long param) {
mess_buffer[15] = tempint & 0xff;
mess_buffer[16] = (tempint >> 8) & 0xff;
- templong = gps->time; // Time of Week (milliseconds) in 4 bytes
+ templong = g_gps->time; // Time of Week (milliseconds) in 4 bytes
mess_buffer[17] = templong & 0xff;
mess_buffer[18] = (templong >> 8) & 0xff;
mess_buffer[19] = (templong >> 16) & 0xff;
diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde
index 3f5a0757d0..80007964d3 100644
--- a/ArduCopterMega/Log.pde
+++ b/ArduCopterMega/Log.pde
@@ -494,7 +494,7 @@ void Log_Read_GPS()
Serial.print(comma);
Serial.print((float)DataFlash.ReadLong()/t7, 7); // Longitude
Serial.print(comma);
- Serial.print((float)DataFlash.ReadLong()/100.0); // Baro/gps altitude mix
+ Serial.print((float)DataFlash.ReadLong()/100.0); // Baro/g_gps altitude mix
Serial.print(comma);
Serial.print((float)DataFlash.ReadLong()/100.0); // GPS altitude
Serial.print(comma);
diff --git a/ArduCopterMega/Makefile b/ArduCopterMega/Makefile
new file mode 100644
index 0000000000..34f8baffd9
--- /dev/null
+++ b/ArduCopterMega/Makefile
@@ -0,0 +1,10 @@
+#
+# Trivial makefile for building APM
+#
+
+#
+# Select 'mega' for the original APM, or 'mega2560' for the V2 APM.
+#
+BOARD = mega
+
+include ../libraries/AP_Common/Arduino.mk
diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h
index d7a25503d2..2643edd682 100644
--- a/ArduCopterMega/Mavlink_Common.h
+++ b/ArduCopterMega/Mavlink_Common.h
@@ -137,8 +137,8 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
{
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(chan,current_loc.lat,
- current_loc.lng,current_loc.alt*10,gps.ground_speed/1.0e2*rot.a.x,
- gps.ground_speed/1.0e2*rot.b.x,gps.ground_speed/1.0e2*rot.c.x);
+ current_loc.lng,current_loc.alt*10,g_gps.ground_speed/1.0e2*rot.a.x,
+ g_gps.ground_speed/1.0e2*rot.b.x,g_gps.ground_speed/1.0e2*rot.c.x);
break;
}
case MSG_LOCAL_LOCATION:
@@ -146,15 +146,15 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_local_position_send(chan,timeStamp,ToRad((current_loc.lat-home.lat)/1.0e7)*radius_of_earth,
ToRad((current_loc.lng-home.lng)/1.0e7)*radius_of_earth*cos(ToRad(home.lat/1.0e7)),
- (current_loc.alt-home.alt)/1.0e2, gps.ground_speed/1.0e2*rot.a.x,
- gps.ground_speed/1.0e2*rot.b.x,gps.ground_speed/1.0e2*rot.c.x);
+ (current_loc.alt-home.alt)/1.0e2, g_gps.ground_speed/1.0e2*rot.a.x,
+ g_gps.ground_speed/1.0e2*rot.b.x,g_gps.ground_speed/1.0e2*rot.c.x);
break;
}
case MSG_GPS_RAW:
{
- mavlink_msg_gps_raw_send(chan,timeStamp,gps.status(),
- gps.latitude/1.0e7,gps.longitude/1.0e7,gps.altitude/100.0,
- gps.hdop,0.0,gps.ground_speed/100.0,gps.ground_course/100.0);
+ mavlink_msg_gps_raw_send(chan,timeStamp,g_gps.status(),
+ g_gps.latitude/1.0e7,g_gps.longitude/1.0e7,g_gps.altitude/100.0,
+ g_gps.hdop,0.0,g_gps.ground_speed/100.0,g_gps.ground_course/100.0);
break;
}
case MSG_SERVO_OUT:
@@ -200,7 +200,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
}
case MSG_VFR_HUD:
{
- mavlink_msg_vfr_hud_send(chan, (float)airspeed/100.0, (float)gps.ground_speed/100.0, dcm.yaw_sensor, current_loc.alt/100.0,
+ mavlink_msg_vfr_hud_send(chan, (float)airspeed/100.0, (float)g_gps.ground_speed/100.0, dcm.yaw_sensor, current_loc.alt/100.0,
climb_rate, (int)rc[CH_THROTTLE]->servo_out);
break;
}
@@ -224,7 +224,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_GPS_STATUS:
{
- mavlink_msg_gps_status_send(chan,gps.num_sats,NULL,NULL,NULL,NULL,NULL);
+ mavlink_msg_gps_status_send(chan,g_gps.num_sats,NULL,NULL,NULL,NULL,NULL);
break;
}
diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h
index baf6f3934e..3b9e28e1fb 100644
--- a/ArduCopterMega/Parameters.h
+++ b/ArduCopterMega/Parameters.h
@@ -57,8 +57,8 @@ public:
k_param_ground_temperature,
k_param_ground_altitude,
k_param_ground_pressure,
- k_param_current,
- k_param_compass,
+ k_param_current,
+ k_param_compass,
k_param_mag_declination,
//
@@ -172,7 +172,7 @@ public:
AP_Int16 ground_pressure;
AP_Int16 RTL_altitude;
AP_Int8 frame_type;
-
+
AP_Int8 current_enabled;
AP_Int8 compass_enabled;
AP_Float mag_declination;
@@ -186,7 +186,7 @@ public:
RC_Channel rc_5;
RC_Channel rc_6;
RC_Channel rc_7;
- RC_Channel rc_8;
+ RC_Channel rc_8;
RC_Channel rc_camera_pitch;
RC_Channel rc_camera_roll;
@@ -256,8 +256,8 @@ public:
rc_7 (k_param_rc_7, PSTR("RC7_")),
rc_8 (k_param_rc_8, PSTR("RC8_")),
- rc_camera_pitch (k_param_rc_8, PSTR("RC9_")),
- rc_camera_roll (k_param_rc_8, PSTR("RC10_")),
+ rc_camera_pitch (k_param_rc_9, PSTR("RC9_")),
+ rc_camera_roll (k_param_rc_10, PSTR("RC10_")),
// PID controller group key name initial P initial I initial D initial imax
@@ -265,7 +265,7 @@ public:
pid_acro_rate_roll (k_param_pid_acro_rate_roll, PSTR("ACR_RLL_"), ACRO_RATE_ROLL_P, ACRO_RATE_ROLL_I, ACRO_RATE_ROLL_D, ACRO_RATE_ROLL_IMAX_CENTIDEGREE),
pid_acro_rate_pitch (k_param_pid_acro_rate_pitch, PSTR("ACR_PIT_"), ACRO_RATE_PITCH_P, ACRO_RATE_PITCH_I, ACRO_RATE_PITCH_D, ACRO_RATE_PITCH_IMAX_CENTIDEGREE),
pid_acro_rate_yaw (k_param_pid_acro_rate_yaw, PSTR("ACR_YAW_"), ACRO_RATE_YAW_P, ACRO_RATE_YAW_I, ACRO_RATE_YAW_D, ACRO_RATE_YAW_IMAX_CENTIDEGREE),
-
+
pid_stabilize_roll (k_param_pid_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_D, STABILIZE_ROLL_IMAX_CENTIDEGREE),
pid_stabilize_pitch (k_param_pid_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_D, STABILIZE_PITCH_IMAX_CENTIDEGREE),
pid_yaw (k_param_pid_yaw, PSTR("STB_YAW_"), YAW_P, YAW_I, YAW_D, YAW_IMAX_CENTIDEGREE),
diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde
index a43be84f46..2001df0e04 100644
--- a/ArduCopterMega/commands.pde
+++ b/ArduCopterMega/commands.pde
@@ -32,13 +32,13 @@ 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 > g.waypoint_total) {
temp.id = CMD_BLANK;
}else{
- // read WP position
+ // read WP position
mem = (WP_START_BYTE) + (i * WP_SIZE);
temp.id = eeprom_read_byte((uint8_t*)mem);
mem++;
@@ -58,7 +58,7 @@ struct Location get_wp_with_index(int i)
void set_wp_with_index(struct Location temp, int i)
{
- i = constrain(i, 0, g.waypoint_total);
+ i = constrain(i, 0, g.waypoint_total.get()); // XXX if i was unsigned this could be simply < g.waypoint_total
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
eeprom_write_byte((uint8_t *) mem, temp.id);
@@ -80,9 +80,9 @@ void increment_WP_index()
{
if(g.waypoint_index < g.waypoint_total){
g.waypoint_index.set_and_save(g.waypoint_index + 1);
- Serial.printf_P(PSTR("WP index is incremented to %d\n"),g.waypoint_index);
+ Serial.printf_P(PSTR("WP index is incremented to %d\n"),(int)g.waypoint_index);
}else{
- Serial.printf_P(PSTR("WP Failed to incremented WP index of %d\n"),g.waypoint_index);
+ Serial.printf_P(PSTR("WP Failed to incremented WP index of %d\n"),(int)g.waypoint_index);
}
SendDebugln(g.waypoint_index,DEC);
}
@@ -95,7 +95,7 @@ void decrement_WP_index()
}
long read_alt_to_hold()
-{
+{
read_EEPROM_alt_RTL();
if(g.RTL_altitude == -1)
return current_loc.alt;
@@ -117,7 +117,7 @@ return_to_launch(void)
// home is WP 0
// ------------
g.waypoint_index.set_and_save(0);
-
+
// Loads WP from Memory
// --------------------
set_next_WP(&home);
@@ -131,8 +131,8 @@ return_to_launch(void)
struct
Location get_LOITER_home_wp()
{
- // read home position
- struct Location temp = get_wp_with_index(0);
+ // read home position
+ struct Location temp = get_wp_with_index(0);
temp.id = CMD_LOITER;
temp.alt = read_alt_to_hold();
return temp;
@@ -151,20 +151,20 @@ set_current_loc_here()
This function stores waypoint commands
It looks to see what the next command type is and finds the last command.
*/
-void
+void
set_next_WP(struct Location *wp)
{
- Serial.printf_P(PSTR("set_next_WP, wp_index %d\n"), g.waypoint_index);
+ Serial.printf_P(PSTR("set_next_WP, wp_index %d\n"), (int)g.waypoint_index);
send_message(MSG_COMMAND, g.waypoint_index);
// copy the current WP into the OldWP slot
// ---------------------------------------
prev_WP = current_loc;
-
+
// Load the next_WP slot
// ---------------------
next_WP = *wp;
-
+
// offset the altitude relative to home position
// ---------------------------------------------
next_WP.alt += home.alt;
@@ -173,27 +173,27 @@ set_next_WP(struct Location *wp)
// -----------------------------------------------
target_altitude = current_loc.alt; // PH: target_altitude = 200
offset_altitude = next_WP.alt - current_loc.alt; // PH: offset_altitude = 0
-
+
// zero out our loiter vals to watch for missed waypoints
loiter_delta = 0;
loiter_sum = 0;
loiter_total = 0;
float rads = (abs(next_WP.lat)/t7) * 0.0174532925;
- //377,173,810 / 10,000,000 = 37.717381 * 0.0174532925 = 0.658292482926943
+ //377,173,810 / 10,000,000 = 37.717381 * 0.0174532925 = 0.658292482926943
scaleLongDown = cos(rads);
scaleLongUp = 1.0f/cos(rads);
// this is handy for the groundstation
wp_totalDistance = getDistance(¤t_loc, &next_WP);
wp_distance = wp_totalDistance;
-
- print_current_waypoints();
-
+
+ print_current_waypoints();
+
target_bearing = get_bearing(¤t_loc, &next_WP);
old_target_bearing = target_bearing;
- // this is used to offset the shrinking longitude as we go towards the poles
-
+ // this is used to offset the shrinking longitude as we go towards the poles
+
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();
@@ -208,14 +208,14 @@ void init_home()
// block until we get a good fix
// -----------------------------
- while (!gps->new_data || !gps->fix) {
- gps->update();
+ while (!g_gps->new_data || !g_gps->fix) {
+ g_gps->update();
}
-
+
home.id = CMD_WAYPOINT;
- home.lng = gps->longitude; // Lon * 10**7
- home.lat = gps->latitude; // Lat * 10**7
- home.alt = gps->altitude;
+ home.lng = g_gps->longitude; // Lon * 10**7
+ home.lat = g_gps->latitude; // Lat * 10**7
+ home.alt = g_gps->altitude;
home_is_set = true;
// ground altitude in centimeters for pressure alt calculations
diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde
index 653844f0a1..228349617c 100644
--- a/ArduCopterMega/commands_process.pde
+++ b/ArduCopterMega/commands_process.pde
@@ -138,7 +138,7 @@ void process_must()
next_WP.lat = current_loc.lat + 10; // so we don't have bad calcs
next_WP.lng = current_loc.lng + 10; // so we don't have bad calcs
next_WP.alt = current_loc.alt + takeoff_altitude;
- takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
+ takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
//set_next_WP(&next_WP);
break;
@@ -158,7 +158,7 @@ void process_must()
next_WP.lat = current_loc.lat;
next_WP.lng = current_loc.lng;
next_WP.alt = home.alt;
- land_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
+ land_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
break;
case CMD_ALTITUDE: // Loiter indefinitely
diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h
index 7107fdf783..06926d6a1d 100644
--- a/ArduCopterMega/defines.h
+++ b/ArduCopterMega/defines.h
@@ -28,7 +28,7 @@
#define GPS_PROTOCOL_MTK 4
// Radio channels
-// Note channels are from 0!
+// Note channels are from 0!
//
// XXX these should be CH_n defines from RC.h at some point.
#define CH_1 0
@@ -107,7 +107,7 @@
// Command IDs - May
#define CMD_DELAY 0x20
#define CMD_CLIMB 0x21 // NOT IMPLEMENTED
-#define CMD_LAND_OPTIONS 0x22 // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
+#define CMD_LAND_OPTIONS 0x22 // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
#define CMD_ANGLE 0x23 // move servo N to PWM value
// Command IDs - Now
@@ -202,8 +202,8 @@
// Command Queues
// ---------------
#define COMMAND_MUST 0
-#define COMMAND_MAY 1
-#define COMMAND_NOW 2
+#define COMMAND_MAY 1
+#define COMMAND_NOW 2
// Events
// ------
@@ -244,7 +244,7 @@
// ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412
// Tested value : 418
-#define ToRad(x) (x * 0.01745329252) // *pi/180
+//#define ToRad(x) (x * 0.01745329252) // *pi/180
//#define ToDeg(x) (x * 57.2957795131) // *180/pi
diff --git a/ArduCopterMega/events.pde b/ArduCopterMega/events.pde
index b42c160306..d92a904053 100644
--- a/ArduCopterMega/events.pde
+++ b/ArduCopterMega/events.pde
@@ -44,7 +44,7 @@ void new_event(struct Location *cmd)
{
Serial.print("New Event Loaded ");
Serial.println(cmd->p1,DEC);
-
+
if(cmd->p1 == STOP_REPEAT){
Serial.println("STOP repeat ");
@@ -64,9 +64,9 @@ void new_event(struct Location *cmd)
event_delay = cmd->lat;
event_repeat = cmd->lng; // convert seconds to millis
event_undo_value = 0;
- repeat_forever = (event_repeat == 0) ? 1:0;
+ repeat_forever = (event_repeat == 0) ? 1:0;
}
-
+
/*
Serial.print("event_id: ");
Serial.println(event_id,DEC);
@@ -93,22 +93,22 @@ void perform_event()
}
switch(event_id) {
case CH_4_TOGGLE:
- event_undo_value = rc_5.radio_out;
+ event_undo_value = g.rc_5.radio_out;
APM_RC.OutputCh(CH_5, event_value); // send to Servos
undo_event = 2;
break;
case CH_5_TOGGLE:
- event_undo_value = rc_6.radio_out;
+ event_undo_value = g.rc_6.radio_out;
APM_RC.OutputCh(CH_6, event_value); // send to Servos
undo_event = 2;
break;
case CH_6_TOGGLE:
- event_undo_value = rc_7.radio_out;
+ event_undo_value = g.rc_7.radio_out;
APM_RC.OutputCh(CH_7, event_value); // send to Servos
undo_event = 2;
break;
case CH_7_TOGGLE:
- event_undo_value = rc_8.radio_out;
+ event_undo_value = g.rc_8.radio_out;
APM_RC.OutputCh(CH_8, event_value); // send to Servos
undo_event = 2;
event_undo_value = 1;
@@ -125,7 +125,7 @@ void perform_event()
Serial.println(PORTL,BIN);
undo_event = 2;
break;
-
+
}
}
@@ -149,12 +149,12 @@ void update_events()
undo_event --;
}
- if(event_timer == -1)
+ if(event_timer == -1)
return;
-
+
if((millis() - event_timer) > event_delay){
perform_event();
-
+
if(event_repeat > 0 || repeat_forever == 1){
event_repeat--;
event_timer = millis();
diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde
index 98b5720838..3d052d18fb 100644
--- a/ArduCopterMega/motors.pde
+++ b/ArduCopterMega/motors.pde
@@ -7,7 +7,7 @@ void arm_motors()
static byte arming_counter;
// Arm motor output : Throttle down and full yaw right for more than 2 seconds
- if (g.rc_3.control_in == 0){
+ if (g.rc_3.control_in == 0){
if (g.rc_4.control_in > 2700) {
if (arming_counter > ARM_DELAY) {
motor_armed = true;
@@ -19,7 +19,7 @@ void arm_motors()
motor_armed = false;
}else{
arming_counter++;
- }
+ }
}else{
arming_counter = 0;
}
@@ -32,7 +32,7 @@ void arm_motors()
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
-void
+void
set_servos_4()
{
static byte num;
@@ -41,15 +41,15 @@ set_servos_4()
// Quadcopter mix
if (motor_armed == true && motor_auto_safe == true) {
int out_min = g.rc_3.radio_min;
-
+
// Throttle is 0 to 1000 only
- g.rc_3.servo_out = constrain(g.rc_3.servo_out.get(), 0, 1000);
+ g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000);
if(g.rc_3.servo_out > 0)
out_min = g.rc_3.radio_min + 50;
-
+
//Serial.printf("out: %d %d %d %d\t\t", g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.servo_out, g.rc_4.servo_out);
-
+
// creates the radio_out and pwm_out values
g.rc_1.calc_pwm();
g.rc_2.calc_pwm();
@@ -58,22 +58,22 @@ set_servos_4()
//Serial.printf("out: %d %d %d %d\n", g.rc_1.radio_out, g.rc_2.radio_out, g.rc_3.radio_out, g.rc_4.radio_out);
//Serial.printf("yaw: %d ", g.rc_4.radio_out);
-
+
if(g.frame_type == PLUS_FRAME){
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out;
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out;
motor_out[CH_3] = g.rc_3.radio_out + g.rc_2.pwm_out;
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out;
-
+
motor_out[CH_1] += g.rc_4.pwm_out; // CCW
motor_out[CH_2] += g.rc_4.pwm_out; // CCW
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_4] -= g.rc_4.pwm_out; // CW
-
-
+
+
}else if(g.frame_type == X_FRAME){
-
+
int roll_out = g.rc_1.pwm_out / 2;
int pitch_out = g.rc_2.pwm_out / 2;
@@ -82,45 +82,45 @@ set_servos_4()
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out;
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out;
-
- //Serial.printf("\tb4: %d %d %d %d ", motor_out[CH_1], motor_out[CH_2], motor_out[CH_3], motor_out[CH_4]);
-
+
+ //Serial.printf("\tb4: %d %d %d %d ", motor_out[CH_1], motor_out[CH_2], motor_out[CH_3], motor_out[CH_4]);
+
motor_out[CH_1] += g.rc_4.pwm_out; // CCW
motor_out[CH_2] += g.rc_4.pwm_out; // CCW
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_4] -= g.rc_4.pwm_out; // CW
-
+
//Serial.printf("\tl8r: %d %d %d %d\n", motor_out[CH_1], motor_out[CH_2], motor_out[CH_3], motor_out[CH_4]);
-
+
}else if(g.frame_type == TRI_FRAME){
// Tri-copter power distribution
-
+
int roll_out = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;
-
+
// front two motors
motor_out[CH_2] = g.rc_3.radio_out + roll_out + pitch_out;
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out;
-
+
// rear motors
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out;
-
+
// servo Yaw
APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
-
+
}else if (g.frame_type == HEXA_FRAME) {
int roll_out = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;
- //left side
+ //left side
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CW
- //right side
+ //right side
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW
motor_out[CH_7] = g.rc_3.radio_out - roll_out + pitch_out; // CCW
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW
@@ -132,36 +132,36 @@ set_servos_4()
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_1] -= g.rc_4.pwm_out; // CW
motor_out[CH_8] -= g.rc_4.pwm_out; // CW
-
+
} else {
-
+
Serial.print("frame error");
-
+
}
-
-
+
+
// limit output so motors don't stop
motor_out[CH_1] = constrain(motor_out[CH_1], out_min, g.rc_3.radio_max.get());
motor_out[CH_2] = constrain(motor_out[CH_2], out_min, g.rc_3.radio_max.get());
motor_out[CH_3] = constrain(motor_out[CH_3], out_min, g.rc_3.radio_max.get());
motor_out[CH_4] = constrain(motor_out[CH_4], out_min, g.rc_3.radio_max.get());
-
+
if (g.frame_type == HEXA_FRAME) {
motor_out[CH_7] = constrain(motor_out[CH_7], out_min, g.rc_3.radio_max.get());
motor_out[CH_8] = constrain(motor_out[CH_8], out_min, g.rc_3.radio_max.get());
}
-
+
num++;
if (num > 10){
num = 0;
//Serial.print("!");
//debugging with Channel 6
-
+
//g.pid_baro_throttle.kD((float)g.rc_6.control_in / 1000); // 0 to 1
//g.pid_baro_throttle.kP((float)g.rc_6.control_in / 4000); // 0 to .25
/*
- // ROLL and PITCH
+ // ROLL and PITCH
// make sure you init_pids() after changing the kP
g.pid_stabilize_roll.kP((float)g.rc_6.control_in / 1000);
init_pids();
@@ -175,37 +175,37 @@ set_servos_4()
g.pid_yaw.kP((float)g.rc_6.control_in / 1000);
init_pids();
//*/
-
+
/*
write_int(motor_out[CH_1]);
write_int(motor_out[CH_2]);
write_int(motor_out[CH_3]);
write_int(motor_out[CH_4]);
-
+
write_int(g.rc_3.servo_out);
write_int((int)(cos_yaw_x * 100));
write_int((int)(sin_yaw_y * 100));
write_int((int)(dcm.yaw_sensor / 100));
write_int((int)(nav_yaw / 100));
-
+
write_int((int)nav_lat);
write_int((int)nav_lon);
write_int((int)nav_roll);
write_int((int)nav_pitch);
-
+
//24
write_long(current_loc.lat); //28
write_long(current_loc.lng); //32
write_int((int)current_loc.alt); //34
-
+
write_long(next_WP.lat);
write_long(next_WP.lng);
write_int((int)next_WP.alt); //44
-
+
flush(10);
//*/
-
+
/*Serial.printf("a %ld, e %ld, i %d, t %d, b %4.2f\n",
current_loc.alt,
altitude_error,
@@ -214,10 +214,10 @@ set_servos_4()
angle_boost());
*/
}
-
+
// Send commands to motors
if(g.rc_3.servo_out > 0){
-
+
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
@@ -231,9 +231,9 @@ set_servos_4()
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
APM_RC.Force_Out6_Out7();
}
-
+
}else{
-
+
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
@@ -248,17 +248,17 @@ set_servos_4()
APM_RC.Force_Out6_Out7();
}
}
-
+
}else{
// our motor is unarmed, we're on the ground
reset_I();
-
+
if(g.rc_3.control_in > 0){
// we have pushed up the throttle
// remove safety
motor_auto_safe = true;
}
-
+
// Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
@@ -269,10 +269,10 @@ set_servos_4()
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
}
-
+
// reset I terms of PID controls
reset_I();
-
+
// Initialize yaw command to actual yaw when throttle is down...
g.rc_4.control_in = ToDeg(yaw);
}
diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde
index d887e0a1ab..421d69c6f5 100644
--- a/ArduCopterMega/navigation.pde
+++ b/ArduCopterMega/navigation.pde
@@ -7,16 +7,16 @@ void navigate()
{
// do not navigate with corrupt data
// ---------------------------------
- if (gps->fix == 0)
+ if (g_gps->fix == 0)
{
- gps->new_data = false;
+ g_gps->new_data = false;
return;
}
-
+
if(next_WP.lat == 0){
return;
}
-
+
// waypoint distance from plane
// ----------------------------
GPS_wp_distance = getDistance(¤t_loc, &next_WP);
@@ -28,7 +28,7 @@ void navigate()
return;
}
- // target_bearing is where we should be heading
+ // target_bearing is where we should be heading
// --------------------------------------------
target_bearing = get_bearing(¤t_loc, &next_WP);
@@ -54,22 +54,22 @@ void calc_nav()
3000 = 33m
10000 = 111m
pitch_max = 22° (2200)
- */
+ */
//temp = dcm.get_dcm_matrix();
//yawvector.y = temp.b.x; // cos
//yawvector.x = temp.a.x; // sin
//yawvector.normalize();
-
+
//cos_yaw_x = yawvector.y; // 0
//sin_yaw_y = yawvector.x; // 1
long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown; // 50 - 30 = 20 pitch right
lat_error = next_WP.lat - current_loc.lat; // 50 - 30 = 20 pitch up
-
+
long_error = constrain(long_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
lat_error = constrain(lat_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
-
+
// Convert distance into ROLL X
nav_lon = long_error * g.pid_nav_lon.kP(); // 1800 * 2 = 3600 or 36°
//nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav2, 1.0);
@@ -80,12 +80,12 @@ void calc_nav()
nav_lat = lat_error * g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
//nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
- // rotate the vector
+ // rotate the vector
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
nav_pitch = -((float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y);
-
- nav_roll = constrain(nav_roll, -g.pitch_max, g.pitch_max);
- nav_pitch = constrain(nav_pitch, -g.pitch_max, g.pitch_max);
+
+ nav_roll = constrain(nav_roll, -g.pitch_max.get(), g.pitch_max.get());
+ nav_pitch = constrain(nav_pitch, -g.pitch_max.get(), g.pitch_max.get());
}
/*
@@ -93,10 +93,10 @@ void verify_missed_wp()
{
// check if we have missed the WP
loiter_delta = (target_bearing - old_target_bearing) / 100;
-
+
// reset the old value
old_target_bearing = target_bearing;
-
+
// wrap values
if (loiter_delta > 170) loiter_delta -= 360;
if (loiter_delta < -170) loiter_delta += 360;
@@ -113,10 +113,10 @@ void calc_bearing_error()
void calc_distance_error()
{
wp_distance = GPS_wp_distance;
-
+
// this wants to work only while moving, but it should filter out jumpy GPS reads
// scale gs to whole deg (50hz / 100) scale bearing error down to whole deg
- //distance_estimate += (float)gps->ground_speed * .0002 * cos(radians(bearing_error / 100));
+ //distance_estimate += (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error / 100));
//distance_estimate -= distance_gain * (float)(distance_estimate - GPS_wp_distance);
//wp_distance = distance_estimate;
}
@@ -128,12 +128,12 @@ void calc_distance_error()
} */
// calculated at 50 hz
-void calc_altitude_error()
+void calc_altitude_error()
{
if(control_mode == AUTO && offset_altitude != 0) {
// limit climb rates - we draw a straight line between first location and edge of waypoint_radius
- target_altitude = next_WP.alt - ((wp_distance * offset_altitude) / (wp_totalDistance - waypoint_radius));
-
+ target_altitude = next_WP.alt - ((wp_distance * offset_altitude) / (wp_totalDistance - g.waypoint_radius));
+
// stay within a certain range
if(prev_WP.alt > next_WP.alt){
target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt);
@@ -186,7 +186,7 @@ void update_crosstrack(void)
// ----------------
if (abs(target_bearing - crosstrack_bearing) < 4500) { // If we are too far off or too close we don't do track following
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line
- nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle, g.crosstrack_entry_angle);
+ nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing = wrap_360(nav_bearing);
}
}
@@ -207,9 +207,9 @@ int get_altitude_above_home(void)
long getDistance(struct Location *loc1, struct Location *loc2)
{
- if(loc1->lat == 0 || loc1->lng == 0)
+ if(loc1->lat == 0 || loc1->lng == 0)
return -1;
- if(loc2->lat == 0 || loc2->lng == 0)
+ if(loc2->lat == 0 || loc2->lng == 0)
return -1;
float dlat = (float)(loc2->lat - loc1->lat);
float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde
index 2899b7a3b3..21e83e1c66 100644
--- a/ArduCopterMega/setup.pde
+++ b/ArduCopterMega/setup.pde
@@ -15,7 +15,7 @@ static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv);
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
-// Command/function table for the setup menu
+// Command/function table for the setup menu
const struct Menu::command setup_menu_commands[] PROGMEM = {
// command function called
// ======= ===============
@@ -61,7 +61,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
uint8_t i;
// clear the area
print_blanks(8);
-
+
report_radio();
report_frame();
report_current();
@@ -88,14 +88,14 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
do {
c = Serial.read();
} while (-1 == c);
-
+
if (('y' != c) && ('Y' != c))
return(-1);
-
+
//zero_eeprom();
default_gains();
-
-
+
+
// setup default values
default_waypoint_info();
default_nav();
@@ -106,7 +106,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
default_logs();
default_current();
print_done();
-
+
// finish
// ------
return(0);
@@ -119,12 +119,12 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
{
Serial.println("\n\nRadio Setup:");
uint8_t i;
-
+
for(i = 0; i < 100;i++){
delay(20);
read_radio();
}
-
+
if(g.rc_1.radio_in < 500){
while(1){
Serial.printf_P(PSTR("\nNo radio; Check connectors."));
@@ -132,7 +132,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
// stop here
}
}
-
+
g.rc_1.radio_min = g.rc_1.radio_in;
g.rc_2.radio_min = g.rc_2.radio_in;
g.rc_3.radio_min = g.rc_3.radio_in;
@@ -163,7 +163,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: "));
while(1){
-
+
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
@@ -177,11 +177,11 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
g.rc_6.update_min_max();
g.rc_7.update_min_max();
g.rc_8.update_min_max();
-
+
if(Serial.available() > 0){
//g.rc_3.radio_max += 250;
Serial.flush();
-
+
save_EEPROM_radio();
//delay(100);
// double checking
@@ -209,9 +209,9 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
print_hit_enter();
delay(1000);
-
+
int out_min = g.rc_3.radio_min + 70;
-
+
while(1){
@@ -222,22 +222,22 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
motor_out[CH_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min;
-
-
+
+
if(frame_type == PLUS_FRAME){
if(g.rc_1.control_in > 0){
motor_out[CH_1] = out_min;
Serial.println("0");
-
+
}else if(g.rc_1.control_in < 0){
motor_out[CH_2] = out_min;
Serial.println("1");
}
-
+
if(g.rc_2.control_in > 0){
motor_out[CH_4] = out_min;
Serial.println("3");
-
+
}else if(g.rc_2.control_in < 0){
motor_out[CH_3] = out_min;
Serial.println("2");
@@ -264,31 +264,31 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
motor_out[CH_1] = out_min;
Serial.println("0");
}
-
+
}else if(frame_type == TRI_FRAME){
if(g.rc_1.control_in > 0){
motor_out[CH_1] = out_min;
-
+
}else if(g.rc_1.control_in < 0){
motor_out[CH_2] = out_min;
}
-
+
if(g.rc_2.control_in > 0){
- motor_out[CH_4] = out_min;
+ motor_out[CH_4] = out_min;
}
if(g.rc_4.control_in > 0){
g.rc_4.servo_out = 2000;
-
+
}else if(g.rc_4.control_in < 0){
g.rc_4.servo_out = -2000;
}
-
+
g.rc_4.calc_pwm();
motor_out[CH_3] = g.rc_4.radio_out;
}
-
+
if(g.rc_3.control_in > 0){
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
@@ -301,7 +301,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
}
-
+
if(Serial.available() > 0){
return (0);
}
@@ -312,7 +312,7 @@ static int8_t
setup_accel(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\nHold ArduCopter completely still and level.\n"));
-
+
imu.init_accel();
imu.print_accel_offsets();
@@ -325,12 +325,12 @@ setup_pid(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("default"))) {
default_gains();
-
+
}else if (!strcmp_P(argv[1].str, PSTR("s_kp"))) {
g.pid_stabilize_roll.kP(argv[2].f);
g.pid_stabilize_pitch.kP(argv[2].f);
save_EEPROM_PID();
-
+
}else if (!strcmp_P(argv[1].str, PSTR("s_kd"))) {
stabilize_dampener = argv[2].f;
save_EEPROM_PID();
@@ -353,7 +353,7 @@ setup_pid(uint8_t argc, const Menu::arg *argv)
}else{
default_gains();
}
-
+
report_gains();
}
@@ -362,20 +362,20 @@ static int8_t
setup_flightmodes(uint8_t argc, const Menu::arg *argv)
{
byte switchPosition, oldSwitchPosition, mode;
-
+
Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
print_hit_enter();
trim_radio();
-
+
while(1){
delay(20);
- read_radio();
+ read_radio();
switchPosition = readSwitch();
-
+
// look for control switch change
if (oldSwitchPosition != switchPosition){
-
+
mode = g.flight_modes[switchPosition];
mode = constrain(mode, 0, NUM_MODES-1);
@@ -383,12 +383,12 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
print_switch(switchPosition, mode);
// Remember switch position
- oldSwitchPosition = switchPosition;
+ oldSwitchPosition = switchPosition;
}
// look for stick input
if (radio_input_switch() == true){
- mode++;
+ mode++;
if(mode >= NUM_MODES)
mode = 0;
@@ -398,7 +398,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
// print new mode
print_switch(switchPosition, mode);
}
-
+
// escape hatch
if(Serial.available() > 0){
save_EEPROM_flight_modes();
@@ -430,16 +430,16 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
if (!strcmp_P(argv[1].str, PSTR("on"))) {
g.compass_enabled = true;
init_compass();
-
+
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.compass_enabled = false;
-
+
} else {
Serial.printf_P(PSTR("\nOptions:[on,off]\n"));
report_compass();
return 0;
}
-
+
save_EEPROM_mag();
report_compass();
return 0;
@@ -453,7 +453,7 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
} else if (!strcmp_P(argv[1].str, PSTR("x"))) {
frame_type = X_FRAME;
-
+
} else if (!strcmp_P(argv[1].str, PSTR("tri"))) {
frame_type = TRI_FRAME;
@@ -465,7 +465,7 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
report_frame();
return 0;
}
-
+
save_EEPROM_frame();
report_frame();
return 0;
@@ -477,20 +477,20 @@ setup_current(uint8_t argc, const Menu::arg *argv)
if (!strcmp_P(argv[1].str, PSTR("on"))) {
current_enabled = true;
save_EEPROM_mag();
-
+
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
current_enabled = false;
save_EEPROM_mag();
-
+
} else if(argv[1].i > 10){
milliamp_hours = argv[1].i;
-
+
} else {
Serial.printf_P(PSTR("\nOptions:[on, off, mAh]\n"));
report_current();
return 0;
}
-
+
save_EEPROM_current();
report_current();
return 0;
@@ -509,7 +509,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft
compass.set_offsets(0, 0, 0); // set offsets to account for surrounding interference
compass.set_declination(ToRad(DECLINATION)); // set local difference between magnetic north and true north
- //int counter = 0;
+ //int counter = 0;
float _min[3], _max[3], _offset[3];
while(1){
@@ -537,15 +537,15 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
offset[0] = -(_max[0] + _min[0]) / 2;
offset[1] = -(_max[1] + _min[1]) / 2;
offset[2] = -(_max[2] + _min[2]) / 2;
-
- // display all to user
+
+ // display all to user
Serial.printf_P(PSTR("Heading: "));
Serial.print(ToDeg(compass.heading));
Serial.print(" \t(");
Serial.print(compass.mag_x);
Serial.print(",");
Serial.print(compass.mag_y);
- Serial.print(",");
+ Serial.print(",");
Serial.print(compass.mag_z);
Serial.print(")\t offsets(");
Serial.print(offset[0]);
@@ -554,18 +554,18 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
Serial.print(",");
Serial.print(offset[2]);
Serial.println(")");
-
+
if(Serial.available() > 0){
-
+
//mag_offset_x = offset[0];
//mag_offset_y = offset[1];
//mag_offset_z = offset[2];
-
+
//save_EEPROM_mag_offset();
-
+
// set offsets to account for surrounding interference
//compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);
-
+
report_compass();
break;
}
@@ -579,7 +579,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
/***************************************************************************/
void default_waypoint_info()
-{
+{
g.waypoint_radius = 4; //TODO: Replace this quick fix with a real way to define wp_radius
g.loiter_radius = 30; //TODO: Replace this quick fix with a real way to define loiter_radius
save_EEPROM_waypoint_info();
@@ -647,7 +647,7 @@ void default_logs()
// convenience macro for testing LOG_* and setting LOGBIT_*
#define LOGBIT(_s) (LOG_ ## _s ? LOGBIT_ ## _s : 0)
- g.log_bitmask =
+ g.log_bitmask =
LOGBIT(ATTITUDE_FAST) |
LOGBIT(ATTITUDE_MED) |
LOGBIT(GPS) |
@@ -659,7 +659,7 @@ void default_logs()
LOGBIT(CMD) |
LOGBIT(CURRENT);
#undef LOGBIT
-
+
save_EEPROM_logs();
}
@@ -672,7 +672,7 @@ default_gains()
g.pid_acro_rate_roll.kI(ACRO_RATE_ROLL_I);
g.pid_acro_rate_roll.kD(0);
g.pid_acro_rate_roll.imax(ACRO_RATE_ROLL_IMAX * 100);
-
+
g.pid_acro_rate_pitch.kP(ACRO_RATE_PITCH_P);
g.pid_acro_rate_pitch.kI(ACRO_RATE_PITCH_I);
g.pid_acro_rate_pitch.kD(0);
@@ -689,7 +689,7 @@ default_gains()
g.pid_stabilize_roll.kI(STABILIZE_ROLL_I);
g.pid_stabilize_roll.kD(0);
g.pid_stabilize_roll.imax(STABILIZE_ROLL_IMAX * 100);
-
+
g.pid_stabilize_pitch.kP(STABILIZE_PITCH_P);
g.pid_stabilize_pitch.kI(STABILIZE_PITCH_I);
g.pid_stabilize_pitch.kD(0);
@@ -705,7 +705,7 @@ default_gains()
// custom dampeners
// roll pitch
stabilize_dampener = STABILIZE_DAMPENER;
-
+
//yaw
hold_yaw_dampener = HOLD_YAW_DAMPENER;
@@ -730,7 +730,7 @@ default_gains()
g.pid_sonar_throttle.kI(THROTTLE_SONAR_I);
g.pid_sonar_throttle.kD(THROTTLE_SONAR_D);
g.pid_sonar_throttle.imax(THROTTLE_SONAR_IMAX);
-
+
save_EEPROM_PID();
}
@@ -760,7 +760,7 @@ void report_frame()
Serial.printf_P(PSTR("Frame\n"));
print_divider();
-
+
if(frame_type == X_FRAME)
Serial.printf_P(PSTR("X "));
else if(frame_type == PLUS_FRAME)
@@ -807,10 +807,10 @@ void report_gains()
print_PID(&g.pid_stabilize_pitch);
Serial.printf_P(PSTR("yaw:\n"));
print_PID(&g.pid_yaw);
-
- Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), stabilize_dampener);
+
+ Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), stabilize_dampener);
Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), hold_yaw_dampener);
-
+
// Nav
Serial.printf_P(PSTR("Nav:\nlat:\n"));
print_PID(&g.pid_nav_lat);
@@ -833,9 +833,9 @@ void report_xtrack()
Serial.printf_P(PSTR("XTRACK: %4.2f\n"
"XTRACK angle: %d\n"
"PITCH_MAX: %ld"),
- g.crosstrack_gain,
- g.crosstrack_entry_angle,
- g.pitch_max);
+ (float)g.crosstrack_gain,
+ (int)g.crosstrack_entry_angle,
+ (long)g.pitch_max);
print_blanks(1);
}
@@ -851,9 +851,9 @@ void report_throttle()
"cruise: %d\n"
"failsafe_enabled: %d\n"
"failsafe_value: %d"),
- g.throttle_min,
- g.throttle_max,
- g.throttle_cruise,
+ (int)g.throttle_min,
+ (int)g.throttle_max,
+ (int)g.throttle_cruise,
throttle_failsafe_enabled,
throttle_failsafe_value);
print_blanks(1);
@@ -872,7 +872,7 @@ void report_imu()
void report_compass()
{
- print_blanks(2);
+ print_blanks(2);
Serial.printf_P(PSTR("Compass\n"));
print_divider();
@@ -881,11 +881,11 @@ void report_compass()
read_EEPROM_compass_offset();
print_enabled(g.compass_enabled);
-
+
// mag declination
- Serial.printf_P(PSTR("Mag Delination: %4.4f\n"),
+ Serial.printf_P(PSTR("Mag Delination: %4.4f\n"),
mag_declination);
-
+
// mag offsets
Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f"),
mag_offset_x,
@@ -897,11 +897,11 @@ void report_compass()
void report_flight_modes()
{
- print_blanks(2);
+ print_blanks(2);
Serial.printf_P(PSTR("Flight modes\n"));
print_divider();
read_EEPROM_flight_modes();
-
+
for(int i = 0; i < 6; i++ ){
print_switch(i, g.flight_modes[i]);
}
@@ -912,23 +912,23 @@ void report_flight_modes()
// CLI utilities
/***************************************************************************/
-void
+void
print_PID(PID * pid)
{
Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"), pid->kP(), pid->kI(), pid->kD(), (long)pid->imax());
}
-void
+void
print_radio_values()
{
- Serial.printf_P(PSTR("CH1: %d | %d\n"), g.rc_1.radio_min, g.rc_1.radio_max);
- Serial.printf_P(PSTR("CH2: %d | %d\n"), g.rc_2.radio_min, g.rc_2.radio_max);
- Serial.printf_P(PSTR("CH3: %d | %d\n"), g.rc_3.radio_min, g.rc_3.radio_max);
- Serial.printf_P(PSTR("CH4: %d | %d\n"), g.rc_4.radio_min, g.rc_4.radio_max);
- Serial.printf_P(PSTR("CH5: %d | %d\n"), g.rc_5.radio_min, g.rc_5.radio_max);
- Serial.printf_P(PSTR("CH6: %d | %d\n"), g.rc_6.radio_min, g.rc_6.radio_max);
- Serial.printf_P(PSTR("CH7: %d | %d\n"), g.rc_7.radio_min, g.rc_7.radio_max);
- Serial.printf_P(PSTR("CH8: %d | %d\n"), g.rc_8.radio_min, g.rc_8.radio_max);
+ Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max);
+ Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max);
+ Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max);
+ Serial.printf_P(PSTR("CH4: %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_max);
+ Serial.printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max);
+ Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max);
+ Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max);
+ Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
}
void
@@ -968,7 +968,7 @@ boolean
radio_input_switch(void)
{
static byte bouncer;
-
+
if (abs(g.rc_1.radio_in - g.rc_1.radio_trim) > 200)
bouncer = 10;
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index fd1abf83c1..de0e4053c9 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -1,9 +1,9 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*****************************************************************************
The init_ardupilot function processes everything we need for an in - air restart
- We will determine later if we are actually on the ground and process a
+ We will determine later if we are actually on the ground and process a
ground start in that case.
-
+
*****************************************************************************/
// Functions called from the top-level menu
@@ -62,7 +62,7 @@ void init_ardupilot()
// Not used if the IMU/X-Plane GPS is in use.
//
// XXX currently the EM406 (SiRF receiver) is nominally configured
- // at 57600, however it's not been supported to date. We should
+ // at 57600, however it's not been supported to date. We should
// probably standardise on 38400.
//
// XXX the 128 byte receive buffer may be too small for NMEA, depending
@@ -88,7 +88,7 @@ void init_ardupilot()
"Init ArduCopterMega 1.0.2 Public Alpha\n\n"
#if TELEMETRY_PORT == 3
"Telemetry is on the xbee port\n"
-#endif
+#endif
"freeRAM: %d\n"),freeRAM());
//
@@ -123,12 +123,12 @@ void init_ardupilot()
APM_BMP085.Init(); // APM Abs Pressure sensor initialization
DataFlash.Init(); // DataFlash log initialization
- gps = &GPS;
- gps->init();
-
+ g_gps = &GPS;
+ g_gps->init();
+
if(g.compass_enabled)
init_compass();
-
+
pinMode(C_LED_PIN, OUTPUT); // GPS status LED
pinMode(A_LED_PIN, OUTPUT); // GPS status LED
pinMode(B_LED_PIN, OUTPUT); // GPS status LED
@@ -156,7 +156,7 @@ void init_ardupilot()
main_menu.run();
}
}
-
+
if(g.log_bitmask > 0){
// Here we will check on the length of the last log
@@ -165,35 +165,35 @@ void init_ardupilot()
last_log_start = eeprom_read_word((uint16_t *) (EE_LOG_1_START+(last_log_num - 1) * 0x02));
last_log_end = eeprom_read_word((uint16_t *) EE_LAST_LOG_PAGE);
- if(last_log_num == 0) {
+ if(last_log_num == 0) {
// The log space is empty. Start a write session on page 1
- DataFlash.StartWrite(1);
+ DataFlash.StartWrite(1);
eeprom_write_byte((uint8_t *) EE_LAST_LOG_NUM, (1));
- eeprom_write_word((uint16_t *) EE_LOG_1_START, (1));
-
- } else if (last_log_end <= last_log_start + 10) {
+ eeprom_write_word((uint16_t *) EE_LOG_1_START, (1));
+
+ } else if (last_log_end <= last_log_start + 10) {
// The last log is small. We consider it junk. Overwrite it.
- DataFlash.StartWrite(last_log_start);
-
+ DataFlash.StartWrite(last_log_start);
+
} else {
// The last log is valid. Start a new log
if(last_log_num >= 19) {
Serial.println("Number of log files exceeds max. Log 19 will be overwritten.");
last_log_num --;
}
- DataFlash.StartWrite(last_log_end + 1);
+ DataFlash.StartWrite(last_log_end + 1);
eeprom_write_byte((uint8_t *) EE_LAST_LOG_NUM, (last_log_num + 1));
- eeprom_write_word((uint16_t *) (EE_LOG_1_START+(last_log_num)*0x02), (last_log_end + 1));
+ eeprom_write_word((uint16_t *) (EE_LOG_1_START+(last_log_num)*0x02), (last_log_end + 1));
}
}
-
+
// read in the flight switches
//update_servo_switches();
-
+
//Serial.print("GROUND START");
send_message(SEVERITY_LOW,"GROUND START");
startup_ground();
-
+
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
@@ -226,17 +226,17 @@ void startup_ground(void)
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
- #if(GROUND_START_DELAY > 0)
+ #if(GROUND_START_DELAY > 0)
send_message(SEVERITY_LOW,"With Delay");
- delay(GROUND_START_DELAY * 1000);
+ delay(GROUND_START_DELAY * 1000);
#endif
-
+
// Output waypoints for confirmation
// --------------------------------
for(int i = 1; i < g.waypoint_total + 1; i++) {
gcs.send_message(MSG_COMMAND_LIST, i);
}
-
+
//IMU ground start
//------------------------
init_pressure_ground();
@@ -248,25 +248,25 @@ void startup_ground(void)
// Save the settings for in-air restart
// ------------------------------------
save_EEPROM_groundstart();
-
+
// initialize commands
// -------------------
init_commands();
-
+
send_message(SEVERITY_LOW,"\n\n Ready to FLY.");
}
void set_mode(byte mode)
{
-
+
if(control_mode == mode){
// don't switch modes if we are already in the correct mode.
return;
}
-
+
control_mode = mode;
control_mode = constrain(control_mode, 0, NUM_MODES - 1);
-
+
// used to stop fly_aways
if(g.rc_1.control_in == 0){
// we are on the ground is this is true
@@ -279,27 +279,27 @@ void set_mode(byte mode)
{
case ACRO:
break;
-
+
case STABILIZE:
set_current_loc_here();
break;
-
+
case ALT_HOLD:
set_current_loc_here();
break;
-
+
case AUTO:
update_auto();
break;
-
+
case POSITION_HOLD:
set_current_loc_here();
break;
-
+
case RTL:
return_to_launch();
break;
-
+
case TAKEOFF:
break;
@@ -309,11 +309,11 @@ void set_mode(byte mode)
default:
break;
}
-
+
// output control mode to the ground station
send_message(MSG_HEARTBEAT);
-
- if (g.log_bitmask & MASK_LOG_MODE)
+
+ if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
}
@@ -333,17 +333,17 @@ void set_failsafe(boolean mode)
// re-read the switch so we can return to our preferred mode
reset_control_switch();
-
+
// Reset control integrators
// ---------------------
reset_I();
-
+
}else{
// We've lost radio contact
// ------------------------
// nothing to do right now
}
-
+
// Let the user know what's up so they can override the behavior
// -------------------------------------------------------------
failsafe_event();
@@ -354,20 +354,20 @@ void update_GPS_light(void)
{
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
// ---------------------------------------------------------------------
- if(gps->fix == 0){
+ if(g_gps->fix == 0){
GPS_light = !GPS_light;
if(GPS_light){
digitalWrite(C_LED_PIN, HIGH);
}else{
digitalWrite(C_LED_PIN, LOW);
- }
+ }
}else{
if(!GPS_light){
GPS_light = true;
digitalWrite(C_LED_PIN, HIGH);
}
}
-
+
if(motor_armed == true){
motor_light = !motor_light;
@@ -376,7 +376,7 @@ void update_GPS_light(void)
digitalWrite(A_LED_PIN, HIGH);
}else{
digitalWrite(A_LED_PIN, LOW);
- }
+ }
}else{
if(!motor_light){
motor_light = true;
@@ -392,7 +392,7 @@ void resetPerfData(void) {
G_Dt_max = 0;
gyro_sat_count = 0;
adc_constraints = 0;
- renorm_sqrt_count = 0;
+ renorm_sqrt_count = 0;
renorm_blowup_count = 0;
gps_fix_count = 0;
perf_mon_timer = millis();
@@ -403,7 +403,7 @@ void
init_compass()
{
dcm.set_compass(&compass);
- compass.init(false);
+ compass.init();
compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft
compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z); // set offsets to account for surrounding interference
compass.set_declination(ToRad(mag_declination)); // set local difference between magnetic north and true north
diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde
index 772a8e7e68..338dc1c6f8 100644
--- a/ArduCopterMega/test.pde
+++ b/ArduCopterMega/test.pde
@@ -28,7 +28,7 @@ static int8_t test_eedump(uint8_t argc, const Menu::arg *argv);
"Commands:\n"
" radio\n"
" servos\n"
- " gps\n"
+ " g_gps\n"
" imu\n"
" battery\n"
"\n"));
@@ -56,7 +56,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"airpressure", test_pressure},
{"compass", test_mag},
{"xbee", test_xbee},
- {"eedump", test_eedump},
+ {"eedump", test_eedump},
};
// A Macro to create the Menu
@@ -89,7 +89,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
-
+
while(1){
delay(20);
@@ -119,21 +119,21 @@ test_radio(uint8_t argc, const Menu::arg *argv)
delay(20);
read_radio();
output_manual_throttle();
-
+
g.rc_1.calc_pwm();
g.rc_2.calc_pwm();
g.rc_3.calc_pwm();
g.rc_4.calc_pwm();
-
+
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"), (g.rc_1.control_in), (g.rc_2.control_in), (g.rc_3.control_in), (g.rc_4.control_in), g.rc_5.control_in, g.rc_6.control_in, g.rc_7.control_in);
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (g.rc_1.servo_out / 100), (g.rc_2.servo_out / 100), g.rc_3.servo_out, (g.rc_4.servo_out / 100));
-
+
/*Serial.printf_P(PSTR( "min: %d"
"\t in: %d"
"\t pwm_in: %d"
"\t sout: %d"
"\t pwm_out %d\n"),
- g.rc_3.radio_min,
+ g.rc_3.radio_min,
g.rc_3.control_in,
g.rc_3.radio_in,
g.rc_3.servo_out,
@@ -156,40 +156,40 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
delay(20);
read_radio();
}
-
+
// read the radio to set trims
// ---------------------------
trim_radio();
-
+
oldSwitchPosition = readSwitch();
-
+
Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
while(g.rc_3.control_in > 0){
delay(20);
read_radio();
}
-
+
while(1){
delay(20);
read_radio();
-
+
if(g.rc_3.control_in > 0){
Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.rc_3.control_in);
fail_test++;
}
-
+
if(oldSwitchPosition != readSwitch()){
Serial.printf_P(PSTR("CONTROL MODE CHANGED: "));
Serial.println(flight_mode_strings[readSwitch()]);
fail_test++;
}
-
+
if(g.throttle_failsafe_enabled && g.rc_3.get_failsafe()){
Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.rc_3.radio_in);
Serial.println(flight_mode_strings[readSwitch()]);
fail_test++;
}
-
+
if(fail_test > 0){
return (0);
}
@@ -204,15 +204,15 @@ static int8_t
test_stabilize(uint8_t argc, const Menu::arg *argv)
{
static byte ts_num;
-
-
+
+
print_hit_enter();
delay(1000);
-
+
// setup the radio
// ---------------
init_rc_in();
-
+
control_mode = STABILIZE;
Serial.printf_P(PSTR("g.pid_stabilize_roll.kP: %4.4f\n"), g.pid_stabilize_roll.kP());
Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
@@ -221,7 +221,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
motor_auto_safe = false;
motor_armed = true;
-
+
while(1){
// 50 hz
if (millis() - fast_loopTimer > 19) {
@@ -243,24 +243,24 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
-
+
// IMU
// ---
read_AHRS();
-
+
// allow us to zero out sensors with control switches
if(g.rc_5.control_in < 600){
dcm.roll_sensor = dcm.pitch_sensor = 0;
}
-
+
// custom code/exceptions for flight modes
// ---------------------------------------
update_current_flight_mode();
-
+
// write out the servo PWM values
// ------------------------------
set_servos_4();
-
+
ts_num++;
if (ts_num > 10){
ts_num = 0;
@@ -272,14 +272,14 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
print_motor_out();
}
// R: 1417, L: 1453 F: 1453 B: 1417
-
+
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
-
+
if(Serial.available() > 0){
return (0);
}
-
+
}
}
}
@@ -288,24 +288,24 @@ static int8_t
test_fbw(uint8_t argc, const Menu::arg *argv)
{
static byte ts_num;
-
+
print_hit_enter();
delay(1000);
-
+
// setup the radio
// ---------------
init_rc_in();
-
+
control_mode = FBW;
//Serial.printf_P(PSTR("g.pid_stabilize_roll.kP: %4.4f\n"), g.pid_stabilize_roll.kP());
//Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
motor_armed = true;
trim_radio();
-
+
nav_yaw = 8000;
scaleLongDown = 1;
-
+
while(1){
// 50 hz
if (millis() - fast_loopTimer > 19) {
@@ -328,30 +328,30 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
-
+
// IMU
// ---
read_AHRS();
-
+
// allow us to zero out sensors with control switches
if(g.rc_5.control_in < 600){
dcm.roll_sensor = dcm.pitch_sensor = 0;
}
-
+
// custom code/exceptions for flight modes
// ---------------------------------------
update_current_flight_mode();
-
+
// write out the servo PWM values
// ------------------------------
set_servos_4();
-
+
ts_num++;
if (ts_num == 5){
// 10 hz
ts_num = 0;
- gps->longitude = 0;
- gps->latitude = 0;
+ g_gps->longitude = 0;
+ g_gps->latitude = 0;
calc_nav();
Serial.printf_P(PSTR("ys:%ld, WP.lat:%ld, WP.lng:%ld, n_lat:%ld, n_lon:%ld, nroll:%ld, npitch:%ld, pmax:%ld, \t- "),
@@ -362,11 +362,11 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
nav_lon,
nav_roll,
nav_pitch,
- g.pitch_max);
-
+ (long)g.pitch_max);
+
print_motor_out();
}
-
+
if(Serial.available() > 0){
return (0);
}
@@ -382,7 +382,7 @@ test_adc(uint8_t argc, const Menu::arg *argv)
delay(1000);
Serial.printf_P(PSTR("ADC\n"));
delay(1000);
-
+
while(1){
for(int i = 0; i < 9; i++){
Serial.printf_P(PSTR("i:%d\t"),adc.Ch(i));
@@ -403,10 +403,10 @@ test_imu(uint8_t argc, const Menu::arg *argv)
print_hit_enter();
delay(1000);
-
+
//float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw;
-
-
+
+
while(1){
delay(20);
if (millis() - fast_loopTimer > 19) {
@@ -415,10 +415,10 @@ test_imu(uint8_t argc, const Menu::arg *argv)
fast_loopTimer = millis();
/*
Matrix3f temp = dcm.get_dcm_matrix();
-
+
sin_pitch = -temp.c.x;
cos_pitch = sqrt(1 - (temp.c.x * temp.c.x));
-
+
cos_roll = temp.c.z / cos_pitch;
sin_roll = temp.c.y / cos_pitch;
@@ -426,7 +426,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
yawvector.y = temp.b.x; // cos
yawvector.normalize();
cos_yaw = yawvector.y; // 0 x = north
- sin_yaw = yawvector.x; // 1 y
+ sin_yaw = yawvector.x; // 1 y
*/
// IMU
@@ -435,7 +435,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
Vector3f accels = imu.get_accel();
Vector3f gyros = imu.get_gyro();
-
+
update_trig();
if(g.compass_enabled){
@@ -446,7 +446,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
medium_loopCounter = 0;
}
}
-
+
// We are using the IMU
// ---------------------
Serial.printf_P(PSTR("A: %4.4f, %4.4f, %4.4f\t"
@@ -467,7 +467,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
cos_yaw_x, // x
sin_yaw_y); // y
}
-
+
if(Serial.available() > 0){
return (0);
}
@@ -479,35 +479,35 @@ test_gps(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
-
+
/*while(1){
delay(100);
-
+
update_GPS();
-
+
if(Serial.available() > 0){
return (0);
}
-
+
if(home.lng != 0){
break;
}
}*/
-
+
while(1){
delay(100);
update_GPS();
-
+
calc_distance_error();
-
- //if (gps->new_data){
+
+ //if (g_gps->new_data){
Serial.printf_P(PSTR("Lat: %3.8f, Lon: %3.8f, alt %dm, spd: %d dist:%d, #sats: %d\n"),
- ((float)gps->latitude / 10000000),
- ((float)gps->longitude / 10000000),
- (int)gps->altitude / 100,
- (int)gps->ground_speed,
+ ((float)g_gps->latitude / 10000000),
+ ((float)g_gps->longitude / 10000000),
+ (int)g_gps->altitude / 100,
+ (int)g_gps->ground_speed,
(int)wp_distance,
- (int)gps->num_sats);
+ (int)g_gps->num_sats);
//}else{
//Serial.print(".");
//}
@@ -525,7 +525,7 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
Serial.printf_P(PSTR("Gyro | Accel\n"));
Vector3f _cam_vector;
Vector3f _out_vector;
-
+
G_Dt = .02;
while(1){
@@ -535,10 +535,10 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
// ---
read_AHRS();
}
-
+
Matrix3f temp = dcm.get_dcm_matrix();
Matrix3f temp_t = dcm.get_dcm_transposed();
-
+
Serial.printf_P(PSTR("dcm\n"
"%4.4f \t %4.4f \t %4.4f \n"
"%4.4f \t %4.4f \t %4.4f \n"
@@ -552,10 +552,10 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
int _yaw = degrees(atan2(temp.b.x, temp.a.x));
Serial.printf_P(PSTR( "angles\n"
"%d \t %d \t %d\n\n"),
- _pitch,
+ _pitch,
_roll,
_yaw);
-
+
//_out_vector = _cam_vector * temp;
//Serial.printf_P(PSTR( "cam\n"
// "%d \t %d \t %d\n\n"),
@@ -576,7 +576,7 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
delay(1000);
Serial.printf_P(PSTR("Gyro | Accel\n"));
delay(1000);
-
+
while(1){
Vector3f accels = dcm.get_accel();
Serial.print("accels.z:");
@@ -601,7 +601,7 @@ test_omega(uint8_t argc, const Menu::arg *argv)
delay(1000);
Serial.printf_P(PSTR("Omega"));
delay(1000);
-
+
G_Dt = .02;
while(1){
@@ -610,9 +610,9 @@ test_omega(uint8_t argc, const Menu::arg *argv)
// ---
read_AHRS();
float my_oz = (dcm.yaw - old_yaw) * 50;
-
+
old_yaw = dcm.yaw;
-
+
ts_num++;
if (ts_num > 2){
ts_num = 0;
@@ -645,7 +645,7 @@ test_battery(uint8_t argc, const Menu::arg *argv)
Serial.println(battery_voltage4, 4);
#else
Serial.printf_P(PSTR("Not enabled\n"));
-
+
#endif
return (0);
}
@@ -655,7 +655,7 @@ test_current(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delta_ms_medium_loop = 100;
-
+
while(1){
delay(100);
read_radio();
@@ -668,7 +668,7 @@ test_current(uint8_t argc, const Menu::arg *argv)
APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
//}
-
+
if(Serial.available() > 0){
return (0);
}
@@ -684,7 +684,7 @@ test_relay(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
-
+
while(1){
Serial.println("Relay A");
relay_A();
@@ -692,7 +692,7 @@ test_relay(uint8_t argc, const Menu::arg *argv)
if(Serial.available() > 0){
return (0);
}
-
+
Serial.println("Relay B");
relay_B();
delay(3000);
@@ -707,24 +707,24 @@ test_wp(uint8_t argc, const Menu::arg *argv)
{
delay(1000);
read_EEPROM_waypoint_info();
-
+
// save the alitude above home option
if(g.RTL_altitude == -1){
Serial.printf_P(PSTR("Hold current altitude\n"));
}else{
- Serial.printf_P(PSTR("Hold altitude of %dm\n"), g.RTL_altitude);
+ Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude);
}
-
- Serial.printf_P(PSTR("%d waypoints\n"), g.waypoint_total);
- Serial.printf_P(PSTR("Hit radius: %d\n"), g.waypoint_radius);
- Serial.printf_P(PSTR("Loiter radius: %d\n\n"), g.loiter_radius);
-
+
+ Serial.printf_P(PSTR("%d waypoints\n"), (int)g.waypoint_total);
+ Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
+ Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
+
for(byte i = 0; i <= g.waypoint_total; i++){
struct Location temp = get_wp_with_index(i);
print_waypoint(&temp, i);
}
-
+
return (0);
}
@@ -736,7 +736,7 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
delay(1000);
Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
while(1){
- delay(250);
+ delay(250);
// Timeout set high enough for X-CTU RSSI Calc over XBee @ 115200
Serial3.printf_P(PSTR("0123456789:;<=>?@ABCDEFGHIJKLMNO\n"));
//Serial.print("X");
@@ -751,11 +751,11 @@ static int8_t
test_pressure(uint8_t argc, const Menu::arg *argv)
{
uint32_t sum;
-
+
Serial.printf_P(PSTR("Uncalibrated Abs Airpressure\n"));
Serial.printf_P(PSTR("Altitude is relative to the start of this test\n"));
print_hit_enter();
-
+
Serial.printf_P(PSTR("\nCalibrating....\n"));
/*
for (int i = 1; i < 301; i++) {
@@ -766,38 +766,38 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
}
abs_pressure_ground = (float)sum / 100.0;
*/
-
+
home.alt = 0;
wp_distance = 0;
init_pressure_ground();
-
+
while(1){
if (millis()-fast_loopTimer > 9) {
delta_ms_fast_loop = millis() - fast_loopTimer;
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
fast_loopTimer = millis();
-
-
+
+
calc_altitude_error();
calc_nav_throttle();
}
-
+
if (millis()-medium_loopTimer > 100) {
medium_loopTimer = millis();
read_radio(); // read the radio first
- next_WP.alt = home.alt + g.rc_6.control_in; // 0 - 2000 (20 meters)
+ next_WP.alt = home.alt + g.rc_6.control_in; // 0 - 2000 (20 meters)
read_trim_switch();
read_barometer();
- Serial.printf_P(PSTR("AP: %ld,\tAlt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \t out:%d\n"),
+ Serial.printf_P(PSTR("AP: %ld,\tAlt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \t out:%d\n"),
abs_pressure,
current_loc.alt,
next_WP.alt,
altitude_error,
g.,
g.rc_3.servo_out);
-
+
/*
Serial.print("Altitude: ");
Serial.print((int)current_loc.alt,DEC);
@@ -813,7 +813,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
//Serial.print(" Raw pressure value: ");
//Serial.println(abs_pressure);
}
-
+
if(Serial.available() > 0){
return (0);
}
@@ -829,7 +829,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
}else{
print_hit_enter();
while(1){
- delay(250);
+ delay(250);
compass.read();
compass.calculate(0,0);
Serial.printf_P(PSTR("Heading: ("));
@@ -857,20 +857,20 @@ void print_hit_enter()
void fake_out_gps()
{
static float rads;
- gps->new_data = true;
- gps->fix = true;
-
+ g_gps->new_data = true;
+ g_gps->fix = true;
+
int length = g.rc_6.control_in;
rads += .05;
-
+
if (rads > 6.28){
rads = 0;
}
-
- gps->latitude = 377696000; // Y
- gps->longitude = -1224319000; // X
- gps->altitude = 9000; // meters * 100
-
+
+ g_gps->latitude = 377696000; // Y
+ g_gps->longitude = -1224319000; // X
+ g_gps->altitude = 9000; // meters * 100
+
//next_WP.lng = home.lng - length * sin(rads); // X
//next_WP.lat = home.lat + length * cos(rads); // Y
}
@@ -878,9 +878,9 @@ void fake_out_gps()
void print_motor_out(){
- Serial.printf("out: R: %d, L: %d F: %d B: %d\n",
+ Serial.printf("out: R: %d, L: %d F: %d B: %d\n",
(motor_out[RIGHT] - g.rc_3.radio_min),
(motor_out[LEFT] - g.rc_3.radio_min),
(motor_out[FRONT] - g.rc_3.radio_min),
(motor_out[BACK] - g.rc_3.radio_min));
-}
\ No newline at end of file
+}