Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: integrate BattMonitor

Browse Source
This commit is contained in:
Randy Mackay 2013-10-01 22:34:44 +09:00
parent 9722276827
commit dab4f032f9
13 changed files with 108 additions and 215 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/AP_State.pde
View File

@ -71,7 +71,7 @@ static void set_failsafe_radio(bool b)
// ---------------------------------------------
void set_low_battery(bool b)
{
ap.low_battery = b;
failsafe.low_battery = b;
AP_Notify::flags.failsafe_battery = b;
}

40
ArduCopter/ArduCopter.pde
View File

@ -117,6 +117,7 @@
#include <AP_Scheduler.h> // main loop scheduler
#include <AP_RCMapper.h> // RC input mapping library
#include <AP_Notify.h> // Notify library
#include <AP_BattMonitor.h> // Battery monitor library
#if SPRAYER == ENABLED
#include <AC_Sprayer.h> // crop sprayer library
#endif
@ -375,19 +376,18 @@ static AP_RangeFinder_MaxsonarXL *sonar;
static union {
struct {
uint8_t home_is_set : 1; // 0
uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE
uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE
uint8_t pre_arm_rc_check : 1; // 5 // true if rc input pre-arm checks have been completed successfully
uint8_t pre_arm_check : 1; // 6 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
uint8_t auto_armed : 1; // 7 // stops auto missions from beginning until throttle is raised
uint8_t logging_started : 1; // 8 // true if dataflash logging has started
uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully
uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised
uint8_t logging_started : 1; // 6 // true if dataflash logging has started
uint8_t low_battery : 1; // 9 // Used to track if the battery is low - LED output flashes when the batt is low
uint8_t do_flip : 1; // 15 // Used to enable flip code
uint8_t takeoff_complete : 1; // 16
uint8_t land_complete : 1; // 17 // true if we have detected a landing
uint8_t compass_status : 1; // 18
uint8_t gps_status : 1; // 19
uint8_t do_flip : 1; // 7 // Used to enable flip code
uint8_t takeoff_complete : 1; // 8
uint8_t land_complete : 1; // 9 // true if we have detected a landing
uint8_t compass_status : 1; // 10 // unused remove
uint8_t gps_status : 1; // 11 // unused remove
};
uint32_t value;
} ap;
@ -425,7 +425,7 @@ static uint8_t receiver_rssi;
static struct {
uint8_t rc_override_active : 1; // 0 // true if rc control are overwritten by ground station
uint8_t radio : 1; // 1 // A status flag for the radio failsafe
uint8_t batt : 1; // 2 // A status flag for the battery failsafe
uint8_t low_battery : 1; // 2 // A status flag for the battery failsafe
uint8_t gps : 1; // 3 // A status flag for the gps failsafe
uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe
@ -624,12 +624,7 @@ static int8_t aux_switch_wp_index;
////////////////////////////////////////////////////////////////////////////////
// Battery Sensors
////////////////////////////////////////////////////////////////////////////////
// Battery Voltage of battery, initialized above threshold for filter
static float battery_voltage1 = LOW_VOLTAGE * 1.05f;
// refers to the instant amp draw – based on an Attopilot Current sensor
static float current_amps1;
// refers to the total amps drawn – based on an Attopilot Current sensor
static float current_total1;
static AP_BattMonitor battery;
////////////////////////////////////////////////////////////////////////////////
@ -899,6 +894,9 @@ void setup() {
// initialise notify system
notify.init();
// initialise battery monitor
battery.init();
#if CONFIG_SONAR == ENABLED
#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
sonar_analog_source = new AP_ADC_AnalogSource(
@ -914,8 +912,6 @@ void setup() {
#endif
rssi_analog_source = hal.analogin->channel(g.rssi_pin);
batt_volt_analog_source = hal.analogin->channel(g.battery_volt_pin);
batt_curr_analog_source = hal.analogin->channel(g.battery_curr_pin);
board_vcc_analog_source = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);
init_ardupilot();
@ -1126,9 +1122,7 @@ static void medium_loop()
medium_loopCounter++;
// read battery before compass because it may be used for motor interference compensation
if (g.battery_monitoring != 0) {
read_battery();
}
read_battery();
#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode
if(g.compass_enabled) {

16
ArduCopter/GCS_Mavlink.pde
View File

@ -189,17 +189,15 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
control_sensors_enabled &= ~(1<<2); // compass
}
uint16_t battery_current = -1;
uint8_t battery_remaining = -1;
int16_t battery_current = -1;
int8_t battery_remaining = -1;
if (current_total1 != 0 && g.pack_capacity != 0) {
battery_remaining = (100.0f * (g.pack_capacity - current_total1) / g.pack_capacity);
}
if (current_total1 != 0) {
battery_current = current_amps1 * 100;
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) {
battery_remaining = battery.capacity_remaining_pct();
battery_current = battery.current_amps() * 100;
}
if (g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) {
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_ONLY) {
/*setting a out-of-range value.
* It informs to external devices that
* it cannot be calculated properly just by voltage*/
@ -212,7 +210,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
control_sensors_enabled,
control_sensors_health,
(uint16_t)(scheduler.load_average(10000) * 1000),
battery_voltage1 * 1000, // mV
battery.voltage() * 1000, // mV
battery_current, // in 10mA units
battery_remaining, // in %
0, // comm drops %,

6
ArduCopter/Log.pde
View File

@ -177,10 +177,10 @@ static void Log_Write_Current()
LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG),
throttle_in : g.rc_3.control_in,
throttle_integrator : throttle_integrator,
battery_voltage : (int16_t) (battery_voltage1 * 100.0f),
current_amps : (int16_t) (current_amps1 * 100.0f),
battery_voltage : (int16_t) (battery.voltage() * 100.0f),
current_amps : (int16_t) (battery.current_amps() * 100.0f),
board_voltage : board_voltage(),
current_total : current_total1
current_total : battery.current_total_mah()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}

32
ArduCopter/Parameters.h
View File

@ -90,7 +90,9 @@ public:
k_param_arming_check_enabled,
k_param_sprayer,
k_param_angle_max,
k_param_gps_hdop_good, // 35
k_param_gps_hdop_good,
k_param_battery,
k_param_fs_batt_mah, // 37
// 65: AP_Limits Library
k_param_limits = 65, // deprecated - remove
@ -135,24 +137,24 @@ public:
// 140: Sensor parameters
//
k_param_imu = 140, // deprecated - can be deleted
k_param_battery_monitoring = 141,
k_param_volt_div_ratio,
k_param_curr_amp_per_volt,
k_param_battery_monitoring = 141, // deprecated - can be deleted
k_param_volt_div_ratio, // deprecated - can be deleted
k_param_curr_amp_per_volt, // deprecated - can be deleted
k_param_input_voltage, // deprecated - can be deleted
k_param_pack_capacity,
k_param_pack_capacity, // deprecated - can be deleted
k_param_compass_enabled,
k_param_compass,
k_param_sonar_enabled,
k_param_frame_orientation,
k_param_optflow_enabled,
k_param_low_voltage,
k_param_fs_batt_voltage,
k_param_ch7_option,
k_param_auto_slew_rate, // deprecated - can be deleted
k_param_sonar_type,
k_param_super_simple = 155,
k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change
k_param_copter_leds_mode,
k_param_ahrs, // AHRS group
k_param_ahrs, // AHRS group // 159
//
// 160: Navigation parameters
@ -174,8 +176,8 @@ public:
//
// Batery monitoring parameters
//
k_param_battery_volt_pin = 168,
k_param_battery_curr_pin, // 169
k_param_battery_volt_pin = 168, // deprecated - can be deleted
k_param_battery_curr_pin, // 169 deprecated - can be deleted
//
// 170: Radio settings
@ -281,26 +283,22 @@ public:
// 2 = XLL (XL with 10m range)
// 3 = HRLV
AP_Float sonar_gain;
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_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 failsafe_battery_enabled; // battery failsafe enabled
AP_Float fs_batt_voltage; // battery voltage below which failsafe will be triggered
AP_Float fs_batt_mah; // battery capacity (in mah) below which failsafe will be triggered
AP_Int8 failsafe_gps_enabled; // gps failsafe enabled
AP_Int8 failsafe_gcs; // ground station failsafe behavior
AP_Int16 gps_hdop_good; // GPS Hdop value below which represent a good position
AP_Int8 compass_enabled;
AP_Int8 optflow_enabled;
AP_Float low_voltage;
AP_Int8 super_simple;
AP_Int16 rtl_alt_final;
AP_Int8 copter_leds_mode; // Operating mode of LED
// lighting system
AP_Int8 battery_volt_pin;
AP_Int8 battery_curr_pin;
AP_Int8 rssi_pin;
AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions
AP_Int16 angle_max; // maximum lean angle of the copter in centi-degrees

87
ArduCopter/Parameters.pde
View File

@ -95,13 +95,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(sonar_gain, "SONAR_GAIN", SONAR_GAIN_DEFAULT),
// @Param: BATT_MONITOR
// @DisplayName: Battery monitoring
// @Description: Controls enabling monitoring of the battery's voltage and current
// @Values: 0:Disabled,3:Voltage Only,4:Voltage and Current
// @User: Standard
GSCALAR(battery_monitoring, "BATT_MONITOR", BATT_MONITOR_DISABLED),
// @Param: FS_BATT_ENABLE
// @DisplayName: Battery Failsafe Enable
// @Description: Controls whether failsafe will be invoked when battery voltage or current runs low
@ -109,6 +102,20 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(failsafe_battery_enabled, "FS_BATT_ENABLE", FS_BATTERY),
// @Param: FS_BATT_VOLTAGE
// @DisplayName: Failsafe battery voltage
// @Description: Battery voltage to trigger failsafe. Set to 0 to disable battery voltage failsafe. If the battery voltage drops below this voltage then the plane will RTL
// @Units: Volts
// @User: Standard
GSCALAR(fs_batt_voltage, "FS_BATT_VOLTAGE", FS_BATT_VOLTAGE_DEFAULT),
// @Param: FS_BATT_MAH
// @DisplayName: Failsafe battery milliAmpHours
// @Description: Battery capacity remaining to trigger failsafe. Set to 0 to disable battery remaining failsafe. If the battery remaining drops below this level then the plane will RTL
// @Units: mAh
// @User: Standard
GSCALAR(fs_batt_mah, "FS_BATT_MAH", FS_BATT_MAH_DEFAULT),
// @Param: FS_GPS_ENABLE
// @DisplayName: GPS Failsafe Enable
// @Description: Controls whether failsafe will be invoked when gps signal is lost
@ -130,25 +137,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Advanced
GSCALAR(gps_hdop_good, "GPS_HDOP_GOOD", GPS_HDOP_GOOD_DEFAULT),
// @Param: VOLT_DIVIDER
// @DisplayName: Voltage Divider
// @Description: Used to convert the voltage of the voltage sensing pin (BATT_VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_DIVIDER). For the 3DR Power brick on APM2 or Pixhawk this should be set to 10.1. For the PX4 using the PX4IO power supply this should be set to 1.
// @User: Advanced
GSCALAR(volt_div_ratio, "VOLT_DIVIDER", VOLT_DIV_RATIO),
// @Param: AMP_PER_VOLT
// @DisplayName: Current Amps per volt
// @Description: Used to convert the voltage on the current sensing pin (BATT_CURR_PIN) to the actual current being consumed in amps. For the 3DR Power brick on APM2 or Pixhawk it should be set to 17.
// @User: Advanced
GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT", CURR_AMP_PER_VOLT),
// @Param: BATT_CAPACITY
// @DisplayName: Battery Capacity
// @Description: Battery capacity in milliamp-hours (mAh)
// @Units: mAh
// @User: Standard
GSCALAR(pack_capacity, "BATT_CAPACITY", BATTERY_CAPACITY_DEFAULT),
// @Param: MAG_ENABLE
// @DisplayName: Enable Compass
// @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass
@ -163,14 +151,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(optflow_enabled, "FLOW_ENABLE", DISABLED),
// @Param: LOW_VOLT
// @DisplayName: Low Voltage
// @Description: Set this to the voltage you want to represent low voltage
// @Range: 0 20
// @Increment: 0.1
// @User: Standard
GSCALAR(low_voltage, "LOW_VOLT", LOW_VOLTAGE),
// @Param: SUPER_SIMPLE
// @DisplayName: Enable Super Simple Mode
// @Description: Setting this to Enabled(1) will enable Super Simple Mode. Setting this to Disabled(0) will disable Super Simple Mode
@ -187,20 +167,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(rtl_alt_final, "RTL_ALT_FINAL", RTL_ALT_FINAL),
// @Param: BATT_VOLT_PIN
// @DisplayName: Battery Voltage sensing pin
// @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. For the 3DR power brick on APM2.5 it should be set to 13. On the PX4 it should be set to 100. On the Pixhawk powered from the PM connector it should be set to 2.
// @Values: -1:Disabled, 0:A0, 1:A1, 2:Pixhawk, 13:A13, 100:PX4
// @User: Standard
GSCALAR(battery_volt_pin, "BATT_VOLT_PIN", BATTERY_VOLT_PIN),
// @Param: BATT_CURR_PIN
// @DisplayName: Battery Current sensing pin
// @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. For the 3DR power brick on APM2.5 it should be set to 12. On the PX4 it should be set to 101. On the Pixhawk powered from the PM connector it should be set to 3.
// @Values: -1:Disabled, 1:A1, 2:A2, 3:Pixhawk, 12:A12, 101:PX4
// @User: Standard
GSCALAR(battery_curr_pin, "BATT_CURR_PIN", BATTERY_CURR_PIN),
// @Param: RSSI_PIN
// @DisplayName: Receiver RSSI sensing pin
// @Description: This selects an analog pin for the receiver RSSI voltage. It assumes the voltage is 5V for max rssi, 0V for minimum
@ -1015,6 +981,10 @@ const AP_Param::Info var_info[] PROGMEM = {
GOBJECT(camera_mount2, "MNT2_", AP_Mount),
#endif
// @Group: BATT_
// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
GOBJECT(battery, "BATT_", AP_BattMonitor),
#if SPRAYER == ENABLED
// @Group: SPRAYER_
// @Path: ../libraries/AC_Sprayer/AC_Sprayer.cpp
@ -1060,6 +1030,27 @@ const AP_Param::Info var_info[] PROGMEM = {
AP_VAREND
};
/*
This is a conversion table from old parameter values to new
parameter names. The startup code looks for saved values of the old
parameters and will copy them across to the new parameters if the
new parameter does not yet have a saved value. It then saves the new
value.
Note that this works even if the old parameter has been removed. It
relies on the old k_param index not being removed
The second column below is the index in the var_info[] table for the
old object. This should be zero for top level parameters.
*/
const AP_Param::ConversionInfo conversion_table[] PROGMEM = {
{ Parameters::k_param_battery_monitoring, 0, AP_PARAM_INT8, "BATT_MONITOR" },
{ Parameters::k_param_battery_volt_pin, 0, AP_PARAM_INT8, "BATT_VOLT_PIN" },
{ Parameters::k_param_battery_curr_pin, 0, AP_PARAM_INT8, "BATT_CURR_PIN" },
{ Parameters::k_param_volt_div_ratio, 0, AP_PARAM_FLOAT, "BATT_VOLT_MULT" },
{ Parameters::k_param_curr_amp_per_volt, 0, AP_PARAM_FLOAT, "BATT_AMP_PERVOLT" },
{ Parameters::k_param_pack_capacity, 0, AP_PARAM_INT32, "BATT_CAPACITY" },
};
static void load_parameters(void)
{

17
ArduCopter/config.h
View File

@ -349,21 +349,12 @@
//////////////////////////////////////////////////////////////////////////////
// Battery monitoring
//
#ifndef LOW_VOLTAGE
# define LOW_VOLTAGE 10.5f
#endif
#ifndef VOLT_DIV_RATIO
# define VOLT_DIV_RATIO 3.56f
#ifndef FS_BATT_VOLTAGE_DEFAULT
# define FS_BATT_VOLTAGE_DEFAULT 10.5f // default battery voltage below which failsafe will be triggered
#endif
#ifndef CURR_AMP_PER_VOLT
# define CURR_AMP_PER_VOLT 27.32f
#endif
#ifndef CURR_AMPS_OFFSET
# define CURR_AMPS_OFFSET 0.0f
#endif
#ifndef BATTERY_CAPACITY_DEFAULT
# define BATTERY_CAPACITY_DEFAULT 3500
#ifndef FS_BATT_MAH_DEFAULT
# define FS_BATT_MAH_DEFAULT 0 // default battery capacity (in mah) below which failsafe will be triggered
#endif
#ifndef BOARD_VOLTAGE_MIN

8
ArduCopter/defines.h
View File

@ -333,14 +333,6 @@ enum ap_message {
#define DATA_SET_SIMPLE_OFF 27
#define DATA_SET_SUPERSIMPLE_ON 28
// battery monitoring macros
#define BATTERY_VOLTAGE(x) (x->voltage_average()*g.volt_div_ratio)
#define CURRENT_AMPS(x) (x->voltage_average()-CURR_AMPS_OFFSET)*g.curr_amp_per_volt
#define BATT_MONITOR_DISABLED 0
#define BATT_MONITOR_VOLTAGE_ONLY 3
#define BATT_MONITOR_VOLTAGE_AND_CURRENT 4
/* ************************************************************** */
/* Expansion PIN's that people can use for various things. */

9
ArduCopter/events.pde
View File

@ -50,7 +50,7 @@ static void failsafe_radio_on_event()
break;
case LAND:
// continue to land if battery failsafe is also active otherwise fall through to default handling
if (g.failsafe_battery_enabled && ap.low_battery) {
if (g.failsafe_battery_enabled && failsafe.low_battery) {
break;
}
default:
@ -85,8 +85,13 @@ static void failsafe_radio_off_event()
static void low_battery_event(void)
{
// return immediately if low battery event has already been triggered
if (failsafe.low_battery) {
return;
}
// failsafe check
if (g.failsafe_battery_enabled && !ap.low_battery && motors.armed()) {
if (g.failsafe_battery_enabled && motors.armed()) {
switch(control_mode) {
case STABILIZE:
case ACRO:

2
ArduCopter/leds.pde
View File

@ -60,7 +60,7 @@ static void update_copter_leds(void)
// motor leds control
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_ENABLED) {
if (motors.armed()) {
if (ap.low_battery) {
if (failsafe.low_battery) {
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BATT_OSCILLATE) {
copter_leds_oscillate(); //if motors are armed, but battery level is low, motor leds fast blink
} else {

38
ArduCopter/sensors.pde
View File

@ -99,45 +99,19 @@ static void init_optflow()
// read_battery - check battery voltage and current and invoke failsafe if necessary
// called at 10hz
#define BATTERY_FS_COUNTER 100 // 100 iterations at 10hz is 10 seconds
static void read_battery(void)
{
static uint8_t low_battery_counter = 0;
battery.read();
if(g.battery_monitoring == BATT_MONITOR_DISABLED) {
battery_voltage1 = 0;
return;
}
if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY || g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT) {
batt_volt_analog_source->set_pin(g.battery_volt_pin);
battery_voltage1 = BATTERY_VOLTAGE(batt_volt_analog_source);
}
if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT) {
static uint32_t last_time_ms;
uint32_t tnow = hal.scheduler->millis();
float dt_millis = tnow - last_time_ms;
current_amps1 = CURRENT_AMPS(batt_curr_analog_source);
if (last_time_ms != 0 && dt_millis < 2000) {
batt_curr_analog_source->set_pin(g.battery_curr_pin);
current_total1 += current_amps1 * 1000 * dt_millis * (1.0f/1000) * (1.0f/3600); //amps * amps to milliamps * milliseconds * milliseconds to seconds * seconds to hours
}
// update compass with current value
compass.set_current(current_amps1);
last_time_ms = tnow;
// update compass with current value
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) {
compass.set_current(battery.current_amps());
}
// check for low voltage or current if the low voltage check hasn't already been triggered
// we only check when we're not powered by USB to avoid false alarms during bench tests
if (!ap_system.usb_connected && !ap.low_battery && ( battery_voltage1 < g.low_voltage || (g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT && current_total1 > g.pack_capacity))) {
low_battery_counter++;
if( low_battery_counter >= BATTERY_FS_COUNTER ) {
low_battery_counter = BATTERY_FS_COUNTER; // ensure counter does not overflow
low_battery_event();
}
}else{
// reset low_battery_counter in case it was a temporary voltage dip
low_battery_counter = 0;
if (!ap_system.usb_connected && !failsafe.low_battery && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) {
low_battery_event();
}
}

16
ArduCopter/setup.pde
View File

@ -127,7 +127,7 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
bool updated = false; // have we updated the compensation vector at least once
// default compensation type to use current if possible
if( g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT ) {
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) {
comp_type = AP_COMPASS_MOT_COMP_CURRENT;
}else{
comp_type = AP_COMPASS_MOT_COMP_THROTTLE;
@ -265,10 +265,10 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
updated = true;
}else{
// current based compensation if more than 3amps being drawn
motor_impact_scaled = motor_impact / current_amps1;
motor_impact_scaled = motor_impact / battery.current_amps();
// adjust the motor compensation to negate the impact if drawing over 3amps
if( current_amps1 >= 3.0f ) {
if( battery.current_amps() >= 3.0f ) {
motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
updated = true;
}
@ -276,7 +276,7 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
// record maximum throttle and current
throttle_pct_max = max(throttle_pct_max, throttle_pct);
current_amps_max = max(current_amps_max, current_amps1);
current_amps_max = max(current_amps_max, battery.current_amps());
// display output at 1hz if throttle is above zero
print_counter++;
@ -334,7 +334,7 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
static void display_compassmot_info(Vector3f& motor_impact, Vector3f& motor_compensation)
{
// print one more time so the last thing printed matches what appears in the report_compass
cliSerial->printf_P(PSTR("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f comp x:%4.2f y:%4.2f z:%4.2f\n"),(int)g.rc_3.control_in, (float)current_amps1, (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z);
cliSerial->printf_P(PSTR("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f comp x:%4.2f y:%4.2f z:%4.2f\n"),(int)g.rc_3.control_in, (float)battery.current_amps(), (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z);
}
static int8_t
@ -967,9 +967,9 @@ static void report_batt_monitor()
{
cliSerial->printf_P(PSTR("\nBatt Mon:\n"));
print_divider();
if(g.battery_monitoring == BATT_MONITOR_DISABLED) print_enabled(false);
if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) cliSerial->printf_P(PSTR("volts"));
if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT) cliSerial->printf_P(PSTR("volts and cur"));
if (battery.monitoring() == AP_BATT_MONITOR_DISABLED) print_enabled(false);
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_ONLY) cliSerial->printf_P(PSTR("volts"));
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) cliSerial->printf_P(PSTR("volts and cur"));
print_blanks(2);
}

50
ArduCopter/test.pde
View File

@ -7,7 +7,6 @@
#if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS
static int8_t test_baro(uint8_t argc, const Menu::arg *argv);
#endif
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
static int8_t test_compass(uint8_t argc, const Menu::arg *argv);
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
static int8_t test_ins(uint8_t argc, const Menu::arg *argv);
@ -32,7 +31,6 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
#if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS
{"baro", test_baro},
#endif
{"battery", test_battery},
{"compass", test_compass},
{"gps", test_gps},
{"ins", test_ins},
@ -88,54 +86,6 @@ test_baro(uint8_t argc, const Menu::arg *argv)
}
#endif
static int8_t
test_battery(uint8_t argc, const Menu::arg *argv)
{
// check if radio is calibration
pre_arm_rc_checks();
if(!ap.pre_arm_rc_check) {
cliSerial->print_P(PSTR("radio not calibrated, exiting"));
return(0);
}
cliSerial->printf_P(PSTR("\nCareful! Motors will spin! Press Enter to start.\n"));
while (cliSerial->read() != -1); /* flush */
while(!cliSerial->available()) { /* wait for input */
delay(100);
}
while (cliSerial->read() != -1); /* flush */
print_hit_enter();
// allow motors to spin
output_min();
motors.armed(true);
while(1) {
delay(100);
read_radio();
read_battery();
if (g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) {
cliSerial->printf_P(PSTR("V: %4.4f\n"),
battery_voltage1,
current_amps1,
current_total1);
} else {
cliSerial->printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"),
battery_voltage1,
current_amps1,
current_total1);
}
motors.throttle_pass_through();
if(cliSerial->available() > 0) {
motors.armed(false);
return (0);
}
}
motors.armed(false);
return (0);
}
static int8_t
test_compass(uint8_t argc, const Menu::arg *argv)
{