Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

updated Param gen - won't compile yet. 

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.
// 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 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_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_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
//

405
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.
*/
////////////////////////////////////////////////////////////////////////////////
// Header includes
////////////////////////////////////////////////////////////////////////////////
// AVR runtime
#include <avr/io.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 <AP_Common.h>
#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_GPS.h> // ArduPilot GPS library
#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_DCM.h> // ArduPilot Mega DCM Library
#include <PID.h> // ArduPilot Mega RC Library
//#include <GCS_MAVLink.h> // MAVLink GCS definitions
// Configuration
@ -40,24 +45,52 @@ version 2.1 of the License, or (at your option) any later version.
// Local modules
#include "defines.h"
#include "Parameters.h"
#include "global_data.h"
////////////////////////////////////////////////////////////////////////////////
// Serial ports
////////////////////////////////////////////////////////////////////////////////
//
// 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
// use.
//
FastSerialPort0(Serial); // FTDI/console
FastSerialPort1(Serial1); // GPS port (except for GPS_PROTOCOL_IMU)
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;
APM_BMP085_Class APM_BMP085;
AP_Compass_HMC5843 compass;
// GPS selection
/*
#if GPS_PROTOCOL == GPS_PROTOCOL_NMEA
AP_GPS_NMEA GPS(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
@ -73,15 +106,33 @@ AP_GPS_None GPS(NULL);
#else
# error Must define GPS_PROTOCOL in your configuration file.
#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_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;
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 = ",";
byte flight_modes[6];
//byte flight_modes[6];
const char* flight_mode_strings[] = {
"ACRO",
"STABILIZE",
@ -117,55 +168,25 @@ const char* flight_mode_strings[] = {
// 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];
byte flight_mode_channel;
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);
//byte flight_mode_channel;
//byte frame_type = PLUS_FRAME;
Vector3f omega;
// roll pitch
float stabilize_dampener;
int max_stabilize_dampener;
// yaw
float hold_yaw_dampener;
int max_yaw_dampener;
//float stabilize_dampener;
//float hold_yaw_dampener;
// used to transition yaw control from Rate control to Yaw hold
boolean rate_yaw_flag;
// PIDs
int max_stabilize_dampener; //
int max_yaw_dampener; //
boolean rate_yaw_flag; // used to transition yaw control from Rate control to Yaw hold
// Nav
PID pid_nav_lat (EE_GAIN_7);
PID pid_nav_lon (EE_GAIN_8);
PID pid_baro_throttle (EE_GAIN_9);
PID pid_sonar_throttle (EE_GAIN_10);
boolean motor_light;
// LED output
// ----------
boolean motor_light; // status of the Motor safety
boolean GPS_light; // status of the GPS light
// 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
float scaleLongUp = 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
// ---------------------
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 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
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_may_index; // current command memory location
@ -210,22 +229,22 @@ float cos_yaw_x;
// Airspeed
// --------
int airspeed; // m/s * 100
float airspeed_error; // m / s * 100
// Throttle Failsafe
// ------------------
boolean motor_armed = false;
boolean motor_auto_safe = false;
byte throttle_failsafe_enabled;
int throttle_failsafe_value;
byte throttle_failsafe_action;
uint16_t log_bitmask;
//byte throttle_failsafe_enabled;
//int throttle_failsafe_value;
//byte throttle_failsafe_action;
//uint16_t log_bitmask;
// Location Errors
// ---------------
long bearing_error; // deg * 100 : 0 to 36000
long altitude_error; // meters * 100 we are off in altitude
float airspeed_error; // m / s * 100
float crosstrack_error; // meters we are off trackline
long distance_error; // distance to the WP
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_total;
int milliamp_hours;
boolean current_enabled = false;
//boolean current_enabled = false;
// Magnetometer variables
// ----------------------
int magnetom_x;
int magnetom_y;
int magnetom_z;
float MAG_Heading;
//int magnetom_x;
//int magnetom_y;
//int magnetom_z;
float mag_offset_x;
float mag_offset_y;
float mag_offset_z;
float mag_declination;
bool compass_enabled;
//float mag_offset_x;
//float mag_offset_y;
//float mag_offset_z;
//float mag_declination;
//bool compass_enabled;
// 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_ground;
int ground_temperature;
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
//int temp_unfilt;
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 land_complete = false;
int landing_pitch; // pitch for landing set by commands
//int takeoff_pitch;
int takeoff_altitude;
int landing_distance; // meters;
int takeoff_altitude;
int landing_distance; // meters;
long old_alt; // used for managing altitude rates
int velocity_land;
// Loiter management
// -----------------
@ -304,7 +313,7 @@ long nav_yaw; // deg * 100 : target yaw angle
long nav_lat; // for error calcs
long nav_lon; // for error calcs
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_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
int command_yaw_speed; // how fast to turn
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
// ---------
long GPS_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
byte wp_total; // # of Commands total including way
byte wp_index; // Current active command index
//byte wp_total; // # of Commands total including way
//byte wp_index; // Current active command index
byte next_wp_index; // Current active command index
// repeating event control
@ -358,7 +358,7 @@ struct Location tell_command; // command for telemetry
struct Location next_command; // command preloaded
long target_altitude; // used for
long offset_altitude; // used for
boolean home_is_set = 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
@ -408,50 +408,13 @@ unsigned long dTnav2; // Delta Time in milliseconds for navigation computa
unsigned long elapsedTime; // for doing custom events
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() {
init_ardupilot();
#if ENABLE_EXTRAINIT
init_extras();
#endif
}
void loop()
@ -477,13 +440,14 @@ void loop()
medium_loop();
if (millis() - perf_mon_timer > 20000) {
send_message(MSG_PERF_REPORT);
if (log_bitmask & MASK_LOG_PM){
Log_Write_Performance();
}
resetPerfData();
perf_mon_timer = millis();
}
if (mainLoop_count != 0) {
gcs.send_message(MSG_PERF_REPORT);
if (g.log_bitmask & MASK_LOG_PM){
Log_Write_Performance();
}
resetPerfData();
}
}
}
}
@ -493,8 +457,8 @@ void fast_loop()
// IMU DCM Algorithm
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)
G_Dt_max = delta_ms_fast_loop;
@ -526,10 +490,12 @@ void medium_loop()
case 0:
medium_loopCounter++;
update_GPS();
readCommands();
if(compass_enabled){
compass.read(); // Read magnetometer
compass.calculate(roll, pitch); // Calculate heading
//readCommands();
if(g.compass_enabled){
compass.read(); // Read magnetometer
compass.calculate(dcm.roll, dcm.pitch); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
}
break;
@ -539,8 +505,8 @@ void medium_loop()
case 1:
medium_loopCounter++;
if(GPS.new_data){
GPS.new_data = false;
if(gps->new_data){
gps->new_data = false;
dTnav = millis() - nav_loopTimer;
nav_loopTimer = millis();
@ -580,26 +546,26 @@ void medium_loop()
case 3:
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);
if (log_bitmask & MASK_LOG_CTUN)
if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning();
if (log_bitmask & MASK_LOG_NTUN)
if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning();
if (log_bitmask & MASK_LOG_GPS){
if (g.log_bitmask & MASK_LOG_GPS){
if(home_is_set)
Log_Write_GPS(GPS.time, current_loc.lat, current_loc.lng, GPS.altitude, current_loc.alt, (long) GPS.ground_speed, GPS.ground_course, GPS.fix, GPS.num_sats);
Log_Write_GPS(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;
// This case controls the slow loop
//---------------------------------
case 4:
if (current_enabled){
if (g.current_enabled){
read_current();
}
@ -629,33 +595,22 @@ void medium_loop()
// guess how close we are - fixed observer calc
calc_distance_error();
if (log_bitmask & MASK_LOG_ATTITUDE_FAST)
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
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();
#if GCS_PROTOCOL == 6 // This is here for Benjamin Pelletier. Please do not remove without checking with me. Doug W
readgcsinput();
#endif
#if ENABLE_HIL
output_HIL();
#endif
#if ENABLE_CAM
camera_stabilization();
#endif
#if ENABLE_AM
flight_lights();
#endif
#if ENABLE_xx
do_something_usefull();
#endif
// kick the GCS to process uplink data
gcs.update();
}
@ -666,31 +621,27 @@ void slow_loop()
switch (slow_loopCounter){
case 0:
slow_loopCounter++;
superslow_loopCounter++;
if(superslow_loopCounter >= 15) {
// 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;
if(superslow_loopCounter == 30) {
// save current data to the flash
if (log_bitmask & MASK_LOG_CUR)
if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current();
}else if(superslow_loopCounter >= 400) {
compass.save_offsets();
//eeprom_write_word((uint16_t *) EE_LAST_LOG_PAGE, DataFlash.GetWritePage());
superslow_loopCounter = 0;
}
break;
case 1:
slow_loopCounter++;
//Serial.println(stabilize_rate_roll_pitch,3);
// Read 3-position switch on radio
// -------------------------------
read_control_switch();
//Serial.print("I: ")
//Serial.println(rc_1.get_integrator(), 1);
read_control_switch();
// Read main battery voltage if hooked up - does not read the 5v from radio
// ------------------------------------------------------------------------
@ -704,6 +655,17 @@ void slow_loop()
slow_loopCounter = 0;
update_events();
// XXX this should be a "GCS slow loop" interface
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(1,5);
// 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;
@ -716,19 +678,21 @@ void slow_loop()
void update_GPS(void)
{
GPS.update();
gps->update();
update_GPS_light();
// !!! comment out after testing
//fake_out_gps();
if (GPS.new_data && GPS.fix) {
//if (gps->new_data && gps->fix) {
if (gps->new_data){
send_message(MSG_LOCATION);
// for performance
// ---------------
gps_fix_count++;
//Serial.printf("gs: %d\n", (int)ground_start_count);
if(ground_start_count > 1){
ground_start_count--;
@ -738,45 +702,55 @@ void update_GPS(void)
// so that the altitude is more accurate
// -------------------------------------
if (current_loc.lat == 0) {
Serial.println("!! bad loc");
SendDebugln("!! bad loc");
ground_start_count = 5;
} else {
//Serial.printf("init Home!");
if (log_bitmask & MASK_LOG_CMD)
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
// reset our nav loop timer
nav_loopTimer = millis();
init_home();
// init altitude
current_loc.alt = GPS.altitude;
current_loc.alt = gps->altitude;
ground_start_count = 0;
}
}
/* disabled for now
// baro_offset is an integrator for the gps altitude error
baro_offset += altitude_gain * (float)(GPS.altitude - current_loc.alt);
baro_offset += altitude_gain * (float)(gps->altitude - current_loc.alt);
*/
current_loc.lng = GPS.longitude; // Lon * 10 * *7
current_loc.lat = GPS.latitude; // Lat * 10 * *7
}
current_loc.lng = gps->longitude; // Lon * 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)
{
if(control_mode == AUTO){
//Serial.print("!");
//crash_checker();
switch(command_must_ID){
//case CMD_TAKEOFF:
//case MAV_CMD_NAV_TAKEOFF:
// break;
//case CMD_LAND:
//case MAV_CMD_NAV_LAND:
// break;
default:
@ -825,14 +799,14 @@ void update_current_flight_mode(void)
// ----------------------------
// 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
}else{
output_stabilize_roll(); // hold yaw
}
// 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
}else{
output_stabilize_pitch(); // hold yaw
@ -880,9 +854,9 @@ void update_current_flight_mode(void)
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
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
@ -910,9 +884,9 @@ void update_current_flight_mode(void)
nav_pitch = 0;
nav_roll = 0;
//if(rc_3.control_in)
//if(g.rc_3.control_in)
// 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
//next_WP.alt -= 500;
@ -1001,7 +975,6 @@ void update_navigation()
if(control_mode == AUTO){
verify_must();
verify_may();
}else{
switch(control_mode){
case RTL:
@ -1017,7 +990,7 @@ void read_AHRS(void)
// Perform IMU calculations and get attitude info
//-----------------------------------------------
dcm.update_DCM(G_Dt);
omega = dcm.get_gyro();
omega = dcm.get_gyro();
// Testing remove !!!
//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
// 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_yaw_dampener = pid_yaw.kP() * 6000; // = .5 * 6000 = 3000
max_stabilize_dampener = g.pid_stabilize_roll.kP() * 2500; // = 0.6 * 2500 = 1500 or 15°
max_yaw_dampener = g.pid_yaw.kP() * 6000; // = .5 * 6000 = 3000
}
@ -13,16 +13,16 @@ void control_nav_mixer()
{
// control +- 45° is mixed with the navigation request by the Autopilot
// output is in degrees = target pitch and roll of copter
rc_1.servo_out = rc_1.control_mix(nav_roll);
rc_2.servo_out = rc_2.control_mix(nav_pitch);
g.rc_1.servo_out = g.rc_1.control_mix(nav_roll);
g.rc_2.servo_out = g.rc_2.control_mix(nav_pitch);
}
void fbw_nav_mixer()
{
// control +- 45° is mixed with the navigation request by the Autopilot
// output is in degrees = target pitch and roll of copter
rc_1.servo_out = nav_roll;
rc_2.servo_out = nav_pitch;
g.rc_1.servo_out = nav_roll;
g.rc_2.servo_out = nav_pitch;
}
void output_stabilize_roll()
@ -30,22 +30,22 @@ void output_stabilize_roll()
float error, rate;
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
error = constrain(error, -2500, 2500);
// 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:
// Rate control through bias corrected gyro rates
// omega is the raw gyro reading
// Limit dampening to be equal to propotional term for symmetry
rate = degrees(omega.x) * 100.0; // 6rad = 34377
dampener = (rate * stabilize_dampener); // 34377 * .175 = 6000
rc_1.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP
rate = degrees(omega.x) * 100.0; // 6rad = 34377
dampener = (rate * g.stabilize_dampener); // 34377 * .175 = 6000
g.rc_1.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP
}
void output_stabilize_pitch()
@ -53,22 +53,22 @@ void output_stabilize_pitch()
float error, rate;
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
error = constrain(error, -2500, 2500);
// 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:
// Rate control through bias corrected gyro rates
// omega is the raw gyro reading
// Limit dampening to be equal to propotional term for symmetry
rate = degrees(omega.y) * 100.0; // 6rad = 34377
dampener = (rate * stabilize_dampener); // 34377 * .175 = 6000
rc_2.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP
rate = degrees(omega.y) * 100.0; // 6rad = 34377
dampener = (rate * g.stabilize_dampener); // 34377 * .175 = 6000
g.rc_2.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP
}
void
@ -113,23 +113,23 @@ void output_yaw_with_hold(boolean hold)
yaw_error = constrain(yaw_error, -6000, 6000); // limit error to 60 degees
// 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
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
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{
// rate control
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
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
rc_4.servo_out = 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 = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
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
long rate = degrees(omega.x) * 100; // 3rad = 17188 , 6rad = 34377
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
rc_1.servo_out = constrain(rc_1.servo_out, -2400, 2400); // limit to 2400
g.rc_1.servo_out = g.pid_acro_rate_roll.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
g.rc_1.servo_out = constrain(g.rc_1.servo_out, -2400, 2400); // limit to 2400
}
void output_rate_pitch()
@ -152,28 +152,28 @@ void output_rate_pitch()
// rate control
long rate = degrees(omega.y) * 100; // 3rad = 17188 , 6rad = 34377
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
rc_2.servo_out = constrain(rc_2.servo_out, -2400, 2400); // limit to 2400
g.rc_2.servo_out = g.pid_acro_rate_pitch.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
g.rc_2.servo_out = constrain(g.rc_2.servo_out, -2400, 2400); // limit to 2400
}
/*
rc_1.servo_out = rc_2.control_in;
rc_2.servo_out = rc_2.control_in;
g.rc_1.servo_out = g.rc_2.control_in;
g.rc_2.servo_out = g.rc_2.control_in;
// Rate control through bias corrected gyro rates
// omega is the raw gyro reading plus Omega_I, so it´s bias corrected
rc_1.servo_out -= (omega.x * 5729.57795 * acro_dampener);
rc_2.servo_out -= (omega.y * 5729.57795 * acro_dampener);
g.rc_1.servo_out -= (omega.x * 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);
// Limit output
rc_1.servo_out = constrain(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_1.servo_out = constrain(g.rc_1.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
void reset_I(void)
{
pid_nav_lat.reset_I();
pid_nav_lon.reset_I();
pid_baro_throttle.reset_I();
pid_sonar_throttle.reset_I();
g.pid_nav_lat.reset_I();
g.pid_nav_lon.reset_I();
g.pid_baro_throttle.reset_I();
g.pid_sonar_throttle.reset_I();
}

8
ArduCopterMega/Camera.pde
View File

@ -1,9 +1,9 @@
void init_camera()
{
rc_camera_pitch.set_angle(4500);
rc_camera_pitch.radio_min = rc_6.radio_min;
rc_camera_pitch.radio_trim = rc_6.radio_trim;
rc_camera_pitch.radio_max = rc_6.radio_max;
rc_camera_pitch.radio_min = g.rc_6.radio_min;
rc_camera_pitch.radio_trim = g.rc_6.radio_trim;
rc_camera_pitch.radio_max = g.rc_6.radio_max;
rc_camera_roll.set_angle(4500);
rc_camera_roll.radio_min = 1000;
@ -33,7 +33,7 @@ camera_stabilization()
//If you want to do control mixing use this function.
// 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();
}

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

10
ArduCopterMega/GCS_IMU_ouput.pde
View File

@ -150,16 +150,16 @@ void print_location(void)
Serial.print(",ALT:");
Serial.print(current_loc.alt/100); // meters
Serial.print(",COG:");
Serial.print(GPS.ground_course);
Serial.print(gps->ground_course);
Serial.print(",SOG:");
Serial.print(GPS.ground_speed);
Serial.print(gps->ground_speed);
Serial.print(",FIX:");
Serial.print((int)GPS.fix);
Serial.print((int)gps->fix);
Serial.print(",SAT:");
Serial.print((int)GPS.num_sats);
Serial.print((int)gps->num_sats);
Serial.print (",");
Serial.print("TOW:");
Serial.print(GPS.time);
Serial.print(gps->time);
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[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[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[14] = (tempint >> 8) & 0xff;
@ -109,7 +109,7 @@ void send_message(byte id, long param) {
mess_buffer[15] = tempint & 0xff;
mess_buffer[16] = (tempint >> 8) & 0xff;
templong = GPS.time; // Time of Week (milliseconds) in 4 bytes
templong = gps->time; // Time of Week (milliseconds) in 4 bytes
mess_buffer[17] = templong & 0xff;
mess_buffer[18] = (templong >> 8) & 0xff;
mess_buffer[19] = (templong >> 16) & 0xff;
@ -199,7 +199,7 @@ void send_message(byte id, long param) {
tempint = param; // item number
mess_buffer[3] = tempint & 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[6] = (tempint >> 8) & 0xff;
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("NWP:");
Serial.print(wp_index,DEC);
Serial.print(g.waypoint_index,DEC);
Serial.print(",\t");
Serial.print(next_WP.lat,DEC);
Serial.print(",\t");
@ -109,7 +109,7 @@ void print_waypoints()
{
Serial.println("Commands in memory");
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
//Location tmp;
Serial.println("Home: ");
@ -117,7 +117,7 @@ void print_waypoints()
print_waypoint(&cmd, 0);
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);
print_waypoint(&cmd, i);
}

20
ArduCopterMega/HIL_output.pde
View File

@ -11,15 +11,15 @@ void output_HIL(void)
{
// output real-time sensor data
Serial.print("AAA"); // Message preamble
output_int((int)(rc_1.servo_out)); // 0 bytes 0, 1
output_int((int)(rc_2.servo_out)); // 1 bytes 2, 3
output_int((int)(rc_3.servo_out)); // 2 bytes 4, 5
output_int((int)(rc_4.servo_out)); // 3 bytes 6, 7
output_int((int)(g.rc_1.servo_out)); // 0 bytes 0, 1
output_int((int)(g.rc_2.servo_out)); // 1 bytes 2, 3
output_int((int)(g.rc_3.servo_out)); // 2 bytes 4, 5
output_int((int)(g.rc_4.servo_out)); // 3 bytes 6, 7
output_int((int)wp_distance); // 4 bytes 8,9
output_int((int)bearing_error); // 5 bytes 10,11
output_int((int)next_WP.alt / 100); // 6 bytes 12, 13
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
// print out the buffer and checksum
@ -33,15 +33,15 @@ void output_HIL_(void)
{
// output real-time sensor data
Serial.print("AAA"); // Message preamble
output_int((int)(rc_1.servo_out)); // 0 bytes 0, 1
output_int((int)(rc_2.servo_out)); // 1 bytes 2, 3
output_int((int)(rc_3.servo_out)); // 2 bytes 4, 5
output_int((int)(rc_4.servo_out)); // 3 bytes 6, 7
output_int((int)(g.rc_1.servo_out)); // 0 bytes 0, 1
output_int((int)(g.rc_2.servo_out)); // 1 bytes 2, 3
output_int((int)(g.rc_3.servo_out)); // 2 bytes 4, 5
output_int((int)(g.rc_4.servo_out)); // 3 bytes 6, 7
output_int((int)wp_distance); // 4 bytes 8, 9
output_int((int)bearing_error); // 5 bytes 10,11
output_int((int)dcm.roll_sensor); // 6 bytes 12,13
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
// 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);
Serial.printf_P(PSTR("logs enabled: "));
if (0 == log_bitmask) {
if (0 == g.log_bitmask) {
Serial.printf_P(PSTR("none"));
} else {
// Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined
// 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.
#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_MED);
PLOG(GPS);
@ -173,9 +173,9 @@ select_logs(uint8_t argc, const Menu::arg *argv)
}
if (!strcasecmp_P(argv[0].str, PSTR("enable"))) {
log_bitmask |= bits;
g.log_bitmask |= bits;
} else {
log_bitmask &= ~bits;
g.log_bitmask &= ~bits;
}
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(LOG_STARTUP_MSG);
DataFlash.WriteByte(type);
DataFlash.WriteByte(wp_total);
DataFlash.WriteByte(g.waypoint_total);
DataFlash.WriteByte(END_BYTE);
// create a location struct to hold the temp Waypoints for printing
struct Location cmd = get_wp_with_index(0);
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);
Log_Write_Cmd(i, &cmd);
}
@ -263,14 +263,14 @@ void Log_Write_Control_Tuning()
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
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)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)dcm.pitch_sensor);
DataFlash.WriteInt((int)(rc_3.servo_out));
DataFlash.WriteInt((int)(rc_4.servo_out));
DataFlash.WriteInt((int)(g.rc_3.servo_out));
DataFlash.WriteInt((int)(g.rc_4.servo_out));
DataFlash.WriteInt((int)(accel.y * 10000));
DataFlash.WriteByte(END_BYTE);
}
@ -347,7 +347,7 @@ void Log_Write_Current()
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
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_amps * 100.0));
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()
{
read_EEPROM_waypoint_info();
wp_index = 0;
g.waypoint_index = 0;
command_must_index = 0;
command_may_index = 0;
next_command.id = CMD_BLANK;
@ -11,19 +11,26 @@ void init_commands()
void update_auto()
{
if (wp_index == wp_total){
if (g.waypoint_index == g.waypoint_total){
return_to_launch();
//wp_index = 0;
//g.waypoint_index = 0;
}
}
void reload_commands()
{
init_commands();
read_command_index(); // Get wp_index = command_must_index from EEPROM
if(wp_index > 0){
decrement_WP_index;
}
g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all?
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
@ -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
// --------------------------------------------------------------------------------
if (i > wp_total) {
if (i > g.waypoint_total) {
temp.id = CMD_BLANK;
}else{
// 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)
{
i = constrain(i,0,wp_total);
i = constrain(i, 0, g.waypoint_total);
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
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()
{
if(wp_index < wp_total){
wp_index++;
Serial.printf_P(PSTR("WP index is incremented to %d\n"),wp_index);
if(g.waypoint_index < g.waypoint_total){
g.waypoint_index++;
Serial.printf_P(PSTR("WP index is incremented to %d\n"),g.waypoint_index);
}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()
{
if(wp_index > 0){
wp_index--;
if(g.waypoint_index > 0){
g.waypoint_index--;
}
}
@ -107,7 +114,7 @@ return_to_launch(void)
// home is WP 0
// ------------
wp_index = 0;
g.waypoint_index = 0;
// Loads WP from Memory
// --------------------
@ -145,8 +152,8 @@ It looks to see what the next command type is and finds the last command.
void
set_next_WP(struct Location *wp)
{
Serial.printf_P(PSTR("set_next_WP, wp_index %d\n"), wp_index);
send_message(MSG_COMMAND, wp_index);
Serial.printf_P(PSTR("set_next_WP, wp_index %d\n"), g.waypoint_index);
send_message(MSG_COMMAND, g.waypoint_index);
// copy the current WP into the OldWP slot
// ---------------------------------------
@ -199,14 +206,14 @@ void init_home()
// block until we get a good fix
// -----------------------------
while (!GPS.new_data || !GPS.fix) {
GPS.update();
while (!gps->new_data || !gps->fix) {
gps->update();
}
home.id = CMD_WAYPOINT;
home.lng = GPS.longitude; // Lon * 10**7
home.lat = GPS.latitude; // Lat * 10**7
home.alt = GPS.altitude;
home.lng = gps->longitude; // Lon * 10**7
home.lat = gps->latitude; // Lat * 10**7
home.alt = gps->altitude;
home_is_set = true;
// 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
// --------------------------------
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){
//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.println(next_command.id,DEC);
}
@ -40,7 +40,7 @@ void load_next_command()
// we are out of commands!
//send_message(SEVERITY_LOW,"out of commands!");
//Serial.print("MSG out of commands, wp_index: ");
//Serial.println(wp_index,DEC);
//Serial.println(g.waypoint_index,DEC);
switch (control_mode){
@ -65,14 +65,14 @@ void process_next_command()
if (next_command.id >= 0x10 && next_command.id <= 0x1F ){
increment_WP_index();
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(next_command.id,DEC);
//Serial.print(" index:");
//Serial.println(command_must_index,DEC);
if (log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(wp_index, &next_command);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
process_must();
}
}
@ -82,11 +82,11 @@ void process_next_command()
if (command_may_index == 0){
if (next_command.id >= 0x20 && next_command.id <= 0x2F ){
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.println(command_may_index,DEC);
if (log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(wp_index, &next_command);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
process_may();
}
}
@ -95,8 +95,8 @@ void process_next_command()
// ---------------------------
if (next_command.id >= 0x30){
increment_WP_index();// this command is from the command list in EEPROM
if (log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(wp_index, &next_command);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
process_now();
}
@ -111,7 +111,7 @@ void process_must()
//Serial.println(command_must_index,DEC);
send_message(SEVERITY_LOW,"New cmd: ");
send_message(MSG_COMMAND, wp_index);
send_message(MSG_COMMAND, g.waypoint_index);
// clear May indexes
command_may_index = 0;
@ -185,7 +185,7 @@ void process_must()
void process_may()
{
//Serial.print("process_may cmd# ");
//Serial.println(wp_index,DEC);
//Serial.println(g.waypoint_index,DEC);
command_may_ID = next_command.id;
// invalidate command so a new one is loaded
@ -193,7 +193,7 @@ void process_may()
next_command.id = 0;
send_message(SEVERITY_LOW,"New cmd: ");
send_message(MSG_COMMAND, wp_index);
send_message(MSG_COMMAND, g.waypoint_index);
// do the command
// --------------
@ -274,7 +274,7 @@ void process_now()
{
const float t5 = 100000.0;
//Serial.print("process_now cmd# ");
//Serial.println(wp_index,DEC);
//Serial.println(g.waypoint_index,DEC);
byte id = next_command.id;
@ -283,7 +283,7 @@ void process_now()
next_command.id = 0;
send_message(SEVERITY_LOW, "New cmd: ");
send_message(MSG_COMMAND, wp_index);
send_message(MSG_COMMAND, g.waypoint_index);
// do the command
// --------------
@ -311,7 +311,7 @@ void process_now()
//break;
case CMD_THROTTLE_CRUISE:
throttle_cruise = next_command.p1;
g.throttle_cruise = next_command.p1;
// todo save to EEPROM
break;
@ -368,7 +368,7 @@ void process_now()
case CMD_INDEX:
command_must_index = 0;
command_may_index = 0;
wp_index = next_command.p1 - 1;
g.waypoint_index = next_command.p1 - 1;
break;
case CMD_RELAY:

94
ArduCopterMega/config.h
View File

@ -53,11 +53,11 @@
//////////////////////////////////////////////////////////////////////////////
// GPS_PROTOCOL
//
#ifndef GPS_PROTOCOL
# error XXX
# error XXX You must define GPS_PROTOCOL in APM_Config.h
# error XXX
#endif
//#ifndef GPS_PROTOCOL
//# error XXX
//# error XXX You must define GPS_PROTOCOL in APM_Config.h
//# error XXX
//#endif
// The X-Plane GCS requires the IMU GPS configuration
#if (ENABLE_HIL == ENABLED) && (GPS_PROTOCOL != GPS_PROTOCOL_IMU)
@ -245,6 +245,7 @@
#ifndef ACRO_RATE_ROLL_IMAX
# define ACRO_RATE_ROLL_IMAX 20
#endif
# define ACRO_RATE_ROLL_IMAX_CENTIDEGREE ACRO_RATE_ROLL_IMAX * 100
#ifndef ACRO_RATE_PITCH_P
# define ACRO_RATE_PITCH_P .190
@ -258,6 +259,7 @@
#ifndef ACRO_RATE_PITCH_IMAX
# define ACRO_RATE_PITCH_IMAX 20
#endif
#define ACRO_RATE_PITCH_IMAX_CENTIDEGREE ACRO_RATE_PITCH_IMAX * 100
#ifndef ACRO_RATE_YAW_P
# define ACRO_RATE_YAW_P .07
@ -271,6 +273,7 @@
#ifndef ACRO_RATE_YAW_IMAX
# define ACRO_RATE_YAW_IMAX 0
#endif
# define ACRO_RATE_YAW_IMAX_CENTIDEGREE ACRO_RATE_YAW_IMAX * 100
#ifndef ACRO_RATE_TRIGGER
# define ACRO_RATE_TRIGGER 100
@ -293,6 +296,7 @@
#ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX 3
#endif
#define STABILIZE_ROLL_IMAX_CENTIDEGREE STABILIZE_ROLL_IMAX * 100
#ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 0.6
@ -306,6 +310,7 @@
#ifndef STABILIZE_PITCH_IMAX
# define STABILIZE_PITCH_IMAX 3
#endif
# define STABILIZE_PITCH_IMAX_CENTIDEGREE STABILIZE_PITCH_IMAX * 100
// STABILZE RATE Control
//
@ -329,6 +334,7 @@
#ifndef YAW_IMAX
# define YAW_IMAX 1
#endif
# define YAW_IMAX_CENTIDEGREE YAW_IMAX * 100
// STABILZE YAW Control
//
@ -357,6 +363,7 @@
#ifndef PITCH_MAX
# define PITCH_MAX 36
#endif
#define PITCH_MAX_CENTIDEGREE PITCH_MAX * 100
//////////////////////////////////////////////////////////////////////////////
@ -374,6 +381,7 @@
#ifndef NAV_IMAX
# define NAV_IMAX 250
#endif
# define NAV_IMAX_CENTIDEGREE NAV_IMAX * 100
//////////////////////////////////////////////////////////////////////////////
@ -391,6 +399,8 @@
#ifndef THROTTLE_BARO_IMAX
# define THROTTLE_BARO_IMAX 50
#endif
# define THROTTLE_BARO_IMAX_CENTIDEGREE THROTTLE_BARO_IMAX * 100
#ifndef THROTTLE_SONAR_P
# define THROTTLE_SONAR_P .8
@ -404,17 +414,20 @@
#ifndef THROTTLE_SONAR_IMAX
# define THROTTLE_SONAR_IMAX 300
#endif
# define THROTTLE_SONAR_IMAX_CENTIDEGREE THROTTLE_SONAR_IMAX * 100
//////////////////////////////////////////////////////////////////////////////
// Crosstrack compensation
//
#ifndef XTRACK_GAIN
# define XTRACK_GAIN 0.01
# define XTRACK_GAIN 1 // deg/m
#endif
#ifndef XTRACK_ENTRY_ANGLE
# define XTRACK_ENTRY_ANGLE 30
# define XTRACK_ENTRY_ANGLE 30 // deg
#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
//
@ -473,5 +478,64 @@
# define LOG_CURRENT DISABLED
#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){
set_mode(flight_modes[switchPosition]);
set_mode(g.flight_modes[switchPosition]);
oldSwitchPosition = switchPosition;
@ -17,13 +17,13 @@ void read_control_switch()
byte readSwitch(void){
#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
int pulsewidth = rc_6.radio_in; //
int pulsewidth = g.rc_6.radio_in; //
#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
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
# error Must define FLIGHT_MODE_CHANNEL as CH_5 - CH_8
#endif
@ -57,7 +57,7 @@ unsigned long trim_timer;
void read_trim_switch()
{
// switch is engaged
if (rc_7.control_in > 800){
if (g.rc_7.control_in > 800){
if(trim_flag == false){
// called once
trim_timer = millis();
@ -76,10 +76,10 @@ void read_trim_switch()
} else {
// set the throttle nominal
if(rc_3.control_in > 50){
throttle_cruise = rc_3.control_in;
Serial.printf("tnom %d\n", throttle_cruise);
save_EEPROM_throttle_cruise();
if(g.rc_3.control_in > 50){
g.throttle_cruise = g.rc_3.control_in;
Serial.printf("tnom %d\n", g.);
save_EEPROM_g.();
}
}
trim_flag = false;
@ -90,15 +90,15 @@ void read_trim_switch()
void trim_accel()
{
if(rc_1.control_in > 0){
if(g.rc_1.control_in > 0){
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);
}
if(rc_2.control_in > 0){
if(g.rc_2.control_in > 0){
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);
}

3
ArduCopterMega/defines.h
View File

@ -6,6 +6,9 @@
#define DEBUG 0
#define LOITER_RANGE 30 // for calculating power outside of loiter radius
#define T6 1000000
#define T7 10000000
// GPS baud rates
// --------------
#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!");
set_mode(RTL);
throttle_cruise = THROTTLE_CRUISE;
g.throttle_cruise = THROTTLE_CRUISE;
}
@ -93,22 +93,22 @@ void perform_event()
}
switch(event_id) {
case CH_4_TOGGLE:
event_undo_value = rc_5.radio_out;
event_undo_value = g.rc_5.radio_out;
APM_RC.OutputCh(CH_5, event_value); // send to Servos
undo_event = 2;
break;
case CH_5_TOGGLE:
event_undo_value = rc_6.radio_out;
event_undo_value = g.rc_6.radio_out;
APM_RC.OutputCh(CH_6, event_value); // send to Servos
undo_event = 2;
break;
case CH_6_TOGGLE:
event_undo_value = rc_7.radio_out;
event_undo_value = g.rc_7.radio_out;
APM_RC.OutputCh(CH_7, event_value); // send to Servos
undo_event = 2;
break;
case CH_7_TOGGLE:
event_undo_value = rc_8.radio_out;
event_undo_value = g.rc_8.radio_out;
APM_RC.OutputCh(CH_8, event_value); // send to Servos
undo_event = 2;
event_undo_value = 1;

28
ArduCopterMega/flight_control.pde
View File

@ -7,16 +7,16 @@ throttle control
// -----------
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
// ---------
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
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()
@ -25,26 +25,26 @@ void calc_nav_throttle()
long error = constrain(altitude_error, -400, 400);
if(altitude_sensor == BARO) {
float t = pid_baro_throttle.kP();
float t = g.pid_baro_throttle.kP();
if(error > 0){ // go up
pid_baro_throttle.kP(t);
g.pid_baro_throttle.kP(t);
}else{ // go down
pid_baro_throttle.kP(t/4.0);
g.pid_baro_throttle.kP(t/4.0);
}
// limit output of throttle control
nav_throttle = pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
nav_throttle = throttle_cruise + constrain(nav_throttle, -30, 80);
nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
pid_baro_throttle.kP(t);
g.pid_baro_throttle.kP(t);
} else {
// 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
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;
@ -65,11 +65,11 @@ yaw control
void output_manual_yaw()
{
if(rc_3.control_in == 0){
if(g.rc_3.control_in == 0){
clear_yaw_control();
} else {
// Yaw control
if(rc_4.control_in == 0){
if(g.rc_4.control_in == 0){
//clear_yaw_control();
output_yaw_with_hold(true); // hold yaw
}else{
@ -94,7 +94,7 @@ void calc_nav_pid()
{
// 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);
}

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;
// Arm motor output : Throttle down and full yaw right for more than 2 seconds
if (rc_3.control_in == 0){
if (rc_4.control_in > 2700) {
if (g.rc_3.control_in == 0){
if (g.rc_4.control_in > 2700) {
if (arming_counter > ARM_DELAY) {
motor_armed = true;
} else{
arming_counter++;
}
}else if (rc_4.control_in < -2700) {
}else if (g.rc_4.control_in < -2700) {
if (arming_counter > DISARM_DELAY){
motor_armed = false;
}else{
@ -40,55 +40,55 @@ set_servos_4()
// Quadcopter mix
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
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)
out_min = rc_3.radio_min + 50;
if(g.rc_3.servo_out > 0)
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
rc_1.calc_pwm();
rc_2.calc_pwm();
rc_3.calc_pwm();
rc_4.calc_pwm();
g.rc_1.calc_pwm();
g.rc_2.calc_pwm();
g.rc_3.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("yaw: %d ", 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 ", g.rc_4.radio_out);
if(frame_type == PLUS_FRAME){
motor_out[CH_1] = rc_3.radio_out - rc_1.pwm_out;
motor_out[CH_2] = rc_3.radio_out + rc_1.pwm_out;
motor_out[CH_3] = rc_3.radio_out + rc_2.pwm_out;
motor_out[CH_4] = rc_3.radio_out - rc_2.pwm_out;
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out;
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out;
motor_out[CH_3] = g.rc_3.radio_out + g.rc_2.pwm_out;
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out;
motor_out[CH_1] += rc_4.pwm_out; // CCW
motor_out[CH_2] += rc_4.pwm_out; // CCW
motor_out[CH_3] -= rc_4.pwm_out; // CW
motor_out[CH_4] -= rc_4.pwm_out; // CW
motor_out[CH_1] += g.rc_4.pwm_out; // CCW
motor_out[CH_2] += g.rc_4.pwm_out; // CCW
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_4] -= g.rc_4.pwm_out; // CW
}else if(frame_type == X_FRAME){
int roll_out = rc_1.pwm_out / 2;
int pitch_out = rc_2.pwm_out / 2;
int roll_out = g.rc_1.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_2] = 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] = g.rc_3.radio_out + roll_out - pitch_out;
motor_out[CH_1] = rc_3.radio_out - roll_out + pitch_out;
motor_out[CH_4] = 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] = 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]);
motor_out[CH_1] += rc_4.pwm_out; // CCW
motor_out[CH_2] += rc_4.pwm_out; // CCW
motor_out[CH_3] -= rc_4.pwm_out; // CW
motor_out[CH_4] -= rc_4.pwm_out; // CW
motor_out[CH_1] += g.rc_4.pwm_out; // CCW
motor_out[CH_2] += g.rc_4.pwm_out; // CCW
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_4] -= g.rc_4.pwm_out; // CW
//Serial.printf("\tl8r: %d %d %d %d\n", motor_out[CH_1], motor_out[CH_2], motor_out[CH_3], motor_out[CH_4]);
@ -96,42 +96,42 @@ set_servos_4()
// Tri-copter power distribution
int roll_out = (float)rc_1.pwm_out * .866;
int pitch_out = rc_2.pwm_out / 2;
int roll_out = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;
// front two motors
motor_out[CH_2] = rc_3.radio_out + roll_out + pitch_out;
motor_out[CH_1] = 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] = g.rc_3.radio_out - roll_out + pitch_out;
// 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
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) {
int roll_out = (float)rc_1.pwm_out * .866;
int pitch_out = rc_2.pwm_out / 2;
int roll_out = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;
//left side
motor_out[CH_2] = rc_3.radio_out + rc_1.pwm_out; // CCW
motor_out[CH_3] = 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_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CW
//right side
motor_out[CH_1] = rc_3.radio_out - rc_1.pwm_out; // CW
motor_out[CH_7] = 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_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW
motor_out[CH_7] = g.rc_3.radio_out - roll_out + pitch_out; // CCW
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW
motor_out[CH_7] += rc_4.pwm_out; // CCW
motor_out[CH_2] += rc_4.pwm_out; // CCW
motor_out[CH_4] += rc_4.pwm_out; // CCW
motor_out[CH_7] += g.rc_4.pwm_out; // CCW
motor_out[CH_2] += g.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_1] -= rc_4.pwm_out; // CW
motor_out[CH_8] -= rc_4.pwm_out; // CW
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_1] -= g.rc_4.pwm_out; // CW
motor_out[CH_8] -= g.rc_4.pwm_out; // CW
} else {
@ -141,14 +141,14 @@ set_servos_4()
// 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_2] = constrain(motor_out[CH_2], out_min, rc_3.radio_max);
motor_out[CH_3] = constrain(motor_out[CH_3], out_min, rc_3.radio_max);
motor_out[CH_4] = constrain(motor_out[CH_4], 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, g.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, g.rc_3.radio_max);
if (frame_type == HEXA_FRAME) {
motor_out[CH_7] = constrain(motor_out[CH_7], out_min, rc_3.radio_max);
motor_out[CH_8] = constrain(motor_out[CH_8], 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, g.rc_3.radio_max);
}
num++;
@ -157,22 +157,22 @@ set_servos_4()
//Serial.print("!");
//debugging with Channel 6
//pid_baro_throttle.kD((float)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.kD((float)g.rc_6.control_in / 1000); // 0 to 1
//g.pid_baro_throttle.kP((float)g.rc_6.control_in / 4000); // 0 to .25
/*
// ROLL and PITCH
// 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();
//Serial.print("kP: ");
//Serial.println(pid_stabilize_roll.kP(),3);
//Serial.println(g.pid_stabilize_roll.kP(),3);
//*/
/*
// YAW
// 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();
//*/
@ -182,7 +182,7 @@ set_servos_4()
write_int(motor_out[CH_3]);
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)(sin_yaw_y * 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",
current_loc.alt,
altitude_error,
(int)pid_baro_throttle.get_integrator(),
(int)g.pid_baro_throttle.get_integrator(),
nav_throttle,
angle_boost());
*/
}
// 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_2, motor_out[CH_2]);
@ -234,17 +234,17 @@ set_servos_4()
}else{
APM_RC.OutputCh(CH_1, rc_3.radio_min);
APM_RC.OutputCh(CH_2, rc_3.radio_min);
APM_RC.OutputCh(CH_3, rc_3.radio_min);
APM_RC.OutputCh(CH_4, rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
if (frame_type == HEXA_FRAME) {
APM_RC.OutputCh(CH_7, rc_3.radio_min);
APM_RC.OutputCh(CH_8, rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.Force_Out6_Out7();
}
}
@ -253,27 +253,27 @@ set_servos_4()
// our motor is unarmed, we're on the ground
reset_I();
if(rc_3.control_in > 0){
if(g.rc_3.control_in > 0){
// we have pushed up the throttle
// remove safety
motor_auto_safe = true;
}
// Send commands to motors
APM_RC.OutputCh(CH_1, rc_3.radio_min);
APM_RC.OutputCh(CH_2, rc_3.radio_min);
APM_RC.OutputCh(CH_3, rc_3.radio_min);
APM_RC.OutputCh(CH_4, rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
if (frame_type == HEXA_FRAME) {
APM_RC.OutputCh(CH_7, rc_3.radio_min);
APM_RC.OutputCh(CH_8, rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
}
// reset I terms of PID controls
reset_I();
// Initialize yaw command to actual yaw when throttle is down...
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
// ---------------------------------
if (GPS.fix == 0)
if (gps->fix == 0)
{
GPS.new_data = false;
gps->new_data = false;
return;
}
@ -71,21 +71,21 @@ void calc_nav()
lat_error = constrain(lat_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
// Convert distance into ROLL X
nav_lon = long_error * pid_nav_lon.kP(); // 1800 * 2 = 3600 or 36°
//nav_lon = pid_nav_lon.get_pid(long_error, dTnav2, 1.0);
nav_lon = long_error * g.pid_nav_lon.kP(); // 1800 * 2 = 3600 or 36°
//nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav2, 1.0);
//nav_lon = constrain(nav_lon, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
// PITCH Y
//nav_lat = 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 = g.pid_nav_lat.get_pid(lat_error, dTnav2, 1.0);
nav_lat = lat_error * g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
//nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
// rotate the vector
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
nav_pitch = -((float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y);
nav_roll = constrain(nav_roll, -pitch_max, pitch_max);
nav_pitch = constrain(nav_pitch, -pitch_max, pitch_max);
nav_roll = constrain(nav_roll, -g.pitch_max, g.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
// 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);
//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
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);
}
}

86
ArduCopterMega/radio.pde
View File

@ -1,29 +1,29 @@
void init_rc_in()
{
read_EEPROM_radio(); // read Radio limits
rc_1.set_angle(4500);
rc_1.dead_zone = 60; // 60 = .6 degrees
rc_2.set_angle(4500);
rc_2.dead_zone = 60;
rc_3.set_range(0,1000);
rc_3.dead_zone = 20;
rc_3.scale_output = .9;
rc_4.set_angle(6000);
rc_4.dead_zone = 500;
rc_5.set_range(0,1000);
rc_5.set_filter(false);
g.rc_1.set_angle(4500);
g.rc_1.dead_zone = 60; // 60 = .6 degrees
g.rc_2.set_angle(4500);
g.rc_2.dead_zone = 60;
g.rc_3.set_range(0,1000);
g.rc_3.dead_zone = 20;
g.rc_3.scale_output = .9;
g.rc_4.set_angle(6000);
g.rc_4.dead_zone = 500;
g.rc_5.set_range(0,1000);
g.rc_5.set_filter(false);
// for kP values
//rc_6.set_range(200,800);
//rc_6.set_range(0,1800); // for faking GPS
rc_6.set_range(0,1000);
//g.rc_6.set_range(200,800);
//g.rc_6.set_range(0,1800); // for faking GPS
g.rc_6.set_range(0,1000);
// for camera angles
//rc_6.set_angle(4500);
//rc_6.dead_zone = 60;
//g.rc_6.set_angle(4500);
//g.rc_6.dead_zone = 60;
rc_7.set_range(0,1000);
rc_8.set_range(0,1000);
g.rc_7.set_range(0,1000);
g.rc_8.set_range(0,1000);
}
void init_rc_out()
@ -32,38 +32,38 @@ void init_rc_out()
motor_armed = 1;
#endif
APM_RC.OutputCh(CH_1, rc_3.radio_min); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, rc_3.radio_min);
APM_RC.OutputCh(CH_3, rc_3.radio_min);
APM_RC.OutputCh(CH_4, rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.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_8, rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.Init(); // APM Radio initialization
APM_RC.OutputCh(CH_1, rc_3.radio_min); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, rc_3.radio_min);
APM_RC.OutputCh(CH_3, rc_3.radio_min);
APM_RC.OutputCh(CH_4, rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.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_8, rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
}
void read_radio()
{
rc_1.set_pwm(APM_RC.InputCh(CH_1));
rc_2.set_pwm(APM_RC.InputCh(CH_2));
rc_3.set_pwm(APM_RC.InputCh(CH_3));
rc_4.set_pwm(APM_RC.InputCh(CH_4));
rc_5.set_pwm(APM_RC.InputCh(CH_5));
rc_6.set_pwm(APM_RC.InputCh(CH_6));
rc_7.set_pwm(APM_RC.InputCh(CH_7));
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);
g.rc_1.set_pwm(APM_RC.InputCh(CH_1));
g.rc_2.set_pwm(APM_RC.InputCh(CH_2));
g.rc_3.set_pwm(APM_RC.InputCh(CH_3));
g.rc_4.set_pwm(APM_RC.InputCh(CH_4));
g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
//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()
@ -71,9 +71,9 @@ void trim_radio()
for (byte i = 0; i < 30; i++){
read_radio();
}
rc_1.trim(); // roll
rc_2.trim(); // pitch
rc_4.trim(); // yaw
g.rc_1.trim(); // roll
g.rc_2.trim(); // pitch
g.rc_4.trim(); // yaw
}
void trim_yaw()
@ -81,5 +81,5 @@ void trim_yaw()
for (byte i = 0; i < 30; i++){
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]){
case 'P':
pid_stabilize_roll.kP((float)value / 1000);
pid_stabilize_pitch.kP((float)value / 1000);
g.pid_stabilize_roll.kP((float)value / 1000);
g.pid_stabilize_pitch.kP((float)value / 1000);
save_EEPROM_PID();
break;
case 'I':
pid_stabilize_roll.kI((float)value / 1000);
pid_stabilize_pitch.kI((float)value / 1000);
g.pid_stabilize_roll.kI((float)value / 1000);
g.pid_stabilize_pitch.kI((float)value / 1000);
save_EEPROM_PID();
break;
case 'D':
//pid_stabilize_roll.kD((float)value / 1000);
//pid_stabilize_pitch.kD((float)value / 1000);
//g.pid_stabilize_roll.kD((float)value / 1000);
//g.pid_stabilize_pitch.kD((float)value / 1000);
//save_EEPROM_PID();
break;
case 'X':
pid_stabilize_roll.imax(value * 100);
pid_stabilize_pitch.imax(value * 100);
g.pid_stabilize_roll.imax(value * 100);
g.pid_stabilize_pitch.imax(value * 100);
save_EEPROM_PID();
break;

2
ArduCopterMega/read_me.text
View File

@ -51,7 +51,7 @@ eedump - raw output of bytes in eeprom
Flight modes:
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.
this control method will change after testing.
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;
temp = ((float)ground_temperature / 10.f) + 273.15;
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

314
ArduCopterMega/setup.pde
View File

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

70
ArduCopterMega/system.pde
View File

@ -91,9 +91,30 @@ void init_ardupilot()
#endif
"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_out(); // sets up the timer libs
@ -101,9 +122,11 @@ void init_ardupilot()
adc.Init(); // APM ADC library initialization
APM_BMP085.Init(); // APM Abs Pressure sensor initialization
DataFlash.Init(); // DataFlash log initialization
GPS.init(); // GPS Initialization
gps = &GPS;
gps->init();
if(compass_enabled)
if(g.compass_enabled)
init_compass();
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
// 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);
@ -171,7 +194,7 @@ void init_ardupilot()
send_message(SEVERITY_LOW,"GROUND START");
startup_ground();
if (log_bitmask & MASK_LOG_CMD)
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
// set the correct flight mode
@ -179,21 +202,6 @@ void init_ardupilot()
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
//********************************************************************************
@ -201,13 +209,13 @@ void startup_ground(void)
{
/*
read_radio();
while (rc_3.control_in > 0){
while (g.rc_3.control_in > 0){
delay(20);
read_radio();
APM_RC.OutputCh(CH_1, rc_3.radio_in);
APM_RC.OutputCh(CH_2, rc_3.radio_in);
APM_RC.OutputCh(CH_3, rc_3.radio_in);
APM_RC.OutputCh(CH_4, rc_3.radio_in);
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
Serial.println("*")
}
*/
@ -215,7 +223,7 @@ void startup_ground(void)
// ---------------------------
trim_radio();
if (log_bitmask & MASK_LOG_CMD)
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
#if(GROUND_START_DELAY > 0)
@ -225,8 +233,8 @@ void startup_ground(void)
// Output waypoints for confirmation
// --------------------------------
for(int i = 1; i < wp_total + 1; i++) {
send_message(MSG_COMMAND, i);
for(int i = 1; i < g.waypoint_total + 1; i++) {
gcs.send_message(MSG_COMMAND_LIST, i);
}
//IMU ground start
@ -260,7 +268,7 @@ void set_mode(byte mode)
control_mode = constrain(control_mode, 0, NUM_MODES - 1);
// 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
// disarm motors temp
motor_auto_safe = false;
@ -305,7 +313,7 @@ void set_mode(byte mode)
// output control mode to the ground station
send_message(MSG_HEARTBEAT);
if (log_bitmask & MASK_LOG_MODE)
if (g.log_bitmask & MASK_LOG_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
// ---------------------------------------------------------------------
if(GPS.fix == 0){
if(gps->fix == 0){
GPS_light = !GPS_light;
if(GPS_light){
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();
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){
return (0);
@ -120,24 +120,24 @@ test_radio(uint8_t argc, const Menu::arg *argv)
read_radio();
output_manual_throttle();
rc_1.calc_pwm();
rc_2.calc_pwm();
rc_3.calc_pwm();
rc_4.calc_pwm();
g.rc_1.calc_pwm();
g.rc_2.calc_pwm();
g.rc_3.calc_pwm();
g.rc_4.calc_pwm();
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"), (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("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("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"), (g.rc_1.control_in), (g.rc_2.control_in), (g.rc_3.control_in), (g.rc_4.control_in), g.rc_5.control_in, g.rc_6.control_in, g.rc_7.control_in);
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (g.rc_1.servo_out / 100), (g.rc_2.servo_out / 100), g.rc_3.servo_out, (g.rc_4.servo_out / 100));
/*Serial.printf_P(PSTR( "min: %d"
"\t in: %d"
"\t pwm_in: %d"
"\t sout: %d"
"\t pwm_out %d\n"),
rc_3.radio_min,
rc_3.control_in,
rc_3.radio_in,
rc_3.servo_out,
rc_3.pwm_out
g.rc_3.radio_min,
g.rc_3.control_in,
g.rc_3.radio_in,
g.rc_3.servo_out,
g.rc_3.pwm_out
);
*/
if(Serial.available() > 0){
@ -164,7 +164,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
oldSwitchPosition = readSwitch();
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);
read_radio();
}
@ -173,8 +173,8 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
delay(20);
read_radio();
if(rc_3.control_in > 0){
Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), rc_3.control_in);
if(g.rc_3.control_in > 0){
Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.rc_3.control_in);
fail_test++;
}
@ -184,8 +184,8 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
fail_test++;
}
if(throttle_failsafe_enabled && rc_3.get_failsafe()){
Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), rc_3.radio_in);
if(g.throttle_failsafe_enabled && g.rc_3.get_failsafe()){
Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.rc_3.radio_in);
Serial.println(flight_mode_strings[readSwitch()]);
fail_test++;
}
@ -214,7 +214,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
init_rc_in();
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);
trim_radio();
@ -229,11 +229,11 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
fast_loopTimer = millis();
G_Dt = (float)delta_ms_fast_loop / 1000.f;
if(compass_enabled){
if(g.compass_enabled){
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(roll, pitch); // Calculate heading
compass.calculate(dcm.roll, dcm.pitch); // Calculate heading
medium_loopCounter = 0;
}
}
@ -249,7 +249,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
read_AHRS();
// 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;
}
@ -267,7 +267,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, "),
(int)(dcm.roll_sensor/100),
(int)(dcm.pitch_sensor/100),
rc_1.pwm_out);
g.rc_1.pwm_out);
print_motor_out();
}
@ -297,7 +297,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
init_rc_in();
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);
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;
if(compass_enabled){
if(g.compass_enabled){
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(roll, pitch); // Calculate heading
compass.calculate(dcm.roll, dcm.pitch); // Calculate heading
medium_loopCounter = 0;
}
}
@ -334,7 +334,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
read_AHRS();
// 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;
}
@ -350,8 +350,8 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
if (ts_num == 5){
// 10 hz
ts_num = 0;
GPS.longitude = 0;
GPS.latitude = 0;
gps->longitude = 0;
gps->latitude = 0;
calc_nav();
Serial.printf_P(PSTR("ys:%ld, WP.lat:%ld, WP.lng:%ld, n_lat:%ld, n_lon:%ld, nroll:%ld, npitch:%ld, pmax:%ld, \t- "),
@ -362,7 +362,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
nav_lon,
nav_roll,
nav_pitch,
pitch_max);
g.pitch_max);
print_motor_out();
}
@ -438,11 +438,11 @@ test_imu(uint8_t argc, const Menu::arg *argv)
update_trig();
if(compass_enabled){
if(g.compass_enabled){
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(roll, pitch); // Calculate heading
compass.calculate(dcm.roll, dcm.pitch); // Calculate heading
medium_loopCounter = 0;
}
}
@ -500,12 +500,14 @@ test_gps(uint8_t argc, const Menu::arg *argv)
calc_distance_error();
//if (GPS.new_data){
Serial.print("Lat:");
Serial.print((float)GPS.latitude/10000000, 10);
Serial.print(" Lon:");
Serial.print((float)GPS.longitude/10000000, 10);
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);
//if (gps->new_data){
Serial.printf_P(PSTR("Lat: %3.8f, Lon: %3.8f, alt %dm, spd: %d dist:%d, #sats: %d\n"),
((float)gps->latitude / 10000000),
((float)gps->longitude / 10000000),
(int)gps->altitude / 100,
(int)gps->ground_speed,
(int)wp_distance,
(int)gps->num_sats);
//}else{
//Serial.print(".");
//}
@ -660,11 +662,11 @@ test_current(uint8_t argc, const Menu::arg *argv)
read_current();
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){
APM_RC.OutputCh(CH_1, rc_3.radio_in);
APM_RC.OutputCh(CH_2, rc_3.radio_in);
APM_RC.OutputCh(CH_3, rc_3.radio_in);
APM_RC.OutputCh(CH_4, rc_3.radio_in);
//if(g.rc_3.control_in > 0){
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
//}
if(Serial.available() > 0){
@ -708,17 +710,17 @@ test_wp(uint8_t argc, const Menu::arg *argv)
// save the alitude above home option
if(alt_to_hold == -1){
if(g.RTL_altitude == -1){
Serial.printf_P(PSTR("Hold current altitude\n"));
}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("Hit radius: %d\n"), wp_radius);
Serial.printf_P(PSTR("Loiter radius: %d\n\n"), loiter_radius);
Serial.printf_P(PSTR("%d waypoints\n"), g.waypoint_total);
Serial.printf_P(PSTR("Hit radius: %d\n"), g.waypoint_radius);
Serial.printf_P(PSTR("Loiter radius: %d\n\n"), g.loiter_radius);
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);
print_waypoint(&temp, i);
}
@ -784,7 +786,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
medium_loopTimer = millis();
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_barometer();
@ -793,8 +795,8 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
current_loc.alt,
next_WP.alt,
altitude_error,
throttle_cruise,
rc_3.servo_out);
g.,
g.rc_3.servo_out);
/*
Serial.print("Altitude: ");
@ -804,9 +806,9 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
Serial.print("\talt_err: ");
Serial.print((int)altitude_error,DEC);
Serial.print("\ttNom: ");
Serial.print(throttle_cruise,DEC);
Serial.print(g.,DEC);
Serial.print("\ttOut: ");
Serial.println(rc_3.servo_out,DEC);
Serial.println(g.rc_3.servo_out,DEC);
*/
//Serial.print(" Raw pressure value: ");
//Serial.println(abs_pressure);
@ -821,7 +823,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
static int8_t
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"));
return (0);
}else{
@ -855,19 +857,19 @@ void print_hit_enter()
void fake_out_gps()
{
static float rads;
GPS.new_data = true;
GPS.fix = true;
gps->new_data = true;
gps->fix = true;
int length = rc_6.control_in;
int length = g.rc_6.control_in;
rads += .05;
if (rads > 6.28){
rads = 0;
}
GPS.latitude = 377696000; // Y
GPS.longitude = -1224319000; // X
GPS.altitude = 9000; // meters * 100
gps->latitude = 377696000; // Y
gps->longitude = -1224319000; // X
gps->altitude = 9000; // meters * 100
//next_WP.lng = home.lng - length * sin(rads); // X
//next_WP.lat = home.lat + length * cos(rads); // Y
@ -877,8 +879,8 @@ void fake_out_gps()
void print_motor_out(){
Serial.printf("out: R: %d, L: %d F: %d B: %d\n",
(motor_out[RIGHT] - rc_3.radio_min),
(motor_out[LEFT] - rc_3.radio_min),
(motor_out[FRONT] - rc_3.radio_min),
(motor_out[BACK] - rc_3.radio_min));
(motor_out[RIGHT] - g.rc_3.radio_min),
(motor_out[LEFT] - g.rc_3.radio_min),
(motor_out[FRONT] - g.rc_3.radio_min),
(motor_out[BACK] - g.rc_3.radio_min));
}