mirror of https://github.com/ArduPilot/ardupilot
Merge branch 'master' of https://code.google.com/p/ardupilot-mega
This commit is contained in:
commit
e785361547
|
@ -418,6 +418,7 @@ static byte led_mode = NORMAL_LEDS;
|
|||
static const float t7 = 10000000.0;
|
||||
// We use atan2 and other trig techniques to calaculate angles
|
||||
// We need to scale the longitude up to make these calcs work
|
||||
// to account for decreasing distance between lines of longitude away from the equator
|
||||
static float scaleLongUp = 1;
|
||||
// Sometimes we need to remove the scaling for distance calcs
|
||||
static float scaleLongDown = 1;
|
||||
|
@ -529,20 +530,12 @@ static int8_t CH7_wp_index;
|
|||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Battery Sensors
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Battery Voltage of total battery, initialized above threshold for filter
|
||||
static float battery_voltage = LOW_VOLTAGE * 1.05;
|
||||
// Battery Voltage of cell 1, initialized above threshold for filter
|
||||
static float battery_voltage1 = LOW_VOLTAGE * 1.05;
|
||||
// Battery Voltage of cells 1 + 2, initialized above threshold for filter
|
||||
static float battery_voltage2 = LOW_VOLTAGE * 1.05;
|
||||
// Battery Voltage of cells 1 + 2+3, initialized above threshold for filter
|
||||
static float battery_voltage3 = LOW_VOLTAGE * 1.05;
|
||||
// Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
|
||||
static float battery_voltage4 = LOW_VOLTAGE * 1.05;
|
||||
// Battery Voltage of battery, initialized above threshold for filter
|
||||
static float battery_voltage1 = LOW_VOLTAGE * 1.05;
|
||||
// refers to the instant amp draw – based on an Attopilot Current sensor
|
||||
static float current_amps;
|
||||
static float current_amps1;
|
||||
// refers to the total amps drawn – based on an Attopilot Current sensor
|
||||
static float current_total;
|
||||
static float current_total1;
|
||||
// Used to track if the battery is low - LED output flashes when the batt is low
|
||||
static bool low_batt = false;
|
||||
|
||||
|
|
|
@ -78,7 +78,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
|||
status = MAV_STATE_STANDBY;
|
||||
}
|
||||
|
||||
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
|
||||
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total1)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
|
||||
|
||||
mavlink_msg_sys_status_send(
|
||||
chan,
|
||||
|
@ -86,7 +86,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
|||
nav_mode,
|
||||
status,
|
||||
0,
|
||||
battery_voltage * 1000,
|
||||
battery_voltage1 * 1000,
|
||||
battery_remaining,
|
||||
packet_drops);
|
||||
}
|
||||
|
|
|
@ -321,9 +321,9 @@ static void Log_Write_Current()
|
|||
|
||||
DataFlash.WriteInt(g.rc_3.control_in); // 1
|
||||
DataFlash.WriteLong(throttle_integrator); // 2
|
||||
DataFlash.WriteInt(battery_voltage * 100.0); // 3
|
||||
DataFlash.WriteInt(current_amps * 100.0); // 4
|
||||
DataFlash.WriteInt(current_total); // 5
|
||||
DataFlash.WriteInt(battery_voltage1 * 100.0); // 3
|
||||
DataFlash.WriteInt(current_amps1 * 100.0); // 4
|
||||
DataFlash.WriteInt(current_total1); // 5
|
||||
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
|
|
@ -198,7 +198,7 @@ public:
|
|||
AP_Int16 RTL_altitude;
|
||||
AP_Int8 sonar_enabled;
|
||||
AP_Int8 sonar_type; // 0 = XL, 1 = LV, 2 = XLL (XL with 10m range)
|
||||
AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current
|
||||
AP_Int8 battery_monitoring; // 0=disabled, 3=voltage only, 4=voltage and current
|
||||
AP_Int16 pack_capacity; // Battery pack capacity less reserve
|
||||
AP_Int8 compass_enabled;
|
||||
AP_Int8 optflow_enabled;
|
||||
|
|
|
@ -118,6 +118,8 @@
|
|||
# define USB_MUX_PIN -1
|
||||
# define CLI_SLIDER_ENABLED DISABLED
|
||||
# define OPTFLOW_CS_PIN 34
|
||||
# define BATTERY_PIN_1 0
|
||||
# define CURRENT_PIN_1 1
|
||||
#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
|
||||
# define A_LED_PIN 27
|
||||
# define B_LED_PIN 26
|
||||
|
@ -129,6 +131,8 @@
|
|||
# define CLI_SLIDER_ENABLED DISABLED
|
||||
# define USB_MUX_PIN 23
|
||||
# define OPTFLOW_CS_PIN A6
|
||||
# define BATTERY_PIN_1 1
|
||||
# define CURRENT_PIN_1 2
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
|
|
@ -61,10 +61,10 @@
|
|||
//
|
||||
//
|
||||
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
|
||||
/* Camera Pitch and Camera Roll: Not yet defined for APM2
|
||||
/* Camera Pitch and Camera Roll: Not yet defined for APM2
|
||||
* They will likely be dependent on the frame config */
|
||||
# define CH_CAM_PITCH (-1)
|
||||
# define CH_CAM_ROLL (-1)
|
||||
# define CH_CAM_PITCH CH_11
|
||||
# define CH_CAM_ROLL CH_10
|
||||
#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
|
||||
# define CH_CAM_PITCH CH_5
|
||||
# define CH_CAM_ROLL CH_6
|
||||
|
|
|
@ -325,15 +325,8 @@ enum gcs_severity {
|
|||
#define AN14 68 // NC
|
||||
#define AN15 69 // NC
|
||||
|
||||
#define VOLTAGE_PIN_0 0 // These are the pins for current sensor: voltage
|
||||
#define CURRENT_PIN_1 1 // and current
|
||||
|
||||
#define RELAY_PIN 47
|
||||
|
||||
#define BATTERY_PIN1 0 // These are the pins for the voltage dividers
|
||||
#define BATTERY_PIN2 1
|
||||
#define BATTERY_PIN3 2
|
||||
#define BATTERY_PIN4 3
|
||||
#define PIEZO_PIN AN5 //Last pin on the back ADC connector
|
||||
|
||||
|
||||
|
|
|
@ -100,30 +100,25 @@ static void init_optflow()
|
|||
|
||||
static void read_battery(void)
|
||||
{
|
||||
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9;
|
||||
battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9;
|
||||
battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9;
|
||||
battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9;
|
||||
|
||||
if(g.battery_monitoring == 1)
|
||||
battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream
|
||||
|
||||
if(g.battery_monitoring == 2)
|
||||
battery_voltage = battery_voltage4;
|
||||
|
||||
if(g.battery_monitoring == 3 || g.battery_monitoring == 4)
|
||||
battery_voltage = battery_voltage1;
|
||||
if(g.battery_monitoring == 0){
|
||||
battery_voltage1 = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
if(g.battery_monitoring == 3 || g.battery_monitoring == 4) {
|
||||
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN_1)) * .1 + battery_voltage1 * .9;
|
||||
}
|
||||
if(g.battery_monitoring == 4) {
|
||||
current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin
|
||||
current_total += current_amps * 0.0278; // called at 100ms on average
|
||||
current_amps1 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps1 * .9; //reads power sensor current pin
|
||||
current_total1 += current_amps1 * 0.02778; // called at 100ms on average, .0002778 is 1/3600 (conversion to hours)
|
||||
}
|
||||
|
||||
#if BATTERY_EVENT == 1
|
||||
//if(battery_voltage < g.low_voltage)
|
||||
// low_battery_event();
|
||||
|
||||
if((battery_voltage < g.low_voltage) || (g.battery_monitoring == 4 && current_total > g.pack_capacity)){
|
||||
if((battery_voltage1 < g.low_voltage) || (g.battery_monitoring == 4 && current_total1 > g.pack_capacity)){
|
||||
low_battery_event();
|
||||
|
||||
#if PIEZO_LOW_VOLTAGE == 1
|
||||
|
|
|
@ -395,7 +395,7 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
|
|||
g.battery_monitoring.set_and_save(argv[1].i);
|
||||
|
||||
} else {
|
||||
Serial.printf_P(PSTR("\nOp: off, 1-4"));
|
||||
Serial.printf_P(PSTR("\nOp: off, 3-4"));
|
||||
}
|
||||
|
||||
report_batt_monitor();
|
||||
|
@ -778,8 +778,6 @@ static void report_batt_monitor()
|
|||
Serial.printf_P(PSTR("\nBatt Mon:\n"));
|
||||
print_divider();
|
||||
if(g.battery_monitoring == 0) print_enabled(false);
|
||||
if(g.battery_monitoring == 1) Serial.printf_P(PSTR("3c"));
|
||||
if(g.battery_monitoring == 2) Serial.printf_P(PSTR("4c"));
|
||||
if(g.battery_monitoring == 3) Serial.printf_P(PSTR("volts"));
|
||||
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("volts and cur"));
|
||||
print_blanks(2);
|
||||
|
|
|
@ -21,7 +21,6 @@ static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
|
|||
//static int8_t test_wp_nav(uint8_t argc, const Menu::arg *argv);
|
||||
//static int8_t test_reverse(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_tuning(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_current(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
|
@ -69,7 +68,6 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
|
|||
{"battery", test_battery},
|
||||
{"tune", test_tuning},
|
||||
//{"tri", test_tri},
|
||||
{"current", test_current},
|
||||
{"relay", test_relay},
|
||||
{"wp", test_wp},
|
||||
//{"nav", test_nav},
|
||||
|
@ -713,26 +711,6 @@ test_gps(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
//*/
|
||||
|
||||
static int8_t
|
||||
test_battery(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
#if BATTERY_EVENT == 1
|
||||
for (int i = 0; i < 20; i++){
|
||||
delay(20);
|
||||
read_battery();
|
||||
}
|
||||
Serial.printf_P(PSTR("Volts: 1:%2.2f, 2:%2.2f, 3:%2.2f, 4:%2.2f\n"),
|
||||
battery_voltage1,
|
||||
battery_voltage2,
|
||||
battery_voltage3,
|
||||
battery_voltage4);
|
||||
#else
|
||||
Serial.printf_P(PSTR("Not enabled\n"));
|
||||
|
||||
#endif
|
||||
return (0);
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_tuning(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
|
@ -751,7 +729,7 @@ test_tuning(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
|
||||
static int8_t
|
||||
test_current(uint8_t argc, const Menu::arg *argv)
|
||||
test_battery(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
//delta_ms_medium_loop = 100;
|
||||
|
@ -760,11 +738,17 @@ test_current(uint8_t argc, const Menu::arg *argv)
|
|||
delay(100);
|
||||
read_radio();
|
||||
read_battery();
|
||||
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
|
||||
battery_voltage,
|
||||
current_amps,
|
||||
current_total);
|
||||
|
||||
if (g.battery_monitoring == 3){
|
||||
Serial.printf_P(PSTR("V: %4.4f\n"),
|
||||
battery_voltage1,
|
||||
current_amps1,
|
||||
current_total1);
|
||||
} else {
|
||||
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"),
|
||||
battery_voltage1,
|
||||
current_amps1,
|
||||
current_total1);
|
||||
}
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_in);
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#define THISFIRMWARE "ArduPlane V2.27"
|
||||
#define THISFIRMWARE "ArduPlane V2.28"
|
||||
/*
|
||||
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler
|
||||
Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier
|
||||
|
@ -240,6 +240,15 @@ AP_AnalogSource_Arduino pitot_analog_source(CONFIG_PITOT_SOURCE_ANALOG_PIN, 4.0)
|
|||
AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter);
|
||||
#endif
|
||||
|
||||
AP_Relay relay;
|
||||
|
||||
// Camera/Antenna mount tracking and stabilisation stuff
|
||||
// --------------------------------------
|
||||
#if MOUNT == ENABLED
|
||||
AP_Mount camera_mount(g_gps, &dcm);
|
||||
#endif
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Global variables
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
@ -354,162 +363,263 @@ static int ground_start_avg;
|
|||
// If we do not detect GPS at startup, we stop trying and assume GPS is not connected
|
||||
static bool GPS_enabled = false;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Location & Navigation
|
||||
// ---------------------
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Constants
|
||||
const float radius_of_earth = 6378100; // meters
|
||||
const float gravity = 9.81; // meters/ sec^2
|
||||
static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
|
||||
static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
|
||||
static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
|
||||
static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
|
||||
// This is the currently calculated direction to fly.
|
||||
// deg * 100 : 0 to 360
|
||||
static long nav_bearing;
|
||||
// This is the direction to the next waypoint or loiter center
|
||||
// deg * 100 : 0 to 360
|
||||
static long target_bearing;
|
||||
//This is the direction from the last waypoint to the next waypoint
|
||||
// deg * 100 : 0 to 360
|
||||
static long crosstrack_bearing;
|
||||
// A gain scaler to account for ground speed/headwind/tailwind
|
||||
static float nav_gain_scaler = 1;
|
||||
// Direction held during phases of takeoff and landing
|
||||
// deg * 100 dir of plane, A value of -1 indicates the course has not been set/is not in use
|
||||
static long hold_course = -1; // deg * 100 dir of plane
|
||||
|
||||
static byte nav_command_index; // active nav command memory location
|
||||
static byte non_nav_command_index; // active non-nav command memory location
|
||||
static byte nav_command_ID = NO_COMMAND; // active nav command ID
|
||||
static byte non_nav_command_ID = NO_COMMAND; // active non-nav command ID
|
||||
// There may be two active commands in Auto mode.
|
||||
// This indicates the active navigation command by index number
|
||||
static byte nav_command_index;
|
||||
// This indicates the active non-navigation command by index number
|
||||
static byte non_nav_command_index;
|
||||
// This is the command type (eg navigate to waypoint) of the active navigation command
|
||||
static byte nav_command_ID = NO_COMMAND;
|
||||
static byte non_nav_command_ID = NO_COMMAND;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Airspeed
|
||||
// --------
|
||||
static int airspeed; // m/s * 100
|
||||
static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range
|
||||
static long target_airspeed; // m/s * 100 (used for Auto-flap deployment in FBW_B mode)
|
||||
static float airspeed_error; // m/s * 100
|
||||
static long energy_error; // energy state error (kinetic + potential) for altitude hold
|
||||
static long airspeed_energy_error; // kinetic portion of energy error (m^2/s^2)
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// The current airspeed estimate/measurement in centimeters per second
|
||||
static int airspeed;
|
||||
// The calculated airspeed to use in FBW-B. Also used in higher modes for insuring min ground speed is met.
|
||||
// Also used for flap deployment criteria. Centimeters per second.static long target_airspeed;
|
||||
static long target_airspeed;
|
||||
// The difference between current and desired airspeed. Used in the pitch controller. Centimeters per second.
|
||||
static float airspeed_error;
|
||||
// The calculated total energy error (kinetic (altitude) plus potential (airspeed)).
|
||||
// Used by the throttle controller
|
||||
static long energy_error;
|
||||
// kinetic portion of energy error (m^2/s^2)
|
||||
static long airspeed_energy_error;
|
||||
// An amount that the airspeed should be increased in auto modes based on the user positioning the
|
||||
// throttle stick in the top half of the range. Centimeters per second.
|
||||
static int airspeed_nudge;
|
||||
// Similar to airspeed_nudge, but used when no airspeed sensor.
|
||||
// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
|
||||
static int throttle_nudge = 0;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Ground speed
|
||||
static long groundspeed_undershoot = 0; // m/s * 100 (>=0, where > 0 => amount below min ground speed)
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// The amount current ground speed is below min ground speed. Centimeters per second
|
||||
static long groundspeed_undershoot = 0;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Location Errors
|
||||
// ---------------
|
||||
static long bearing_error; // deg * 100 : 0 to 36000
|
||||
static long altitude_error; // meters * 100 we are off in altitude
|
||||
static float crosstrack_error; // meters we are off trackline
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Difference between current bearing and desired bearing. Hundredths of a degree
|
||||
static long bearing_error;
|
||||
// Difference between current altitude and desired altitude. Centimeters
|
||||
static long altitude_error;
|
||||
// Distance perpandicular to the course line that we are off trackline. Meters
|
||||
static float crosstrack_error;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Battery Sensors
|
||||
// ---------------
|
||||
static float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter
|
||||
static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter
|
||||
static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter
|
||||
static float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter
|
||||
static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Battery pack 1 voltage. Initialized above the low voltage threshold to pre-load the filter and prevent low voltage events at startup.
|
||||
static float battery_voltage1 = LOW_VOLTAGE * 1.05;
|
||||
// Battery pack 1 instantaneous currrent draw. Amperes
|
||||
static float current_amps1;
|
||||
// Totalized current (Amp-hours) from battery 1
|
||||
static float current_total1;
|
||||
|
||||
static float current_amps;
|
||||
static float current_total;
|
||||
// To Do - Add support for second battery pack
|
||||
//static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery 2 Voltage, initialized above threshold for filter
|
||||
//static float current_amps2; // Current (Amperes) draw from battery 2
|
||||
//static float current_total2; // Totalized current (Amp-hours) from battery 2
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Airspeed Sensors
|
||||
// ----------------
|
||||
static float airspeed_raw; // Airspeed Sensor - is a float to better handle filtering
|
||||
static float airspeed_pressure; // airspeed as a pressure value
|
||||
|
||||
// Barometer Sensor variables
|
||||
// --------------------------
|
||||
static unsigned long abs_pressure;
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Raw differential pressure measurement (filtered). ADC units
|
||||
static float airspeed_raw;
|
||||
// Raw differential pressure less the zero pressure offset. ADC units
|
||||
static float airspeed_pressure;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Altitude Sensor variables
|
||||
// ----------------------
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Raw absolute pressure measurement (filtered). ADC units
|
||||
static unsigned long abs_pressure;
|
||||
// Altitude from the sonar sensor. Meters. Not yet implemented.
|
||||
static int sonar_alt;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// flight mode specific
|
||||
// --------------------
|
||||
static bool takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes.
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Flag for using gps ground course instead of IMU yaw. Set false when takeoff command in process.
|
||||
static bool takeoff_complete = true;
|
||||
// Flag to indicate if we have landed.
|
||||
//Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
|
||||
static bool land_complete;
|
||||
// Altitude threshold to complete a takeoff command in autonomous modes. Centimeters
|
||||
static long takeoff_altitude;
|
||||
// static int landing_distance; // meters;
|
||||
static int landing_pitch; // pitch for landing set by commands
|
||||
// Pitch to hold during landing command in the no airspeed sensor case. Hundredths of a degree
|
||||
static int landing_pitch;
|
||||
// Minimum pitch to hold during takeoff command execution. Hundredths of a degree
|
||||
static int takeoff_pitch;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Loiter management
|
||||
// -----------------
|
||||
static long old_target_bearing; // deg * 100
|
||||
static int loiter_total; // deg : how many times to loiter * 360
|
||||
static int loiter_delta; // deg : how far we just turned
|
||||
static int loiter_sum; // deg : how far we have turned around a waypoint
|
||||
static long loiter_time; // millis : when we started LOITER mode
|
||||
static int loiter_time_max; // millis : how long to stay in LOITER mode
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Previous target bearing. Used to calculate loiter rotations. Hundredths of a degree
|
||||
static long old_target_bearing;
|
||||
// Total desired rotation in a loiter. Used for Loiter Turns commands. Degrees
|
||||
static int loiter_total;
|
||||
// The amount in degrees we have turned since recording old_target_bearing
|
||||
static int loiter_delta;
|
||||
// Total rotation in a loiter. Used for Loiter Turns commands and to check for missed waypoints. Degrees
|
||||
static int loiter_sum;
|
||||
// The amount of time we have been in a Loiter. Used for the Loiter Time command. Milliseconds.
|
||||
static long loiter_time;
|
||||
// The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds.
|
||||
static int loiter_time_max;
|
||||
|
||||
// these are the values for navigation control functions
|
||||
// ----------------------------------------------------
|
||||
static long nav_roll; // deg * 100 : target roll angle
|
||||
static long nav_pitch; // deg * 100 : target pitch angle
|
||||
static int throttle_nudge = 0; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Navigation control variables
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// The instantaneous desired bank angle. Hundredths of a degree
|
||||
static long nav_roll;
|
||||
// The instantaneous desired pitch angle. Hundredths of a degree
|
||||
static long nav_pitch;
|
||||
|
||||
// Waypoints
|
||||
// ---------
|
||||
static long wp_distance; // meters - distance between plane and next waypoint
|
||||
static long wp_totalDistance; // meters - distance between old and next waypoint
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Waypoint distances
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Distance between plane and next waypoint. Meters
|
||||
static long wp_distance;
|
||||
// Distance between previous and next waypoint. Meters
|
||||
static long wp_totalDistance;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// repeating event control
|
||||
// -----------------------
|
||||
static byte event_id; // what to do - see defines
|
||||
static long event_timer; // when the event was asked for in ms
|
||||
static uint16_t event_delay; // how long to delay the next firing of event in millis
|
||||
static int event_repeat = 0; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
|
||||
static int event_value; // per command value, such as PWM for servos
|
||||
static int event_undo_value; // the value used to cycle events (alternate value to event_value)
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Flag indicating current event type
|
||||
static byte event_id;
|
||||
// when the event was started in ms
|
||||
static long event_timer;
|
||||
// how long to delay the next firing of event in millis
|
||||
static uint16_t event_delay;
|
||||
// how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
|
||||
static int event_repeat = 0;
|
||||
// per command value, such as PWM for servos
|
||||
static int event_value;
|
||||
// the value used to cycle events (alternate value to event_value)
|
||||
static int event_undo_value;
|
||||
|
||||
// delay command
|
||||
// --------------
|
||||
static long condition_value; // used in condition commands (eg delay, change alt, etc.)
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Conditional command
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// A value used in condition commands (eg delay, change alt, etc.)
|
||||
// For example in a change altitude command, it is the altitude to change to.
|
||||
static long condition_value;
|
||||
// A starting value used to check the status of a conditional command.
|
||||
// For example in a delay command the condition_start records that start time for the delay
|
||||
static long condition_start;
|
||||
static int condition_rate;
|
||||
// A value used in condition commands. For example the rate at which to change altitude.
|
||||
static int condition_rate;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// 3D Location vectors
|
||||
// -------------------
|
||||
static struct Location home; // home location
|
||||
static struct Location prev_WP; // last waypoint
|
||||
static struct Location current_loc; // current location
|
||||
static struct Location next_WP; // next waypoint
|
||||
static struct Location guided_WP; // guided mode waypoint
|
||||
static struct Location next_nav_command; // command preloaded
|
||||
static struct Location next_nonnav_command; // command preloaded
|
||||
static long target_altitude; // used for altitude management between waypoints
|
||||
static long offset_altitude; // used for altitude management between waypoints
|
||||
static bool home_is_set; // Flag for if we have g_gps lock and have set the home location
|
||||
// Location structure defined in AP_Common
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// The home location used for RTL. The location is set when we first get stable GPS lock
|
||||
static struct Location home;
|
||||
// Flag for if we have g_gps lock and have set the home location
|
||||
static bool home_is_set;
|
||||
// The location of the previous waypoint. Used for track following and altitude ramp calculations
|
||||
static struct Location prev_WP;
|
||||
// The plane's current location
|
||||
static struct Location current_loc;
|
||||
// The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations.
|
||||
static struct Location next_WP;
|
||||
// The location of the active waypoint in Guided mode.
|
||||
static struct Location guided_WP;
|
||||
// The location structure information from the Nav command being processed
|
||||
static struct Location next_nav_command;
|
||||
// The location structure information from the Non-Nav command being processed
|
||||
static struct Location next_nonnav_command;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Altitude / Climb rate control
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// The current desired altitude. Altitude is linearly ramped between waypoints. Centimeters
|
||||
static long target_altitude;
|
||||
// Altitude difference between previous and current waypoint. Centimeters
|
||||
static long offset_altitude;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// IMU variables
|
||||
// -------------
|
||||
static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// The main loop execution time. Seconds
|
||||
//This is the time between calls to the DCM algorithm and is the Integration time for the gyros.
|
||||
static float G_Dt = 0.02;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Performance monitoring
|
||||
// ----------------------
|
||||
static long perf_mon_timer; // Metric based on accel gain deweighting
|
||||
static int G_Dt_max = 0; // Max main loop cycle time in milliseconds
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Timer used to accrue data and trigger recording of the performanc monitoring log message
|
||||
static long perf_mon_timer;
|
||||
// The maximum main loop execution time recorded in the current performance monitoring interval
|
||||
static int G_Dt_max = 0;
|
||||
// The number of gps fixes recorded in the current performance monitoring interval
|
||||
static int gps_fix_count = 0;
|
||||
// A variable used by developers to track performanc metrics.
|
||||
// Currently used to record the number of GCS heartbeat messages received
|
||||
static int pmTest1 = 0;
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// System Timers
|
||||
// --------------
|
||||
static unsigned long fast_loopTimer; // Time in miliseconds of main control loop
|
||||
static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete
|
||||
static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Time in miliseconds of start of main control loop. Milliseconds
|
||||
static unsigned long fast_loopTimer;
|
||||
// Time Stamp when fast loop was complete. Milliseconds
|
||||
static unsigned long fast_loopTimeStamp;
|
||||
// Number of milliseconds used in last main loop cycle
|
||||
static uint8_t delta_ms_fast_loop;
|
||||
// Counter of main loop executions. Used for performance monitoring and failsafe processing
|
||||
static uint16_t mainLoop_count;
|
||||
|
||||
static unsigned long medium_loopTimer; // Time in miliseconds of medium loop
|
||||
static byte medium_loopCounter; // Counters for branching from main control loop to slower loops
|
||||
// Time in miliseconds of start of medium control loop. Milliseconds
|
||||
static unsigned long medium_loopTimer;
|
||||
// Counters for branching from main control loop to slower loops
|
||||
static byte medium_loopCounter;
|
||||
// Number of milliseconds used in last medium loop cycle
|
||||
static uint8_t delta_ms_medium_loop;
|
||||
|
||||
// Counters for branching from medium control loop to slower loops
|
||||
static byte slow_loopCounter;
|
||||
// Counter to trigger execution of very low rate processes
|
||||
static byte superslow_loopCounter;
|
||||
static byte counter_one_herz;
|
||||
// Counter to trigger execution of 1 Hz processes
|
||||
static byte counter_one_herz;
|
||||
|
||||
static unsigned long nav_loopTimer; // used to track the elapsed time for GPS nav
|
||||
// used to track the elapsed time for navigation PID integral terms
|
||||
static unsigned long nav_loopTimer;
|
||||
// Elapsed time since last call to navigation pid functions
|
||||
static unsigned long dTnav;
|
||||
// % MCU cycles used
|
||||
static float load;
|
||||
|
||||
static unsigned long dTnav; // Delta Time in milliseconds for navigation computations
|
||||
static float load; // % MCU cycles used
|
||||
|
||||
AP_Relay relay;
|
||||
|
||||
// Camera/Antenna mount tracking and stabilisation stuff
|
||||
// --------------------------------------
|
||||
#if MOUNT == ENABLED
|
||||
AP_Mount camera_mount(g_gps, &dcm);
|
||||
#endif
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
|
|
@ -206,11 +206,11 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
|||
uint16_t battery_current = -1;
|
||||
uint8_t battery_remaining = -1;
|
||||
|
||||
if (current_total != 0 && g.pack_capacity != 0) {
|
||||
battery_remaining = (100.0 * (g.pack_capacity - current_total) / g.pack_capacity);
|
||||
if (current_total1 != 0 && g.pack_capacity != 0) {
|
||||
battery_remaining = (100.0 * (g.pack_capacity - current_total1) / g.pack_capacity);
|
||||
}
|
||||
if (current_total != 0) {
|
||||
battery_current = current_amps * 100;
|
||||
if (current_total1 != 0) {
|
||||
battery_current = current_amps1 * 100;
|
||||
}
|
||||
|
||||
mavlink_msg_sys_status_send(
|
||||
|
@ -219,7 +219,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
|||
control_sensors_enabled,
|
||||
control_sensors_health,
|
||||
(uint16_t)(load * 1000),
|
||||
battery_voltage * 1000, // mV
|
||||
battery_voltage1 * 1000, // mV
|
||||
battery_current, // in 10mA units
|
||||
battery_remaining, // in %
|
||||
0, // comm drops %,
|
||||
|
@ -270,7 +270,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
|||
}
|
||||
|
||||
uint8_t status = MAV_STATE_ACTIVE;
|
||||
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
|
||||
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total1)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
|
||||
|
||||
mavlink_msg_sys_status_send(
|
||||
chan,
|
||||
|
@ -278,7 +278,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
|||
nav_mode,
|
||||
status,
|
||||
load * 1000,
|
||||
battery_voltage * 1000,
|
||||
battery_voltage1 * 1000,
|
||||
battery_remaining,
|
||||
packet_drops);
|
||||
#endif // MAVLINK10
|
||||
|
|
|
@ -386,9 +386,9 @@ static void Log_Write_Current()
|
|||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_CURRENT_MSG);
|
||||
DataFlash.WriteInt(g.channel_throttle.control_in);
|
||||
DataFlash.WriteInt((int)(battery_voltage * 100.0));
|
||||
DataFlash.WriteInt((int)(current_amps * 100.0));
|
||||
DataFlash.WriteInt((int)current_total);
|
||||
DataFlash.WriteInt((int)(battery_voltage1 * 100.0));
|
||||
DataFlash.WriteInt((int)(current_amps1 * 100.0));
|
||||
DataFlash.WriteInt((int)current_total1);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
||||
|
|
|
@ -330,7 +330,7 @@ public:
|
|||
AP_Int32 ground_pressure;
|
||||
AP_Int8 compass_enabled;
|
||||
AP_Int16 angle_of_attack;
|
||||
AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current
|
||||
AP_Int8 battery_monitoring; // 0=disabled, 3=voltage only, 4=voltage and current
|
||||
AP_Float volt_div_ratio;
|
||||
AP_Float curr_amp_per_volt;
|
||||
AP_Float input_voltage;
|
||||
|
|
|
@ -64,20 +64,6 @@ handle_process_condition_command()
|
|||
do_change_alt();
|
||||
break;
|
||||
|
||||
/* case MAV_CMD_NAV_LAND_OPTIONS: // TODO - Add the command or equiv to MAVLink (repair in verify_condition() also)
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("Landing options set"));
|
||||
|
||||
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
|
||||
landing_pitch = next_nav_command.lng * 100;
|
||||
g.airspeed_cruise = next_nav_command.alt * 100;
|
||||
g.throttle_cruise = next_nav_command.lat;
|
||||
landing_distance = next_nav_command.p1;
|
||||
|
||||
SendDebug_P("MSG: throttle_cruise = ");
|
||||
SendDebugln(g.throttle_cruise,DEC);
|
||||
break;
|
||||
*/
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
|
|
@ -87,6 +87,8 @@
|
|||
# define PUSHBUTTON_PIN 41
|
||||
# define USB_MUX_PIN -1
|
||||
# define CONFIG_RELAY ENABLED
|
||||
# define BATTERY_PIN_1 0
|
||||
# define CURRENT_PIN_1 1
|
||||
#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
|
||||
# define A_LED_PIN 27
|
||||
# define B_LED_PIN 26
|
||||
|
@ -97,6 +99,8 @@
|
|||
# define PUSHBUTTON_PIN (-1)
|
||||
# define CLI_SLIDER_ENABLED DISABLED
|
||||
# define USB_MUX_PIN 23
|
||||
# define BATTERY_PIN_1 1
|
||||
# define CURRENT_PIN_1 2
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
@ -161,10 +165,10 @@
|
|||
# endif
|
||||
#elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN
|
||||
# ifndef CONFIG_PITOT_SOURCE_ANALOG_PIN
|
||||
# define CONFIG_PITOT_SOURCE_ANALOG_PIN AN4
|
||||
# define CONFIG_PITOT_SOURCE_ANALOG_PIN 0
|
||||
# endif
|
||||
#else
|
||||
# warning Invalid value for CONFIG_PITOT_SOURCE, disabling sonar
|
||||
# warning Invalid value for CONFIG_PITOT_SOURCE, disabling airspeed
|
||||
# undef PITOT_ENABLED
|
||||
# define PITOT_ENABLED DISABLED
|
||||
#endif
|
||||
|
@ -173,10 +177,9 @@
|
|||
# define SONAR_TYPE MAX_SONAR_LV // MAX_SONAR_XL,
|
||||
#endif
|
||||
|
||||
/* In ArduPlane PITOT usually takes the place of SONAR, but some bits
|
||||
* still depend on SONAR.
|
||||
*/
|
||||
#define SONAR_ENABLED PITOT_ENABLED
|
||||
#ifndef SONAR_ENABLED
|
||||
#define SONAR_ENABLED DISABLED
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// HIL_MODE OPTIONAL
|
||||
|
@ -225,12 +228,17 @@
|
|||
# define LOW_VOLTAGE 9.6
|
||||
#endif
|
||||
#ifndef VOLT_DIV_RATIO
|
||||
# define VOLT_DIV_RATIO 3.56
|
||||
# define VOLT_DIV_RATIO 3.56 // This is the proper value for an on-board APM1 voltage divider with a 3.9kOhm resistor
|
||||
//# define VOLT_DIV_RATIO 15.70 // This is the proper value for the AttoPilot 50V/90A sensor
|
||||
//# define VOLT_DIV_RATIO 4.127 // This is the proper value for the AttoPilot 13.6V/45A sensor
|
||||
|
||||
#endif
|
||||
|
||||
#ifndef CURR_AMP_PER_VOLT
|
||||
# define CURR_AMP_PER_VOLT 27.32
|
||||
# define CURR_AMP_PER_VOLT 27.32 // This is the proper value for the AttoPilot 50V/90A sensor
|
||||
//# define CURR_AMP_PER_VOLT 13.66 // This is the proper value for the AttoPilot 13.6V/45A sensor
|
||||
#endif
|
||||
|
||||
#ifndef CURR_AMPS_OFFSET
|
||||
# define CURR_AMPS_OFFSET 0.0
|
||||
#endif
|
||||
|
|
|
@ -183,18 +183,8 @@ enum gcs_severity {
|
|||
|
||||
|
||||
#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*g.volt_div_ratio
|
||||
|
||||
#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt
|
||||
|
||||
#define AIRSPEED_CH 7 // The external ADC channel for the airspeed sensor
|
||||
#define BATTERY_PIN1 0 // These are the pins for the voltage dividers
|
||||
#define BATTERY_PIN2 1
|
||||
#define BATTERY_PIN3 2
|
||||
#define BATTERY_PIN4 3
|
||||
|
||||
#define VOLTAGE_PIN_0 0 // These are the pins for current sensor: voltage
|
||||
#define CURRENT_PIN_1 1 // and current
|
||||
|
||||
#define RELAY_PIN 47
|
||||
|
||||
|
||||
|
|
|
@ -106,25 +106,21 @@ static void zero_airspeed(void)
|
|||
|
||||
static void read_battery(void)
|
||||
{
|
||||
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9;
|
||||
battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9;
|
||||
battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9;
|
||||
battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9;
|
||||
|
||||
if(g.battery_monitoring == 1)
|
||||
battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream
|
||||
if(g.battery_monitoring == 2)
|
||||
battery_voltage = battery_voltage4;
|
||||
if(g.battery_monitoring == 0) {
|
||||
battery_voltage1 = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
if(g.battery_monitoring == 3 || g.battery_monitoring == 4)
|
||||
battery_voltage = battery_voltage1;
|
||||
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN_1)) * .1 + battery_voltage1 * .9;
|
||||
if(g.battery_monitoring == 4) {
|
||||
current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin
|
||||
current_total += current_amps * (float)delta_ms_medium_loop * 0.000278;
|
||||
current_amps1 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps1 * .9; //reads power sensor current pin
|
||||
current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours)
|
||||
}
|
||||
|
||||
#if BATTERY_EVENT == ENABLED
|
||||
if(battery_voltage < LOW_VOLTAGE) low_battery_event();
|
||||
if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event();
|
||||
if(battery_voltage1 < LOW_VOLTAGE) low_battery_event();
|
||||
if(g.battery_monitoring == 4 && current_total1 > g.pack_capacity) low_battery_event();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
@ -301,8 +301,11 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
|
|||
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
||||
g.compass_enabled = false;
|
||||
|
||||
} else if (!strcmp_P(argv[1].str, PSTR("reset"))) {
|
||||
compass.set_offsets(0,0,0);
|
||||
|
||||
} else {
|
||||
Serial.printf_P(PSTR("\nOptions:[on,off]\n"));
|
||||
Serial.printf_P(PSTR("\nOptions:[on,off,reset]\n"));
|
||||
report_compass();
|
||||
return 0;
|
||||
}
|
||||
|
@ -319,7 +322,7 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
|
|||
g.battery_monitoring.set_and_save(argv[1].i);
|
||||
|
||||
} else {
|
||||
Serial.printf_P(PSTR("\nOptions: 0-4"));
|
||||
Serial.printf_P(PSTR("\nOptions: 3-4"));
|
||||
}
|
||||
|
||||
report_batt_monitor();
|
||||
|
@ -336,8 +339,6 @@ static void report_batt_monitor()
|
|||
Serial.printf_P(PSTR("Batt Mointor\n"));
|
||||
print_divider();
|
||||
if(g.battery_monitoring == 0) Serial.printf_P(PSTR("Batt monitoring disabled"));
|
||||
if(g.battery_monitoring == 1) Serial.printf_P(PSTR("Monitoring 3 cell"));
|
||||
if(g.battery_monitoring == 2) Serial.printf_P(PSTR("Monitoring 4 cell"));
|
||||
if(g.battery_monitoring == 3) Serial.printf_P(PSTR("Monitoring batt volts"));
|
||||
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current"));
|
||||
print_blanks(2);
|
||||
|
|
|
@ -14,7 +14,6 @@ static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
|
|||
#endif
|
||||
static int8_t test_imu(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_current(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv);
|
||||
|
@ -34,18 +33,18 @@ static int8_t test_logging(uint8_t argc, const Menu::arg *argv);
|
|||
// User enters the string in the console to call the functions on the right.
|
||||
// See class Menu in AP_Common for implementation details
|
||||
static const struct Menu::command test_menu_commands[] PROGMEM = {
|
||||
{"pwm", test_radio_pwm},
|
||||
{"radio", test_radio},
|
||||
{"passthru", test_passthru},
|
||||
{"failsafe", test_failsafe},
|
||||
{"battery", test_battery},
|
||||
{"relay", test_relay},
|
||||
{"waypoints", test_wp},
|
||||
{"xbee", test_xbee},
|
||||
{"eedump", test_eedump},
|
||||
{"modeswitch", test_modeswitch},
|
||||
{"pwm", test_radio_pwm},
|
||||
{"radio", test_radio},
|
||||
{"passthru", test_passthru},
|
||||
{"failsafe", test_failsafe},
|
||||
{"battery", test_battery},
|
||||
{"relay", test_relay},
|
||||
{"waypoints", test_wp},
|
||||
{"xbee", test_xbee},
|
||||
{"eedump", test_eedump},
|
||||
{"modeswitch", test_modeswitch},
|
||||
#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
|
||||
{"dipswitches", test_dipswitches},
|
||||
{"dipswitches", test_dipswitches},
|
||||
#endif
|
||||
|
||||
// Tests below here are for hardware sensors only present
|
||||
|
@ -60,7 +59,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
|
|||
{"airspeed", test_airspeed},
|
||||
{"airpressure", test_pressure},
|
||||
{"compass", test_mag},
|
||||
{"current", test_current},
|
||||
#elif HIL_MODE == HIL_MODE_SENSORS
|
||||
{"adc", test_adc},
|
||||
{"gps", test_gps},
|
||||
|
@ -260,26 +258,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
|
|||
static int8_t
|
||||
test_battery(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
if (g.battery_monitoring >=1 && g.battery_monitoring < 4) {
|
||||
for (int i = 0; i < 80; i++){ // Need to get many samples for filter to stabilize
|
||||
delay(20);
|
||||
read_battery();
|
||||
}
|
||||
Serial.printf_P(PSTR("Volts: 1:%2.2f, 2:%2.2f, 3:%2.2f, 4:%2.2f\n"),
|
||||
battery_voltage1,
|
||||
battery_voltage2,
|
||||
battery_voltage3,
|
||||
battery_voltage4);
|
||||
} else {
|
||||
Serial.printf_P(PSTR("Not enabled\n"));
|
||||
}
|
||||
return (0);
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_current(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
if (g.battery_monitoring == 4) {
|
||||
if (g.battery_monitoring == 3 || g.battery_monitoring == 4) {
|
||||
print_hit_enter();
|
||||
delta_ms_medium_loop = 100;
|
||||
|
||||
|
@ -287,10 +266,17 @@ if (g.battery_monitoring == 4) {
|
|||
delay(100);
|
||||
read_radio();
|
||||
read_battery();
|
||||
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
|
||||
battery_voltage,
|
||||
current_amps,
|
||||
current_total);
|
||||
if (g.battery_monitoring == 3){
|
||||
Serial.printf_P(PSTR("V: %4.4f\n"),
|
||||
battery_voltage1,
|
||||
current_amps1,
|
||||
current_total1);
|
||||
} else {
|
||||
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
|
||||
battery_voltage1,
|
||||
current_amps1,
|
||||
current_total1);
|
||||
}
|
||||
|
||||
// write out the servo PWM values
|
||||
// ------------------------------
|
||||
|
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue