From 7acf9a15fcd942b00676efaeca168e57d98d164d Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sun, 20 Feb 2011 02:03:01 +0000 Subject: [PATCH] AP_Var integration continued git-svn-id: https://arducopter.googlecode.com/svn/trunk@1697 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/ArduCopterMega.pde | 98 +++++++------------ ArduCopterMega/Attitude.pde | 133 ++++++++++++++++++++------ ArduCopterMega/EEPROM.pde | 43 +++++---- ArduCopterMega/Parameters.h | 44 ++++----- ArduCopterMega/config.h | 15 +-- ArduCopterMega/defines.h | 6 +- ArduCopterMega/flight_control.pde | 154 ------------------------------ ArduCopterMega/sensors.pde | 14 +-- ArduCopterMega/setup.pde | 25 +++-- ArduCopterMega/system.pde | 8 +- ArduCopterMega/test.pde | 3 +- 11 files changed, 216 insertions(+), 327 deletions(-) diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 49294ae1b3..0a18da46e8 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -1,4 +1,4 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- /* ArduCopterMega Version 0.1.3 Experimental @@ -6,7 +6,8 @@ Authors: Jason Short Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier -This firmware is free software; you can redistribute it and / or + +This firmware is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. @@ -26,19 +27,19 @@ version 2.1 of the License, or (at your option) any later version. #include #include #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 -#include // ArduPilot Mega BMP085 Library #include // ArduPilot Mega Flash Memory Library -#include // ArduPilot Mega Magnetometer Library +#include // ArduPilot Mega Analog to Digital Converter Library +#include // ArduPilot Mega BMP085 Library +#include // ArduPilot Mega Magnetometer Library #include // ArduPilot Mega Vector/Matrix math Library #include // ArduPilot Mega IMU Library -#include // ArduPilot Mega DCM Library +#include // ArduPilot Mega DCM Library #include // ArduPilot Mega RC Library -#include // MAVLink GCS definitions +#include // RC Channel Library +#include // MAVLink GCS definitions // Configuration #include "config.h" @@ -57,9 +58,9 @@ version 2.1 of the License, or (at your option) any later version. // 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) +FastSerialPort0(Serial); // FTDI/console +FastSerialPort1(Serial1); // GPS port +FastSerialPort3(Serial3); // Telemetry port //////////////////////////////////////////////////////////////////////////////// // Parameters @@ -83,15 +84,12 @@ Parameters g; // // All GPS access should be through this pointer. -GPS *g_gps; - -//#if HIL_MODE == HIL_MODE_NONE +GPS *g_gps; // real sensors AP_ADC_ADS7844 adc; -APM_BMP085_Class APM_BMP085; -AP_Compass_HMC5843 compass; - +APM_BMP085_Class barometer; +AP_Compass_HMC5843 compass(Parameters::k_param_compass); // real GPS selection #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO @@ -135,14 +133,12 @@ byte control_mode = STABILIZE; boolean failsafe = false; // did our throttle dip below the failsafe value? boolean ch3_failsafe = false; byte oldSwitchPosition; // for remembering the control mode switch -byte fbw_timer; // for limiting the execution of FBW input const char *comma = ","; -//byte flight_modes[6]; const char* flight_mode_strings[] = { - "ACRO", "STABILIZE", + "ACRO", "ALT_HOLD", "FBW", "AUTO", @@ -166,15 +162,8 @@ const char* flight_mode_strings[] = { // Radio // ----- int motor_out[4]; -//byte flight_mode_channel; -//byte frame_type = PLUS_FRAME; - Vector3f omega; - -//float stabilize_dampener; -//float hold_yaw_dampener; - // PIDs int max_stabilize_dampener; // int max_yaw_dampener; // @@ -189,10 +178,10 @@ float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TO // GPS variables // ------------- -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 +byte ground_start_count = 5; // have we achieved first lock and set Home? // Magnetometer variables // ---------------------- @@ -213,16 +202,9 @@ byte command_may_index; // current command memory location byte command_must_ID; // current command ID byte command_may_ID; // current command ID -//float altitude_gain; // in nav - -float cos_roll_x; -float sin_roll_y; - -float cos_pitch_x; -float sin_pitch_y; - -float sin_yaw_y; -float cos_yaw_x; +float cos_roll_x, sin_roll_y; +float cos_pitch_x, sin_pitch_y; +float sin_yaw_y, cos_yaw_x; // Airspeed // -------- @@ -234,11 +216,6 @@ float airspeed_error; // m / s * 100 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; - // Location Errors // --------------- long bearing_error; // deg * 100 : 0 to 36000 @@ -260,7 +237,7 @@ float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + float current_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter float current_amps; float current_total; -int milliamp_hours; +//int milliamp_hours; //boolean current_enabled = false; // Magnetometer variables @@ -273,7 +250,6 @@ int milliamp_hours; //float mag_offset_y; //float mag_offset_z; //float mag_declination; -//bool compass_enabled; // Barometer Sensor variables // -------------------------- @@ -392,6 +368,7 @@ byte medium_count; byte slow_loopCounter; byte superslow_loopCounter; +byte fbw_timer; // for limiting the execution of FBW input unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav unsigned long nav2_loopTimer; // used to track the elapsed ime for GPS nav @@ -609,7 +586,6 @@ void medium_loop() gcs.update(); } - void slow_loop() { // This is the slow (3 1/3 Hz) loop pieces @@ -617,17 +593,14 @@ void slow_loop() switch (slow_loopCounter){ case 0: slow_loopCounter++; - superslow_loopCounter++; - if(superslow_loopCounter == 30) { - // save current data to the flash + if(superslow_loopCounter >= 200){ 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()); + if(g.compass_enabled){ + compass.save_offsets(); + } superslow_loopCounter = 0; } break; @@ -677,18 +650,17 @@ void update_GPS(void) g_gps->update(); update_GPS_light(); - // !!! comment out after testing - //fake_out_gps(); - - //if (g_gps->new_data && g_gps->fix) { - if (g_gps->new_data){ - send_message(MSG_LOCATION); + if (g_gps->new_data && g_gps->fix) { + // XXX We should be sending GPS data off one of the regular loops so that we send + // no-GPS-fix data too + #if GCS_PROTOCOL != GCS_PROTOCOL_MAVLINK + gcs.send_message(MSG_LOCATION); + #endif // for performance // --------------- gps_fix_count++; - //Serial.printf("gs: %d\n", (int)ground_start_count); if(ground_start_count > 1){ ground_start_count--; @@ -710,17 +682,13 @@ void update_GPS(void) // reset our nav loop timer nav_loopTimer = millis(); init_home(); + // init altitude current_loc.alt = g_gps->altitude; ground_start_count = 0; } } - /* disabled for now - // baro_offset is an integrator for the g_gps altitude error - baro_offset += altitude_gain * (float)(g_gps->altitude - current_loc.alt); - */ - current_loc.lng = g_gps->longitude; // Lon * 10 * *7 current_loc.lat = g_gps->latitude; // Lat * 10 * *7 diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 51092f5dc6..03caebc8c7 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -1,5 +1,6 @@ -void init_pids() +void +init_pids() { // create limits to how much dampening we'll allow // this creates symmetry with the P gain value preventing oscillations @@ -9,7 +10,8 @@ void init_pids() } -void control_nav_mixer() +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 @@ -17,7 +19,8 @@ void control_nav_mixer() g.rc_2.servo_out = g.rc_2.control_mix(nav_pitch); } -void fbw_nav_mixer() +void +fbw_nav_mixer() { // control +- 45° is mixed with the navigation request by the Autopilot // output is in degrees = target pitch and roll of copter @@ -25,7 +28,8 @@ void fbw_nav_mixer() g.rc_2.servo_out = nav_pitch; } -void output_stabilize_roll() +void +output_stabilize_roll() { float error, rate; int dampener; @@ -48,7 +52,8 @@ void output_stabilize_roll() g.rc_1.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP } -void output_stabilize_pitch() +void +output_stabilize_pitch() { float error, rate; int dampener; @@ -80,7 +85,8 @@ clear_yaw_control() yaw_error = 0; } -void output_yaw_with_hold(boolean hold) +void +output_yaw_with_hold(boolean hold) { if(hold){ // look to see if we have exited rate control properly - ie stopped turning @@ -134,9 +140,8 @@ void output_yaw_with_hold(boolean hold) } } - - -void output_rate_roll() +void +output_rate_roll() { // rate control long rate = degrees(omega.x) * 100; // 3rad = 17188 , 6rad = 34377 @@ -147,7 +152,8 @@ void output_rate_roll() g.rc_1.servo_out = constrain(g.rc_1.servo_out, -2400, 2400); // limit to 2400 } -void output_rate_pitch() +void +output_rate_pitch() { // rate control long rate = degrees(omega.y) * 100; // 3rad = 17188 , 6rad = 34377 @@ -158,29 +164,10 @@ void output_rate_pitch() g.rc_2.servo_out = constrain(g.rc_2.servo_out, -2400, 2400); // limit to 2400 } -/* - - g.rc_1.servo_out = g.rc_2.control_in; - g.rc_2.servo_out = g.rc_2.control_in; - - // Rate control through bias corrected gyro rates - // omega is the raw gyro reading plus Omega_I, so it´s bias corrected - g.rc_1.servo_out -= (omega.x * 5729.57795 * acro_dampener); - g.rc_2.servo_out -= (omega.y * 5729.57795 * acro_dampener); - - //Serial.printf("\trated out %d, omega ", g.rc_1.servo_out); - //Serial.print((Omega[0] * 5729.57795 * stabilize_rate_roll_pitch), 3); - - // Limit 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); - */ -//} - - // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. // Keeps outdated data out of our calculations -void reset_I(void) +void +reset_I(void) { g.pid_nav_lat.reset_I(); g.pid_nav_lon.reset_I(); @@ -190,3 +177,87 @@ void reset_I(void) + +/************************************************************* +throttle control +****************************************************************/ + +// user input: +// ----------- +void output_manual_throttle() +{ + g.rc_3.servo_out = (float)g.rc_3.control_in * angle_boost(); +} + +// Autopilot +// --------- +void output_auto_throttle() +{ + g.rc_3.servo_out = (float)nav_throttle * angle_boost(); + // make sure we never send a 0 throttle that will cut the motors + g.rc_3.servo_out = max(g.rc_3.servo_out, 1); +} + +void calc_nav_throttle() +{ + // limit error + long error = constrain(altitude_error, -400, 400); + + if(altitude_sensor == BARO) { + float t = g.pid_baro_throttle.kP(); + + if(error > 0){ // go up + g.pid_baro_throttle.kP(t); + }else{ // go down + g.pid_baro_throttle.kP(t/4.0); + } + + // limit output of throttle control + 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); + + g.pid_baro_throttle.kP(t); + + } else { + // SONAR + nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0); + + // limit output of throttle control + nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 100); + } + + nav_throttle = (nav_throttle + nav_throttle_old) >> 1; + nav_throttle_old = nav_throttle; +} + +float angle_boost() +{ + float temp = cos_pitch_x * cos_roll_x; + temp = 2.0 - constrain(temp, .7, 1.0); + return temp; +} + + +/************************************************************* +yaw control +****************************************************************/ + +void output_manual_yaw() +{ + if(g.rc_3.control_in == 0){ + clear_yaw_control(); + } else { + // Yaw control + if(g.rc_4.control_in == 0){ + //clear_yaw_control(); + output_yaw_with_hold(true); // hold yaw + }else{ + output_yaw_with_hold(false); // rate control yaw + } + } +} + +void auto_yaw() +{ + output_yaw_with_hold(true); // hold yaw +} diff --git a/ArduCopterMega/EEPROM.pde b/ArduCopterMega/EEPROM.pde index 8bb344d00b..6ec07d3587 100644 --- a/ArduCopterMega/EEPROM.pde +++ b/ArduCopterMega/EEPROM.pde @@ -34,7 +34,7 @@ void save_EEPROM_alt_RTL(void) { g.RTL_altitude.save(); - + } void read_EEPROM_alt_RTL(void) @@ -81,16 +81,16 @@ 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 g.stabilize_dampener.load(); @@ -101,14 +101,15 @@ void read_EEPROM_PID(void) void save_EEPROM_PID(void) { - g.pid_acro_rate_roll.save_gains(); + + 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(); @@ -210,7 +211,7 @@ void save_EEPROM_pressure(void) void read_EEPROM_pressure(void) { g.ground_pressure.load(); - g.ground_temperature.load(); + g.ground_temperature.load(); // to prime the filter abs_pressure = g.ground_pressure; @@ -231,7 +232,7 @@ void read_EEPROM_radio(void) } void save_EEPROM_radio(void) -{ +{ g.rc_1.save_eeprom(); g.rc_2.save_eeprom(); g.rc_3.save_eeprom(); @@ -249,9 +250,9 @@ void read_EEPROM_throttle(void) g.throttle_min.load(); g.throttle_max.load(); g.throttle_cruise.load(); - g.throttle_failsafe_enabled.load(); - g.throttle_failsafe_action.load(); - g.throttle_failsafe_value.load(); + g.throttle_fs_enabled.load(); + g.throttle_fs_action.load(); + g.throttle_fs_value.load(); } void save_EEPROM_throttle(void) @@ -259,9 +260,9 @@ void save_EEPROM_throttle(void) g.throttle_min.load(); g.throttle_max.load(); g.throttle_cruise.save(); - g.throttle_failsafe_enabled.load(); - g.throttle_failsafe_action.load(); - g.throttle_failsafe_value.load(); + g.throttle_fs_enabled.load(); + g.throttle_fs_action.load(); + g.throttle_fs_value.load(); } /********************************************************************************/ @@ -286,7 +287,7 @@ void read_EEPROM_flight_modes(void) void save_EEPROM_flight_modes(void) { g.flight_modes.save(); - + } /********************************************************************************/ @@ -298,8 +299,8 @@ read_EE_float(int address) byte bytes[4]; float value; } _floatOut; - - for (int i = 0; i < 4; i++) + + for (int i = 0; i < 4; i++) _floatOut.bytes[i] = eeprom_read_byte((uint8_t *) (address + i)); return _floatOut.value; } @@ -310,9 +311,9 @@ void write_EE_float(float value, int address) byte bytes[4]; float value; } _floatIn; - + _floatIn.value = value; - for (int i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]); } */ @@ -322,7 +323,7 @@ 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); } diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h index e702136e5c..5553bdb555 100644 --- a/ArduCopterMega/Parameters.h +++ b/ArduCopterMega/Parameters.h @@ -58,6 +58,7 @@ public: k_param_ground_pressure, k_param_current, k_param_milliamp_hours, + k_param_compass_enabled, k_param_compass, // @@ -83,9 +84,9 @@ public: 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_fs_enabled, + k_param_throttle_fs_action, + k_param_throttle_fs_value, k_param_throttle_cruise, k_param_flight_mode_channel, k_param_flight_modes, @@ -96,7 +97,7 @@ public: k_param_waypoint_mode = 220, k_param_waypoint_total, k_param_waypoint_index, - k_param_command_must_index, + k_param_command_must_index, k_param_waypoint_radius, k_param_loiter_radius, @@ -146,9 +147,9 @@ public: // AP_Int16 throttle_min; AP_Int16 throttle_max; - AP_Int8 throttle_failsafe_enabled; - AP_Int8 throttle_failsafe_action; - AP_Int16 throttle_failsafe_value; + AP_Int8 throttle_fs_enabled; + AP_Int8 throttle_fs_action; + AP_Int16 throttle_fs_value; AP_Int16 throttle_cruise; // Flight modes @@ -214,13 +215,13 @@ public: crosstrack_gain (XTRACK_GAIN * 100, k_param_crosstrack_gain, PSTR("XTRK_GAIN")), crosstrack_entry_angle (XTRACK_ENTRY_ANGLE * 100, k_param_crosstrack_entry_angle, PSTR("XTRACK_ANGLE")), - + frame_type (FRAME_CONFIG, k_param_frame_type, PSTR("FRAME_CONFIG")), - + current_enabled (DISABLED, k_param_current, PSTR("CURRENT_ENABLE")), - milliamp_hours (2100, k_param_milliamp_hours, PSTR("MAH")), - compass_enabled (DISABLED, k_param_compass, PSTR("COMPASS_ENABLE")), - + milliamp_hours (CURR_AMP_HOURS, k_param_milliamp_hours, PSTR("MAH")), + compass_enabled (DISABLED, k_param_compass_enabled, PSTR("COMPASS_ENABLE")), + 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")), @@ -228,18 +229,18 @@ public: 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")), - + throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")), + throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")), + throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")), + throttle_fs_action (THROTTLE_FAILSAFE_ACTION, k_param_throttle_fs_action, PSTR("THR_FS_ACTION")), + throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_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_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")), @@ -258,7 +259,6 @@ public: rc_camera_pitch (k_param_rc_9, PSTR("RC9_")), rc_camera_roll (k_param_rc_10, PSTR("RC10_")), - // PID controller group key name initial P initial I initial D initial imax //-------------------------------------------------------------------------------------------------------------------------------------------------------------------- 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 * 100), diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 4186547086..9bbf650cc6 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -25,12 +25,12 @@ /// /// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that /// change in your local copy of APM_Config.h. -/// +/// #include "APM_Config.h" // <== THIS INCLUDE, DO NOT EDIT IT /// /// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that /// change in your local copy of APM_Config.h. -/// +/// // Just so that it's completely clear... #define ENABLED 1 @@ -67,7 +67,7 @@ ////////////////////////////////////////////////////////////////////////////// // FRAME_CONFIG -// +// #ifndef FRAME_CONFIG # define FRAME_CONFIG PLUS_FRAME #endif @@ -122,6 +122,9 @@ #ifndef CURR_AMP_DIV_RATIO # define CURR_AMP_DIV_RATIO 30.35 #endif +#ifndef CURR_AMP_HOURS +# define CURR_AMP_HOURS 2000 +#endif ////////////////////////////////////////////////////////////////////////////// @@ -257,7 +260,7 @@ # define ACRO_RATE_PITCH_D 0.0 #endif #ifndef ACRO_RATE_PITCH_IMAX -# define ACRO_RATE_PITCH_IMAX 20 +# define ACRO_RATE_PITCH_IMAX 20 #endif #define ACRO_RATE_PITCH_IMAX_CENTIDEGREE ACRO_RATE_PITCH_IMAX * 100 @@ -426,8 +429,8 @@ #ifndef XTRACK_ENTRY_ANGLE # define XTRACK_ENTRY_ANGLE 30 // deg #endif -# define XTRACK_GAIN_SCALED XTRACK_GAIN * 100 -# define XTRACK_ENTRY_ANGLE_CENTIDEGREE XTRACK_ENTRY_ANGLE * 100 +//# define XTRACK_GAIN_SCALED XTRACK_GAIN * 100 +//# define XTRACK_ENTRY_ANGLE_CENTIDEGREE XTRACK_ENTRY_ANGLE * 100 ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 5dd1e1f40a..f084c62052 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -97,9 +97,9 @@ // Auto Pilot modes // ---------------- -#define ACRO 0 // rate control -#define STABILIZE 1 // hold level position -#define ALT_HOLD 2 // AUTO control +#define STABILIZE 0 // hold level position +#define ACRO 1 // rate control +#define ALT_HOLD 2 // AUTO control #define FBW 3 // AUTO control #define AUTO 4 // AUTO control #define POSITION_HOLD 5 // Hold a single location diff --git a/ArduCopterMega/flight_control.pde b/ArduCopterMega/flight_control.pde index 09220cb1ff..e69de29bb2 100644 --- a/ArduCopterMega/flight_control.pde +++ b/ArduCopterMega/flight_control.pde @@ -1,154 +0,0 @@ - -/************************************************************* -throttle control -****************************************************************/ - -// user input: -// ----------- -void output_manual_throttle() -{ - g.rc_3.servo_out = (float)g.rc_3.control_in * angle_boost(); -} - -// Autopilot -// --------- -void output_auto_throttle() -{ - g.rc_3.servo_out = (float)nav_throttle * angle_boost(); - // make sure we never send a 0 throttle that will cut the motors - g.rc_3.servo_out = max(g.rc_3.servo_out, 1); -} - -void calc_nav_throttle() -{ - // limit error - long error = constrain(altitude_error, -400, 400); - - if(altitude_sensor == BARO) { - float t = g.pid_baro_throttle.kP(); - - if(error > 0){ // go up - g.pid_baro_throttle.kP(t); - }else{ // go down - g.pid_baro_throttle.kP(t/4.0); - } - - // limit output of throttle control - 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); - - g.pid_baro_throttle.kP(t); - - } else { - // SONAR - nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0); - - // limit output of throttle control - nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 100); - } - - nav_throttle = (nav_throttle + nav_throttle_old) >> 1; - nav_throttle_old = nav_throttle; -} - -float angle_boost() -{ - float temp = cos_pitch_x * cos_roll_x; - temp = 2.0 - constrain(temp, .7, 1.0); - return temp; -} - - -/************************************************************* -yaw control -****************************************************************/ - -void output_manual_yaw() -{ - if(g.rc_3.control_in == 0){ - clear_yaw_control(); - } else { - // Yaw control - if(g.rc_4.control_in == 0){ - //clear_yaw_control(); - output_yaw_with_hold(true); // hold yaw - }else{ - output_yaw_with_hold(false); // rate control yaw - } - } -} - -void auto_yaw() -{ - output_yaw_with_hold(true); // hold yaw -} - -/************************************************************* -picth and roll control -****************************************************************/ - - -/*// how hard to tilt towards the target -// ----------------------------------- -void calc_nav_pid() -{ - // how hard to pitch to target - - nav_angle = g.pid_nav.get_pid(wp_distance * 100, dTnav, 1.0); - nav_angle = constrain(nav_angle, -pitch_max, pitch_max); -} - -// distribute the pitch angle based on our orientation -// --------------------------------------------------- -void calc_nav_pitch() -{ - // how hard to pitch to target - - long angle = wrap_360(nav_bearing - dcm.yaw_sensor); - - bool rev = false; - float roll_out; - - if(angle > 18000){ - angle -= 18000; - rev = true; - } - - roll_out = abs(angle - 18000); - roll_out = (9000.0 - roll_out) / 9000.0; - roll_out = (rev) ? roll_out : -roll_out; - - nav_pitch = (float)nav_angle * roll_out; -} - -// distribute the roll angle based on our orientation -// -------------------------------------------------- -void calc_nav_roll() -{ - long angle = wrap_360(nav_bearing - dcm.yaw_sensor); - - bool rev = false; - float roll_out; - - if(angle > 18000){ - angle -= 18000; - rev = true; - } - - roll_out = abs(angle - 9000); - roll_out = (9000.0 - roll_out) / 9000.0; - roll_out = (rev) ? -roll_out : roll_out; - - nav_roll = (float)nav_angle * roll_out; -} -*/ - - - - - - - - - - diff --git a/ArduCopterMega/sensors.pde b/ArduCopterMega/sensors.pde index 1043bbd2eb..5e5c3fc981 100644 --- a/ArduCopterMega/sensors.pde +++ b/ArduCopterMega/sensors.pde @@ -5,20 +5,20 @@ void init_pressure_ground(void) { for(int i = 0; i < 300; i++){ // We take some readings... delay(20); - APM_BMP085.Read(); // Get initial data from absolute pressure sensor - ground_pressure = (ground_pressure * 9l + APM_BMP085.Press) / 10l; - ground_temperature = (ground_temperature * 9 + APM_BMP085.Temp) / 10; + barometer.Read(); // Get initial data from absolute pressure sensor + ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l; + ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10; } - abs_pressure = APM_BMP085.Press; + abs_pressure = barometer.Press; } void read_barometer(void){ float x, scaling, temp; - APM_BMP085.Read(); // Get new data from absolute pressure sensor + barometer.Read(); // Get new data from absolute pressure sensor - //abs_pressure = (abs_pressure + APM_BMP085.Press) >> 1; // Small filtering - abs_pressure = ((float)abs_pressure * .7) + ((float)APM_BMP085.Press * .3); // large filtering + //abs_pressure = (abs_pressure + barometer.Press) >> 1; // Small filtering + abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.Press * .3); // large filtering scaling = (float)ground_pressure / (float)abs_pressure; temp = ((float)ground_temperature / 10.f) + 273.15; x = log(scaling) * temp * 29271.267f; diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index e249905df8..7627345a0f 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -96,7 +96,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue")); for (;;) { } - + // note, cannot actually return here return(0); @@ -491,7 +491,7 @@ setup_current(uint8_t argc, const Menu::arg *argv) save_EEPROM_mag(); } else if(argv[1].i > 10){ - milliamp_hours = argv[1].i; + g.milliamp_hours = argv[1].i; } else { Serial.printf_P(PSTR("\nOptions:[on, off, mAh]\n")); @@ -515,8 +515,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv) compass.init(); // Initialization compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft - compass.set_offsets(0, 0, 0); // set offsets to account for surrounding interference - compass.set_declination(ToRad(DECLINATION)); // set local difference between magnetic north and true north + //compass.set_offsets(0, 0, 0); // set offsets to account for surrounding interference //int counter = 0; float _min[3], _max[3], _offset[3]; @@ -621,7 +620,7 @@ default_frame() void default_current() { - milliamp_hours = 2000; + g.milliamp_hours = 2000; g.current_enabled.set(false); save_EEPROM_current(); } @@ -644,9 +643,9 @@ default_throttle() g.throttle_min = THROTTLE_MIN; g.throttle_max = THROTTLE_MAX; g.throttle_cruise = THROTTLE_CRUISE; - g.throttle_failsafe_enabled = THROTTLE_FAILSAFE; - g.throttle_failsafe_action = THROTTLE_FAILSAFE_ACTION; - g.throttle_failsafe_value = THROTTLE_FS_VALUE; + g.throttle_fs_enabled = THROTTLE_FAILSAFE; + g.throttle_fs_action = THROTTLE_FAILSAFE_ACTION; + g.throttle_fs_value = THROTTLE_FS_VALUE; save_EEPROM_throttle(); } @@ -742,7 +741,7 @@ default_gains() save_EEPROM_PID(); Serial.printf("EL8R %4.2f\n ",g.pid_stabilize_roll.kP()); - + } @@ -759,7 +758,7 @@ void report_current() print_divider(); print_enabled(g.current_enabled.get()); - Serial.printf_P(PSTR("mah: %d"),milliamp_hours); + Serial.printf_P(PSTR("mah: %d"),g.milliamp_hours); print_blanks(1); } @@ -865,8 +864,8 @@ void report_throttle() (int)g.throttle_min, (int)g.throttle_max, (int)g.throttle_cruise, - (int)g.throttle_failsafe_enabled, - (int)g.throttle_failsafe_value); + (int)g.throttle_fs_enabled, + (int)g.throttle_fs_value); print_blanks(1); } @@ -896,7 +895,7 @@ void report_compass() // mag declination Serial.printf_P(PSTR("Mag Delination: %4.4f\n"), degrees(compass.get_declination())); - + Vector3f offsets = compass.get_offsets(); // mag offsets diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 2fb17b8299..8845afac5a 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -116,7 +116,7 @@ void init_ardupilot() init_rc_out(); // sets up the timer libs init_camera(); adc.Init(); // APM ADC library initialization - APM_BMP085.Init(); // APM Abs Pressure sensor initialization + barometer.Init(); // APM Abs Pressure sensor initialization DataFlash.Init(); // DataFlash log initialization // Do GPS init @@ -130,8 +130,8 @@ void init_ardupilot() #else gcs.init(&Serial); #endif - - + + if(g.compass_enabled) @@ -386,7 +386,7 @@ init_compass() dcm.set_compass(&compass); bool junkbool = compass.init(); compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft - Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference + Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference } diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index e3ad9ed93d..bc9001c174 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -184,7 +184,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv) fail_test++; } - if(g.throttle_failsafe_enabled && g.rc_3.get_failsafe()){ + if(g.throttle_fs_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++; @@ -237,6 +237,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv) medium_loopCounter = 0; } } + // for trim features read_trim_switch();