From 1716a41a97205d41a13618db106348b2e394fd2b Mon Sep 17 00:00:00 2001 From: jasonshort Date: Thu, 17 Feb 2011 07:09:13 +0000 Subject: [PATCH] updated Param gen - won't compile yet. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1666 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/APM_Config.h | 3 +- ArduCopterMega/APM_Config.h.reference | 4 +- ArduCopterMega/ArduCopterMega.pde | 405 ++++++++++++-------------- ArduCopterMega/Attitude.pde | 78 ++--- ArduCopterMega/Camera.pde | 8 +- ArduCopterMega/EEPROM.txt | 390 +++++++++++++++++++++++++ ArduCopterMega/GCS_Ardupilot.pde | 10 +- ArduCopterMega/GCS_IMU_ouput.pde | 10 +- ArduCopterMega/GCS_Standard.pde | 8 +- ArduCopterMega/GCS_Xplane.pde | 6 +- ArduCopterMega/HIL_output.pde | 20 +- ArduCopterMega/Log.pde | 22 +- ArduCopterMega/Mavlink_Common.h | 253 ++++++++++++++++ ArduCopterMega/Parameters.h | 285 ++++++++++++++++++ ArduCopterMega/commands.pde | 53 ++-- ArduCopterMega/commands_process.pde | 36 +-- ArduCopterMega/config.h | 94 +++++- ArduCopterMega/control_modes.pde | 28 +- ArduCopterMega/defines.h | 3 + ArduCopterMega/events.pde | 10 +- ArduCopterMega/flight_control.pde | 28 +- ArduCopterMega/global_data.h | 55 ++++ ArduCopterMega/global_data.pde | 20 ++ ArduCopterMega/motors.pde | 160 +++++----- ArduCopterMega/navigation.pde | 20 +- ArduCopterMega/radio.pde | 86 +++--- ArduCopterMega/read_commands.pde | 16 +- ArduCopterMega/read_me.text | 2 +- ArduCopterMega/sensors.pde | 2 +- ArduCopterMega/setup.pde | 314 ++++++++++---------- ArduCopterMega/system.pde | 70 +++-- ArduCopterMega/test.pde | 130 +++++---- 32 files changed, 1845 insertions(+), 784 deletions(-) create mode 100644 ArduCopterMega/EEPROM.txt create mode 100644 ArduCopterMega/Mavlink_Common.h create mode 100644 ArduCopterMega/Parameters.h create mode 100644 ArduCopterMega/global_data.h create mode 100644 ArduCopterMega/global_data.pde diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index 16e2cfc70c..7566b24a5f 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -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 diff --git a/ArduCopterMega/APM_Config.h.reference b/ArduCopterMega/APM_Config.h.reference index 5d575ac328..46dfb00ab6 100644 --- a/ArduCopterMega/APM_Config.h.reference +++ b/ArduCopterMega/APM_Config.h.reference @@ -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 // diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 620696479e..ea201a4d38 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -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 #include @@ -22,7 +26,7 @@ version 2.1 of the License, or (at your option) any later version. #include #include #include // ArduPilot Mega RC Library -#include // ArduPilot Mega RC Library +#include // RC Channel Library #include // ArduPilot Mega Analog to Digital Converter Library #include // ArduPilot GPS library #include // Arduino I2C lib @@ -33,6 +37,7 @@ version 2.1 of the License, or (at your option) any later version. #include // ArduPilot Mega IMU Library #include // ArduPilot Mega DCM Library #include // ArduPilot Mega RC Library +//#include // 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; diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index df962fce47..72e5852203 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -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(); } diff --git a/ArduCopterMega/Camera.pde b/ArduCopterMega/Camera.pde index c3ba7d1b71..3b1b5df194 100644 --- a/ArduCopterMega/Camera.pde +++ b/ArduCopterMega/Camera.pde @@ -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(); } diff --git a/ArduCopterMega/EEPROM.txt b/ArduCopterMega/EEPROM.txt new file mode 100644 index 0000000000..6747a74f37 --- /dev/null +++ b/ArduCopterMega/EEPROM.txt @@ -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); +} diff --git a/ArduCopterMega/GCS_Ardupilot.pde b/ArduCopterMega/GCS_Ardupilot.pde index 7be04d7a2f..9ddb309d55 100644 --- a/ArduCopterMega/GCS_Ardupilot.pde +++ b/ArduCopterMega/GCS_Ardupilot.pde @@ -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(",***"); } diff --git a/ArduCopterMega/GCS_IMU_ouput.pde b/ArduCopterMega/GCS_IMU_ouput.pde index 9456a4b16c..26ea49d4ed 100644 --- a/ArduCopterMega/GCS_IMU_ouput.pde +++ b/ArduCopterMega/GCS_IMU_ouput.pde @@ -150,16 +150,16 @@ void print_location(void) Serial.print(",ALT:"); Serial.print(current_loc.alt/100); // meters Serial.print(",COG:"); - Serial.print(GPS.ground_course); + Serial.print(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("***"); } diff --git a/ArduCopterMega/GCS_Standard.pde b/ArduCopterMega/GCS_Standard.pde index e72cd1b020..5a96acc68e 100644 --- a/ArduCopterMega/GCS_Standard.pde +++ b/ArduCopterMega/GCS_Standard.pde @@ -97,11 +97,11 @@ void send_message(byte id, long param) { mess_buffer[9] = (templong >> 16) & 0xff; mess_buffer[10] = (templong >> 24) & 0xff; - tempint = GPS.altitude / 100; // Altitude MSL in meters * 10 in 2 bytes + tempint = 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); diff --git a/ArduCopterMega/GCS_Xplane.pde b/ArduCopterMega/GCS_Xplane.pde index e6f4c2f235..9ac670789c 100644 --- a/ArduCopterMega/GCS_Xplane.pde +++ b/ArduCopterMega/GCS_Xplane.pde @@ -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; icast_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 diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h new file mode 100644 index 0000000000..baf6f3934e --- /dev/null +++ b/ArduCopterMega/Parameters.h @@ -0,0 +1,285 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef PARAMETERS_H +#define PARAMETERS_H + +#include + +// 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 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 diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index 1c9e977ef1..310bbd07d1 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -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 diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde index 76050bbe93..9498a875b2 100644 --- a/ArduCopterMega/commands_process.pde +++ b/ArduCopterMega/commands_process.pde @@ -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: diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index dadf75cbce..4186547086 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -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) diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde index 63ff046094..e4bcb52569 100644 --- a/ArduCopterMega/control_modes.pde +++ b/ArduCopterMega/control_modes.pde @@ -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); } diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 030bd07179..7107fdf783 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -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 diff --git a/ArduCopterMega/events.pde b/ArduCopterMega/events.pde index b42c160306..4a5cd6109f 100644 --- a/ArduCopterMega/events.pde +++ b/ArduCopterMega/events.pde @@ -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; diff --git a/ArduCopterMega/flight_control.pde b/ArduCopterMega/flight_control.pde index 8e2d4e2ffe..09220cb1ff 100644 --- a/ArduCopterMega/flight_control.pde +++ b/ArduCopterMega/flight_control.pde @@ -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); } diff --git a/ArduCopterMega/global_data.h b/ArduCopterMega/global_data.h new file mode 100644 index 0000000000..ef01589f8e --- /dev/null +++ b/ArduCopterMega/global_data.h @@ -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 diff --git a/ArduCopterMega/global_data.pde b/ArduCopterMega/global_data.pde new file mode 100644 index 0000000000..5dcd11cfb5 --- /dev/null +++ b/ArduCopterMega/global_data.pde @@ -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) +{ +} diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index ac04a5aab3..cd28dbc350 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -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); } } diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index c201ac14ab..ddd2bf28cc 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -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); } } diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index 02324c894a..76a4978366 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -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 } diff --git a/ArduCopterMega/read_commands.pde b/ArduCopterMega/read_commands.pde index 430e57dc44..3d9ceae888 100644 --- a/ArduCopterMega/read_commands.pde +++ b/ArduCopterMega/read_commands.pde @@ -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; diff --git a/ArduCopterMega/read_me.text b/ArduCopterMega/read_me.text index c5c4e339a8..73dcd9b9e1 100644 --- a/ArduCopterMega/read_me.text +++ b/ArduCopterMega/read_me.text @@ -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. diff --git a/ArduCopterMega/sensors.pde b/ArduCopterMega/sensors.pde index 333d0ae824..ef7ef5f4bd 100644 --- a/ArduCopterMega/sensors.pde +++ b/ArduCopterMega/sensors.pde @@ -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 diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index f131a45c40..2899b7a3b3 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -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) diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 9390f4c4a5..fd1abf83c1 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -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); diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index cbb537a91b..772a8e7e68 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -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)); } \ No newline at end of file