Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

updated Param gen - won't compile yet.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1666 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-02-17 07:09:13 +00:00
parent 637ca47b4a
commit 1716a41a97
32 changed files with 1845 additions and 784 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopterMega/APM_Config.h
View File

@ -2,7 +2,8 @@
// Once you upload the code, run the factory "reset" to save all config values to EEPROM. // Once you upload the code, run the factory "reset" to save all config values to EEPROM.
// After reset, use the setup mode to set your radio limits for CH1-4, and to set your flight modes. // After reset, use the setup mode to set your radio limits for CH1-4, and to set your flight modes.
#define GPS_PROTOCOL GPS_PROTOCOL_MTK // GPS is auto-selected
#define GCS_PROTOCOL GCS_PROTOCOL_NONE #define GCS_PROTOCOL GCS_PROTOCOL_NONE
//#define MAGORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK //#define MAGORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK

4
ArduCopterMega/APM_Config.h.reference
View File

@ -65,10 +65,10 @@
// //
// GPS_PROTOCOL_NONE No GPS attached // GPS_PROTOCOL_NONE No GPS attached
// GPS_PROTOCOL_IMU X-Plane interface or ArduPilot IMU. // GPS_PROTOCOL_IMU X-Plane interface or ArduPilot IMU.
// GPS_PROTOCOL_MTK MediaTek-based GPS. // GPS_PROTOCOL_MTK MediaTek-based GPS
// GPS_PROTOCOL_UBLOX UBLOX GPS // GPS_PROTOCOL_UBLOX UBLOX GPS
// GPS_PROTOCOL_SIRF SiRF-based GPS in Binary mode. NOT TESTED // GPS_PROTOCOL_SIRF SiRF-based GPS in Binary mode. NOT TESTED
// GPS_PROTOCOL_NMEA Standard NMEA GPS. NOT SUPPORTED (yet?) // GPS_PROTOCOL_NMEA Standard NMEA GPS NOT SUPPORTED (yet?)
// //
//#define GPS_PROTOCOL GPS_PROTOCOL_UBLOX //#define GPS_PROTOCOL GPS_PROTOCOL_UBLOX
// //

395
ArduCopterMega/ArduCopterMega.pde
View File

@ -12,6 +12,10 @@ License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. version 2.1 of the License, or (at your option) any later version.
*/ */
////////////////////////////////////////////////////////////////////////////////
// Header includes
////////////////////////////////////////////////////////////////////////////////
// AVR runtime // AVR runtime
#include <avr/io.h> #include <avr/io.h>
#include <avr/eeprom.h> #include <avr/eeprom.h>
@ -22,7 +26,7 @@ version 2.1 of the License, or (at your option) any later version.
#include <FastSerial.h> #include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <APM_RC.h> // ArduPilot Mega RC Library #include <APM_RC.h> // ArduPilot Mega RC Library
#include <RC_Channel.h> // ArduPilot Mega RC Library #include <RC_Channel.h> // RC Channel Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library #include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_GPS.h> // ArduPilot GPS library #include <AP_GPS.h> // ArduPilot GPS library
#include <Wire.h> // Arduino I2C lib #include <Wire.h> // Arduino I2C lib
@ -33,6 +37,7 @@ version 2.1 of the License, or (at your option) any later version.
#include <AP_IMU.h> // ArduPilot Mega IMU Library #include <AP_IMU.h> // ArduPilot Mega IMU Library
#include <AP_DCM.h> // ArduPilot Mega DCM Library #include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <PID.h> // ArduPilot Mega RC Library #include <PID.h> // ArduPilot Mega RC Library
//#include <GCS_MAVLink.h> // MAVLink GCS definitions
// Configuration // Configuration
@ -40,24 +45,52 @@ version 2.1 of the License, or (at your option) any later version.
// Local modules // Local modules
#include "defines.h" #include "defines.h"
#include "Parameters.h"
#include "global_data.h"
////////////////////////////////////////////////////////////////////////////////
// Serial ports // Serial ports
////////////////////////////////////////////////////////////////////////////////
// //
// Note that FastSerial port buffers are allocated at ::begin time, // Note that FastSerial port buffers are allocated at ::begin time,
// so there is not much of a penalty to defining ports that we don't // so there is not much of a penalty to defining ports that we don't
// use. // use.
//
FastSerialPort0(Serial); // FTDI/console FastSerialPort0(Serial); // FTDI/console
FastSerialPort1(Serial1); // GPS port (except for GPS_PROTOCOL_IMU) FastSerialPort1(Serial1); // GPS port (except for GPS_PROTOCOL_IMU)
FastSerialPort3(Serial3); // Telemetry port (optional, Standard and ArduPilot protocols only) FastSerialPort3(Serial3); // Telemetry port (optional, Standard and ArduPilot protocols only)
// standard sensors for live flight ////////////////////////////////////////////////////////////////////////////////
// Parameters
////////////////////////////////////////////////////////////////////////////////
//
// Global parameters are all contained within the 'g' class.
//
Parameters g;
////////////////////////////////////////////////////////////////////////////////
// Sensors
////////////////////////////////////////////////////////////////////////////////
//
// There are three basic options related to flight sensor selection.
//
// - Normal flight mode. Real sensors are used.
// - HIL Attitude mode. Most sensors are disabled, as the HIL
// protocol supplies attitude information directly.
// - HIL Sensors mode. Synthetic sensors are configured that
// supply data from the simulation.
//
//#if HIL_MODE == HIL_MODE_NONE
// real sensors
AP_ADC_ADS7844 adc; AP_ADC_ADS7844 adc;
APM_BMP085_Class APM_BMP085; APM_BMP085_Class APM_BMP085;
AP_Compass_HMC5843 compass; AP_Compass_HMC5843 compass;
// GPS selection // GPS selection
/*
#if GPS_PROTOCOL == GPS_PROTOCOL_NMEA #if GPS_PROTOCOL == GPS_PROTOCOL_NMEA
AP_GPS_NMEA GPS(&Serial1); AP_GPS_NMEA GPS(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF #elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
@ -73,15 +106,33 @@ AP_GPS_None GPS(NULL);
#else #else
# error Must define GPS_PROTOCOL in your configuration file. # error Must define GPS_PROTOCOL in your configuration file.
#endif #endif
*/
#if GPS_PROTOCOL == GPS_PROTOCOL_NONE
AP_GPS_None gps(NULL);
#else
GPS *gps;
AP_GPS_Auto GPS(&Serial1, &gps);
#endif // GPS PROTOCOL
AP_IMU_Oilpan imu(&adc, EE_IMU_OFFSET); AP_IMU_Oilpan imu(&adc, EE_IMU_OFFSET);
AP_DCM dcm(&imu, &GPS); AP_DCM dcm(&imu, &GPS);
//AP_DCM dcm(&imu, &gps, &compass); ////////////////////////////////////////////////////////////////////////////////
// GCS selection
////////////////////////////////////////////////////////////////////////////////
//
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
GCS_MAVLINK gcs;
#else
// If we are not using a GCS, we need a stub that does nothing.
GCS_Class gcs;
#endif
////////////////////////////////////////////////////////////////////////////////
// GENERAL VARIABLE DECLARATIONS // Global variables
// -------------------------------------------- ////////////////////////////////////////////////////////////////////////////////
byte control_mode = STABILIZE; byte control_mode = STABILIZE;
boolean failsafe = false; // did our throttle dip below the failsafe value? boolean failsafe = false; // did our throttle dip below the failsafe value?
@ -91,7 +142,7 @@ byte fbw_timer; // for limiting the execution of FBW input
const char *comma = ","; const char *comma = ",";
byte flight_modes[6]; //byte flight_modes[6];
const char* flight_mode_strings[] = { const char* flight_mode_strings[] = {
"ACRO", "ACRO",
"STABILIZE", "STABILIZE",
@ -117,55 +168,25 @@ const char* flight_mode_strings[] = {
// Radio // Radio
// ----- // -----
RC_Channel rc_1(EE_RADIO_1);
RC_Channel rc_2(EE_RADIO_2);
RC_Channel rc_3(EE_RADIO_3);
RC_Channel rc_4(EE_RADIO_4);
RC_Channel rc_5(EE_RADIO_5);
RC_Channel rc_6(EE_RADIO_6);
RC_Channel rc_7(EE_RADIO_7);
RC_Channel rc_8(EE_RADIO_8);
RC_Channel rc_camera_pitch(EE_RADIO_9);
RC_Channel rc_camera_roll(EE_RADIO_10);
int motor_out[4]; int motor_out[4];
byte flight_mode_channel; //byte flight_mode_channel;
byte frame_type = PLUS_FRAME; //byte frame_type = PLUS_FRAME;
// PIDs and gains
// ---------------
//Acro
PID pid_acro_rate_roll (EE_GAIN_1);
PID pid_acro_rate_pitch (EE_GAIN_2);
PID pid_acro_rate_yaw (EE_GAIN_3);
//Stabilize
PID pid_stabilize_roll (EE_GAIN_4);
PID pid_stabilize_pitch (EE_GAIN_5);
PID pid_yaw (EE_GAIN_6);
Vector3f omega; Vector3f omega;
// roll pitch
float stabilize_dampener;
int max_stabilize_dampener;
// yaw //float stabilize_dampener;
float hold_yaw_dampener; //float hold_yaw_dampener;
int max_yaw_dampener;
// used to transition yaw control from Rate control to Yaw hold // PIDs
boolean rate_yaw_flag; int max_stabilize_dampener; //
int max_yaw_dampener; //
boolean rate_yaw_flag; // used to transition yaw control from Rate control to Yaw hold
// Nav // LED output
PID pid_nav_lat (EE_GAIN_7); // ----------
PID pid_nav_lon (EE_GAIN_8); boolean motor_light; // status of the Motor safety
PID pid_baro_throttle (EE_GAIN_9); boolean GPS_light; // status of the GPS light
PID pid_sonar_throttle (EE_GAIN_10);
boolean motor_light;
// GPS variables // GPS variables
// ------------- // -------------
@ -173,22 +194,20 @@ byte ground_start_count = 5; // have we achieved first lock and set Home?
const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage
float scaleLongUp = 1; // used to reverse longtitude scaling float scaleLongUp = 1; // used to reverse longtitude scaling
float scaleLongDown = 1; // used to reverse longtitude scaling float scaleLongDown = 1; // used to reverse longtitude scaling
boolean GPS_light = false; // status of the GPS light
// Magnetometer variables
// ----------------------
Vector3f mag_offsets;
// Location & Navigation // Location & Navigation
// --------------------- // ---------------------
byte wp_radius = 3; // meters const float radius_of_earth = 6378100; // meters
const float gravity = 9.81; // meters/ sec^2
//byte wp_radius = 3; // meters
long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
byte loiter_radius; // meters
float x_track_gain;
int x_track_angle;
long alt_to_hold; // how high we should be for RTL
long nav_angle; // how much to pitch towards target
long pitch_max;
byte command_must_index; // current command memory location byte command_must_index; // current command memory location
byte command_may_index; // current command memory location byte command_may_index; // current command memory location
@ -210,22 +229,22 @@ float cos_yaw_x;
// Airspeed // Airspeed
// -------- // --------
int airspeed; // m/s * 100 int airspeed; // m/s * 100
float airspeed_error; // m / s * 100
// Throttle Failsafe // Throttle Failsafe
// ------------------ // ------------------
boolean motor_armed = false; boolean motor_armed = false;
boolean motor_auto_safe = false; boolean motor_auto_safe = false;
byte throttle_failsafe_enabled; //byte throttle_failsafe_enabled;
int throttle_failsafe_value; //int throttle_failsafe_value;
byte throttle_failsafe_action; //byte throttle_failsafe_action;
uint16_t log_bitmask; //uint16_t log_bitmask;
// Location Errors // 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 long altitude_error; // meters * 100 we are off in altitude
float airspeed_error; // m / s * 100
float crosstrack_error; // meters we are off trackline float crosstrack_error; // meters we are off trackline
long distance_error; // distance to the WP long distance_error; // distance to the WP
long yaw_error; // how off are we pointed long yaw_error; // how off are we pointed
@ -244,37 +263,27 @@ float current_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2
float current_amps; float current_amps;
float current_total; float current_total;
int milliamp_hours; int milliamp_hours;
boolean current_enabled = false; //boolean current_enabled = false;
// Magnetometer variables // Magnetometer variables
// ---------------------- // ----------------------
int magnetom_x; //int magnetom_x;
int magnetom_y; //int magnetom_y;
int magnetom_z; //int magnetom_z;
float MAG_Heading;
float mag_offset_x; //float mag_offset_x;
float mag_offset_y; //float mag_offset_y;
float mag_offset_z; //float mag_offset_z;
float mag_declination; //float mag_declination;
bool compass_enabled; //bool compass_enabled;
// Barometer Sensor variables // Barometer Sensor variables
// -------------------------- // --------------------------
int baro_offset; // used to correct drift of absolute pressue sensor //int baro_offset; // used to correct drift of absolute pressue sensor
unsigned long abs_pressure; unsigned long abs_pressure;
unsigned long abs_pressure_ground; unsigned long abs_pressure_ground;
int ground_temperature; int ground_temperature;
int temp_unfilt; //int temp_unfilt;
// From IMU
// --------
//long roll_sensor; // degrees * 100
//long pitch_sensor; // degrees * 100
//long yaw_sensor; // degrees * 100
float roll; // radians
float pitch; // radians
float yaw; // radians
byte altitude_sensor = BARO; // used to know whic sensor is active, BARO or SONAR byte altitude_sensor = BARO; // used to know whic sensor is active, BARO or SONAR
@ -282,10 +291,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 takeoff_complete = false; // Flag for using take-off controls
boolean land_complete = false; boolean land_complete = false;
int landing_pitch; // pitch for landing set by commands int takeoff_altitude;
//int takeoff_pitch; int landing_distance; // meters;
int takeoff_altitude; long old_alt; // used for managing altitude rates
int landing_distance; // meters; int velocity_land;
// Loiter management // Loiter management
// ----------------- // -----------------
@ -304,7 +313,7 @@ long nav_yaw; // deg * 100 : target yaw angle
long nav_lat; // for error calcs long nav_lat; // for error calcs
long nav_lon; // for error calcs long nav_lon; // for error calcs
int nav_throttle; // 0-1000 for throttle control int nav_throttle; // 0-1000 for throttle control
int nav_throttle_old; // 0-1000 for throttle control int nav_throttle_old; // for filtering
long command_yaw_start; // what angle were we to begin with long command_yaw_start; // what angle were we to begin with
long command_yaw_start_time; // when did we start turning long command_yaw_start_time; // when did we start turning
@ -313,23 +322,14 @@ long command_yaw_end; // what angle are we trying to be
long command_yaw_delta; // how many degrees will we turn long command_yaw_delta; // how many degrees will we turn
int command_yaw_speed; // how fast to turn int command_yaw_speed; // how fast to turn
byte command_yaw_dir; byte command_yaw_dir;
long old_alt; // used for managing altitude rates
int velocity_land;
long altitude_estimate; // for smoothing GPS output
long distance_estimate; // for smoothing GPS output
int throttle_min; // 0 - 1000 : Min throttle output - copter should be 0
int throttle_cruise; // 0 - 1000 : what will make the copter hover
int throttle_max; // 0 - 1000 : Max throttle output
// Waypoints // Waypoints
// --------- // ---------
long GPS_wp_distance; // meters - distance between plane and next waypoint long GPS_wp_distance; // meters - distance between plane and next waypoint
long wp_distance; // meters - distance between plane and next waypoint long wp_distance; // meters - distance between plane and next waypoint
long wp_totalDistance; // meters - distance between old and next waypoint long wp_totalDistance; // meters - distance between old and next waypoint
byte wp_total; // # of Commands total including way //byte wp_total; // # of Commands total including way
byte wp_index; // Current active command index //byte wp_index; // Current active command index
byte next_wp_index; // Current active command index byte next_wp_index; // Current active command index
// repeating event control // repeating event control
@ -358,7 +358,7 @@ struct Location tell_command; // command for telemetry
struct Location next_command; // command preloaded struct Location next_command; // command preloaded
long target_altitude; // used for long target_altitude; // used for
long offset_altitude; // used for long offset_altitude; // used for
boolean home_is_set = false; // Flag for if we have gps lock and have set the home location boolean home_is_set; // Flag for if we have gps lock and have set the home location
// IMU variables // IMU variables
@ -408,50 +408,13 @@ unsigned long dTnav2; // Delta Time in milliseconds for navigation computa
unsigned long elapsedTime; // for doing custom events unsigned long elapsedTime; // for doing custom events
float load; // % MCU cycles used float load; // % MCU cycles used
byte FastLoopGate = 9;
////////////////////////////////////////////////////////////////////////////////
// Top-level logic
////////////////////////////////////////////////////////////////////////////////
// AC generic variables for future use
byte gled_status = HIGH;
long gled_timer;
int gled_speed = 200;
long cli_timer;
byte cli_status = LOW;
byte cli_step;
byte fled_status;
byte res1;
byte res2;
byte res3;
byte res4;
byte res5;
byte cam_mode;
byte cam1;
byte cam2;
byte cam3;
int ires1;
int ires2;
int ires3;
int ires4;
boolean SW_DIP1; // closest to SW2 slider switch
boolean SW_DIP2;
boolean SW_DIP3;
boolean SW_DIP4; // closest to header pins
// Basic Initialization
//---------------------
void setup() { void setup() {
init_ardupilot(); init_ardupilot();
#if ENABLE_EXTRAINIT
init_extras();
#endif
} }
void loop() void loop()
@ -477,13 +440,14 @@ void loop()
medium_loop(); medium_loop();
if (millis() - perf_mon_timer > 20000) { if (millis() - perf_mon_timer > 20000) {
send_message(MSG_PERF_REPORT); if (mainLoop_count != 0) {
if (log_bitmask & MASK_LOG_PM){ gcs.send_message(MSG_PERF_REPORT);
Log_Write_Performance(); if (g.log_bitmask & MASK_LOG_PM){
} Log_Write_Performance();
resetPerfData(); }
perf_mon_timer = millis(); resetPerfData();
} }
}
} }
} }
@ -493,8 +457,8 @@ void fast_loop()
// IMU DCM Algorithm // IMU DCM Algorithm
read_AHRS(); read_AHRS();
// This is the fast loop - we want it to execute at 200Hz if possible // 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; G_Dt_max = delta_ms_fast_loop;
@ -526,10 +490,12 @@ void medium_loop()
case 0: case 0:
medium_loopCounter++; medium_loopCounter++;
update_GPS(); update_GPS();
readCommands(); //readCommands();
if(compass_enabled){
compass.read(); // Read magnetometer if(g.compass_enabled){
compass.calculate(roll, pitch); // Calculate heading compass.read(); // Read magnetometer
compass.calculate(dcm.roll, dcm.pitch); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
} }
break; break;
@ -539,8 +505,8 @@ void medium_loop()
case 1: case 1:
medium_loopCounter++; medium_loopCounter++;
if(GPS.new_data){ if(gps->new_data){
GPS.new_data = false; gps->new_data = false;
dTnav = millis() - nav_loopTimer; dTnav = millis() - nav_loopTimer;
nav_loopTimer = millis(); nav_loopTimer = millis();
@ -580,26 +546,26 @@ void medium_loop()
case 3: case 3:
medium_loopCounter++; medium_loopCounter++;
if (log_bitmask & MASK_LOG_ATTITUDE_MED && (log_bitmask & MASK_LOG_ATTITUDE_FAST == 0)) 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); Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
if (log_bitmask & MASK_LOG_CTUN) if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning(); Log_Write_Control_Tuning();
if (log_bitmask & MASK_LOG_NTUN) if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning(); Log_Write_Nav_Tuning();
if (log_bitmask & MASK_LOG_GPS){ if (g.log_bitmask & MASK_LOG_GPS){
if(home_is_set) 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(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);
} }
send_message(MSG_ATTITUDE); // Sends attitude data gcs.send_message(MSG_ATTITUDE); // Sends attitude data
break; break;
// This case controls the slow loop // This case controls the slow loop
//--------------------------------- //---------------------------------
case 4: case 4:
if (current_enabled){ if (g.current_enabled){
read_current(); read_current();
} }
@ -629,33 +595,22 @@ void medium_loop()
// guess how close we are - fixed observer calc // guess how close we are - fixed observer calc
calc_distance_error(); calc_distance_error();
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
if (log_bitmask & MASK_LOG_ATTITUDE_FAST)
Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor); Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
if (log_bitmask & MASK_LOG_RAW) if (g.log_bitmask & MASK_LOG_RAW)
Log_Write_Raw(); Log_Write_Raw();
#if GCS_PROTOCOL == 6 // This is here for Benjamin Pelletier. Please do not remove without checking with me. Doug W #if GCS_PROTOCOL == 6 // This is here for Benjamin Pelletier. Please do not remove without checking with me. Doug W
readgcsinput(); readgcsinput();
#endif #endif
#if ENABLE_HIL
output_HIL();
#endif
#if ENABLE_CAM #if ENABLE_CAM
camera_stabilization(); camera_stabilization();
#endif #endif
#if ENABLE_AM // kick the GCS to process uplink data
flight_lights(); gcs.update();
#endif
#if ENABLE_xx
do_something_usefull();
#endif
} }
@ -666,32 +621,28 @@ void slow_loop()
switch (slow_loopCounter){ switch (slow_loopCounter){
case 0: case 0:
slow_loopCounter++; slow_loopCounter++;
superslow_loopCounter++; superslow_loopCounter++;
if(superslow_loopCounter >= 15) { if(superslow_loopCounter == 30) {
// keep track of what page is in use in the log
// *** We need to come up with a better scheme to handle this...
eeprom_write_word((uint16_t *) EE_LAST_LOG_PAGE, DataFlash.GetWritePage());
superslow_loopCounter = 0;
// save current data to the flash // save current data to the flash
if (log_bitmask & MASK_LOG_CUR) if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current(); 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; break;
case 1: case 1:
slow_loopCounter++; slow_loopCounter++;
//Serial.println(stabilize_rate_roll_pitch,3);
// Read 3-position switch on radio // Read 3-position switch on radio
// ------------------------------- // -------------------------------
read_control_switch(); read_control_switch();
//Serial.print("I: ")
//Serial.println(rc_1.get_integrator(), 1);
// Read main battery voltage if hooked up - does not read the 5v from radio // Read main battery voltage if hooked up - does not read the 5v from radio
// ------------------------------------------------------------------------ // ------------------------------------------------------------------------
#if BATTERY_EVENT == 1 #if BATTERY_EVENT == 1
@ -704,6 +655,17 @@ void slow_loop()
slow_loopCounter = 0; slow_loopCounter = 0;
update_events(); update_events();
// XXX this should be a "GCS slow loop" interface
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(1,5);
// send all requested output streams with rates requested
// between 1 and 5 Hz
#else
gcs.send_message(MSG_LOCATION);
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; break;
@ -716,19 +678,21 @@ void slow_loop()
void update_GPS(void) void update_GPS(void)
{ {
GPS.update(); gps->update();
update_GPS_light(); update_GPS_light();
// !!! comment out after testing // !!! comment out after testing
//fake_out_gps(); //fake_out_gps();
if (GPS.new_data && GPS.fix) { //if (gps->new_data && gps->fix) {
if (gps->new_data){
send_message(MSG_LOCATION); send_message(MSG_LOCATION);
// for performance // for performance
// --------------- // ---------------
gps_fix_count++; gps_fix_count++;
//Serial.printf("gs: %d\n", (int)ground_start_count);
if(ground_start_count > 1){ if(ground_start_count > 1){
ground_start_count--; ground_start_count--;
@ -738,45 +702,55 @@ void update_GPS(void)
// so that the altitude is more accurate // so that the altitude is more accurate
// ------------------------------------- // -------------------------------------
if (current_loc.lat == 0) { if (current_loc.lat == 0) {
Serial.println("!! bad loc"); SendDebugln("!! bad loc");
ground_start_count = 5; ground_start_count = 5;
} else { } else {
//Serial.printf("init Home!");
if (log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG); Log_Write_Startup(TYPE_GROUNDSTART_MSG);
// reset our nav loop timer // reset our nav loop timer
nav_loopTimer = millis(); nav_loopTimer = millis();
init_home(); init_home();
// init altitude // init altitude
current_loc.alt = GPS.altitude; current_loc.alt = gps->altitude;
ground_start_count = 0; ground_start_count = 0;
} }
} }
/* disabled for now /* disabled for now
// baro_offset is an integrator for the gps altitude error // baro_offset is an integrator for the gps altitude error
baro_offset += altitude_gain * (float)(GPS.altitude - current_loc.alt); baro_offset += altitude_gain * (float)(gps->altitude - current_loc.alt);
*/ */
current_loc.lng = GPS.longitude; // Lon * 10 * *7 current_loc.lng = gps->longitude; // Lon * 10 * *7
current_loc.lat = GPS.latitude; // Lat * 10 * *7 current_loc.lat = 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,
(int)wp_distance,
(int)gps->num_sats);
*/
}else{
//Serial.println("No fix");
} }
} }
void update_current_flight_mode(void) void update_current_flight_mode(void)
{ {
if(control_mode == AUTO){ if(control_mode == AUTO){
//Serial.print("!");
//crash_checker();
switch(command_must_ID){ switch(command_must_ID){
//case CMD_TAKEOFF: //case MAV_CMD_NAV_TAKEOFF:
// break; // break;
//case CMD_LAND: //case MAV_CMD_NAV_LAND:
// break; // break;
default: default:
@ -825,14 +799,14 @@ void update_current_flight_mode(void)
// ---------------------------- // ----------------------------
// Roll control // Roll control
if(abs(rc_1.control_in) >= ACRO_RATE_TRIGGER){ if(abs(g.rc_1.control_in) >= ACRO_RATE_TRIGGER){
output_rate_roll(); // rate control yaw output_rate_roll(); // rate control yaw
}else{ }else{
output_stabilize_roll(); // hold yaw output_stabilize_roll(); // hold yaw
} }
// Roll control // Roll control
if(abs(rc_2.control_in) >= ACRO_RATE_TRIGGER){ if(abs(g.rc_2.control_in) >= ACRO_RATE_TRIGGER){
output_rate_pitch(); // rate control yaw output_rate_pitch(); // rate control yaw
}else{ }else{
output_stabilize_pitch(); // hold yaw output_stabilize_pitch(); // hold yaw
@ -880,9 +854,9 @@ void update_current_flight_mode(void)
dTnav = 200; dTnav = 200;
} }
next_WP.lng = home.lng + 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 // forward is negative so we reverse it to get a positive North translation
next_WP.lat = home.lat - 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 // Output Pitch, Roll, Yaw and Throttle
@ -910,9 +884,9 @@ void update_current_flight_mode(void)
nav_pitch = 0; nav_pitch = 0;
nav_roll = 0; nav_roll = 0;
//if(rc_3.control_in) //if(g.rc_3.control_in)
// get desired height from the throttle // get desired height from the throttle
next_WP.alt = home.alt + (rc_3.control_in * 4); // 0 - 1000 (40 meters) next_WP.alt = home.alt + (g.rc_3.control_in * 4); // 0 - 1000 (40 meters)
// !!! testing // !!! testing
//next_WP.alt -= 500; //next_WP.alt -= 500;
@ -1001,7 +975,6 @@ void update_navigation()
if(control_mode == AUTO){ if(control_mode == AUTO){
verify_must(); verify_must();
verify_may(); verify_may();
}else{ }else{
switch(control_mode){ switch(control_mode){
case RTL: case RTL:
@ -1017,7 +990,7 @@ void read_AHRS(void)
// Perform IMU calculations and get attitude info // Perform IMU calculations and get attitude info
//----------------------------------------------- //-----------------------------------------------
dcm.update_DCM(G_Dt); dcm.update_DCM(G_Dt);
omega = dcm.get_gyro(); omega = dcm.get_gyro();
// Testing remove !!! // Testing remove !!!
//dcm.pitch_sensor = 0; //dcm.pitch_sensor = 0;

78
ArduCopterMega/Attitude.pde
View File

@ -4,8 +4,8 @@ void init_pids()
// create limits to how much dampening we'll allow // create limits to how much dampening we'll allow
// this creates symmetry with the P gain value preventing oscillations // this creates symmetry with the P gain value preventing oscillations
max_stabilize_dampener = 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 = pid_yaw.kP() * 6000; // = .5 * 6000 = 3000 max_yaw_dampener = g.pid_yaw.kP() * 6000; // = .5 * 6000 = 3000
} }
@ -13,16 +13,16 @@ void control_nav_mixer()
{ {
// control +- 45° is mixed with the navigation request by the Autopilot // control +- 45° is mixed with the navigation request by the Autopilot
// output is in degrees = target pitch and roll of copter // output is in degrees = target pitch and roll of copter
rc_1.servo_out = rc_1.control_mix(nav_roll); g.rc_1.servo_out = g.rc_1.control_mix(nav_roll);
rc_2.servo_out = rc_2.control_mix(nav_pitch); g.rc_2.servo_out = g.rc_2.control_mix(nav_pitch);
} }
void fbw_nav_mixer() void fbw_nav_mixer()
{ {
// control +- 45° is mixed with the navigation request by the Autopilot // control +- 45° is mixed with the navigation request by the Autopilot
// output is in degrees = target pitch and roll of copter // output is in degrees = target pitch and roll of copter
rc_1.servo_out = nav_roll; g.rc_1.servo_out = nav_roll;
rc_2.servo_out = nav_pitch; g.rc_2.servo_out = nav_pitch;
} }
void output_stabilize_roll() void output_stabilize_roll()
@ -30,22 +30,22 @@ void output_stabilize_roll()
float error, rate; float error, rate;
int dampener; int dampener;
error = 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 // limit the error we're feeding to the PID
error = constrain(error, -2500, 2500); error = constrain(error, -2500, 2500);
// write out angles back to servo out - this will be converted to PWM by RC_Channel // write out angles back to servo out - this will be converted to PWM by RC_Channel
rc_1.servo_out = pid_stabilize_roll.get_pid(error, delta_ms_fast_loop, 1.0); g.rc_1.servo_out = g.g.pid_stabilize_roll.get_pid(error, delta_ms_fast_loop, 1.0);
// We adjust the output by the rate of rotation: // We adjust the output by the rate of rotation:
// Rate control through bias corrected gyro rates // Rate control through bias corrected gyro rates
// omega is the raw gyro reading // omega is the raw gyro reading
// Limit dampening to be equal to propotional term for symmetry // Limit dampening to be equal to propotional term for symmetry
rate = degrees(omega.x) * 100.0; // 6rad = 34377 rate = degrees(omega.x) * 100.0; // 6rad = 34377
dampener = (rate * stabilize_dampener); // 34377 * .175 = 6000 dampener = (rate * g.stabilize_dampener); // 34377 * .175 = 6000
rc_1.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP g.rc_1.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP
} }
void output_stabilize_pitch() void output_stabilize_pitch()
@ -53,22 +53,22 @@ void output_stabilize_pitch()
float error, rate; float error, rate;
int dampener; int dampener;
error = rc_2.servo_out - dcm.pitch_sensor; error = g.rc_2.servo_out - dcm.pitch_sensor;
// limit the error we're feeding to the PID // limit the error we're feeding to the PID
error = constrain(error, -2500, 2500); error = constrain(error, -2500, 2500);
// write out angles back to servo out - this will be converted to PWM by RC_Channel // write out angles back to servo out - this will be converted to PWM by RC_Channel
rc_2.servo_out = pid_stabilize_pitch.get_pid(error, delta_ms_fast_loop, 1.0); g.rc_2.servo_out = g.pid_stabilize_pitch.get_pid(error, delta_ms_fast_loop, 1.0);
// We adjust the output by the rate of rotation: // We adjust the output by the rate of rotation:
// Rate control through bias corrected gyro rates // Rate control through bias corrected gyro rates
// omega is the raw gyro reading // omega is the raw gyro reading
// Limit dampening to be equal to propotional term for symmetry // Limit dampening to be equal to propotional term for symmetry
rate = degrees(omega.y) * 100.0; // 6rad = 34377 rate = degrees(omega.y) * 100.0; // 6rad = 34377
dampener = (rate * stabilize_dampener); // 34377 * .175 = 6000 dampener = (rate * g.stabilize_dampener); // 34377 * .175 = 6000
rc_2.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP g.rc_2.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP
} }
void void
@ -113,23 +113,23 @@ void output_yaw_with_hold(boolean hold)
yaw_error = constrain(yaw_error, -6000, 6000); // limit error to 60 degees yaw_error = constrain(yaw_error, -6000, 6000); // limit error to 60 degees
// Apply PID and save the new angle back to RC_Channel // Apply PID and save the new angle back to RC_Channel
rc_4.servo_out = pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); // .5 * 6000 = 3000 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 // We adjust the output by the rate of rotation
long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377 long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377
int dampener = ((float)rate * hold_yaw_dampener); // 18000 * .17 = 3000 int dampener = ((float)rate * g.hold_yaw_dampener); // 18000 * .17 = 3000
// Limit dampening to be equal to propotional term for symmetry // Limit dampening to be equal to propotional term for symmetry
rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000 g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000
}else{ }else{
// rate control // rate control
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun! rate = constrain(rate, -36000, 36000); // limit to something fun!
long error = ((long)rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000 long error = ((long)g.rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000
// -error = CCW, +error = CW // -error = CCW, +error = CW
rc_4.servo_out = pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700 g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
rc_4.servo_out = constrain(rc_4.servo_out, -2400, 2400); // limit to 2400 g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2400, 2400); // limit to 2400
} }
} }
@ -141,10 +141,10 @@ void output_rate_roll()
// rate control // rate control
long rate = degrees(omega.x) * 100; // 3rad = 17188 , 6rad = 34377 long rate = degrees(omega.x) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun! rate = constrain(rate, -36000, 36000); // limit to something fun!
long error = ((long)rc_1.control_in * 8) - rate; // control is += 4500 * 8 = 36000 long error = ((long)g.rc_1.control_in * 8) - rate; // control is += 4500 * 8 = 36000
rc_1.servo_out = pid_acro_rate_roll.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700 g.rc_1.servo_out = g.pid_acro_rate_roll.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
rc_1.servo_out = constrain(rc_1.servo_out, -2400, 2400); // limit to 2400 g.rc_1.servo_out = constrain(g.rc_1.servo_out, -2400, 2400); // limit to 2400
} }
void output_rate_pitch() void output_rate_pitch()
@ -152,28 +152,28 @@ void output_rate_pitch()
// rate control // rate control
long rate = degrees(omega.y) * 100; // 3rad = 17188 , 6rad = 34377 long rate = degrees(omega.y) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun! rate = constrain(rate, -36000, 36000); // limit to something fun!
long error = ((long)rc_2.control_in * 8) - rate; // control is += 4500 * 8 = 36000 long error = ((long)g.rc_2.control_in * 8) - rate; // control is += 4500 * 8 = 36000
rc_2.servo_out = pid_acro_rate_pitch.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700 g.rc_2.servo_out = g.pid_acro_rate_pitch.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
rc_2.servo_out = constrain(rc_2.servo_out, -2400, 2400); // limit to 2400 g.rc_2.servo_out = constrain(g.rc_2.servo_out, -2400, 2400); // limit to 2400
} }
/* /*
rc_1.servo_out = rc_2.control_in; g.rc_1.servo_out = g.rc_2.control_in;
rc_2.servo_out = rc_2.control_in; g.rc_2.servo_out = g.rc_2.control_in;
// Rate control through bias corrected gyro rates // Rate control through bias corrected gyro rates
// omega is the raw gyro reading plus Omega_I, so it´s bias corrected // omega is the raw gyro reading plus Omega_I, so it´s bias corrected
rc_1.servo_out -= (omega.x * 5729.57795 * acro_dampener); g.rc_1.servo_out -= (omega.x * 5729.57795 * acro_dampener);
rc_2.servo_out -= (omega.y * 5729.57795 * acro_dampener); g.rc_2.servo_out -= (omega.y * 5729.57795 * acro_dampener);
//Serial.printf("\trated out %d, omega ", rc_1.servo_out); //Serial.printf("\trated out %d, omega ", g.rc_1.servo_out);
//Serial.print((Omega[0] * 5729.57795 * stabilize_rate_roll_pitch), 3); //Serial.print((Omega[0] * 5729.57795 * stabilize_rate_roll_pitch), 3);
// Limit output // Limit output
rc_1.servo_out = constrain(rc_1.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT); g.rc_1.servo_out = constrain(g.rc_1.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT);
rc_2.servo_out = constrain(rc_2.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT); g.rc_2.servo_out = constrain(g.rc_2.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT);
*/ */
//} //}
@ -182,10 +182,10 @@ void output_rate_pitch()
// Keeps outdated data out of our calculations // Keeps outdated data out of our calculations
void reset_I(void) void reset_I(void)
{ {
pid_nav_lat.reset_I(); g.pid_nav_lat.reset_I();
pid_nav_lon.reset_I(); g.pid_nav_lon.reset_I();
pid_baro_throttle.reset_I(); g.pid_baro_throttle.reset_I();
pid_sonar_throttle.reset_I(); g.pid_sonar_throttle.reset_I();
} }

8
ArduCopterMega/Camera.pde
View File

@ -1,9 +1,9 @@
void init_camera() void init_camera()
{ {
rc_camera_pitch.set_angle(4500); rc_camera_pitch.set_angle(4500);
rc_camera_pitch.radio_min = rc_6.radio_min; rc_camera_pitch.radio_min = g.rc_6.radio_min;
rc_camera_pitch.radio_trim = rc_6.radio_trim; rc_camera_pitch.radio_trim = g.rc_6.radio_trim;
rc_camera_pitch.radio_max = rc_6.radio_max; rc_camera_pitch.radio_max = g.rc_6.radio_max;
rc_camera_roll.set_angle(4500); rc_camera_roll.set_angle(4500);
rc_camera_roll.radio_min = 1000; rc_camera_roll.radio_min = 1000;
@ -33,7 +33,7 @@ camera_stabilization()
//If you want to do control mixing use this function. //If you want to do control mixing use this function.
// set servo_out to the control input from radio // set servo_out to the control input from radio
//rc_camera_roll = rc_2.control_mix(dcm.pitch_sensor); //rc_camera_roll = g.rc_2.control_mix(dcm.pitch_sensor);
//rc_camera_roll.calc_pwm(); //rc_camera_roll.calc_pwm();
} }

390
ArduCopterMega/EEPROM.txt Normal file
View File

@ -0,0 +1,390 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// ************************************************************************************
// This function gets critical data from EEPROM to get us underway if restarting in air
// ************************************************************************************
// Macro functions
// ---------------
void read_EEPROM_startup(void)
{
read_EEPROM_PID();
read_EEPROM_frame();
read_EEPROM_throttle();
read_EEPROM_logs();
read_EEPROM_flight_modes();
read_EEPROM_waypoint_info();
imu.load_gyro_eeprom();
imu.load_accel_eeprom();
read_EEPROM_alt_RTL();
read_EEPROM_current();
read_EEPROM_nav();
// magnatometer
read_EEPROM_compass();
read_EEPROM_compass_declination();
read_EEPROM_compass_offset();
}
void read_EEPROM_airstart_critical(void)
{
// Read the home location
//-----------------------
home = get_wp_with_index(0);
// Read pressure sensor values
//----------------------------
read_EEPROM_pressure();
}
void save_EEPROM_groundstart(void)
{
g.rc_1.save_trim();
g.rc_2.save_trim();
g.rc_3.save_trim();
g.rc_4.save_trim();
g.rc_5.save_trim();
g.rc_6.save_trim();
g.rc_7.save_trim();
g.rc_8.save_trim();
// pressure sensor data saved by init_home
}
/********************************************************************************/
long read_alt_to_hold()
{
read_EEPROM_alt_RTL();
if(g.RTL_altitude == -1)
return current_loc.alt;
else
return g.RTL_altitude + home.alt;
}
/********************************************************************************/
void save_EEPROM_alt_RTL(void)
{
eeprom_write_dword((uint32_t *)EE_ALT_HOLD_HOME, alt_to_hold);
}
void read_EEPROM_alt_RTL(void)
{
alt_to_hold = eeprom_read_dword((const uint32_t *) EE_ALT_HOLD_HOME);
}
/********************************************************************************/
void read_EEPROM_waypoint_info(void)
{
g.waypoint_total = eeprom_read_byte((uint8_t *) EE_WP_TOTAL);
wp_radius = eeprom_read_byte((uint8_t *) EE_WP_RADIUS);
loiter_radius = eeprom_read_byte((uint8_t *) EE_LOITER_RADIUS);
}
void save_EEPROM_waypoint_info(void)
{
eeprom_write_byte((uint8_t *) EE_WP_TOTAL, g.waypoint_total);
eeprom_write_byte((uint8_t *) EE_WP_RADIUS, wp_radius);
eeprom_write_byte((uint8_t *) EE_LOITER_RADIUS, loiter_radius);
}
/********************************************************************************/
void read_EEPROM_nav(void)
{
// for nav estimation
distance_gain = read_EE_compressed_float(EE_DISTANCE_GAIN, 4);
altitude_gain = read_EE_compressed_float(EE_ALTITUDE_GAIN, 4);
// stored as degree * 100
g.crosstrack_gain = read_EE_compressed_float(EE_XTRACK_GAIN, 4);
g.crosstrack_entry_angle = eeprom_read_word((uint16_t *) EE_XTRACK_ANGLE) * 100;
pitch_max = eeprom_read_word((uint16_t *) EE_PITCH_MAX); // stored as degress * 100
}
void save_EEPROM_nav(void)
{
// for nav estimation
write_EE_compressed_float(altitude_gain, EE_ALTITUDE_GAIN, 4);
write_EE_compressed_float(distance_gain, EE_DISTANCE_GAIN, 4);
write_EE_compressed_float(crosstrack_gain, EE_XTRACK_GAIN, 4);
// stored as degrees
eeprom_write_word((uint16_t *) EE_XTRACK_ANGLE, g.crosstrack_entry_angle / 100);
// stored as degrees
eeprom_write_word((uint16_t *) EE_PITCH_MAX, pitch_max);
}
/********************************************************************************/
void read_EEPROM_PID(void)
{
g.pid_acro_rate_roll.load_gains();
g.pid_acro_rate_pitch.load_gains();
g.pid_acro_rate_yaw.load_gains();
g.pid_stabilize_roll.load_gains();
g.pid_stabilize_pitch.load_gains();
g.pid_yaw.load_gains();
g.pid_nav_lon.load_gains();
g.pid_nav_lat.load_gains();
g.pid_baro_throttle.load_gains();
g.pid_sonar_throttle.load_gains();
// roll pitch
stabilize_dampener = read_EE_compressed_float(EE_STAB_DAMPENER, 4);
// yaw
hold_yaw_dampener = read_EE_compressed_float(EE_HOLD_YAW_DAMPENER, 4);
init_pids();
}
void save_EEPROM_PID(void)
{
g.pid_acro_rate_roll.save_gains();
g.pid_acro_rate_pitch.save_gains();
g.pid_acro_rate_yaw.save_gains();
g.pid_stabilize_roll.save_gains();
g.pid_stabilize_pitch.save_gains();
g.pid_yaw.save_gains();
g.pid_nav_lon.save_gains();
g.pid_nav_lat.save_gains();
g.pid_baro_throttle.save_gains();
g.pid_sonar_throttle.save_gains();
// roll pitch
write_EE_compressed_float(stabilize_dampener, EE_STAB_DAMPENER, 4);
// yaw
write_EE_compressed_float(hold_yaw_dampener, EE_HOLD_YAW_DAMPENER, 4);
}
/********************************************************************************/
void save_EEPROM_frame(void)
{
eeprom_write_byte((uint8_t *)EE_FRAME, frame_type);
}
void read_EEPROM_frame(void)
{
frame_type = eeprom_read_byte((uint8_t *) EE_FRAME);
}
/********************************************************************************/
void save_EEPROM_g.(void)
{
eeprom_write_word((uint16_t *)EE_THROTTLE_CRUISE, g.);
}
void read_EEPROM_g.(void)
{
g.throttle_cruise = eeprom_read_word((uint16_t *) EE_THROTTLE_CRUISE);
}
/********************************************************************************/
void save_EEPROM_mag_declination(void)
{
write_EE_compressed_float(mag_declination, EE_MAG_DECLINATION, 1);
}
void read_EEPROM_compass_declination(void)
{
mag_declination = read_EE_compressed_float(EE_MAG_DECLINATION, 1);
}
/********************************************************************************/
void save_EEPROM_current(void)
{
eeprom_write_byte((uint8_t *) EE_CURRENT_SENSOR, current_enabled);
eeprom_write_word((uint16_t *) EE_CURRENT_MAH, milliamp_hours);
}
void read_EEPROM_current(void)
{
current_enabled = eeprom_read_byte((uint8_t *) EE_CURRENT_SENSOR);
milliamp_hours = eeprom_read_word((uint16_t *) EE_CURRENT_MAH);
}
/********************************************************************************/
/*
void save_EEPROM_mag_offset(void)
{
write_EE_compressed_float(mag_offset_x, EE_MAG_X, 2);
write_EE_compressed_float(mag_offset_y, EE_MAG_Y, 2);
write_EE_compressed_float(mag_offset_z, EE_MAG_Z, 2);
}
void read_EEPROM_compass_offset(void)
{
mag_offset_x = read_EE_compressed_float(EE_MAG_X, 2);
mag_offset_y = read_EE_compressed_float(EE_MAG_Y, 2);
mag_offset_z = read_EE_compressed_float(EE_MAG_Z, 2);
}
*/
/********************************************************************************/
void read_EEPROM_compass(void)
{
compass_enabled = eeprom_read_byte((uint8_t *) EE_COMPASS);
}
void save_EEPROM_mag(void)
{
eeprom_write_byte((uint8_t *) EE_COMPASS, compass_enabled);
}
/********************************************************************************/
void save_command_index(void)
{
eeprom_write_byte((uint8_t *) EE_WP_INDEX, command_must_index);
}
void read_command_index(void)
{
g.waypoint_index = command_must_index = eeprom_read_byte((uint8_t *) EE_WP_INDEX);
}
/********************************************************************************/
void save_EEPROM_pressure(void)
{
eeprom_write_dword((uint32_t *)EE_ABS_PRESS_GND, abs_pressure_ground);
eeprom_write_word((uint16_t *)EE_GND_TEMP, ground_temperature);
}
void read_EEPROM_pressure(void)
{
abs_pressure_ground = eeprom_read_dword((uint32_t *) EE_ABS_PRESS_GND);
// Better than zero for an air start value
abs_pressure = abs_pressure_ground;
ground_temperature = eeprom_read_word((uint16_t *) EE_GND_TEMP);
}
/********************************************************************************/
void read_EEPROM_radio(void)
{
g.rc_1.load_eeprom();
g.rc_2.load_eeprom();
g.rc_3.load_eeprom();
g.rc_4.load_eeprom();
g.rc_5.load_eeprom();
g.rc_6.load_eeprom();
g.rc_7.load_eeprom();
g.rc_8.load_eeprom();
}
void save_EEPROM_radio(void)
{
g.rc_1.save_eeprom();
g.rc_2.save_eeprom();
g.rc_3.save_eeprom();
g.rc_4.save_eeprom();
g.rc_5.save_eeprom();
g.rc_6.save_eeprom();
g.rc_7.save_eeprom();
g.rc_8.save_eeprom();
}
/********************************************************************************/
// configs are the basics
void read_EEPROM_throttle(void)
{
throttle_min = eeprom_read_word((uint16_t *) EE_THROTTLE_MIN);
throttle_max = eeprom_read_word((uint16_t *) EE_THROTTLE_MAX);
read_EEPROM_g.();
throttle_failsafe_enabled = eeprom_read_byte((byte *) EE_THROTTLE_FAILSAFE);
throttle_failsafe_action = eeprom_read_byte((byte *) EE_THROTTLE_FAILSAFE_ACTION);
throttle_failsafe_value = eeprom_read_word((uint16_t *) EE_THROTTLE_FS_VALUE);
}
void save_EEPROM_throttle(void)
{
eeprom_write_word((uint16_t *) EE_THROTTLE_MIN, throttle_min);
eeprom_write_word((uint16_t *) EE_THROTTLE_MAX, throttle_max);
save_EEPROM_g.();
eeprom_write_byte((byte *) EE_THROTTLE_FAILSAFE, throttle_failsafe_enabled);
eeprom_write_byte((byte *) EE_THROTTLE_FAILSAFE_ACTION,throttle_failsafe_action);
eeprom_write_word((uint16_t *) EE_THROTTLE_FS_VALUE, throttle_failsafe_value);
}
/********************************************************************************/
void read_EEPROM_logs(void)
{
g.log_bitmask = eeprom_read_word((uint16_t *) EE_LOG_BITMASK);
}
void save_EEPROM_logs(void)
{
eeprom_write_word((uint16_t *) EE_LOG_BITMASK, log_bitmask);
}
/********************************************************************************/
void read_EEPROM_flight_modes(void)
{
// Read Radio min/max settings
eeprom_read_block((void*)&flight_modes, (const void*)EE_FLIGHT_MODES, sizeof(flight_modes));
}
void save_EEPROM_flight_modes(void)
{
// Save Radio min/max settings
eeprom_write_block((const void *)&flight_modes, (void *)EE_FLIGHT_MODES, sizeof(flight_modes));
}
/********************************************************************************/
float
read_EE_float(int address)
{
union {
byte bytes[4];
float value;
} _floatOut;
for (int i = 0; i < 4; i++)
_floatOut.bytes[i] = eeprom_read_byte((uint8_t *) (address + i));
return _floatOut.value;
}
void write_EE_float(float value, int address)
{
union {
byte bytes[4];
float value;
} _floatIn;
_floatIn.value = value;
for (int i = 0; i < 4; i++)
eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]);
}
/********************************************************************************/
float
read_EE_compressed_float(int address, byte places)
{
float scale = pow(10, places);
int temp = eeprom_read_word((uint16_t *) address);
return ((float)temp / scale);
}
void write_EE_compressed_float(float value, int address, byte places)
{
float scale = pow(10, places);
int temp = value * scale;
eeprom_write_word((uint16_t *) address, temp);
}

10
ArduCopterMega/GCS_Ardupilot.pde
View File

@ -76,7 +76,7 @@ void print_attitude(void)
SendSer("ASP:"); SendSer("ASP:");
SendSer((int)airspeed / 100, DEC); SendSer((int)airspeed / 100, DEC);
SendSer(",THH:"); SendSer(",THH:");
SendSer(rc_3.servo_out, DEC); SendSer(g.rc_3.servo_out, DEC);
SendSer (",RLL:"); SendSer (",RLL:");
SendSer(dcm.roll_sensor / 100, DEC); SendSer(dcm.roll_sensor / 100, DEC);
SendSer (",PCH:"); SendSer (",PCH:");
@ -99,7 +99,7 @@ void print_position(void)
SendSer(",LON:"); SendSer(",LON:");
SendSer(current_loc.lng/10,DEC); //wp_current_lat SendSer(current_loc.lng/10,DEC); //wp_current_lat
SendSer(",SPD:"); SendSer(",SPD:");
SendSer(GPS.ground_speed/100,DEC); SendSer(gps->ground_speed/100,DEC);
SendSer(",CRT:"); SendSer(",CRT:");
SendSer(climb_rate,DEC); SendSer(climb_rate,DEC);
SendSer(",ALT:"); SendSer(",ALT:");
@ -111,15 +111,15 @@ void print_position(void)
SendSer(",BER:"); SendSer(",BER:");
SendSer(target_bearing/100,DEC); SendSer(target_bearing/100,DEC);
SendSer(",WPN:"); SendSer(",WPN:");
SendSer(wp_index,DEC);//Actually is the waypoint. SendSer(g.waypoint_index,DEC);//Actually is the waypoint.
SendSer(",DST:"); SendSer(",DST:");
SendSer(wp_distance,DEC); SendSer(wp_distance,DEC);
SendSer(",BTV:"); SendSer(",BTV:");
SendSer(battery_voltage,DEC); SendSer(battery_voltage,DEC);
SendSer(",RSP:"); SendSer(",RSP:");
SendSer(rc_1.servo_out/100,DEC); SendSer(g.rc_1.servo_out/100,DEC);
SendSer(",TOW:"); SendSer(",TOW:");
SendSer(GPS.time); SendSer(gps->time);
SendSerln(",***"); SendSerln(",***");
} }

10
ArduCopterMega/GCS_IMU_ouput.pde
View File

@ -150,16 +150,16 @@ void print_location(void)
Serial.print(",ALT:"); Serial.print(",ALT:");
Serial.print(current_loc.alt/100); // meters Serial.print(current_loc.alt/100); // meters
Serial.print(",COG:"); Serial.print(",COG:");
Serial.print(GPS.ground_course); Serial.print(gps->ground_course);
Serial.print(",SOG:"); Serial.print(",SOG:");
Serial.print(GPS.ground_speed); Serial.print(gps->ground_speed);
Serial.print(",FIX:"); Serial.print(",FIX:");
Serial.print((int)GPS.fix); Serial.print((int)gps->fix);
Serial.print(",SAT:"); Serial.print(",SAT:");
Serial.print((int)GPS.num_sats); Serial.print((int)gps->num_sats);
Serial.print (","); Serial.print (",");
Serial.print("TOW:"); Serial.print("TOW:");
Serial.print(GPS.time); Serial.print(gps->time);
Serial.println("***"); Serial.println("***");
} }

8
ArduCopterMega/GCS_Standard.pde
View File

@ -97,11 +97,11 @@ void send_message(byte id, long param) {
mess_buffer[9] = (templong >> 16) & 0xff; mess_buffer[9] = (templong >> 16) & 0xff;
mess_buffer[10] = (templong >> 24) & 0xff; mess_buffer[10] = (templong >> 24) & 0xff;
tempint = GPS.altitude / 100; // Altitude MSL in meters * 10 in 2 bytes tempint = gps->altitude / 100; // Altitude MSL in meters * 10 in 2 bytes
mess_buffer[11] = tempint & 0xff; mess_buffer[11] = tempint & 0xff;
mess_buffer[12] = (tempint >> 8) & 0xff; mess_buffer[12] = (tempint >> 8) & 0xff;
tempint = GPS.ground_speed; // Speed in M / S * 100 in 2 bytes tempint = gps->ground_speed; // Speed in M / S * 100 in 2 bytes
mess_buffer[13] = tempint & 0xff; mess_buffer[13] = tempint & 0xff;
mess_buffer[14] = (tempint >> 8) & 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[15] = tempint & 0xff;
mess_buffer[16] = (tempint >> 8) & 0xff; mess_buffer[16] = (tempint >> 8) & 0xff;
templong = GPS.time; // Time of Week (milliseconds) in 4 bytes templong = gps->time; // Time of Week (milliseconds) in 4 bytes
mess_buffer[17] = templong & 0xff; mess_buffer[17] = templong & 0xff;
mess_buffer[18] = (templong >> 8) & 0xff; mess_buffer[18] = (templong >> 8) & 0xff;
mess_buffer[19] = (templong >> 16) & 0xff; mess_buffer[19] = (templong >> 16) & 0xff;
@ -199,7 +199,7 @@ void send_message(byte id, long param) {
tempint = param; // item number tempint = param; // item number
mess_buffer[3] = tempint & 0xff; mess_buffer[3] = tempint & 0xff;
mess_buffer[4] = (tempint >> 8) & 0xff; mess_buffer[4] = (tempint >> 8) & 0xff;
tempint = wp_total; // list length (# of commands in mission) tempint = g.waypoint_total; // list length (# of commands in mission)
mess_buffer[5] = tempint & 0xff; mess_buffer[5] = tempint & 0xff;
mess_buffer[6] = (tempint >> 8) & 0xff; mess_buffer[6] = (tempint >> 8) & 0xff;
tell_command = get_wp_with_index((int)param); tell_command = get_wp_with_index((int)param);

6
ArduCopterMega/GCS_Xplane.pde
View File

@ -67,7 +67,7 @@ void print_current_waypoints()
Serial.print("MSG: "); Serial.print("MSG: ");
Serial.print("NWP:"); Serial.print("NWP:");
Serial.print(wp_index,DEC); Serial.print(g.waypoint_index,DEC);
Serial.print(",\t"); Serial.print(",\t");
Serial.print(next_WP.lat,DEC); Serial.print(next_WP.lat,DEC);
Serial.print(",\t"); Serial.print(",\t");
@ -109,7 +109,7 @@ void print_waypoints()
{ {
Serial.println("Commands in memory"); Serial.println("Commands in memory");
Serial.print("commands total: "); Serial.print("commands total: ");
Serial.println(wp_total, DEC); Serial.println(g.waypoint_total, DEC);
// create a location struct to hold the temp Waypoints for printing // create a location struct to hold the temp Waypoints for printing
//Location tmp; //Location tmp;
Serial.println("Home: "); Serial.println("Home: ");
@ -117,7 +117,7 @@ void print_waypoints()
print_waypoint(&cmd, 0); print_waypoint(&cmd, 0);
Serial.println("Commands: "); Serial.println("Commands: ");
for (int i=1; i<wp_total; i++){ for (int i=1; i < g.waypoint_total; i++){
cmd = get_wp_with_index(i); cmd = get_wp_with_index(i);
print_waypoint(&cmd, i); print_waypoint(&cmd, i);
} }

20
ArduCopterMega/HIL_output.pde
View File

@ -11,15 +11,15 @@ void output_HIL(void)
{ {
// output real-time sensor data // output real-time sensor data
Serial.print("AAA"); // Message preamble Serial.print("AAA"); // Message preamble
output_int((int)(rc_1.servo_out)); // 0 bytes 0, 1 output_int((int)(g.rc_1.servo_out)); // 0 bytes 0, 1
output_int((int)(rc_2.servo_out)); // 1 bytes 2, 3 output_int((int)(g.rc_2.servo_out)); // 1 bytes 2, 3
output_int((int)(rc_3.servo_out)); // 2 bytes 4, 5 output_int((int)(g.rc_3.servo_out)); // 2 bytes 4, 5
output_int((int)(rc_4.servo_out)); // 3 bytes 6, 7 output_int((int)(g.rc_4.servo_out)); // 3 bytes 6, 7
output_int((int)wp_distance); // 4 bytes 8,9 output_int((int)wp_distance); // 4 bytes 8,9
output_int((int)bearing_error); // 5 bytes 10,11 output_int((int)bearing_error); // 5 bytes 10,11
output_int((int)next_WP.alt / 100); // 6 bytes 12, 13 output_int((int)next_WP.alt / 100); // 6 bytes 12, 13
output_int((int)energy_error); // 7 bytes 14,15 output_int((int)energy_error); // 7 bytes 14,15
output_byte(wp_index); // 8 bytes 16 output_byte(g.waypoint_index); // 8 bytes 16
output_byte(control_mode); // 9 bytes 17 output_byte(control_mode); // 9 bytes 17
// print out the buffer and checksum // print out the buffer and checksum
@ -33,15 +33,15 @@ void output_HIL_(void)
{ {
// output real-time sensor data // output real-time sensor data
Serial.print("AAA"); // Message preamble Serial.print("AAA"); // Message preamble
output_int((int)(rc_1.servo_out)); // 0 bytes 0, 1 output_int((int)(g.rc_1.servo_out)); // 0 bytes 0, 1
output_int((int)(rc_2.servo_out)); // 1 bytes 2, 3 output_int((int)(g.rc_2.servo_out)); // 1 bytes 2, 3
output_int((int)(rc_3.servo_out)); // 2 bytes 4, 5 output_int((int)(g.rc_3.servo_out)); // 2 bytes 4, 5
output_int((int)(rc_4.servo_out)); // 3 bytes 6, 7 output_int((int)(g.rc_4.servo_out)); // 3 bytes 6, 7
output_int((int)wp_distance); // 4 bytes 8, 9 output_int((int)wp_distance); // 4 bytes 8, 9
output_int((int)bearing_error); // 5 bytes 10,11 output_int((int)bearing_error); // 5 bytes 10,11
output_int((int)dcm.roll_sensor); // 6 bytes 12,13 output_int((int)dcm.roll_sensor); // 6 bytes 12,13
output_int((int)loiter_total); // 7 bytes 14,15 output_int((int)loiter_total); // 7 bytes 14,15
output_byte(wp_index); // 8 bytes 16 output_byte(g.waypoint_index); // 8 bytes 16
output_byte(control_mode); // 9 bytes 17 output_byte(control_mode); // 9 bytes 17
// print out the buffer and checksum // print out the buffer and checksum

22
ArduCopterMega/Log.pde
View File

@ -52,14 +52,14 @@ print_log_menu(void)
byte last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM); byte last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM);
Serial.printf_P(PSTR("logs enabled: ")); Serial.printf_P(PSTR("logs enabled: "));
if (0 == log_bitmask) { if (0 == g.log_bitmask) {
Serial.printf_P(PSTR("none")); Serial.printf_P(PSTR("none"));
} else { } else {
// Macro to make the following code a bit easier on the eye. // Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined // Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for // in defines.h but without the LOG_ prefix. It will check for
// the bit being set and print the name of the log option to suit. // the bit being set and print the name of the log option to suit.
#define PLOG(_s) if (log_bitmask & LOGBIT_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s)) #define PLOG(_s) if (g.log_bitmask & LOGBIT_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s))
PLOG(ATTITUDE_FAST); PLOG(ATTITUDE_FAST);
PLOG(ATTITUDE_MED); PLOG(ATTITUDE_MED);
PLOG(GPS); PLOG(GPS);
@ -173,9 +173,9 @@ select_logs(uint8_t argc, const Menu::arg *argv)
} }
if (!strcasecmp_P(argv[0].str, PSTR("enable"))) { if (!strcasecmp_P(argv[0].str, PSTR("enable"))) {
log_bitmask |= bits; g.log_bitmask |= bits;
} else { } else {
log_bitmask &= ~bits; g.log_bitmask &= ~bits;
} }
save_EEPROM_logs(); // XXX this is a bit heavyweight... save_EEPROM_logs(); // XXX this is a bit heavyweight...
@ -240,14 +240,14 @@ void Log_Write_Startup(byte type)
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_STARTUP_MSG); DataFlash.WriteByte(LOG_STARTUP_MSG);
DataFlash.WriteByte(type); DataFlash.WriteByte(type);
DataFlash.WriteByte(wp_total); DataFlash.WriteByte(g.waypoint_total);
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
// create a location struct to hold the temp Waypoints for printing // create a location struct to hold the temp Waypoints for printing
struct Location cmd = get_wp_with_index(0); struct Location cmd = get_wp_with_index(0);
Log_Write_Cmd(0, &cmd); Log_Write_Cmd(0, &cmd);
for (int i = 1; i < wp_total; i++){ for (int i = 1; i < g.waypoint_total; i++){
cmd = get_wp_with_index(i); cmd = get_wp_with_index(i);
Log_Write_Cmd(i, &cmd); Log_Write_Cmd(i, &cmd);
} }
@ -263,14 +263,14 @@ void Log_Write_Control_Tuning()
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
DataFlash.WriteInt((int)(rc_1.servo_out)); DataFlash.WriteInt((int)(g.rc_1.servo_out));
DataFlash.WriteInt((int)nav_roll); DataFlash.WriteInt((int)nav_roll);
DataFlash.WriteInt((int)dcm.roll_sensor); DataFlash.WriteInt((int)dcm.roll_sensor);
DataFlash.WriteInt((int)(rc_2.servo_out)); DataFlash.WriteInt((int)(g.rc_2.servo_out));
DataFlash.WriteInt((int)nav_pitch); DataFlash.WriteInt((int)nav_pitch);
DataFlash.WriteInt((int)dcm.pitch_sensor); DataFlash.WriteInt((int)dcm.pitch_sensor);
DataFlash.WriteInt((int)(rc_3.servo_out)); DataFlash.WriteInt((int)(g.rc_3.servo_out));
DataFlash.WriteInt((int)(rc_4.servo_out)); DataFlash.WriteInt((int)(g.rc_4.servo_out));
DataFlash.WriteInt((int)(accel.y * 10000)); DataFlash.WriteInt((int)(accel.y * 10000));
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
@ -347,7 +347,7 @@ void Log_Write_Current()
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CURRENT_MSG); DataFlash.WriteByte(LOG_CURRENT_MSG);
DataFlash.WriteInt(rc_3.control_in); DataFlash.WriteInt(g.rc_3.control_in);
DataFlash.WriteInt((int)(current_voltage * 100.0)); DataFlash.WriteInt((int)(current_voltage * 100.0));
DataFlash.WriteInt((int)(current_amps * 100.0)); DataFlash.WriteInt((int)(current_amps * 100.0));
DataFlash.WriteInt((int)current_total); DataFlash.WriteInt((int)current_total);

253
ArduCopterMega/Mavlink_Common.h Normal file
View File

@ -0,0 +1,253 @@
#ifndef Mavlink_Common_H
#define Mavlink_Common_H
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK || GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
uint16_t system_type = MAV_FIXED_WING;
byte mavdelay = 0;
static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
{
if (sysid != mavlink_system.sysid)
{
return 1;
}
else if (compid != mavlink_system.compid)
{
gcs.send_text(SEVERITY_LOW,"component id mismatch");
return 0; // XXX currently not receiving correct compid from gcs
}
else return 0; // no error
}
/**
* @brief Send low-priority messages at a maximum rate of xx Hertz
*
* This function sends messages at a lower rate to not exceed the wireless
* bandwidth. It sends one message each time it is called until the buffer is empty.
* Call this function with xx Hertz to increase/decrease the bandwidth.
*/
static void mavlink_queued_send(mavlink_channel_t chan)
{
AP_Var *vp;
float value;
// send parameters one by one and prevent cross port comms
if (NULL != global_data.parameter_p && global_data.requested_interface == chan) {
// if the value can't be represented as a float, we will skip it here
vp = global_data.parameter_p;
value = vp->cast_to_float();
if (!isnan(value)) {
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
vp->copy_name(param_name, sizeof(param_name));
mavlink_msg_param_value_send(chan,
(int8_t*)param_name,
value,
256,
vp->meta_get_handle());
}
// remember the next variable we're going to send
global_data.parameter_p = vp->next();
}
// this is called at 50hz, count runs to prevent flooding serialport and delayed to allow eeprom write
mavdelay++;
// request waypoints one by one
if (global_data.waypoint_receiving && global_data.requested_interface == chan &&
global_data.waypoint_request_i <= g.waypoint_total && mavdelay > 15) // limits to 3.33 hz
{
mavlink_msg_waypoint_request_send(chan,
global_data.waypoint_dest_sysid,
global_data.waypoint_dest_compid ,global_data.waypoint_request_i);
mavdelay = 0;
}
}
void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops)
{
uint64_t timeStamp = micros();
switch(id) {
case MSG_HEARTBEAT:
mavlink_msg_heartbeat_send(chan,system_type,MAV_AUTOPILOT_ARDUPILOTMEGA);
break;
case MSG_EXTENDED_STATUS:
{
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
switch(control_mode) {
case MANUAL:
mode = MAV_MODE_MANUAL;
break;
case CIRCLE:
mode = MAV_MODE_TEST3;
break;
case STABILIZE:
mode = MAV_MODE_GUIDED;
break;
case FLY_BY_WIRE_A:
mode = MAV_MODE_TEST1;
break;
case FLY_BY_WIRE_B:
mode = MAV_MODE_TEST2;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
break;
case TAKEOFF:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LIFTOFF;
break;
case LAND:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LANDING;
break;
}
uint8_t status = MAV_STATE_ACTIVE;
uint8_t motor_block = false;
mavlink_msg_sys_status_send(chan,mode,nav_mode,status,load*1000,
battery_voltage1*1000,motor_block,packet_drops);
break;
}
case MSG_ATTITUDE:
{
Vector3f omega = dcm.get_gyro();
mavlink_msg_attitude_send(chan,timeStamp,dcm.roll,dcm.pitch,dcm.yaw,
omega.x,omega.y,omega.z);
break;
}
case MSG_LOCATION:
{
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);
break;
}
case MSG_LOCAL_LOCATION:
{
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);
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);
break;
}
case MSG_SERVO_OUT:
{
uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers
mavlink_msg_rc_channels_scaled_send(chan,
10000*rc[0]->norm_output(),
10000*rc[1]->norm_output(),
10000*rc[2]->norm_output(),
10000*rc[3]->norm_output(),
0,0,0,0,rssi);
break;
}
case MSG_RADIO_IN:
{
uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send(chan,
rc[0]->radio_in,
rc[1]->radio_in,
rc[2]->radio_in,
rc[3]->radio_in,
0/*rc[4]->radio_in*/, // XXX currently only 4 RC channels defined
0/*rc[5]->radio_in*/,
0/*rc[6]->radio_in*/,
0/*rc[7]->radio_in*/,
rssi);
break;
}
case MSG_RADIO_OUT:
{
mavlink_msg_servo_output_raw_send(chan,
rc[0]->radio_out,
rc[1]->radio_out,
rc[2]->radio_out,
rc[3]->radio_out,
0/*rc[4]->radio_out*/, // XXX currently only 4 RC channels defined
0/*rc[5]->radio_out*/,
0/*rc[6]->radio_out*/,
0/*rc[7]->radio_out*/);
break;
}
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,
climb_rate, (int)rc[CH_THROTTLE]->servo_out);
break;
}
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU:
{
Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro();
//Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z);
//Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z);
mavlink_msg_raw_imu_send(chan,timeStamp,
accel.x*1000.0/gravity,accel.y*1000.0/gravity,accel.z*1000.0/gravity,
gyro.x*1000.0,gyro.y*1000.0,gyro.z*1000.0,
compass.mag_x,compass.mag_y,compass.mag_z);
mavlink_msg_raw_pressure_send(chan,timeStamp,
adc.Ch(AIRSPEED_CH),barometer.RawPress,0,0);
break;
}
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
case MSG_GPS_STATUS:
{
mavlink_msg_gps_status_send(chan,gps.num_sats,NULL,NULL,NULL,NULL,NULL);
break;
}
case MSG_CURRENT_WAYPOINT:
{
mavlink_msg_waypoint_current_send(chan,g.waypoint_index);
break;
}
defualt:
break;
}
}
void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str)
{
mavlink_msg_statustext_send(chan,severity,(const int8_t*)str);
}
void mavlink_acknowledge(mavlink_channel_t chan, uint8_t id, uint8_t sum1, uint8_t sum2)
{
}
#endif // mavlink in use
#endif // inclusion guard

285
ArduCopterMega/Parameters.h Normal file
View File

@ -0,0 +1,285 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef PARAMETERS_H
#define PARAMETERS_H
#include <AP_Common.h>
// Global parameter class.
//
class Parameters {
public:
// The version of the layout as described by the parameter enum.
//
// When changing the parameter enum in an incompatible fashion, this
// value should be incremented by one.
//
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 1;
//
// Parameter identities.
//
// The enumeration defined here is used to ensure that every parameter
// or parameter group has a unique ID number. This number is used by
// AP_Var to store and locate parameters in EEPROM.
//
// Note that entries without a number are assigned the next number after
// the entry preceding them. When adding new entries, ensure that they
// don't overlap.
//
// Try to group related variables together, and assign them a set
// range in the enumeration. Place these groups in numerical order
// at the end of the enumeration.
//
// WARNING: Care should be taken when editing this enumeration as the
// AP_Var load/save code depends on the values here to identify
// variables saved in EEPROM.
//
//
enum {
// Layout version number, always key zero.
//
k_param_format_version = 0,
// Misc
//
k_param_log_bitmask,
k_param_frame_type,
//
// 140: Sensor parameters
//
k_param_IMU_calibration = 140,
k_param_ground_temperature,
k_param_ground_altitude,
k_param_ground_pressure,
k_param_current,
k_param_compass,
k_param_mag_declination,
//
// 160: Navigation parameters
//
k_param_crosstrack_gain = 160,
k_param_crosstrack_entry_angle,
k_param_pitch_max,
k_param_RTL_altitude,
//
// 180: Radio settings
//
k_param_rc_1 = 180,
k_param_rc_2,
k_param_rc_3,
k_param_rc_4,
k_param_rc_5,
k_param_rc_6,
k_param_rc_7,
k_param_rc_8,
k_param_rc_9,
k_param_rc_10,
k_param_throttle_min,
k_param_throttle_max,
k_param_throttle_failsafe_enabled,
k_param_throttle_failsafe_action,
k_param_throttle_failsafe_value,
k_param_throttle_cruise,
k_param_flight_mode_channel,
k_param_flight_modes,
//
// 220: Waypoint data
//
k_param_waypoint_mode = 220,
k_param_waypoint_total,
k_param_waypoint_index,
k_param_waypoint_radius,
k_param_loiter_radius,
//
// 240: PID Controllers
//
// Heading-to-roll PID:
// heading error from commnd to roll command deviation from trim
// (bank to turn strategy)
//
k_param_pid_acro_rate_roll = 240,
k_param_pid_acro_rate_pitch,
k_param_pid_acro_rate_yaw,
k_param_pid_stabilize_roll,
k_param_pid_stabilize_pitch,
k_param_pid_yaw,
k_param_pid_nav_lat,
k_param_pid_nav_lon,
k_param_pid_baro_throttle,
k_param_pid_sonar_throttle,
// special D term alternatives
k_param_stabilize_dampener,
k_param_hold_yaw_dampener,
// 255: reserved
};
AP_Int16 format_version;
// Crosstrack navigation
//
AP_Float crosstrack_gain;
AP_Int16 crosstrack_entry_angle;
// Waypoints
//
AP_Int8 waypoint_mode;
AP_Int8 waypoint_total;
AP_Int8 waypoint_index;
AP_Int8 waypoint_radius;
AP_Int8 loiter_radius;
// Throttle
//
AP_Int8 throttle_min;
AP_Int8 throttle_max;
AP_Int8 throttle_failsafe_enabled;
AP_Int8 throttle_failsafe_action;
AP_Int16 throttle_failsafe_value;
AP_Int8 throttle_cruise;
// Flight modes
//
AP_Int8 flight_mode_channel;
AP_VarA<uint8_t,6> flight_modes;
// Radio settings
//
//AP_Var_group pwm_roll;
//AP_Var_group pwm_pitch;
//AP_Var_group pwm_throttle;
//AP_Var_group pwm_yaw;
AP_Int16 pitch_max;
// Misc
//
AP_Int16 log_bitmask;
AP_Int16 ground_temperature;
AP_Int16 ground_altitude;
AP_Int16 ground_pressure;
AP_Int16 RTL_altitude;
AP_Int8 frame_type;
AP_Int8 current_enabled;
AP_Int8 compass_enabled;
AP_Float mag_declination;
// RC channels
RC_Channel rc_1;
RC_Channel rc_2;
RC_Channel rc_3;
RC_Channel rc_4;
RC_Channel rc_5;
RC_Channel rc_6;
RC_Channel rc_7;
RC_Channel rc_8;
RC_Channel rc_camera_pitch;
RC_Channel rc_camera_roll;
// PID controllers
PID pid_acro_rate_roll;
PID pid_acro_rate_pitch;
PID pid_acro_rate_yaw;
PID pid_stabilize_roll;
PID pid_stabilize_pitch;
PID pid_yaw;
PID pid_nav_lat;
PID pid_nav_lon;
PID pid_baro_throttle;
PID pid_sonar_throttle;
AP_Float stabilize_dampener;
AP_Float hold_yaw_dampener;
uint8_t junk;
Parameters() :
// variable default key name
//-------------------------------------------------------------------------------------------------------
format_version (k_format_version, k_param_format_version, NULL),
crosstrack_gain (XTRACK_GAIN_SCALED, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SCALED")),
crosstrack_entry_angle (XTRACK_ENTRY_ANGLE_CENTIDEGREE,k_param_crosstrack_entry_angle, PSTR("XTRACK_ENTRY_ANGLE_CENTIDEGREE")),
frame_type (FRAME_CONFIG, k_param_frame_type, PSTR("FRAME_CONFIG")),
current_enabled (DISABLED, k_param_current, PSTR("CURRENT_ENABLE")),
compass_enabled (DISABLED, k_param_compass, PSTR("COMPASS_ENABLE")),
mag_declination (0, k_param_mag_declination, PSTR("MAG_DEC")),
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")),
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")),
loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("LOITER_RADIUS")),
throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")),
throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")),
throttle_failsafe_enabled (THROTTLE_FAILSAFE, k_param_throttle_failsafe_enabled, PSTR("THR_FAILSAFE")),
throttle_failsafe_action (THROTTLE_FAILSAFE_ACTION, k_param_throttle_failsafe_action, PSTR("THR_FS_ACTION")),
throttle_failsafe_value (THROTTLE_FS_VALUE, k_param_throttle_failsafe_value, PSTR("THR_FS_VALUE")),
throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")),
flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLIGHT_MODE_CH")),
flight_modes (k_param_flight_modes, PSTR("FLIGHT_MODES")),
pitch_max (PITCH_MAX_CENTIDEGREE, k_param_pitch_max, PSTR("PITCH_MAX_CENTIDEGREE")),
log_bitmask (0, k_param_log_bitmask, PSTR("LOG_BITMASK")),
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
ground_altitude (0, k_param_ground_altitude, PSTR("GND_ALT_CM")),
ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")),
RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
// RC channel group key name
//----------------------------------------------------------------------
rc_1 (k_param_rc_1, PSTR("RC1_")),
rc_2 (k_param_rc_2, PSTR("RC2_")),
rc_3 (k_param_rc_3, PSTR("RC3_")),
rc_4 (k_param_rc_4, PSTR("RC4_")),
rc_5 (k_param_rc_5, PSTR("RC5_")),
rc_6 (k_param_rc_6, PSTR("RC6_")),
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_")),
// PID controller group key name initial P initial I initial D initial imax
//---------------------------------------------------------------------------------------------------------------------------------------
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),
pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_D, NAV_IMAX_CENTIDEGREE),
pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_D, NAV_IMAX_CENTIDEGREE),
pid_baro_throttle (k_param_pid_baro_throttle, PSTR("THR_BAR_"), THROTTLE_BARO_P, THROTTLE_BARO_I, THROTTLE_BARO_D, THROTTLE_BARO_IMAX_CENTIDEGREE),
pid_sonar_throttle (k_param_pid_sonar_throttle, PSTR("THR_SON_"), THROTTLE_SONAR_P, THROTTLE_SONAR_I, THROTTLE_SONAR_D, THROTTLE_SONAR_IMAX_CENTIDEGREE),
stabilize_dampener (STABILIZE_DAMPENER, k_param_stabilize_dampener, PSTR("STB_DAMP")),
hold_yaw_dampener (HOLD_YAW_DAMPENER, k_param_hold_yaw_dampener, PSTR("YAW_DAMP")),
junk(0) // XXX just so that we can add things without worrying about the trailing comma
{
}
};
#endif // PARAMETERS_H

53
ArduCopterMega/commands.pde
View File

@ -3,7 +3,7 @@
void init_commands() void init_commands()
{ {
read_EEPROM_waypoint_info(); read_EEPROM_waypoint_info();
wp_index = 0; g.waypoint_index = 0;
command_must_index = 0; command_must_index = 0;
command_may_index = 0; command_may_index = 0;
next_command.id = CMD_BLANK; next_command.id = CMD_BLANK;
@ -11,19 +11,26 @@ void init_commands()
void update_auto() void update_auto()
{ {
if (wp_index == wp_total){ if (g.waypoint_index == g.waypoint_total){
return_to_launch(); return_to_launch();
//wp_index = 0; //g.waypoint_index = 0;
} }
} }
void reload_commands() void reload_commands()
{ {
init_commands(); init_commands();
read_command_index(); // Get wp_index = command_must_index from EEPROM g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all?
if(wp_index > 0){ decrement_WP_index();
decrement_WP_index; }
}
long read_alt_to_hold()
{
read_EEPROM_alt_RTL();
if(g.RTL_altitude == -1)
return current_loc.alt;
else
return g.RTL_altitude + home.alt;
} }
// Getters // Getters
@ -36,7 +43,7 @@ struct Location get_wp_with_index(int i)
// Find out proper location in memory by using the start_byte position + the index // Find out proper location in memory by using the start_byte position + the index
// -------------------------------------------------------------------------------- // --------------------------------------------------------------------------------
if (i > wp_total) { if (i > g.waypoint_total) {
temp.id = CMD_BLANK; temp.id = CMD_BLANK;
}else{ }else{
// read WP position // read WP position
@ -59,7 +66,7 @@ struct Location get_wp_with_index(int i)
void set_wp_with_index(struct Location temp, int i) void set_wp_with_index(struct Location temp, int i)
{ {
i = constrain(i,0,wp_total); i = constrain(i, 0, g.waypoint_total);
uint32_t mem = WP_START_BYTE + (i * WP_SIZE); uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
eeprom_write_byte((uint8_t *) mem, temp.id); eeprom_write_byte((uint8_t *) mem, temp.id);
@ -79,17 +86,17 @@ void set_wp_with_index(struct Location temp, int i)
void increment_WP_index() void increment_WP_index()
{ {
if(wp_index < wp_total){ if(g.waypoint_index < g.waypoint_total){
wp_index++; g.waypoint_index++;
Serial.printf_P(PSTR("WP index is incremented to %d\n"),wp_index); Serial.printf_P(PSTR("WP index is incremented to %d\n"),g.waypoint_index);
}else{ }else{
Serial.printf_P(PSTR("WP Failed to incremented WP index of %d\n"),wp_index); Serial.printf_P(PSTR("WP Failed to incremented WP index of %d\n"),g.waypoint_index);
} }
} }
void decrement_WP_index() void decrement_WP_index()
{ {
if(wp_index > 0){ if(g.waypoint_index > 0){
wp_index--; g.waypoint_index--;
} }
} }
@ -107,7 +114,7 @@ return_to_launch(void)
// home is WP 0 // home is WP 0
// ------------ // ------------
wp_index = 0; g.waypoint_index = 0;
// Loads WP from Memory // Loads WP from Memory
// -------------------- // --------------------
@ -145,8 +152,8 @@ It looks to see what the next command type is and finds the last command.
void void
set_next_WP(struct Location *wp) set_next_WP(struct Location *wp)
{ {
Serial.printf_P(PSTR("set_next_WP, wp_index %d\n"), wp_index); Serial.printf_P(PSTR("set_next_WP, wp_index %d\n"), g.waypoint_index);
send_message(MSG_COMMAND, wp_index); send_message(MSG_COMMAND, g.waypoint_index);
// copy the current WP into the OldWP slot // copy the current WP into the OldWP slot
// --------------------------------------- // ---------------------------------------
@ -199,14 +206,14 @@ void init_home()
// block until we get a good fix // block until we get a good fix
// ----------------------------- // -----------------------------
while (!GPS.new_data || !GPS.fix) { while (!gps->new_data || !gps->fix) {
GPS.update(); gps->update();
} }
home.id = CMD_WAYPOINT; home.id = CMD_WAYPOINT;
home.lng = GPS.longitude; // Lon * 10**7 home.lng = gps->longitude; // Lon * 10**7
home.lat = GPS.latitude; // Lat * 10**7 home.lat = gps->latitude; // Lat * 10**7
home.alt = GPS.altitude; home.alt = gps->altitude;
home_is_set = true; home_is_set = true;
// ground altitude in centimeters for pressure alt calculations // ground altitude in centimeters for pressure alt calculations

36
ArduCopterMega/commands_process.pde
View File

@ -24,10 +24,10 @@ void load_next_command()
// fetch next command if it's empty // fetch next command if it's empty
// -------------------------------- // --------------------------------
if(next_command.id == CMD_BLANK){ if(next_command.id == CMD_BLANK){
next_command = get_wp_with_index(wp_index+1); next_command = get_wp_with_index(g.waypoint_index + 1);
if(next_command.id != CMD_BLANK){ if(next_command.id != CMD_BLANK){
//Serial.print("MSG fetch found new cmd from list at index: "); //Serial.print("MSG fetch found new cmd from list at index: ");
//Serial.println((wp_index+1),DEC); //Serial.println((g.waypoint_index + 1),DEC);
//Serial.print("MSG cmd id: "); //Serial.print("MSG cmd id: ");
//Serial.println(next_command.id,DEC); //Serial.println(next_command.id,DEC);
} }
@ -40,7 +40,7 @@ void load_next_command()
// we are out of commands! // we are out of commands!
//send_message(SEVERITY_LOW,"out of commands!"); //send_message(SEVERITY_LOW,"out of commands!");
//Serial.print("MSG out of commands, wp_index: "); //Serial.print("MSG out of commands, wp_index: ");
//Serial.println(wp_index,DEC); //Serial.println(g.waypoint_index,DEC);
switch (control_mode){ switch (control_mode){
@ -65,14 +65,14 @@ void process_next_command()
if (next_command.id >= 0x10 && next_command.id <= 0x1F ){ if (next_command.id >= 0x10 && next_command.id <= 0x1F ){
increment_WP_index(); increment_WP_index();
save_command_index(); // to Recover from in air Restart save_command_index(); // to Recover from in air Restart
command_must_index = wp_index; command_must_index = g.waypoint_index;
//Serial.print("MSG new command_must_id "); //Serial.print("MSG new command_must_id ");
//Serial.print(next_command.id,DEC); //Serial.print(next_command.id,DEC);
//Serial.print(" index:"); //Serial.print(" index:");
//Serial.println(command_must_index,DEC); //Serial.println(command_must_index,DEC);
if (log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(wp_index, &next_command); Log_Write_Cmd(g.waypoint_index, &next_command);
process_must(); process_must();
} }
} }
@ -82,11 +82,11 @@ void process_next_command()
if (command_may_index == 0){ if (command_may_index == 0){
if (next_command.id >= 0x20 && next_command.id <= 0x2F ){ if (next_command.id >= 0x20 && next_command.id <= 0x2F ){
increment_WP_index();// this command is from the command list in EEPROM increment_WP_index();// this command is from the command list in EEPROM
command_may_index = wp_index; command_may_index = g.waypoint_index;
//Serial.print("new command_may_index "); //Serial.print("new command_may_index ");
//Serial.println(command_may_index,DEC); //Serial.println(command_may_index,DEC);
if (log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(wp_index, &next_command); Log_Write_Cmd(g.waypoint_index, &next_command);
process_may(); process_may();
} }
} }
@ -95,8 +95,8 @@ void process_next_command()
// --------------------------- // ---------------------------
if (next_command.id >= 0x30){ if (next_command.id >= 0x30){
increment_WP_index();// this command is from the command list in EEPROM increment_WP_index();// this command is from the command list in EEPROM
if (log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(wp_index, &next_command); Log_Write_Cmd(g.waypoint_index, &next_command);
process_now(); process_now();
} }
@ -111,7 +111,7 @@ void process_must()
//Serial.println(command_must_index,DEC); //Serial.println(command_must_index,DEC);
send_message(SEVERITY_LOW,"New cmd: "); send_message(SEVERITY_LOW,"New cmd: ");
send_message(MSG_COMMAND, wp_index); send_message(MSG_COMMAND, g.waypoint_index);
// clear May indexes // clear May indexes
command_may_index = 0; command_may_index = 0;
@ -185,7 +185,7 @@ void process_must()
void process_may() void process_may()
{ {
//Serial.print("process_may cmd# "); //Serial.print("process_may cmd# ");
//Serial.println(wp_index,DEC); //Serial.println(g.waypoint_index,DEC);
command_may_ID = next_command.id; command_may_ID = next_command.id;
// invalidate command so a new one is loaded // invalidate command so a new one is loaded
@ -193,7 +193,7 @@ void process_may()
next_command.id = 0; next_command.id = 0;
send_message(SEVERITY_LOW,"New cmd: "); send_message(SEVERITY_LOW,"New cmd: ");
send_message(MSG_COMMAND, wp_index); send_message(MSG_COMMAND, g.waypoint_index);
// do the command // do the command
// -------------- // --------------
@ -274,7 +274,7 @@ void process_now()
{ {
const float t5 = 100000.0; const float t5 = 100000.0;
//Serial.print("process_now cmd# "); //Serial.print("process_now cmd# ");
//Serial.println(wp_index,DEC); //Serial.println(g.waypoint_index,DEC);
byte id = next_command.id; byte id = next_command.id;
@ -283,7 +283,7 @@ void process_now()
next_command.id = 0; next_command.id = 0;
send_message(SEVERITY_LOW, "New cmd: "); send_message(SEVERITY_LOW, "New cmd: ");
send_message(MSG_COMMAND, wp_index); send_message(MSG_COMMAND, g.waypoint_index);
// do the command // do the command
// -------------- // --------------
@ -311,7 +311,7 @@ void process_now()
//break; //break;
case CMD_THROTTLE_CRUISE: case CMD_THROTTLE_CRUISE:
throttle_cruise = next_command.p1; g.throttle_cruise = next_command.p1;
// todo save to EEPROM // todo save to EEPROM
break; break;
@ -368,7 +368,7 @@ void process_now()
case CMD_INDEX: case CMD_INDEX:
command_must_index = 0; command_must_index = 0;
command_may_index = 0; command_may_index = 0;
wp_index = next_command.p1 - 1; g.waypoint_index = next_command.p1 - 1;
break; break;
case CMD_RELAY: case CMD_RELAY:

94
ArduCopterMega/config.h
View File

@ -53,11 +53,11 @@
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// GPS_PROTOCOL // GPS_PROTOCOL
// //
#ifndef GPS_PROTOCOL //#ifndef GPS_PROTOCOL
# error XXX //# error XXX
# error XXX You must define GPS_PROTOCOL in APM_Config.h //# error XXX You must define GPS_PROTOCOL in APM_Config.h
# error XXX //# error XXX
#endif //#endif
// The X-Plane GCS requires the IMU GPS configuration // The X-Plane GCS requires the IMU GPS configuration
#if (ENABLE_HIL == ENABLED) && (GPS_PROTOCOL != GPS_PROTOCOL_IMU) #if (ENABLE_HIL == ENABLED) && (GPS_PROTOCOL != GPS_PROTOCOL_IMU)
@ -245,6 +245,7 @@
#ifndef ACRO_RATE_ROLL_IMAX #ifndef ACRO_RATE_ROLL_IMAX
# define ACRO_RATE_ROLL_IMAX 20 # define ACRO_RATE_ROLL_IMAX 20
#endif #endif
# define ACRO_RATE_ROLL_IMAX_CENTIDEGREE ACRO_RATE_ROLL_IMAX * 100
#ifndef ACRO_RATE_PITCH_P #ifndef ACRO_RATE_PITCH_P
# define ACRO_RATE_PITCH_P .190 # define ACRO_RATE_PITCH_P .190
@ -258,6 +259,7 @@
#ifndef ACRO_RATE_PITCH_IMAX #ifndef ACRO_RATE_PITCH_IMAX
# define ACRO_RATE_PITCH_IMAX 20 # define ACRO_RATE_PITCH_IMAX 20
#endif #endif
#define ACRO_RATE_PITCH_IMAX_CENTIDEGREE ACRO_RATE_PITCH_IMAX * 100
#ifndef ACRO_RATE_YAW_P #ifndef ACRO_RATE_YAW_P
# define ACRO_RATE_YAW_P .07 # define ACRO_RATE_YAW_P .07
@ -271,6 +273,7 @@
#ifndef ACRO_RATE_YAW_IMAX #ifndef ACRO_RATE_YAW_IMAX
# define ACRO_RATE_YAW_IMAX 0 # define ACRO_RATE_YAW_IMAX 0
#endif #endif
# define ACRO_RATE_YAW_IMAX_CENTIDEGREE ACRO_RATE_YAW_IMAX * 100
#ifndef ACRO_RATE_TRIGGER #ifndef ACRO_RATE_TRIGGER
# define ACRO_RATE_TRIGGER 100 # define ACRO_RATE_TRIGGER 100
@ -293,6 +296,7 @@
#ifndef STABILIZE_ROLL_IMAX #ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX 3 # define STABILIZE_ROLL_IMAX 3
#endif #endif
#define STABILIZE_ROLL_IMAX_CENTIDEGREE STABILIZE_ROLL_IMAX * 100
#ifndef STABILIZE_PITCH_P #ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 0.6 # define STABILIZE_PITCH_P 0.6
@ -306,6 +310,7 @@
#ifndef STABILIZE_PITCH_IMAX #ifndef STABILIZE_PITCH_IMAX
# define STABILIZE_PITCH_IMAX 3 # define STABILIZE_PITCH_IMAX 3
#endif #endif
# define STABILIZE_PITCH_IMAX_CENTIDEGREE STABILIZE_PITCH_IMAX * 100
// STABILZE RATE Control // STABILZE RATE Control
// //
@ -329,6 +334,7 @@
#ifndef YAW_IMAX #ifndef YAW_IMAX
# define YAW_IMAX 1 # define YAW_IMAX 1
#endif #endif
# define YAW_IMAX_CENTIDEGREE YAW_IMAX * 100
// STABILZE YAW Control // STABILZE YAW Control
// //
@ -357,6 +363,7 @@
#ifndef PITCH_MAX #ifndef PITCH_MAX
# define PITCH_MAX 36 # define PITCH_MAX 36
#endif #endif
#define PITCH_MAX_CENTIDEGREE PITCH_MAX * 100
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
@ -374,6 +381,7 @@
#ifndef NAV_IMAX #ifndef NAV_IMAX
# define NAV_IMAX 250 # define NAV_IMAX 250
#endif #endif
# define NAV_IMAX_CENTIDEGREE NAV_IMAX * 100
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
@ -391,6 +399,8 @@
#ifndef THROTTLE_BARO_IMAX #ifndef THROTTLE_BARO_IMAX
# define THROTTLE_BARO_IMAX 50 # define THROTTLE_BARO_IMAX 50
#endif #endif
# define THROTTLE_BARO_IMAX_CENTIDEGREE THROTTLE_BARO_IMAX * 100
#ifndef THROTTLE_SONAR_P #ifndef THROTTLE_SONAR_P
# define THROTTLE_SONAR_P .8 # define THROTTLE_SONAR_P .8
@ -404,17 +414,20 @@
#ifndef THROTTLE_SONAR_IMAX #ifndef THROTTLE_SONAR_IMAX
# define THROTTLE_SONAR_IMAX 300 # define THROTTLE_SONAR_IMAX 300
#endif #endif
# define THROTTLE_SONAR_IMAX_CENTIDEGREE THROTTLE_SONAR_IMAX * 100
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Crosstrack compensation // Crosstrack compensation
// //
#ifndef XTRACK_GAIN #ifndef XTRACK_GAIN
# define XTRACK_GAIN 0.01 # define XTRACK_GAIN 1 // deg/m
#endif #endif
#ifndef XTRACK_ENTRY_ANGLE #ifndef XTRACK_ENTRY_ANGLE
# define XTRACK_ENTRY_ANGLE 30 # define XTRACK_ENTRY_ANGLE 30 // deg
#endif #endif
# define XTRACK_GAIN_SCALED XTRACK_GAIN * 100
# define XTRACK_ENTRY_ANGLE_CENTIDEGREE XTRACK_ENTRY_ANGLE * 100
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
@ -423,14 +436,6 @@
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// DEBUG_SUBSYSTEM
//
#ifndef DEBUG_SUBSYSTEM
# define DEBUG_SUBSYSTEM 0
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// DEBUG_LEVEL // DEBUG_LEVEL
// //
@ -473,5 +478,64 @@
# define LOG_CURRENT DISABLED # define LOG_CURRENT DISABLED
#endif #endif
#ifndef DEBUG_PORT
# define DEBUG_PORT 0
#endif
#if DEBUG_PORT == 0
# define SendDebug_P(a) Serial.print_P(PSTR(a))
# define SendDebugln_P(a) Serial.println_P(PSTR(a))
# define SendDebug Serial.print
# define SendDebugln Serial.println
#elif DEBUG_PORT == 1
# define SendDebug_P(a) Serial1.print_P(PSTR(a))
# define SendDebugln_P(a) Serial1.println_P(PSTR(a))
# define SendDebug Serial1.print
# define SendDebugln Serial1.println
#elif DEBUG_PORT == 2
# define SendDebug_P(a) Serial2.print_P(PSTR(a))
# define SendDebugln_P(a) Serial2.println_P(PSTR(a))
# define SendDebug Serial2.print
# define SendDebugln Serial2.println
#elif DEBUG_PORT == 3
# define SendDebug_P(a) Serial3.print_P(PSTR(a))
# define SendDebugln_P(a) Serial3.println_P(PSTR(a))
# define SendDebug Serial3.print
# define SendDebugln Serial3.println
#endif
//////////////////////////////////////////////////////////////////////////////
// Navigation defaults
//
#ifndef WP_RADIUS_DEFAULT
# define WP_RADIUS_DEFAULT 3
#endif
#ifndef LOITER_RADIUS_DEFAULT
# define LOITER_RADIUS_DEFAULT 10
#endif
#ifndef ALT_HOLD_HOME
# define ALT_HOLD_HOME 8
#endif
#define ALT_HOLD_HOME_CM ALT_HOLD_HOME*100
#ifndef USE_CURRENT_ALT
# define USE_CURRENT_ALT FALSE
#endif
#if USE_CURRENT_ALT == TRUE
# define CONFIG_OPTIONS 0
#else
# define CONFIG_OPTIONS HOLD_ALT_ABOVE_HOME
#endif
//////////////////////////////////////////////////////////////////////////////
// Developer Items
//
#ifndef STANDARD_SPEED
# define STANDARD_SPEED 15.0
#define STANDARD_SPEED_SQUARED (STANDARD_SPEED * STANDARD_SPEED)
#endif
#define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE)

28
ArduCopterMega/control_modes.pde
View File

@ -5,7 +5,7 @@ void read_control_switch()
if (oldSwitchPosition != switchPosition){ if (oldSwitchPosition != switchPosition){
set_mode(flight_modes[switchPosition]); set_mode(g.flight_modes[switchPosition]);
oldSwitchPosition = switchPosition; oldSwitchPosition = switchPosition;
@ -17,13 +17,13 @@ void read_control_switch()
byte readSwitch(void){ byte readSwitch(void){
#if FLIGHT_MODE_CHANNEL == CH_5 #if FLIGHT_MODE_CHANNEL == CH_5
int pulsewidth = rc_5.radio_in; // default for Arducopter int pulsewidth = g.rc_5.radio_in; // default for Arducopter
#elif FLIGHT_MODE_CHANNEL == CH_6 #elif FLIGHT_MODE_CHANNEL == CH_6
int pulsewidth = rc_6.radio_in; // int pulsewidth = g.rc_6.radio_in; //
#elif FLIGHT_MODE_CHANNEL == CH_7 #elif FLIGHT_MODE_CHANNEL == CH_7
int pulsewidth = rc_7.radio_in; // int pulsewidth = g.rc_7.radio_in; //
#elif FLIGHT_MODE_CHANNEL == CH_8 #elif FLIGHT_MODE_CHANNEL == CH_8
int pulsewidth = rc_8.radio_in; // default for Ardupilot. Don't use for Arducopter! it has a hardware failsafe mux! int pulsewidth = g.rc_8.radio_in; // default for Ardupilot. Don't use for Arducopter! it has a hardware failsafe mux!
#else #else
# error Must define FLIGHT_MODE_CHANNEL as CH_5 - CH_8 # error Must define FLIGHT_MODE_CHANNEL as CH_5 - CH_8
#endif #endif
@ -57,7 +57,7 @@ unsigned long trim_timer;
void read_trim_switch() void read_trim_switch()
{ {
// switch is engaged // switch is engaged
if (rc_7.control_in > 800){ if (g.rc_7.control_in > 800){
if(trim_flag == false){ if(trim_flag == false){
// called once // called once
trim_timer = millis(); trim_timer = millis();
@ -76,10 +76,10 @@ void read_trim_switch()
} else { } else {
// set the throttle nominal // set the throttle nominal
if(rc_3.control_in > 50){ if(g.rc_3.control_in > 50){
throttle_cruise = rc_3.control_in; g.throttle_cruise = g.rc_3.control_in;
Serial.printf("tnom %d\n", throttle_cruise); Serial.printf("tnom %d\n", g.);
save_EEPROM_throttle_cruise(); save_EEPROM_g.();
} }
} }
trim_flag = false; trim_flag = false;
@ -90,15 +90,15 @@ void read_trim_switch()
void trim_accel() void trim_accel()
{ {
if(rc_1.control_in > 0){ if(g.rc_1.control_in > 0){
imu.ay(imu.ay() + 1); imu.ay(imu.ay() + 1);
}else if (rc_1.control_in < 0){ }else if (g.rc_1.control_in < 0){
imu.ay(imu.ay() - 1); imu.ay(imu.ay() - 1);
} }
if(rc_2.control_in > 0){ if(g.rc_2.control_in > 0){
imu.ax(imu.ax() + 1); imu.ax(imu.ax() + 1);
}else if (rc_2.control_in < 0){ }else if (g.rc_2.control_in < 0){
imu.ax(imu.ax() - 1); imu.ax(imu.ax() - 1);
} }

3
ArduCopterMega/defines.h
View File

@ -6,6 +6,9 @@
#define DEBUG 0 #define DEBUG 0
#define LOITER_RANGE 30 // for calculating power outside of loiter radius #define LOITER_RANGE 30 // for calculating power outside of loiter radius
#define T6 1000000
#define T7 10000000
// GPS baud rates // GPS baud rates
// -------------- // --------------
#define NO_GPS 38400 #define NO_GPS 38400

10
ArduCopterMega/events.pde
View File

@ -20,7 +20,7 @@ void low_battery_event(void)
{ {
send_message(SEVERITY_HIGH,"Low Battery!"); send_message(SEVERITY_HIGH,"Low Battery!");
set_mode(RTL); set_mode(RTL);
throttle_cruise = THROTTLE_CRUISE; g.throttle_cruise = THROTTLE_CRUISE;
} }
@ -93,22 +93,22 @@ void perform_event()
} }
switch(event_id) { switch(event_id) {
case CH_4_TOGGLE: 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 APM_RC.OutputCh(CH_5, event_value); // send to Servos
undo_event = 2; undo_event = 2;
break; break;
case CH_5_TOGGLE: 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 APM_RC.OutputCh(CH_6, event_value); // send to Servos
undo_event = 2; undo_event = 2;
break; break;
case CH_6_TOGGLE: 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 APM_RC.OutputCh(CH_7, event_value); // send to Servos
undo_event = 2; undo_event = 2;
break; break;
case CH_7_TOGGLE: 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 APM_RC.OutputCh(CH_8, event_value); // send to Servos
undo_event = 2; undo_event = 2;
event_undo_value = 1; event_undo_value = 1;

28
ArduCopterMega/flight_control.pde
View File

@ -7,16 +7,16 @@ throttle control
// ----------- // -----------
void output_manual_throttle() void output_manual_throttle()
{ {
rc_3.servo_out = (float)rc_3.control_in * angle_boost(); g.rc_3.servo_out = (float)g.rc_3.control_in * angle_boost();
} }
// Autopilot // Autopilot
// --------- // ---------
void output_auto_throttle() void output_auto_throttle()
{ {
rc_3.servo_out = (float)nav_throttle * angle_boost(); g.rc_3.servo_out = (float)nav_throttle * angle_boost();
// make sure we never send a 0 throttle that will cut the motors // make sure we never send a 0 throttle that will cut the motors
rc_3.servo_out = max(rc_3.servo_out, 1); g.rc_3.servo_out = max(g.rc_3.servo_out, 1);
} }
void calc_nav_throttle() void calc_nav_throttle()
@ -25,26 +25,26 @@ void calc_nav_throttle()
long error = constrain(altitude_error, -400, 400); long error = constrain(altitude_error, -400, 400);
if(altitude_sensor == BARO) { if(altitude_sensor == BARO) {
float t = pid_baro_throttle.kP(); float t = g.pid_baro_throttle.kP();
if(error > 0){ // go up if(error > 0){ // go up
pid_baro_throttle.kP(t); g.pid_baro_throttle.kP(t);
}else{ // go down }else{ // go down
pid_baro_throttle.kP(t/4.0); g.pid_baro_throttle.kP(t/4.0);
} }
// limit output of throttle control // limit output of throttle control
nav_throttle = pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0); nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
nav_throttle = throttle_cruise + constrain(nav_throttle, -30, 80); nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
pid_baro_throttle.kP(t); g.pid_baro_throttle.kP(t);
} else { } else {
// SONAR // SONAR
nav_throttle = pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0); nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
// limit output of throttle control // limit output of throttle control
nav_throttle = throttle_cruise + constrain(nav_throttle, -60, 100); nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 100);
} }
nav_throttle = (nav_throttle + nav_throttle_old) >> 1; nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
@ -65,11 +65,11 @@ yaw control
void output_manual_yaw() void output_manual_yaw()
{ {
if(rc_3.control_in == 0){ if(g.rc_3.control_in == 0){
clear_yaw_control(); clear_yaw_control();
} else { } else {
// Yaw control // Yaw control
if(rc_4.control_in == 0){ if(g.rc_4.control_in == 0){
//clear_yaw_control(); //clear_yaw_control();
output_yaw_with_hold(true); // hold yaw output_yaw_with_hold(true); // hold yaw
}else{ }else{
@ -94,7 +94,7 @@ void calc_nav_pid()
{ {
// how hard to pitch to target // how hard to pitch to target
nav_angle = pid_nav.get_pid(wp_distance * 100, dTnav, 1.0); nav_angle = g.pid_nav.get_pid(wp_distance * 100, dTnav, 1.0);
nav_angle = constrain(nav_angle, -pitch_max, pitch_max); nav_angle = constrain(nav_angle, -pitch_max, pitch_max);
} }

55
ArduCopterMega/global_data.h Normal file
View File

@ -0,0 +1,55 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef __GLOBAL_DATA_H
#define __GLOBAL_DATA_H
#define ONBOARD_PARAM_NAME_LENGTH 15
#define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe
#define FIRMWARE_VERSION 18 // <-- change on param data struct modifications
#ifdef __cplusplus
///
// global data structure
// This structure holds all the vehicle parameters.
// TODO: bring in varibles floating around in ArduPilotMega.pde
//
struct global_struct
{
// parameters
uint16_t requested_interface; // store port to use
AP_Var *parameter_p; // parameter pointer
// waypoints
uint16_t waypoint_request_i; // request index
uint16_t waypoint_dest_sysid; // where to send requests
uint16_t waypoint_dest_compid; // "
bool waypoint_sending; // currently in send process
bool waypoint_receiving; // currently receiving
uint16_t waypoint_count;
uint32_t waypoint_timelast_send; // milliseconds
uint32_t waypoint_timelast_receive; // milliseconds
uint16_t waypoint_send_timeout; // milliseconds
uint16_t waypoint_receive_timeout; // milliseconds
float junk; //used to return a junk value for interface
// data stream rates
uint16_t streamRateRawSensors;
uint16_t streamRateExtendedStatus;
uint16_t streamRateRCChannels;
uint16_t streamRateRawController;
uint16_t streamRatePosition;
uint16_t streamRateExtra1;
uint16_t streamRateExtra2;
uint16_t streamRateExtra3;
// struct constructor
global_struct();
} global_data;
extern "C" const char *param_nametab[];
#endif // __cplusplus
#endif // __GLOBAL_DATA_H

20
ArduCopterMega/global_data.pde Normal file
View File

@ -0,0 +1,20 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
global_struct::global_struct() :
// parameters
// note, all values not explicitly initialised here are zeroed
waypoint_send_timeout(1000), // 1 second
waypoint_receive_timeout(1000), // 1 second
// stream rates
streamRateRawSensors(0),
streamRateExtendedStatus(0),
streamRateRCChannels(0),
streamRateRawController(0),
//streamRateRawSensorFusion(0),
streamRatePosition(0),
streamRateExtra1(0),
streamRateExtra2(0),
streamRateExtra3(0)
{
}

160
ArduCopterMega/motors.pde
View File

@ -7,14 +7,14 @@ void arm_motors()
static byte arming_counter; static byte arming_counter;
// Arm motor output : Throttle down and full yaw right for more than 2 seconds // Arm motor output : Throttle down and full yaw right for more than 2 seconds
if (rc_3.control_in == 0){ if (g.rc_3.control_in == 0){
if (rc_4.control_in > 2700) { if (g.rc_4.control_in > 2700) {
if (arming_counter > ARM_DELAY) { if (arming_counter > ARM_DELAY) {
motor_armed = true; motor_armed = true;
} else{ } else{
arming_counter++; arming_counter++;
} }
}else if (rc_4.control_in < -2700) { }else if (g.rc_4.control_in < -2700) {
if (arming_counter > DISARM_DELAY){ if (arming_counter > DISARM_DELAY){
motor_armed = false; motor_armed = false;
}else{ }else{
@ -40,55 +40,55 @@ set_servos_4()
// Quadcopter mix // Quadcopter mix
if (motor_armed == true && motor_auto_safe == true) { if (motor_armed == true && motor_auto_safe == true) {
int out_min = rc_3.radio_min; int out_min = g.rc_3.radio_min;
// Throttle is 0 to 1000 only // Throttle is 0 to 1000 only
rc_3.servo_out = constrain(rc_3.servo_out, 0, 1000); g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000);
if(rc_3.servo_out > 0) if(g.rc_3.servo_out > 0)
out_min = rc_3.radio_min + 50; out_min = g.rc_3.radio_min + 50;
//Serial.printf("out: %d %d %d %d\t\t", rc_1.servo_out, rc_2.servo_out, rc_3.servo_out, rc_4.servo_out); //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 // creates the radio_out and pwm_out values
rc_1.calc_pwm(); g.rc_1.calc_pwm();
rc_2.calc_pwm(); g.rc_2.calc_pwm();
rc_3.calc_pwm(); g.rc_3.calc_pwm();
rc_4.calc_pwm(); g.rc_4.calc_pwm();
//Serial.printf("out: %d %d %d %d\n", rc_1.radio_out, rc_2.radio_out, rc_3.radio_out, rc_4.radio_out); //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 ", rc_4.radio_out); //Serial.printf("yaw: %d ", g.rc_4.radio_out);
if(frame_type == PLUS_FRAME){ if(frame_type == PLUS_FRAME){
motor_out[CH_1] = rc_3.radio_out - rc_1.pwm_out; motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out;
motor_out[CH_2] = rc_3.radio_out + rc_1.pwm_out; motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out;
motor_out[CH_3] = rc_3.radio_out + rc_2.pwm_out; motor_out[CH_3] = g.rc_3.radio_out + g.rc_2.pwm_out;
motor_out[CH_4] = rc_3.radio_out - rc_2.pwm_out; motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out;
motor_out[CH_1] += rc_4.pwm_out; // CCW motor_out[CH_1] += g.rc_4.pwm_out; // CCW
motor_out[CH_2] += rc_4.pwm_out; // CCW motor_out[CH_2] += g.rc_4.pwm_out; // CCW
motor_out[CH_3] -= rc_4.pwm_out; // CW motor_out[CH_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_4] -= rc_4.pwm_out; // CW motor_out[CH_4] -= g.rc_4.pwm_out; // CW
}else if(frame_type == X_FRAME){ }else if(frame_type == X_FRAME){
int roll_out = rc_1.pwm_out / 2; int roll_out = g.rc_1.pwm_out / 2;
int pitch_out = rc_2.pwm_out / 2; int pitch_out = g.rc_2.pwm_out / 2;
motor_out[CH_3] = rc_3.radio_out + roll_out + pitch_out; motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out;
motor_out[CH_2] = rc_3.radio_out + roll_out - pitch_out; motor_out[CH_2] = g.rc_3.radio_out + roll_out - pitch_out;
motor_out[CH_1] = rc_3.radio_out - roll_out + pitch_out; motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out;
motor_out[CH_4] = 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] += rc_4.pwm_out; // CCW motor_out[CH_1] += g.rc_4.pwm_out; // CCW
motor_out[CH_2] += rc_4.pwm_out; // CCW motor_out[CH_2] += g.rc_4.pwm_out; // CCW
motor_out[CH_3] -= rc_4.pwm_out; // CW motor_out[CH_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_4] -= 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]); //Serial.printf("\tl8r: %d %d %d %d\n", motor_out[CH_1], motor_out[CH_2], motor_out[CH_3], motor_out[CH_4]);
@ -96,42 +96,42 @@ set_servos_4()
// Tri-copter power distribution // Tri-copter power distribution
int roll_out = (float)rc_1.pwm_out * .866; int roll_out = (float)g.rc_1.pwm_out * .866;
int pitch_out = rc_2.pwm_out / 2; int pitch_out = g.rc_2.pwm_out / 2;
// front two motors // front two motors
motor_out[CH_2] = rc_3.radio_out + roll_out + pitch_out; motor_out[CH_2] = g.rc_3.radio_out + roll_out + pitch_out;
motor_out[CH_1] = rc_3.radio_out - roll_out + pitch_out; motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out;
// rear motors // rear motors
motor_out[CH_4] = rc_3.radio_out - rc_2.pwm_out; motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out;
// servo Yaw // servo Yaw
APM_RC.OutputCh(CH_7, rc_4.radio_out); APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
}else if (frame_type == HEXA_FRAME) { }else if (frame_type == HEXA_FRAME) {
int roll_out = (float)rc_1.pwm_out * .866; int roll_out = (float)g.rc_1.pwm_out * .866;
int pitch_out = rc_2.pwm_out / 2; int pitch_out = g.rc_2.pwm_out / 2;
//left side //left side
motor_out[CH_2] = rc_3.radio_out + rc_1.pwm_out; // CCW motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW
motor_out[CH_3] = rc_3.radio_out + roll_out + pitch_out; // CW motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
motor_out[CH_8] = 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] = rc_3.radio_out - rc_1.pwm_out; // CW motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW
motor_out[CH_7] = rc_3.radio_out - roll_out + pitch_out; // CCW motor_out[CH_7] = g.rc_3.radio_out - roll_out + pitch_out; // CCW
motor_out[CH_4] = rc_3.radio_out - roll_out - pitch_out; // CCW motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW
motor_out[CH_7] += rc_4.pwm_out; // CCW motor_out[CH_7] += g.rc_4.pwm_out; // CCW
motor_out[CH_2] += rc_4.pwm_out; // CCW motor_out[CH_2] += g.rc_4.pwm_out; // CCW
motor_out[CH_4] += rc_4.pwm_out; // CCW motor_out[CH_4] += g.rc_4.pwm_out; // CCW
motor_out[CH_3] -= rc_4.pwm_out; // CW motor_out[CH_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_1] -= rc_4.pwm_out; // CW motor_out[CH_1] -= g.rc_4.pwm_out; // CW
motor_out[CH_8] -= rc_4.pwm_out; // CW motor_out[CH_8] -= g.rc_4.pwm_out; // CW
} else { } else {
@ -141,14 +141,14 @@ set_servos_4()
// limit output so motors don't stop // limit output so motors don't stop
motor_out[CH_1] = constrain(motor_out[CH_1], out_min, rc_3.radio_max); motor_out[CH_1] = constrain(motor_out[CH_1], out_min, g.rc_3.radio_max);
motor_out[CH_2] = constrain(motor_out[CH_2], out_min, rc_3.radio_max); motor_out[CH_2] = constrain(motor_out[CH_2], out_min, g.rc_3.radio_max);
motor_out[CH_3] = constrain(motor_out[CH_3], out_min, rc_3.radio_max); motor_out[CH_3] = constrain(motor_out[CH_3], out_min, g.rc_3.radio_max);
motor_out[CH_4] = constrain(motor_out[CH_4], out_min, rc_3.radio_max); motor_out[CH_4] = constrain(motor_out[CH_4], out_min, g.rc_3.radio_max);
if (frame_type == HEXA_FRAME) { if (frame_type == HEXA_FRAME) {
motor_out[CH_7] = constrain(motor_out[CH_7], out_min, rc_3.radio_max); motor_out[CH_7] = constrain(motor_out[CH_7], out_min, g.rc_3.radio_max);
motor_out[CH_8] = constrain(motor_out[CH_8], out_min, rc_3.radio_max); motor_out[CH_8] = constrain(motor_out[CH_8], out_min, g.rc_3.radio_max);
} }
num++; num++;
@ -157,22 +157,22 @@ set_servos_4()
//Serial.print("!"); //Serial.print("!");
//debugging with Channel 6 //debugging with Channel 6
//pid_baro_throttle.kD((float)rc_6.control_in / 1000); // 0 to 1 //g.pid_baro_throttle.kD((float)g.rc_6.control_in / 1000); // 0 to 1
//pid_baro_throttle.kP((float)rc_6.control_in / 4000); // 0 to .25 //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 // make sure you init_pids() after changing the kP
pid_stabilize_roll.kP((float)rc_6.control_in / 1000); g.pid_stabilize_roll.kP((float)g.rc_6.control_in / 1000);
init_pids(); init_pids();
//Serial.print("kP: "); //Serial.print("kP: ");
//Serial.println(pid_stabilize_roll.kP(),3); //Serial.println(g.pid_stabilize_roll.kP(),3);
//*/ //*/
/* /*
// YAW // YAW
// make sure you init_pids() after changing the kP // make sure you init_pids() after changing the kP
pid_yaw.kP((float)rc_6.control_in / 1000); g.pid_yaw.kP((float)g.rc_6.control_in / 1000);
init_pids(); init_pids();
//*/ //*/
@ -182,7 +182,7 @@ set_servos_4()
write_int(motor_out[CH_3]); write_int(motor_out[CH_3]);
write_int(motor_out[CH_4]); write_int(motor_out[CH_4]);
write_int(rc_3.servo_out); write_int(g.rc_3.servo_out);
write_int((int)(cos_yaw_x * 100)); write_int((int)(cos_yaw_x * 100));
write_int((int)(sin_yaw_y * 100)); write_int((int)(sin_yaw_y * 100));
write_int((int)(dcm.yaw_sensor / 100)); write_int((int)(dcm.yaw_sensor / 100));
@ -209,14 +209,14 @@ set_servos_4()
/*Serial.printf("a %ld, e %ld, i %d, t %d, b %4.2f\n", /*Serial.printf("a %ld, e %ld, i %d, t %d, b %4.2f\n",
current_loc.alt, current_loc.alt,
altitude_error, altitude_error,
(int)pid_baro_throttle.get_integrator(), (int)g.pid_baro_throttle.get_integrator(),
nav_throttle, nav_throttle,
angle_boost()); angle_boost());
*/ */
} }
// Send commands to motors // Send commands to motors
if(rc_3.servo_out > 0){ if(g.rc_3.servo_out > 0){
APM_RC.OutputCh(CH_1, motor_out[CH_1]); APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]); APM_RC.OutputCh(CH_2, motor_out[CH_2]);
@ -234,17 +234,17 @@ set_servos_4()
}else{ }else{
APM_RC.OutputCh(CH_1, rc_3.radio_min); APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, rc_3.radio_min); APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, rc_3.radio_min); APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, rc_3.radio_min); APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
// InstantPWM // InstantPWM
APM_RC.Force_Out0_Out1(); APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3(); APM_RC.Force_Out2_Out3();
if (frame_type == HEXA_FRAME) { if (frame_type == HEXA_FRAME) {
APM_RC.OutputCh(CH_7, rc_3.radio_min); APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, rc_3.radio_min); APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.Force_Out6_Out7(); APM_RC.Force_Out6_Out7();
} }
} }
@ -253,27 +253,27 @@ set_servos_4()
// our motor is unarmed, we're on the ground // our motor is unarmed, we're on the ground
reset_I(); reset_I();
if(rc_3.control_in > 0){ if(g.rc_3.control_in > 0){
// we have pushed up the throttle // we have pushed up the throttle
// remove safety // remove safety
motor_auto_safe = true; motor_auto_safe = true;
} }
// Send commands to motors // Send commands to motors
APM_RC.OutputCh(CH_1, rc_3.radio_min); APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, rc_3.radio_min); APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, rc_3.radio_min); APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, rc_3.radio_min); APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
if (frame_type == HEXA_FRAME) { if (frame_type == HEXA_FRAME) {
APM_RC.OutputCh(CH_7, rc_3.radio_min); APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, rc_3.radio_min); APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
} }
// reset I terms of PID controls // reset I terms of PID controls
reset_I(); reset_I();
// Initialize yaw command to actual yaw when throttle is down... // Initialize yaw command to actual yaw when throttle is down...
rc_4.control_in = ToDeg(yaw); g.rc_4.control_in = ToDeg(yaw);
} }
} }

20
ArduCopterMega/navigation.pde
View File

@ -7,9 +7,9 @@ void navigate()
{ {
// do not navigate with corrupt data // do not navigate with corrupt data
// --------------------------------- // ---------------------------------
if (GPS.fix == 0) if (gps->fix == 0)
{ {
GPS.new_data = false; gps->new_data = false;
return; return;
} }
@ -71,21 +71,21 @@ void calc_nav()
lat_error = constrain(lat_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 // Convert distance into ROLL X
nav_lon = long_error * pid_nav_lon.kP(); // 1800 * 2 = 3600 or 36° nav_lon = long_error * g.pid_nav_lon.kP(); // 1800 * 2 = 3600 or 36°
//nav_lon = pid_nav_lon.get_pid(long_error, dTnav2, 1.0); //nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav2, 1.0);
//nav_lon = constrain(nav_lon, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command //nav_lon = constrain(nav_lon, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
// PITCH Y // PITCH Y
//nav_lat = pid_nav_lat.get_pid(lat_error, dTnav2, 1.0); //nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav2, 1.0);
nav_lat = lat_error * pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36° 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 //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_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_pitch = -((float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y);
nav_roll = constrain(nav_roll, -pitch_max, pitch_max); nav_roll = constrain(nav_roll, -g.pitch_max, g.pitch_max);
nav_pitch = constrain(nav_pitch, -pitch_max, pitch_max); nav_pitch = constrain(nav_pitch, -g.pitch_max, g.pitch_max);
} }
/* /*
@ -116,7 +116,7 @@ void calc_distance_error()
// this wants to work only while moving, but it should filter out jumpy GPS reads // 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 // 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)gps->ground_speed * .0002 * cos(radians(bearing_error / 100));
//distance_estimate -= distance_gain * (float)(distance_estimate - GPS_wp_distance); //distance_estimate -= distance_gain * (float)(distance_estimate - GPS_wp_distance);
//wp_distance = distance_estimate; //wp_distance = distance_estimate;
} }
@ -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 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 crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line
nav_bearing += constrain(crosstrack_error * x_track_gain, -x_track_angle, x_track_angle); nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle, g.crosstrack_entry_angle);
nav_bearing = wrap_360(nav_bearing); nav_bearing = wrap_360(nav_bearing);
} }
} }

86
ArduCopterMega/radio.pde
View File

@ -1,29 +1,29 @@
void init_rc_in() void init_rc_in()
{ {
read_EEPROM_radio(); // read Radio limits read_EEPROM_radio(); // read Radio limits
rc_1.set_angle(4500); g.rc_1.set_angle(4500);
rc_1.dead_zone = 60; // 60 = .6 degrees g.rc_1.dead_zone = 60; // 60 = .6 degrees
rc_2.set_angle(4500); g.rc_2.set_angle(4500);
rc_2.dead_zone = 60; g.rc_2.dead_zone = 60;
rc_3.set_range(0,1000); g.rc_3.set_range(0,1000);
rc_3.dead_zone = 20; g.rc_3.dead_zone = 20;
rc_3.scale_output = .9; g.rc_3.scale_output = .9;
rc_4.set_angle(6000); g.rc_4.set_angle(6000);
rc_4.dead_zone = 500; g.rc_4.dead_zone = 500;
rc_5.set_range(0,1000); g.rc_5.set_range(0,1000);
rc_5.set_filter(false); g.rc_5.set_filter(false);
// for kP values // for kP values
//rc_6.set_range(200,800); //g.rc_6.set_range(200,800);
//rc_6.set_range(0,1800); // for faking GPS //g.rc_6.set_range(0,1800); // for faking GPS
rc_6.set_range(0,1000); g.rc_6.set_range(0,1000);
// for camera angles // for camera angles
//rc_6.set_angle(4500); //g.rc_6.set_angle(4500);
//rc_6.dead_zone = 60; //g.rc_6.dead_zone = 60;
rc_7.set_range(0,1000); g.rc_7.set_range(0,1000);
rc_8.set_range(0,1000); g.rc_8.set_range(0,1000);
} }
void init_rc_out() void init_rc_out()
@ -32,38 +32,38 @@ void init_rc_out()
motor_armed = 1; motor_armed = 1;
#endif #endif
APM_RC.OutputCh(CH_1, rc_3.radio_min); // Initialization of servo outputs APM_RC.OutputCh(CH_1, g.rc_3.radio_min); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, rc_3.radio_min); APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, rc_3.radio_min); APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, rc_3.radio_min); APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, rc_3.radio_min); APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, rc_3.radio_min); APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.Init(); // APM Radio initialization APM_RC.Init(); // APM Radio initialization
APM_RC.OutputCh(CH_1, rc_3.radio_min); // Initialization of servo outputs APM_RC.OutputCh(CH_1, g.rc_3.radio_min); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, rc_3.radio_min); APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, rc_3.radio_min); APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, rc_3.radio_min); APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, rc_3.radio_min); APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, rc_3.radio_min); APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
} }
void read_radio() void read_radio()
{ {
rc_1.set_pwm(APM_RC.InputCh(CH_1)); g.rc_1.set_pwm(APM_RC.InputCh(CH_1));
rc_2.set_pwm(APM_RC.InputCh(CH_2)); g.rc_2.set_pwm(APM_RC.InputCh(CH_2));
rc_3.set_pwm(APM_RC.InputCh(CH_3)); g.rc_3.set_pwm(APM_RC.InputCh(CH_3));
rc_4.set_pwm(APM_RC.InputCh(CH_4)); g.rc_4.set_pwm(APM_RC.InputCh(CH_4));
rc_5.set_pwm(APM_RC.InputCh(CH_5)); g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
rc_6.set_pwm(APM_RC.InputCh(CH_6)); g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
rc_7.set_pwm(APM_RC.InputCh(CH_7)); g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
rc_8.set_pwm(APM_RC.InputCh(CH_8)); g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), rc_1.control_in, rc_2.control_in, rc_3.control_in, rc_4.control_in); //Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), g.rc_1.control_in, g.rc_2.control_in, g.rc_3.control_in, g.rc_4.control_in);
} }
void trim_radio() void trim_radio()
@ -71,9 +71,9 @@ void trim_radio()
for (byte i = 0; i < 30; i++){ for (byte i = 0; i < 30; i++){
read_radio(); read_radio();
} }
rc_1.trim(); // roll g.rc_1.trim(); // roll
rc_2.trim(); // pitch g.rc_2.trim(); // pitch
rc_4.trim(); // yaw g.rc_4.trim(); // yaw
} }
void trim_yaw() void trim_yaw()
@ -81,5 +81,5 @@ void trim_yaw()
for (byte i = 0; i < 30; i++){ for (byte i = 0; i < 30; i++){
read_radio(); read_radio();
} }
rc_4.trim(); // yaw g.rc_4.trim(); // yaw
} }

16
ArduCopterMega/read_commands.pde
View File

@ -77,26 +77,26 @@ void parseCommand(char *buffer)
///* ///*
switch(cmd[0]){ switch(cmd[0]){
case 'P': case 'P':
pid_stabilize_roll.kP((float)value / 1000); g.pid_stabilize_roll.kP((float)value / 1000);
pid_stabilize_pitch.kP((float)value / 1000); g.pid_stabilize_pitch.kP((float)value / 1000);
save_EEPROM_PID(); save_EEPROM_PID();
break; break;
case 'I': case 'I':
pid_stabilize_roll.kI((float)value / 1000); g.pid_stabilize_roll.kI((float)value / 1000);
pid_stabilize_pitch.kI((float)value / 1000); g.pid_stabilize_pitch.kI((float)value / 1000);
save_EEPROM_PID(); save_EEPROM_PID();
break; break;
case 'D': case 'D':
//pid_stabilize_roll.kD((float)value / 1000); //g.pid_stabilize_roll.kD((float)value / 1000);
//pid_stabilize_pitch.kD((float)value / 1000); //g.pid_stabilize_pitch.kD((float)value / 1000);
//save_EEPROM_PID(); //save_EEPROM_PID();
break; break;
case 'X': case 'X':
pid_stabilize_roll.imax(value * 100); g.pid_stabilize_roll.imax(value * 100);
pid_stabilize_pitch.imax(value * 100); g.pid_stabilize_pitch.imax(value * 100);
save_EEPROM_PID(); save_EEPROM_PID();
break; break;

2
ArduCopterMega/read_me.text
View File

@ -51,7 +51,7 @@ eedump - raw output of bytes in eeprom
Flight modes: Flight modes:
stabilize - copter will hold -45 to 45° angle, throttle is manual. stabilize - copter will hold -45 to 45° angle, throttle is manual.
Alt_hold - You need to set your throttle_cruise value by toggling ch_7 for less than 1 second. (Mine is 330), Alt_hold - You need to set your g. value by toggling ch_7 for less than 1 second. (Mine is 330),
altitude is controlled by the throttle lever using absolute position - from 0 to 40 meters. altitude is controlled by the throttle lever using absolute position - from 0 to 40 meters.
this control method will change after testing. this control method will change after testing.
FBW - Simulates GPS Hold with the sticks being the position offset. Manual Throttle. FBW - Simulates GPS Hold with the sticks being the position offset. Manual Throttle.

2
ArduCopterMega/sensors.pde
View File

@ -22,7 +22,7 @@ void read_barometer(void){
scaling = (float)abs_pressure_ground / (float)abs_pressure; scaling = (float)abs_pressure_ground / (float)abs_pressure;
temp = ((float)ground_temperature / 10.f) + 273.15; temp = ((float)ground_temperature / 10.f) + 273.15;
x = log(scaling) * temp * 29271.267f; x = log(scaling) * temp * 29271.267f;
current_loc.alt = (long)(x / 10) + home.alt + baro_offset; // Pressure altitude in centimeters current_loc.alt = (long)(x / 10) + home.alt;// + baro_offset; // Pressure altitude in centimeters
} }
// in M/S * 100 // in M/S * 100

314
ArduCopterMega/setup.pde
View File

@ -125,7 +125,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
read_radio(); read_radio();
} }
if(rc_1.radio_in < 500){ if(g.rc_1.radio_in < 500){
while(1){ while(1){
Serial.printf_P(PSTR("\nNo radio; Check connectors.")); Serial.printf_P(PSTR("\nNo radio; Check connectors."));
delay(1000); delay(1000);
@ -133,32 +133,32 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
} }
} }
rc_1.radio_min = rc_1.radio_in; g.rc_1.radio_min = g.rc_1.radio_in;
rc_2.radio_min = rc_2.radio_in; g.rc_2.radio_min = g.rc_2.radio_in;
rc_3.radio_min = rc_3.radio_in; g.rc_3.radio_min = g.rc_3.radio_in;
rc_4.radio_min = rc_4.radio_in; g.rc_4.radio_min = g.rc_4.radio_in;
rc_5.radio_min = rc_5.radio_in; g.rc_5.radio_min = g.rc_5.radio_in;
rc_6.radio_min = rc_6.radio_in; g.rc_6.radio_min = g.rc_6.radio_in;
rc_7.radio_min = rc_7.radio_in; g.rc_7.radio_min = g.rc_7.radio_in;
rc_8.radio_min = rc_8.radio_in; g.rc_8.radio_min = g.rc_8.radio_in;
rc_1.radio_max = rc_1.radio_in; g.rc_1.radio_max = g.rc_1.radio_in;
rc_2.radio_max = rc_2.radio_in; g.rc_2.radio_max = g.rc_2.radio_in;
rc_3.radio_max = rc_3.radio_in; g.rc_3.radio_max = g.rc_3.radio_in;
rc_4.radio_max = rc_4.radio_in; g.rc_4.radio_max = g.rc_4.radio_in;
rc_5.radio_max = rc_5.radio_in; g.rc_5.radio_max = g.rc_5.radio_in;
rc_6.radio_max = rc_6.radio_in; g.rc_6.radio_max = g.rc_6.radio_in;
rc_7.radio_max = rc_7.radio_in; g.rc_7.radio_max = g.rc_7.radio_in;
rc_8.radio_max = rc_8.radio_in; g.rc_8.radio_max = g.rc_8.radio_in;
rc_1.radio_trim = rc_1.radio_in; g.rc_1.radio_trim = g.rc_1.radio_in;
rc_2.radio_trim = rc_2.radio_in; g.rc_2.radio_trim = g.rc_2.radio_in;
rc_4.radio_trim = rc_4.radio_in; g.rc_4.radio_trim = g.rc_4.radio_in;
// 3 is not trimed // 3 is not trimed
rc_5.radio_trim = 1500; g.rc_5.radio_trim = 1500;
rc_6.radio_trim = 1500; g.rc_6.radio_trim = 1500;
rc_7.radio_trim = 1500; g.rc_7.radio_trim = 1500;
rc_8.radio_trim = 1500; g.rc_8.radio_trim = 1500;
Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: ")); Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: "));
@ -169,17 +169,17 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
// ---------------------------------------------------------- // ----------------------------------------------------------
read_radio(); read_radio();
rc_1.update_min_max(); g.rc_1.update_min_max();
rc_2.update_min_max(); g.rc_2.update_min_max();
rc_3.update_min_max(); g.rc_3.update_min_max();
rc_4.update_min_max(); g.rc_4.update_min_max();
rc_5.update_min_max(); g.rc_5.update_min_max();
rc_6.update_min_max(); g.rc_6.update_min_max();
rc_7.update_min_max(); g.rc_7.update_min_max();
rc_8.update_min_max(); g.rc_8.update_min_max();
if(Serial.available() > 0){ if(Serial.available() > 0){
//rc_3.radio_max += 250; //g.rc_3.radio_max += 250;
Serial.flush(); Serial.flush();
save_EEPROM_radio(); save_EEPROM_radio();
@ -210,35 +210,35 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
delay(1000); delay(1000);
int out_min = rc_3.radio_min + 70; int out_min = g.rc_3.radio_min + 70;
while(1){ while(1){
delay(20); delay(20);
read_radio(); read_radio();
motor_out[CH_1] = rc_3.radio_min; motor_out[CH_1] = g.rc_3.radio_min;
motor_out[CH_2] = rc_3.radio_min; motor_out[CH_2] = g.rc_3.radio_min;
motor_out[CH_3] = rc_3.radio_min; motor_out[CH_3] = g.rc_3.radio_min;
motor_out[CH_4] = rc_3.radio_min; motor_out[CH_4] = g.rc_3.radio_min;
if(frame_type == PLUS_FRAME){ if(frame_type == PLUS_FRAME){
if(rc_1.control_in > 0){ if(g.rc_1.control_in > 0){
motor_out[CH_1] = out_min; motor_out[CH_1] = out_min;
Serial.println("0"); Serial.println("0");
}else if(rc_1.control_in < 0){ }else if(g.rc_1.control_in < 0){
motor_out[CH_2] = out_min; motor_out[CH_2] = out_min;
Serial.println("1"); Serial.println("1");
} }
if(rc_2.control_in > 0){ if(g.rc_2.control_in > 0){
motor_out[CH_4] = out_min; motor_out[CH_4] = out_min;
Serial.println("3"); Serial.println("3");
}else if(rc_2.control_in < 0){ }else if(g.rc_2.control_in < 0){
motor_out[CH_3] = out_min; motor_out[CH_3] = out_min;
Serial.println("2"); Serial.println("2");
} }
@ -246,55 +246,55 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
}else if(frame_type == X_FRAME){ }else if(frame_type == X_FRAME){
// lower right // lower right
if((rc_1.control_in > 0) && (rc_2.control_in > 0)){ if((g.rc_1.control_in > 0) && (g.rc_2.control_in > 0)){
motor_out[CH_4] = out_min; motor_out[CH_4] = out_min;
Serial.println("3"); Serial.println("3");
// lower left // lower left
}else if((rc_1.control_in < 0) && (rc_2.control_in > 0)){ }else if((g.rc_1.control_in < 0) && (g.rc_2.control_in > 0)){
motor_out[CH_2] = out_min; motor_out[CH_2] = out_min;
Serial.println("1"); Serial.println("1");
// upper left // upper left
}else if((rc_1.control_in < 0) && (rc_2.control_in < 0)){ }else if((g.rc_1.control_in < 0) && (g.rc_2.control_in < 0)){
motor_out[CH_3] = out_min; motor_out[CH_3] = out_min;
Serial.println("2"); Serial.println("2");
// upper right // upper right
}else if((rc_1.control_in > 0) && (rc_2.control_in < 0)){ }else if((g.rc_1.control_in > 0) && (g.rc_2.control_in < 0)){
motor_out[CH_1] = out_min; motor_out[CH_1] = out_min;
Serial.println("0"); Serial.println("0");
} }
}else if(frame_type == TRI_FRAME){ }else if(frame_type == TRI_FRAME){
if(rc_1.control_in > 0){ if(g.rc_1.control_in > 0){
motor_out[CH_1] = out_min; motor_out[CH_1] = out_min;
}else if(rc_1.control_in < 0){ }else if(g.rc_1.control_in < 0){
motor_out[CH_2] = out_min; motor_out[CH_2] = out_min;
} }
if(rc_2.control_in > 0){ if(g.rc_2.control_in > 0){
motor_out[CH_4] = out_min; motor_out[CH_4] = out_min;
} }
if(rc_4.control_in > 0){ if(g.rc_4.control_in > 0){
rc_4.servo_out = 2000; g.rc_4.servo_out = 2000;
}else if(rc_4.control_in < 0){ }else if(g.rc_4.control_in < 0){
rc_4.servo_out = -2000; g.rc_4.servo_out = -2000;
} }
rc_4.calc_pwm(); g.rc_4.calc_pwm();
motor_out[CH_3] = rc_4.radio_out; motor_out[CH_3] = g.rc_4.radio_out;
} }
if(rc_3.control_in > 0){ if(g.rc_3.control_in > 0){
APM_RC.OutputCh(CH_1, rc_3.radio_in); APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
APM_RC.OutputCh(CH_2, rc_3.radio_in); APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
APM_RC.OutputCh(CH_3, rc_3.radio_in); APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
if(frame_type != TRI_FRAME) if(frame_type != TRI_FRAME)
APM_RC.OutputCh(CH_4, rc_3.radio_in); APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
}else{ }else{
APM_RC.OutputCh(CH_1, motor_out[CH_1]); APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]); APM_RC.OutputCh(CH_2, motor_out[CH_2]);
@ -327,8 +327,8 @@ setup_pid(uint8_t argc, const Menu::arg *argv)
default_gains(); default_gains();
}else if (!strcmp_P(argv[1].str, PSTR("s_kp"))) { }else if (!strcmp_P(argv[1].str, PSTR("s_kp"))) {
pid_stabilize_roll.kP(argv[2].f); g.pid_stabilize_roll.kP(argv[2].f);
pid_stabilize_pitch.kP(argv[2].f); g.pid_stabilize_pitch.kP(argv[2].f);
save_EEPROM_PID(); save_EEPROM_PID();
}else if (!strcmp_P(argv[1].str, PSTR("s_kd"))) { }else if (!strcmp_P(argv[1].str, PSTR("s_kd"))) {
@ -336,19 +336,19 @@ setup_pid(uint8_t argc, const Menu::arg *argv)
save_EEPROM_PID(); save_EEPROM_PID();
}else if (!strcmp_P(argv[1].str, PSTR("y_kp"))) { }else if (!strcmp_P(argv[1].str, PSTR("y_kp"))) {
pid_yaw.kP(argv[2].f); g.pid_yaw.kP(argv[2].f);
save_EEPROM_PID(); save_EEPROM_PID();
}else if (!strcmp_P(argv[1].str, PSTR("s_kd"))) { }else if (!strcmp_P(argv[1].str, PSTR("s_kd"))) {
pid_yaw.kD(argv[2].f); g.pid_yaw.kD(argv[2].f);
save_EEPROM_PID(); save_EEPROM_PID();
}else if (!strcmp_P(argv[1].str, PSTR("t_kp"))) { }else if (!strcmp_P(argv[1].str, PSTR("t_kp"))) {
pid_baro_throttle.kP(argv[2].f); g.pid_baro_throttle.kP(argv[2].f);
save_EEPROM_PID(); save_EEPROM_PID();
}else if (!strcmp_P(argv[1].str, PSTR("t_kd"))) { }else if (!strcmp_P(argv[1].str, PSTR("t_kd"))) {
pid_baro_throttle.kD(argv[2].f); g.pid_baro_throttle.kD(argv[2].f);
save_EEPROM_PID(); save_EEPROM_PID();
}else{ }else{
default_gains(); default_gains();
@ -376,7 +376,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
// look for control switch change // look for control switch change
if (oldSwitchPosition != switchPosition){ if (oldSwitchPosition != switchPosition){
mode = flight_modes[switchPosition]; mode = g.flight_modes[switchPosition];
mode = constrain(mode, 0, NUM_MODES-1); mode = constrain(mode, 0, NUM_MODES-1);
// update the user // update the user
@ -393,7 +393,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
mode = 0; mode = 0;
// save new mode // save new mode
flight_modes[switchPosition] = mode; g.flight_modes[switchPosition] = mode;
// print new mode // print new mode
print_switch(switchPosition, mode); print_switch(switchPosition, mode);
@ -428,11 +428,11 @@ static int8_t
setup_compass(uint8_t argc, const Menu::arg *argv) setup_compass(uint8_t argc, const Menu::arg *argv)
{ {
if (!strcmp_P(argv[1].str, PSTR("on"))) { if (!strcmp_P(argv[1].str, PSTR("on"))) {
compass_enabled = true; g.compass_enabled = true;
init_compass(); init_compass();
} else if (!strcmp_P(argv[1].str, PSTR("off"))) { } else if (!strcmp_P(argv[1].str, PSTR("off"))) {
compass_enabled = false; g.compass_enabled = false;
} else { } else {
Serial.printf_P(PSTR("\nOptions:[on,off]\n")); Serial.printf_P(PSTR("\nOptions:[on,off]\n"));
@ -557,14 +557,14 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
if(Serial.available() > 0){ if(Serial.available() > 0){
mag_offset_x = offset[0]; //mag_offset_x = offset[0];
mag_offset_y = offset[1]; //mag_offset_y = offset[1];
mag_offset_z = offset[2]; //mag_offset_z = offset[2];
save_EEPROM_mag_offset(); //save_EEPROM_mag_offset();
// set offsets to account for surrounding interference // set offsets to account for surrounding interference
compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z); //compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);
report_compass(); report_compass();
break; break;
@ -580,8 +580,8 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
void default_waypoint_info() void default_waypoint_info()
{ {
wp_radius = 4; //TODO: Replace this quick fix with a real way to define wp_radius g.waypoint_radius = 4; //TODO: Replace this quick fix with a real way to define wp_radius
loiter_radius = 30; //TODO: Replace this quick fix with a real way to define loiter_radius g.loiter_radius = 30; //TODO: Replace this quick fix with a real way to define loiter_radius
save_EEPROM_waypoint_info(); save_EEPROM_waypoint_info();
} }
@ -590,9 +590,9 @@ void
default_nav() default_nav()
{ {
// nav control // nav control
x_track_gain = XTRACK_GAIN * 100; g.crosstrack_gain = XTRACK_GAIN * 100;
x_track_angle = XTRACK_ENTRY_ANGLE * 100; g.crosstrack_entry_angle = XTRACK_ENTRY_ANGLE * 100;
pitch_max = PITCH_MAX * 100; g.pitch_max = PITCH_MAX * 100;
save_EEPROM_nav(); save_EEPROM_nav();
} }
@ -621,21 +621,21 @@ default_current()
void void
default_flight_modes() default_flight_modes()
{ {
flight_modes[0] = FLIGHT_MODE_1; g.flight_modes[0] = FLIGHT_MODE_1;
flight_modes[1] = FLIGHT_MODE_2; g.flight_modes[1] = FLIGHT_MODE_2;
flight_modes[2] = FLIGHT_MODE_3; g.flight_modes[2] = FLIGHT_MODE_3;
flight_modes[3] = FLIGHT_MODE_4; g.flight_modes[3] = FLIGHT_MODE_4;
flight_modes[4] = FLIGHT_MODE_5; g.flight_modes[4] = FLIGHT_MODE_5;
flight_modes[5] = FLIGHT_MODE_6; g.flight_modes[5] = FLIGHT_MODE_6;
save_EEPROM_flight_modes(); save_EEPROM_flight_modes();
} }
void void
default_throttle() default_throttle()
{ {
throttle_min = THROTTLE_MIN; g.throttle_min = THROTTLE_MIN;
throttle_max = THROTTLE_MAX; g.throttle_max = THROTTLE_MAX;
throttle_cruise = THROTTLE_CRUISE; g.throttle_cruise = THROTTLE_CRUISE;
throttle_failsafe_enabled = THROTTLE_FAILSAFE; throttle_failsafe_enabled = THROTTLE_FAILSAFE;
throttle_failsafe_action = THROTTLE_FAILSAFE_ACTION; throttle_failsafe_action = THROTTLE_FAILSAFE_ACTION;
throttle_failsafe_value = THROTTLE_FS_VALUE; throttle_failsafe_value = THROTTLE_FS_VALUE;
@ -647,7 +647,7 @@ void default_logs()
// convenience macro for testing LOG_* and setting LOGBIT_* // convenience macro for testing LOG_* and setting LOGBIT_*
#define LOGBIT(_s) (LOG_ ## _s ? LOGBIT_ ## _s : 0) #define LOGBIT(_s) (LOG_ ## _s ? LOGBIT_ ## _s : 0)
log_bitmask = g.log_bitmask =
LOGBIT(ATTITUDE_FAST) | LOGBIT(ATTITUDE_FAST) |
LOGBIT(ATTITUDE_MED) | LOGBIT(ATTITUDE_MED) |
LOGBIT(GPS) | LOGBIT(GPS) |
@ -668,38 +668,38 @@ void
default_gains() default_gains()
{ {
// acro, angular rate // acro, angular rate
pid_acro_rate_roll.kP(ACRO_RATE_ROLL_P); g.pid_acro_rate_roll.kP(ACRO_RATE_ROLL_P);
pid_acro_rate_roll.kI(ACRO_RATE_ROLL_I); g.pid_acro_rate_roll.kI(ACRO_RATE_ROLL_I);
pid_acro_rate_roll.kD(0); g.pid_acro_rate_roll.kD(0);
pid_acro_rate_roll.imax(ACRO_RATE_ROLL_IMAX * 100); g.pid_acro_rate_roll.imax(ACRO_RATE_ROLL_IMAX * 100);
pid_acro_rate_pitch.kP(ACRO_RATE_PITCH_P); g.pid_acro_rate_pitch.kP(ACRO_RATE_PITCH_P);
pid_acro_rate_pitch.kI(ACRO_RATE_PITCH_I); g.pid_acro_rate_pitch.kI(ACRO_RATE_PITCH_I);
pid_acro_rate_pitch.kD(0); g.pid_acro_rate_pitch.kD(0);
pid_acro_rate_pitch.imax(ACRO_RATE_PITCH_IMAX * 100); g.pid_acro_rate_pitch.imax(ACRO_RATE_PITCH_IMAX * 100);
pid_acro_rate_yaw.kP(ACRO_RATE_YAW_P); g.pid_acro_rate_yaw.kP(ACRO_RATE_YAW_P);
pid_acro_rate_yaw.kI(ACRO_RATE_YAW_I); g.pid_acro_rate_yaw.kI(ACRO_RATE_YAW_I);
pid_acro_rate_yaw.kD(0); g.pid_acro_rate_yaw.kD(0);
pid_acro_rate_yaw.imax(ACRO_RATE_YAW_IMAX * 100); g.pid_acro_rate_yaw.imax(ACRO_RATE_YAW_IMAX * 100);
// stabilize, angle error // stabilize, angle error
pid_stabilize_roll.kP(STABILIZE_ROLL_P); g.pid_stabilize_roll.kP(STABILIZE_ROLL_P);
pid_stabilize_roll.kI(STABILIZE_ROLL_I); g.pid_stabilize_roll.kI(STABILIZE_ROLL_I);
pid_stabilize_roll.kD(0); g.pid_stabilize_roll.kD(0);
pid_stabilize_roll.imax(STABILIZE_ROLL_IMAX * 100); g.pid_stabilize_roll.imax(STABILIZE_ROLL_IMAX * 100);
pid_stabilize_pitch.kP(STABILIZE_PITCH_P); g.pid_stabilize_pitch.kP(STABILIZE_PITCH_P);
pid_stabilize_pitch.kI(STABILIZE_PITCH_I); g.pid_stabilize_pitch.kI(STABILIZE_PITCH_I);
pid_stabilize_pitch.kD(0); g.pid_stabilize_pitch.kD(0);
pid_stabilize_pitch.imax(STABILIZE_PITCH_IMAX * 100); g.pid_stabilize_pitch.imax(STABILIZE_PITCH_IMAX * 100);
// YAW hold // YAW hold
pid_yaw.kP(YAW_P); g.pid_yaw.kP(YAW_P);
pid_yaw.kI(YAW_I); g.pid_yaw.kI(YAW_I);
pid_yaw.kD(0); g.pid_yaw.kD(0);
pid_yaw.imax(YAW_IMAX * 100); g.pid_yaw.imax(YAW_IMAX * 100);
// custom dampeners // custom dampeners
@ -711,25 +711,25 @@ default_gains()
// navigation // navigation
pid_nav_lat.kP(NAV_P); g.pid_nav_lat.kP(NAV_P);
pid_nav_lat.kI(NAV_I); g.pid_nav_lat.kI(NAV_I);
pid_nav_lat.kD(NAV_D); g.pid_nav_lat.kD(NAV_D);
pid_nav_lat.imax(NAV_IMAX); g.pid_nav_lat.imax(NAV_IMAX);
pid_nav_lon.kP(NAV_P); g.pid_nav_lon.kP(NAV_P);
pid_nav_lon.kI(NAV_I); g.pid_nav_lon.kI(NAV_I);
pid_nav_lon.kD(NAV_D); g.pid_nav_lon.kD(NAV_D);
pid_nav_lon.imax(NAV_IMAX); g.pid_nav_lon.imax(NAV_IMAX);
pid_baro_throttle.kP(THROTTLE_BARO_P); g.pid_baro_throttle.kP(THROTTLE_BARO_P);
pid_baro_throttle.kI(THROTTLE_BARO_I); g.pid_baro_throttle.kI(THROTTLE_BARO_I);
pid_baro_throttle.kD(THROTTLE_BARO_D); g.pid_baro_throttle.kD(THROTTLE_BARO_D);
pid_baro_throttle.imax(THROTTLE_BARO_IMAX); g.pid_baro_throttle.imax(THROTTLE_BARO_IMAX);
pid_sonar_throttle.kP(THROTTLE_SONAR_P); g.pid_sonar_throttle.kP(THROTTLE_SONAR_P);
pid_sonar_throttle.kI(THROTTLE_SONAR_I); g.pid_sonar_throttle.kI(THROTTLE_SONAR_I);
pid_sonar_throttle.kD(THROTTLE_SONAR_D); g.pid_sonar_throttle.kD(THROTTLE_SONAR_D);
pid_sonar_throttle.imax(THROTTLE_SONAR_IMAX); g.pid_sonar_throttle.imax(THROTTLE_SONAR_IMAX);
save_EEPROM_PID(); save_EEPROM_PID();
} }
@ -794,32 +794,32 @@ void report_gains()
read_EEPROM_PID(); read_EEPROM_PID();
// Acro // Acro
Serial.printf_P(PSTR("Acro:\nroll:\n")); Serial.printf_P(PSTR("Acro:\nroll:\n"));
print_PID(&pid_acro_rate_roll); print_PID(&g.pid_acro_rate_roll);
Serial.printf_P(PSTR("pitch:\n")); Serial.printf_P(PSTR("pitch:\n"));
print_PID(&pid_acro_rate_pitch); print_PID(&g.pid_acro_rate_pitch);
Serial.printf_P(PSTR("yaw:\n")); Serial.printf_P(PSTR("yaw:\n"));
print_PID(&pid_acro_rate_yaw); print_PID(&g.pid_acro_rate_yaw);
// Stabilize // Stabilize
Serial.printf_P(PSTR("\nStabilize:\nroll:\n")); Serial.printf_P(PSTR("\nStabilize:\nroll:\n"));
print_PID(&pid_stabilize_roll); print_PID(&g.pid_stabilize_roll);
Serial.printf_P(PSTR("pitch:\n")); Serial.printf_P(PSTR("pitch:\n"));
print_PID(&pid_stabilize_pitch); print_PID(&g.pid_stabilize_pitch);
Serial.printf_P(PSTR("yaw:\n")); Serial.printf_P(PSTR("yaw:\n"));
print_PID(&pid_yaw); 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); Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), hold_yaw_dampener);
// Nav // Nav
Serial.printf_P(PSTR("Nav:\nlat:\n")); Serial.printf_P(PSTR("Nav:\nlat:\n"));
print_PID(&pid_nav_lat); print_PID(&g.pid_nav_lat);
Serial.printf_P(PSTR("long:\n")); Serial.printf_P(PSTR("long:\n"));
print_PID(&pid_nav_lon); print_PID(&g.pid_nav_lon);
Serial.printf_P(PSTR("baro throttle:\n")); Serial.printf_P(PSTR("baro throttle:\n"));
print_PID(&pid_baro_throttle); print_PID(&g.pid_baro_throttle);
Serial.printf_P(PSTR("sonar throttle:\n")); Serial.printf_P(PSTR("sonar throttle:\n"));
print_PID(&pid_sonar_throttle); print_PID(&g.pid_sonar_throttle);
print_blanks(1); print_blanks(1);
} }
@ -833,9 +833,9 @@ void report_xtrack()
Serial.printf_P(PSTR("XTRACK: %4.2f\n" Serial.printf_P(PSTR("XTRACK: %4.2f\n"
"XTRACK angle: %d\n" "XTRACK angle: %d\n"
"PITCH_MAX: %ld"), "PITCH_MAX: %ld"),
x_track_gain, g.crosstrack_gain,
x_track_angle, g.crosstrack_entry_angle,
pitch_max); g.pitch_max);
print_blanks(1); print_blanks(1);
} }
@ -851,9 +851,9 @@ void report_throttle()
"cruise: %d\n" "cruise: %d\n"
"failsafe_enabled: %d\n" "failsafe_enabled: %d\n"
"failsafe_value: %d"), "failsafe_value: %d"),
throttle_min, g.throttle_min,
throttle_max, g.throttle_max,
throttle_cruise, g.throttle_cruise,
throttle_failsafe_enabled, throttle_failsafe_enabled,
throttle_failsafe_value); throttle_failsafe_value);
print_blanks(1); print_blanks(1);
@ -880,7 +880,7 @@ void report_compass()
read_EEPROM_compass_declination(); read_EEPROM_compass_declination();
read_EEPROM_compass_offset(); read_EEPROM_compass_offset();
print_enabled(compass_enabled); print_enabled(g.compass_enabled);
// mag declination // mag declination
Serial.printf_P(PSTR("Mag Delination: %4.4f\n"), Serial.printf_P(PSTR("Mag Delination: %4.4f\n"),
@ -903,7 +903,7 @@ void report_flight_modes()
read_EEPROM_flight_modes(); read_EEPROM_flight_modes();
for(int i = 0; i < 6; i++ ){ for(int i = 0; i < 6; i++ ){
print_switch(i, flight_modes[i]); print_switch(i, g.flight_modes[i]);
} }
print_blanks(1); print_blanks(1);
} }
@ -921,14 +921,14 @@ print_PID(PID * pid)
void void
print_radio_values() print_radio_values()
{ {
Serial.printf_P(PSTR("CH1: %d | %d\n"), rc_1.radio_min, rc_1.radio_max); 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"), rc_2.radio_min, rc_2.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"), rc_3.radio_min, rc_3.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"), rc_4.radio_min, rc_4.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"), rc_5.radio_min, rc_5.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"), rc_6.radio_min, rc_6.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"), rc_7.radio_min, rc_7.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"), rc_8.radio_min, rc_8.radio_max); Serial.printf_P(PSTR("CH8: %d | %d\n"), g.rc_8.radio_min, g.rc_8.radio_max);
} }
void void
@ -969,7 +969,7 @@ radio_input_switch(void)
{ {
static byte bouncer; static byte bouncer;
if (abs(rc_1.radio_in - rc_1.radio_trim) > 200) if (abs(g.rc_1.radio_in - g.rc_1.radio_trim) > 200)
bouncer = 10; bouncer = 10;
if (bouncer > 0) if (bouncer > 0)

70
ArduCopterMega/system.pde
View File

@ -91,9 +91,30 @@ void init_ardupilot()
#endif #endif
"freeRAM: %d\n"),freeRAM()); "freeRAM: %d\n"),freeRAM());
//
// Check the EEPROM format version before loading any parameters from EEPROM.
//
if (!g.format_version.load() ||
g.format_version != Parameters::k_format_version) {
Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)"
"\n\nForcing complete parameter reset..."),
g.format_version.get(), Parameters::k_format_version);
read_EEPROM_startup(); // Read critical config information to start // erase all parameters
AP_Var::erase_all();
// save the new format version
g.format_version.set_and_save(Parameters::k_format_version);
Serial.println_P(PSTR("done."));
} else {
// Load all auto-loaded EEPROM variables
AP_Var::load_all();
}
//read_EEPROM_startup(); // Read critical config information to start
init_rc_in(); // sets up rc channels from radio init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up the timer libs init_rc_out(); // sets up the timer libs
@ -101,9 +122,11 @@ void init_ardupilot()
adc.Init(); // APM ADC library initialization adc.Init(); // APM ADC library initialization
APM_BMP085.Init(); // APM Abs Pressure sensor initialization APM_BMP085.Init(); // APM Abs Pressure sensor initialization
DataFlash.Init(); // DataFlash log initialization DataFlash.Init(); // DataFlash log initialization
GPS.init(); // GPS Initialization
if(compass_enabled) gps = &GPS;
gps->init();
if(g.compass_enabled)
init_compass(); init_compass();
pinMode(C_LED_PIN, OUTPUT); // GPS status LED pinMode(C_LED_PIN, OUTPUT); // GPS status LED
@ -135,7 +158,7 @@ void init_ardupilot()
} }
if(log_bitmask > 0){ if(g.log_bitmask > 0){
// Here we will check on the length of the last log // Here we will check on the length of the last log
// We don't want to create a bunch of little logs due to powering on and off // We don't want to create a bunch of little logs due to powering on and off
last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM); last_log_num = eeprom_read_byte((uint8_t *) EE_LAST_LOG_NUM);
@ -171,7 +194,7 @@ void init_ardupilot()
send_message(SEVERITY_LOW,"GROUND START"); send_message(SEVERITY_LOW,"GROUND START");
startup_ground(); startup_ground();
if (log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG); Log_Write_Startup(TYPE_GROUNDSTART_MSG);
// set the correct flight mode // set the correct flight mode
@ -179,21 +202,6 @@ void init_ardupilot()
reset_control_switch(); reset_control_switch();
} }
/*
byte startup_check(void){
if(DEBUG_SUBSYSTEM > 0){
debug_subsystem();
}else{
if (rc_3.radio_in < (rc_3.radio_in + 25)){
// we are on the ground
return 1;
}else{
return 0;
}
}
}
*/
//******************************************************************************** //********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start //This function does all the calibrations, etc. that we need during a ground start
//******************************************************************************** //********************************************************************************
@ -201,13 +209,13 @@ void startup_ground(void)
{ {
/* /*
read_radio(); read_radio();
while (rc_3.control_in > 0){ while (g.rc_3.control_in > 0){
delay(20); delay(20);
read_radio(); read_radio();
APM_RC.OutputCh(CH_1, rc_3.radio_in); APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
APM_RC.OutputCh(CH_2, rc_3.radio_in); APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
APM_RC.OutputCh(CH_3, rc_3.radio_in); APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
APM_RC.OutputCh(CH_4, rc_3.radio_in); APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
Serial.println("*") Serial.println("*")
} }
*/ */
@ -215,7 +223,7 @@ void startup_ground(void)
// --------------------------- // ---------------------------
trim_radio(); trim_radio();
if (log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG); Log_Write_Startup(TYPE_GROUNDSTART_MSG);
#if(GROUND_START_DELAY > 0) #if(GROUND_START_DELAY > 0)
@ -225,8 +233,8 @@ void startup_ground(void)
// Output waypoints for confirmation // Output waypoints for confirmation
// -------------------------------- // --------------------------------
for(int i = 1; i < wp_total + 1; i++) { for(int i = 1; i < g.waypoint_total + 1; i++) {
send_message(MSG_COMMAND, i); gcs.send_message(MSG_COMMAND_LIST, i);
} }
//IMU ground start //IMU ground start
@ -260,7 +268,7 @@ void set_mode(byte mode)
control_mode = constrain(control_mode, 0, NUM_MODES - 1); control_mode = constrain(control_mode, 0, NUM_MODES - 1);
// used to stop fly_aways // used to stop fly_aways
if(rc_1.control_in == 0){ if(g.rc_1.control_in == 0){
// we are on the ground is this is true // we are on the ground is this is true
// disarm motors temp // disarm motors temp
motor_auto_safe = false; motor_auto_safe = false;
@ -305,7 +313,7 @@ void set_mode(byte mode)
// output control mode to the ground station // output control mode to the ground station
send_message(MSG_HEARTBEAT); send_message(MSG_HEARTBEAT);
if (log_bitmask & MASK_LOG_MODE) if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode); Log_Write_Mode(control_mode);
} }
@ -346,7 +354,7 @@ void update_GPS_light(void)
{ {
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data // GPS LED on if we have a fix or Blink GPS LED if we are receiving data
// --------------------------------------------------------------------- // ---------------------------------------------------------------------
if(GPS.fix == 0){ if(gps->fix == 0){
GPS_light = !GPS_light; GPS_light = !GPS_light;
if(GPS_light){ if(GPS_light){
digitalWrite(C_LED_PIN, HIGH); digitalWrite(C_LED_PIN, HIGH);

130
ArduCopterMega/test.pde
View File

@ -97,7 +97,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
// ---------------------------------------------------------- // ----------------------------------------------------------
read_radio(); read_radio();
Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), rc_1.radio_in, rc_2.radio_in, rc_3.radio_in, rc_4.radio_in, rc_5.radio_in, rc_6.radio_in, rc_7.radio_in, rc_8.radio_in); Serial.printf_P(PSTR("IN: 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), g.rc_1.radio_in, g.rc_2.radio_in, g.rc_3.radio_in, g.rc_4.radio_in, g.rc_5.radio_in, g.rc_6.radio_in, g.rc_7.radio_in, g.rc_8.radio_in);
if(Serial.available() > 0){ if(Serial.available() > 0){
return (0); return (0);
@ -120,24 +120,24 @@ test_radio(uint8_t argc, const Menu::arg *argv)
read_radio(); read_radio();
output_manual_throttle(); output_manual_throttle();
rc_1.calc_pwm(); g.rc_1.calc_pwm();
rc_2.calc_pwm(); g.rc_2.calc_pwm();
rc_3.calc_pwm(); g.rc_3.calc_pwm();
rc_4.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"), (rc_1.control_in), (rc_2.control_in), (rc_3.control_in), (rc_4.control_in), rc_5.control_in, rc_6.control_in, rc_7.control_in); 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"), (rc_1.servo_out / 100), (rc_2.servo_out / 100), rc_3.servo_out, (rc_4.servo_out / 100)); //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" /*Serial.printf_P(PSTR( "min: %d"
"\t in: %d" "\t in: %d"
"\t pwm_in: %d" "\t pwm_in: %d"
"\t sout: %d" "\t sout: %d"
"\t pwm_out %d\n"), "\t pwm_out %d\n"),
rc_3.radio_min, g.rc_3.radio_min,
rc_3.control_in, g.rc_3.control_in,
rc_3.radio_in, g.rc_3.radio_in,
rc_3.servo_out, g.rc_3.servo_out,
rc_3.pwm_out g.rc_3.pwm_out
); );
*/ */
if(Serial.available() > 0){ if(Serial.available() > 0){
@ -164,7 +164,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
oldSwitchPosition = readSwitch(); oldSwitchPosition = readSwitch();
Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
while(rc_3.control_in > 0){ while(g.rc_3.control_in > 0){
delay(20); delay(20);
read_radio(); read_radio();
} }
@ -173,8 +173,8 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
delay(20); delay(20);
read_radio(); read_radio();
if(rc_3.control_in > 0){ if(g.rc_3.control_in > 0){
Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), rc_3.control_in); Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.rc_3.control_in);
fail_test++; fail_test++;
} }
@ -184,8 +184,8 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
fail_test++; fail_test++;
} }
if(throttle_failsafe_enabled && rc_3.get_failsafe()){ if(g.throttle_failsafe_enabled && g.rc_3.get_failsafe()){
Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), rc_3.radio_in); Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.rc_3.radio_in);
Serial.println(flight_mode_strings[readSwitch()]); Serial.println(flight_mode_strings[readSwitch()]);
fail_test++; fail_test++;
} }
@ -214,7 +214,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
init_rc_in(); init_rc_in();
control_mode = STABILIZE; control_mode = STABILIZE;
Serial.printf_P(PSTR("pid_stabilize_roll.kP: %4.4f\n"), pid_stabilize_roll.kP()); 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); Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
trim_radio(); trim_radio();
@ -229,11 +229,11 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
fast_loopTimer = millis(); fast_loopTimer = millis();
G_Dt = (float)delta_ms_fast_loop / 1000.f; G_Dt = (float)delta_ms_fast_loop / 1000.f;
if(compass_enabled){ if(g.compass_enabled){
medium_loopCounter++; medium_loopCounter++;
if(medium_loopCounter == 5){ if(medium_loopCounter == 5){
compass.read(); // Read magnetometer compass.read(); // Read magnetometer
compass.calculate(roll, pitch); // Calculate heading compass.calculate(dcm.roll, dcm.pitch); // Calculate heading
medium_loopCounter = 0; medium_loopCounter = 0;
} }
} }
@ -249,7 +249,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
read_AHRS(); read_AHRS();
// allow us to zero out sensors with control switches // allow us to zero out sensors with control switches
if(rc_5.control_in < 600){ if(g.rc_5.control_in < 600){
dcm.roll_sensor = dcm.pitch_sensor = 0; dcm.roll_sensor = dcm.pitch_sensor = 0;
} }
@ -267,7 +267,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, "), Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, "),
(int)(dcm.roll_sensor/100), (int)(dcm.roll_sensor/100),
(int)(dcm.pitch_sensor/100), (int)(dcm.pitch_sensor/100),
rc_1.pwm_out); g.rc_1.pwm_out);
print_motor_out(); print_motor_out();
} }
@ -297,7 +297,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
init_rc_in(); init_rc_in();
control_mode = FBW; control_mode = FBW;
//Serial.printf_P(PSTR("pid_stabilize_roll.kP: %4.4f\n"), pid_stabilize_roll.kP()); //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); //Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
motor_armed = true; motor_armed = true;
@ -314,11 +314,11 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
G_Dt = (float)delta_ms_fast_loop / 1000.f; G_Dt = (float)delta_ms_fast_loop / 1000.f;
if(compass_enabled){ if(g.compass_enabled){
medium_loopCounter++; medium_loopCounter++;
if(medium_loopCounter == 5){ if(medium_loopCounter == 5){
compass.read(); // Read magnetometer compass.read(); // Read magnetometer
compass.calculate(roll, pitch); // Calculate heading compass.calculate(dcm.roll, dcm.pitch); // Calculate heading
medium_loopCounter = 0; medium_loopCounter = 0;
} }
} }
@ -334,7 +334,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
read_AHRS(); read_AHRS();
// allow us to zero out sensors with control switches // allow us to zero out sensors with control switches
if(rc_5.control_in < 600){ if(g.rc_5.control_in < 600){
dcm.roll_sensor = dcm.pitch_sensor = 0; dcm.roll_sensor = dcm.pitch_sensor = 0;
} }
@ -350,8 +350,8 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
if (ts_num == 5){ if (ts_num == 5){
// 10 hz // 10 hz
ts_num = 0; ts_num = 0;
GPS.longitude = 0; gps->longitude = 0;
GPS.latitude = 0; gps->latitude = 0;
calc_nav(); 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- "), 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,7 +362,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
nav_lon, nav_lon,
nav_roll, nav_roll,
nav_pitch, nav_pitch,
pitch_max); g.pitch_max);
print_motor_out(); print_motor_out();
} }
@ -438,11 +438,11 @@ test_imu(uint8_t argc, const Menu::arg *argv)
update_trig(); update_trig();
if(compass_enabled){ if(g.compass_enabled){
medium_loopCounter++; medium_loopCounter++;
if(medium_loopCounter == 5){ if(medium_loopCounter == 5){
compass.read(); // Read magnetometer compass.read(); // Read magnetometer
compass.calculate(roll, pitch); // Calculate heading compass.calculate(dcm.roll, dcm.pitch); // Calculate heading
medium_loopCounter = 0; medium_loopCounter = 0;
} }
} }
@ -500,12 +500,14 @@ test_gps(uint8_t argc, const Menu::arg *argv)
calc_distance_error(); calc_distance_error();
//if (GPS.new_data){ //if (gps->new_data){
Serial.print("Lat:"); Serial.printf_P(PSTR("Lat: %3.8f, Lon: %3.8f, alt %dm, spd: %d dist:%d, #sats: %d\n"),
Serial.print((float)GPS.latitude/10000000, 10); ((float)gps->latitude / 10000000),
Serial.print(" Lon:"); ((float)gps->longitude / 10000000),
Serial.print((float)GPS.longitude/10000000, 10); (int)gps->altitude / 100,
Serial.printf_P(PSTR(" alt %dm, spd: %d dist:%d, #sats: %d\n"), (int)GPS.altitude/100, (int)GPS.ground_speed, (int)wp_distance, (int)GPS.num_sats); (int)gps->ground_speed,
(int)wp_distance,
(int)gps->num_sats);
//}else{ //}else{
//Serial.print("."); //Serial.print(".");
//} //}
@ -660,11 +662,11 @@ test_current(uint8_t argc, const Menu::arg *argv)
read_current(); read_current();
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"), current_voltage, current_amps, current_total); Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"), current_voltage, current_amps, current_total);
//if(rc_3.control_in > 0){ //if(g.rc_3.control_in > 0){
APM_RC.OutputCh(CH_1, rc_3.radio_in); APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
APM_RC.OutputCh(CH_2, rc_3.radio_in); APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
APM_RC.OutputCh(CH_3, rc_3.radio_in); APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
APM_RC.OutputCh(CH_4, rc_3.radio_in); APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
//} //}
if(Serial.available() > 0){ if(Serial.available() > 0){
@ -708,17 +710,17 @@ test_wp(uint8_t argc, const Menu::arg *argv)
// save the alitude above home option // save the alitude above home option
if(alt_to_hold == -1){ if(g.RTL_altitude == -1){
Serial.printf_P(PSTR("Hold current altitude\n")); Serial.printf_P(PSTR("Hold current altitude\n"));
}else{ }else{
Serial.printf_P(PSTR("Hold altitude of %dm\n"), alt_to_hold/100); Serial.printf_P(PSTR("Hold altitude of %dm\n"), g.RTL_altitude);
} }
Serial.printf_P(PSTR("%d waypoints\n"), wp_total); Serial.printf_P(PSTR("%d waypoints\n"), g.waypoint_total);
Serial.printf_P(PSTR("Hit radius: %d\n"), wp_radius); Serial.printf_P(PSTR("Hit radius: %d\n"), g.waypoint_radius);
Serial.printf_P(PSTR("Loiter radius: %d\n\n"), loiter_radius); Serial.printf_P(PSTR("Loiter radius: %d\n\n"), g.loiter_radius);
for(byte i = 0; i <= wp_total; i++){ for(byte i = 0; i <= g.waypoint_total; i++){
struct Location temp = get_wp_with_index(i); struct Location temp = get_wp_with_index(i);
print_waypoint(&temp, i); print_waypoint(&temp, i);
} }
@ -784,7 +786,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
medium_loopTimer = millis(); medium_loopTimer = millis();
read_radio(); // read the radio first read_radio(); // read the radio first
next_WP.alt = home.alt + 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_trim_switch();
read_barometer(); read_barometer();
@ -793,8 +795,8 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
current_loc.alt, current_loc.alt,
next_WP.alt, next_WP.alt,
altitude_error, altitude_error,
throttle_cruise, g.,
rc_3.servo_out); g.rc_3.servo_out);
/* /*
Serial.print("Altitude: "); Serial.print("Altitude: ");
@ -804,9 +806,9 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
Serial.print("\talt_err: "); Serial.print("\talt_err: ");
Serial.print((int)altitude_error,DEC); Serial.print((int)altitude_error,DEC);
Serial.print("\ttNom: "); Serial.print("\ttNom: ");
Serial.print(throttle_cruise,DEC); Serial.print(g.,DEC);
Serial.print("\ttOut: "); Serial.print("\ttOut: ");
Serial.println(rc_3.servo_out,DEC); Serial.println(g.rc_3.servo_out,DEC);
*/ */
//Serial.print(" Raw pressure value: "); //Serial.print(" Raw pressure value: ");
//Serial.println(abs_pressure); //Serial.println(abs_pressure);
@ -821,7 +823,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
test_mag(uint8_t argc, const Menu::arg *argv) test_mag(uint8_t argc, const Menu::arg *argv)
{ {
if(compass_enabled == false){ if(g.compass_enabled == false){
Serial.printf_P(PSTR("Compass disabled\n")); Serial.printf_P(PSTR("Compass disabled\n"));
return (0); return (0);
}else{ }else{
@ -855,19 +857,19 @@ void print_hit_enter()
void fake_out_gps() void fake_out_gps()
{ {
static float rads; static float rads;
GPS.new_data = true; gps->new_data = true;
GPS.fix = true; gps->fix = true;
int length = rc_6.control_in; int length = g.rc_6.control_in;
rads += .05; rads += .05;
if (rads > 6.28){ if (rads > 6.28){
rads = 0; rads = 0;
} }
GPS.latitude = 377696000; // Y gps->latitude = 377696000; // Y
GPS.longitude = -1224319000; // X gps->longitude = -1224319000; // X
GPS.altitude = 9000; // meters * 100 gps->altitude = 9000; // meters * 100
//next_WP.lng = home.lng - length * sin(rads); // X //next_WP.lng = home.lng - length * sin(rads); // X
//next_WP.lat = home.lat + length * cos(rads); // Y //next_WP.lat = home.lat + length * cos(rads); // Y
@ -877,8 +879,8 @@ void fake_out_gps()
void print_motor_out(){ 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] - rc_3.radio_min), (motor_out[RIGHT] - g.rc_3.radio_min),
(motor_out[LEFT] - rc_3.radio_min), (motor_out[LEFT] - g.rc_3.radio_min),
(motor_out[FRONT] - rc_3.radio_min), (motor_out[FRONT] - g.rc_3.radio_min),
(motor_out[BACK] - rc_3.radio_min)); (motor_out[BACK] - g.rc_3.radio_min));
} }