Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Added APM's current sensing.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2234 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-05-09 15:46:56 +00:00
parent 6edb299243
commit 0979876ccc
10 changed files with 95 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopterMega/APM_Config.h
View File

@ -4,6 +4,7 @@
// GPS is auto-selected
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
#define NAV_TEST 0 // 0 = traditional, 1 = rate controlled
@ -37,7 +38,7 @@
# define LOG_CTUN ENABLED
# define LOG_NTUN ENABLED
# define LOG_MODE ENABLED
# define LOG_RAW DISABLED
# define LOG_RAW ENABLED
# define LOG_CMD ENABLED
# define LOG_CURRENT DISABLED

4
ArduCopterMega/ArduCopterMega.pde
View File

@ -669,8 +669,8 @@ void medium_loop()
case 4:
medium_loopCounter = 0;
if (g.current_enabled){
read_current();
if (g.battery_monitoring != 0){
read_battery();
}
// Accel trims = hold > 2 seconds

20
ArduCopterMega/Log.pde
View File

@ -362,6 +362,25 @@ void Log_Write_Nav_Tuning()
DataFlash.WriteByte(END_BYTE);
}
/*
Matrix3f tempmat = dcm.get_dcm_matrix();
DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);
DataFlash.WriteInt((int)wp_distance);
DataFlash.WriteInt((uint16_t)target_bearing);
DataFlash.WriteInt((uint16_t)nav_bearing);
DataFlash.WriteInt(altitude_error);
DataFlash.WriteInt((int)airspeed);
DataFlash.WriteInt((int)(nav_gain_scaler*1000));
DataFlash.WriteInt((int)(wrap_360(ToDeg(compass.heading)*100)/100)); // Just a temp hack
DataFlash.WriteLong(compass.last_update); // Just a temp hack
DataFlash.WriteInt((int)(tempmat.b.x*1000)); // Just a temp hack
DataFlash.WriteInt((int)(compass.heading_x*1000)); // Just a temp hack
DataFlash.WriteInt((int)(tempmat.a.x*1000)); // Just a temp hack
DataFlash.WriteInt((int)(compass.heading_y*1000)); // Just a temp hack
*/
// 1 2 3 4 5 6 7 8 9 10 11
//NTUN, 236, 0, 132, 10, 0, 0, 29, 2963, 16545, 16682, 108
@ -387,6 +406,7 @@ void Log_Read_Nav_Tuning()
}
// Write a mode packet. Total length : 5 bytes
void Log_Write_Mode(byte mode)
{

12
ArduCopterMega/Parameters.h
View File

@ -61,8 +61,8 @@ public:
k_param_IMU_calibration = 140,
k_param_ground_temperature,
k_param_ground_pressure,
k_param_current,
k_param_milliamp_hours,
k_param_battery_monitoring,
k_param_pack_capacity,
k_param_compass_enabled,
k_param_compass,
k_param_sonar,
@ -183,8 +183,8 @@ public:
AP_Int8 frame_type;
AP_Int8 sonar_enabled;
AP_Int8 current_enabled;
AP_Int16 milliamp_hours;
AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current
AP_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 compass_enabled;
AP_Int8 esc_calibrate;
@ -229,8 +229,8 @@ public:
frame_type (FRAME_CONFIG, k_param_frame_type, PSTR("FRAME_CONFIG")),
sonar_enabled (DISABLED, k_param_sonar, PSTR("SONAR_ENABLE")),
current_enabled (DISABLED, k_param_current, PSTR("CURRENT_ENABLE")),
milliamp_hours (CURR_AMP_HOURS, k_param_milliamp_hours, PSTR("MAH")),
battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")),
pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")),
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),

32
ArduCopterMega/config.h
View File

@ -77,14 +77,16 @@
#define HIL_MODE HIL_MODE_DISABLED
#endif
#ifndef HIL_PROTOCOL
#ifndef HIL_PROTOCOL
#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
#endif
#endif
#ifndef HIL_PORT
#ifndef HIL_PORT
#define HIL_PORT 0
#endif
#endif
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
# undef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_NONE
@ -131,6 +133,10 @@
# define GCS_PORT 3
#endif
#ifndef MAV_SYSTEM_ID
# define MAV_SYSTEM_ID 1
#endif
//////////////////////////////////////////////////////////////////////////////
// Serial port speeds.
//
@ -148,27 +154,23 @@
#ifndef BATTERY_EVENT
# define BATTERY_EVENT DISABLED
#endif
#ifndef BATTERY_TYPE
# define BATTERY_TYPE 0
#endif
#ifndef LOW_VOLTAGE
# define LOW_VOLTAGE 9.6
#endif
#ifndef VOLT_DIV_RATIO
# define VOLT_DIV_RATIO 3.0
# define VOLT_DIV_RATIO 3.56
#endif
#ifndef CURR_VOLT_DIV_RATIO
# define CURR_VOLT_DIV_RATIO 15.7
#ifndef CURR_AMP_PER_VOLT
# define CURR_AMP_PER_VOLT 27.32
#endif
#ifndef CURR_AMP_DIV_RATIO
# define CURR_AMP_DIV_RATIO 30.35
#ifndef CURR_AMPS_OFFSET
# define CURR_AMPS_OFFSET 0.0
#endif
#ifndef CURR_AMP_HOURS
# define CURR_AMP_HOURS 2000
#ifndef HIGH_DISCHARGE
# define HIGH_DISCHARGE 1760
#endif
//////////////////////////////////////////////////////////////////////////////
// INPUT_VOLTAGE
//

4
ArduCopterMega/defines.h
View File

@ -266,9 +266,7 @@
#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
#define CURRENT_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*CURR_VOLT_DIV_RATIO
#define CURRENT_AMPS(x) (x*(INPUT_VOLTAGE/1024.0))*CURR_AMP_DIV_RATIO
#define CURRENT_AMPS(x) ((x*(INPUT_VOLTAGE/1024.0))-CURR_AMPS_OFFSET)*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

31
ArduCopterMega/sensors.pde
View File

@ -86,7 +86,6 @@ void zero_airspeed(void)
#endif // HIL_MODE != HIL_MODE_ATTITUDE
#if BATTERY_EVENT == 1
void read_battery(void)
{
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9;
@ -94,27 +93,21 @@ void read_battery(void)
battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9;
battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9;
#if BATTERY_TYPE == 0
if(battery_voltage3 < LOW_VOLTAGE)
low_battery_event();
if(g.battery_monitoring == 1)
battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream
#endif
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 == 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;
}
#if BATTERY_TYPE == 1
if(battery_voltage4 < LOW_VOLTAGE)
low_battery_event();
battery_voltage = battery_voltage4; // set total battery voltage, for telemetry stream
#if BATTERY_EVENT == 1
if(battery_voltage < LOW_VOLTAGE) low_battery_event();
if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event();
#endif
}
#endif
void read_current(void)
{
current_voltage = CURRENT_VOLTAGE(analogRead(VOLTAGE_PIN_0)) * .1 + current_voltage * .9; //reads power sensor voltage pin
current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin
current_total += (current_amps * 0.27777) / delta_ms_medium_loop;
}
//v: 10.9453, a: 17.4023, mah: 8.2

51
ArduCopterMega/setup.pde
View File

@ -9,7 +9,7 @@ static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv);
static int8_t setup_pid (uint8_t argc, const Menu::arg *argv);
static int8_t setup_frame (uint8_t argc, const Menu::arg *argv);
static int8_t setup_current (uint8_t argc, const Menu::arg *argv);
static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv);
static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
@ -29,7 +29,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
{"level", setup_accel},
{"modes", setup_flightmodes},
{"frame", setup_frame},
{"current", setup_current},
{"battery", setup_batt_monitor},
{"sonar", setup_sonar},
{"compass", setup_compass},
{"declination", setup_declination},
@ -73,7 +73,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
report_version();
report_radio();
report_frame();
report_current();
report_batt_monitor();
report_sonar();
report_gains();
report_xtrack();
@ -521,24 +521,16 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
}
static int8_t
setup_current(uint8_t argc, const Menu::arg *argv)
setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("on"))) {
g.current_enabled.set_and_save(true);
if(argv[1].i >= 0 && argv[1].i <= 4){
g.battery_monitoring.set_and_save(argv[1].i);
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.current_enabled.set_and_save(false);
} else if(argv[1].i > 10){
g.milliamp_hours.set_and_save(argv[1].i);
}else{
Serial.printf_P(PSTR("\nOptions:[on, off, mAh]\n"));
report_current();
return 0;
} else {
Serial.printf_P(PSTR("\nOptions: 0-4"));
}
report_current();
report_batt_monitor();
return 0;
}
@ -674,13 +666,14 @@ default_frame()
g.frame_type.set_and_save(PLUS_FRAME);
}
void
/*void
default_current()
{
g.milliamp_hours = 2000;
g.current_enabled.set(false);
save_EEPROM_current();
}
*/
void
default_flight_modes()
@ -805,6 +798,19 @@ default_gains()
/***************************************************************************/
// CLI reports
/***************************************************************************/
void report_batt_monitor()
{
//print_blanks(2);
Serial.printf_P(PSTR("Batt Mointor\n"));
print_divider();
if(g.battery_monitoring == 0) Serial.printf_P(PSTR("Batt mon. disabled"));
if(g.battery_monitoring == 1) Serial.printf_P(PSTR("3 cells"));
if(g.battery_monitoring == 2) Serial.printf_P(PSTR("4 cells"));
if(g.battery_monitoring == 3) Serial.printf_P(PSTR("batt volts"));
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("volts and cur"));
print_blanks(2);
}
void report_wp(byte index = 255)
{
if(index == 255){
@ -830,6 +836,7 @@ void print_wp(struct Location *cmd, byte index)
cmd->lng);
}
/*
void report_current()
{
//read_EEPROM_current();
@ -840,7 +847,7 @@ void report_current()
Serial.printf_P(PSTR("mah: %d"),(int)g.milliamp_hours.get());
print_blanks(2);
}
*/
void report_gps()
{
Serial.printf_P(PSTR("\nGPS\n"));
@ -1242,10 +1249,10 @@ void save_EEPROM_PID(void)
/********************************************************************************/
void save_EEPROM_current(void)
/*void save_EEPROM_current(void)
{
g.current_enabled.save();
g.milliamp_hours.save();
//g.milliamp_hours.save();
}
void read_EEPROM_current(void)
@ -1253,7 +1260,7 @@ void read_EEPROM_current(void)
g.current_enabled.load();
g.milliamp_hours.load();
}
*/
/********************************************************************************/
void read_EEPROM_radio(void)

2
ArduCopterMega/system.pde
View File

@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
};
// Create the top-level menu object.
MENU(main_menu, "AC 2.0 Beta", main_menu_commands);
MENU(main_menu, "AC 2.0.1 Beta", main_menu_commands);
void init_ardupilot()
{

12
ArduCopterMega/test.pde
View File

@ -652,18 +652,16 @@ test_current(uint8_t argc, const Menu::arg *argv)
while(1){
delay(100);
read_radio();
read_current();
read_battery();
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
current_voltage,
current_amps,
current_total);
//if(g.rc_3.control_in > 0){
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
//}
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
if(Serial.available() > 0){
return (0);