5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-12 10:58:30 -04:00

Added low voltage code flashing code.

This commit is contained in:
Jason Short 2011-09-16 18:56:51 -07:00
parent 867e01cab5
commit 3d307c9fde
4 changed files with 34 additions and 19 deletions

View File

@ -17,7 +17,7 @@ public:
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 107;
static const uint16_t k_format_version = 108;
// The parameter software_type is set up solely for ground station use
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
@ -78,6 +78,7 @@ public:
k_param_top_bottom_ratio,
k_param_optflow_enabled,
k_param_input_voltage,
k_param_low_voltage,
//
// 160: Navigation parameters
@ -237,6 +238,7 @@ public:
AP_Float top_bottom_ratio;
AP_Int8 optflow_enabled;
AP_Float input_voltage;
AP_Float low_voltage;
#if FRAME_CONFIG == HELI_FRAME
// Heli
@ -300,6 +302,7 @@ public:
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
optflow_enabled (OPTFLOW, k_param_optflow_enabled, PSTR("FLOW_ENABLE")),
input_voltage (INPUT_VOLTAGE, k_param_input_voltage, PSTR("IN_VOLT")),
low_voltage (LOW_VOLTAGE, k_param_low_voltage, PSTR("LOW_VOLT")),
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),

View File

@ -52,7 +52,7 @@
#define FR_LED AN12 // Mega PE4 pin, OUT7
#define RE_LED AN14 // Mega PE5 pin, OUT6
#define RI_LED AN10 // Mega PH4 pin, OUT5
#define LE_LED AN8 // Mega PH5 pin, OUT4
#define LE_LED AN8 // Mega PH5 pin, OUT4
// Internal defines, don't edit and expect things to work
// -------------------------------------------------------

View File

@ -96,26 +96,35 @@ static void clear_leds()
#if MOTOR_LEDS == 1
static void update_motor_leds(void)
{
// blink rear
static bool blink = false;
if (motor_armed == true){
if (low_batt == true){
// blink rear
static bool blink = false;
if (blink){
digitalWrite(RE_LED, HIGH);
digitalWrite(FR_LED, HIGH);
digitalWrite(RI_LED, LOW);
digitalWrite(LE_LED, LOW);
}else{
if (blink){
digitalWrite(RE_LED, HIGH);
digitalWrite(FR_LED, HIGH);
digitalWrite(RI_LED, LOW);
digitalWrite(LE_LED, LOW);
}else{
digitalWrite(RE_LED, LOW);
digitalWrite(FR_LED, LOW);
digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH);
}
blink = !blink;
}else{
digitalWrite(RE_LED, HIGH);
digitalWrite(FR_LED, HIGH);
digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH);
}
}else {
digitalWrite(RE_LED, LOW);
digitalWrite(FR_LED, LOW);
digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH);
digitalWrite(RI_LED, LOW);
digitalWrite(LE_LED, LOW);
}
blink = !blink;
// the variable low_batt is here to let people know the voltage is low or the pack capacity is finished
// I don't know what folks want here.
// low_batt
}
#endif

View File

@ -113,17 +113,20 @@ static void read_battery(void)
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 == 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_EVENT == 1
if(battery_voltage < LOW_VOLTAGE)
if(battery_voltage < g.low_voltage)
low_battery_event();
if(g.battery_monitoring == 4 && current_total > g.pack_capacity)