3 files changed, 411 insertions, 373 deletions

diff --git a/apps/debug_menu.c b/apps/debug_menu.c
index 1d42cf8312..9d5028af7c 100644
--- a/apps/debug_menu.c
+++ b/apps/debug_menu.c
@@ -1247,9 +1247,10 @@ bool view_battery(void)
                 lcd_puts(0, 3, buf);
 #endif
 #ifdef HAVE_CHARGE_CTRL
-                snprintf(buf, 30, "Charging: %s", 
-                         charger_enabled ? "yes" : "no");
-                lcd_puts(0, 4, buf);
+                snprintf(buf, 30, "Chgr: %s %s", 
+                         charger_inserted() ? "present" : "absent",
+                         charger_enabled ? "on" : "off");
+                lcd_puts(0, 3, buf);
                 snprintf(buf, 30, "short delta: %d", short_delta);
                 lcd_puts(0, 5, buf);
                 snprintf(buf, 30, "long delta: %d", long_delta);
@@ -1271,7 +1272,7 @@ bool view_battery(void)
                 }
                 break;
 
-            case 3: /* remeining time estimation: */
+            case 3: /* remaining time estimation: */
                 lcd_clear_display();
 
 #ifdef HAVE_CHARGE_CTRL
@@ -1283,23 +1284,24 @@ bool view_battery(void)
 
                 snprintf(buf, 30, "Lvl@cyc st: %d%%", powermgmt_last_cycle_level);
                 lcd_puts(0, 2, buf);
+
+                snprintf(buf, 30, "P=%2d I=%2d", pid_p, pid_i);
+                lcd_puts(0, 3, buf);
+
+                snprintf(buf, 30, "Trickle sec: %d/60", trickle_sec);
+                lcd_puts(0, 4, buf);
 #endif
 
                 snprintf(buf, 30, "Last PwrHist: %d.%02d V",
                     power_history[0] / 100,
                     power_history[0] % 100);
-                lcd_puts(0, 3, buf);
-
-                snprintf(buf, 30, "battery level: %d%%", battery_level());
                 lcd_puts(0, 5, buf);
 
-                snprintf(buf, 30, "Est. remain: %d m", battery_time());
+                snprintf(buf, 30, "battery level: %d%%", battery_level());
                 lcd_puts(0, 6, buf);
 
-#ifdef HAVE_CHARGE_CTRL
-                snprintf(buf, 30, "Trickle sec: %d/60", trickle_sec);
+                snprintf(buf, 30, "Est. remain: %d m", battery_time());
                 lcd_puts(0, 7, buf);
-#endif                
                 break;
         }
         
diff --git a/firmware/export/powermgmt.h b/firmware/export/powermgmt.h
index 3bccb0f59f..ccfdede9f9 100644
--- a/firmware/export/powermgmt.h
+++ b/firmware/export/powermgmt.h
@@ -57,12 +57,13 @@
 #ifndef SIMULATOR
 
 #ifdef HAVE_CHARGE_CTRL
+#define START_TOPOFF_CHG    85  /* Battery % to start at top-off */
+#define START_TRICKLE_CHG   95  /* Battery % to start at trickle */
+
 #define POWER_MESSAGE_LEN 32     /* power thread status message */
 #define CHARGE_MAX_TIME_1500 450 /* minutes: maximum charging time for 1500 mAh batteries */
                                  /* actual max time depends also on BATTERY_CAPACITY! */
 #define CHARGE_MIN_TIME   10     /* minutes: minimum charging time */
-#define CHARGE_RESTART    85     /* %: when to restart charging in 'charge' mode */
-                                 /* attention: if set too high, normal charging is started in trickle mode */
 #define TOPOFF_MAX_TIME   90     /* After charging, go to top off charge. How long should top off charge be? */
 #define TOPOFF_VOLTAGE    565    /* which voltage is best? (centivolts) */
 #define TRICKLE_MAX_TIME  12*60  /* After top off charge, go to trickle charge. How long should trickle charge be? */
@@ -71,6 +72,9 @@
 #define START_TOPOFF_SEC    25   /* initial trickle_sec for topoff */
 #define START_TRICKLE_SEC   15   /* initial trickle_sec for trickle */
 
+#define PID_PCONST          2   /* PID proportional constant */
+#define PID_DEADZONE        2   /* PID proportional deadzone */
+
 extern char power_message[POWER_MESSAGE_LEN];
 
 extern int long_delta;          /* long term delta battery voltage */
@@ -79,6 +83,8 @@ extern int short_delta;         /* short term delta battery voltage */
 extern int powermgmt_last_cycle_startstop_min; /* how many minutes ago was the charging started or stopped? */
 extern int powermgmt_last_cycle_level;         /* which level had the batteries at this time? */
 
+extern int pid_p;                /* PID proportional term */
+extern int pid_i;                /* PID integral term */
 extern int trickle_sec;          /* trickle charge: How many seconds per minute are we charging actually? */
 
 #endif /* HAVE_CHARGE_CTRL */
diff --git a/firmware/powermgmt.c b/firmware/powermgmt.c
index dd2233efb1..0366896bb4 100644
--- a/firmware/powermgmt.c
+++ b/firmware/powermgmt.c
@@ -57,17 +57,11 @@
 static char debug_message[DEBUG_MESSAGE_LEN];
 #define DEBUG_STACK ((0x1000)/sizeof(long))
 static int fd;          /* write debug information to this file */
+static int wrcount;
 #else
 #define DEBUG_STACK 0
 #endif
 
-long last_event_tick;
-
-void reset_poweroff_timer(void)
-{
-    last_event_tick = current_tick;
-}
-
 #ifdef SIMULATOR /***********************************************************/
 
 int battery_level(void)
@@ -125,24 +119,38 @@ static const short percent_to_volt_discharge[BATTERY_TYPES_COUNT][11] =
 #endif
 };
 
-static int battery_type = 0;
+#ifdef HAVE_CHARGING
+/* voltages (centivolt) of 0%, 10%, ... 100% when charging enabled */
+static const short percent_to_volt_charge[11] =
+{
+    /* values guessed, see
+       http://www.seattlerobotics.org/encoder/200210/LiIon2.pdf until someone
+       measures voltages over a charging cycle */
+    476, 544, 551, 556, 561, 564, 566, 576, 582, 584, 585 /* NiMH */
+};
+#endif /* HAVE_CHARGING */
 
 #if defined(HAVE_CHARGE_CTRL) || CONFIG_BATTERY == BATT_LIION2200
 charge_state_type charge_state;     /* charging mode */
-int  charge_timer;                  /* charge timer (minutes, dec to zero) */
 #endif
 
 #ifdef HAVE_CHARGING
-/* Flag that the charger has been plugged in */
-static bool charger_was_inserted = false;   /* for power off logic */
-static bool charger_power_is_on;            /* for car adapter mode */
-#endif
+/*
+ * Flag that the charger has been plugged in/removed: this is set for exactly
+ * one time through the power loop when the charger has been plugged in.
+ */
+static enum {
+    NO_CHARGER,
+    CHARGER_UNPLUGGED,              /* transient state */
+    CHARGER_PLUGGED,                /* transient state */
+    CHARGER
+} charger_input_state;
 
-/* Power history: power_history[0] is the newest sample */
-unsigned short power_history[POWER_HISTORY_LEN];
+static bool waiting_to_resume_play = false;
+static long play_resume_time;
+#endif
 
 #ifdef HAVE_CHARGE_CTRL
-
 int long_delta;                     /* long term delta battery voltage */
 int short_delta;                    /* short term delta battery voltage */
 
@@ -155,52 +163,56 @@ int powermgmt_last_cycle_startstop_min = 0;    /* how many minutes ago was the
                                                   stopped? */
 int powermgmt_last_cycle_level = 0;            /* which level had the
                                                   batteries at this time? */
-int  trickle_sec = 0;                          /* how many seconds should the
+int trickle_sec = 0;                           /* how many seconds should the
                                                   charger be enabled per
                                                   minute for trickle
                                                   charging? */
-/* voltages (centivolt) of 0%, 10%, ... 100% when charging enabled */
-static const short percent_to_volt_charge[11] =
-{
-    /* values guessed, see
-       http://www.seattlerobotics.org/encoder/200210/LiIon2.pdf until someone
-       measures voltages over a charging cycle */
-    476, 544, 551, 556, 561, 564, 566, 576, 582, 584, 585 /* NiMH */
-};
-
-#endif /* HAVE_CHARGE_CTRL || CONFIG_BATTERY == BATT_LIION2200 */
+int pid_p = 0;                      /* PID proportional term */
+int pid_i = 0;                      /* PID integral term */
+#endif /* HAVE_CHARGE_CTRL */
 
 /*
  * Average battery voltage and charger voltage, filtered via a digital
  * exponential filter.
  */
-unsigned int battery_centivolts;/* filtered battery voltage, centvolts */
-static unsigned int avgbat;     /* average battery voltage */
+static unsigned int battery_centivolts;/* filtered battery voltage, centvolts */
+static unsigned int avgbat;     /* average battery voltage (filtering) */
 #define BATT_AVE_SAMPLES        32  /* filter constant / @ 2Hz sample rate */
 
-int battery_capacity = BATTERY_CAPACITY_MIN; /* only a default value */
-
 /* battery level (0-100%) of this minute, updated once per minute */
-static int battery_percent = -1;
+static int battery_percent  = -1;
+static int battery_capacity = BATTERY_CAPACITY_MIN; /* default value, mAH */
+static int battery_type     = 0;
+
+/* Power history: power_history[0] is the newest sample */
+unsigned short power_history[POWER_HISTORY_LEN];
 
 static bool car_adapter_mode_enabled = false;
 
 static char power_stack[DEFAULT_STACK_SIZE + DEBUG_STACK];
 static const char power_thread_name[] = "power";
 
-static int  poweroff_timeout = 0;
-static long last_charge_time = 0;
-int powermgmt_est_runningtime_min = -1;
+static int poweroff_timeout = 0;
+static int powermgmt_est_runningtime_min = -1;
 
 static bool sleeptimer_active = false;
-static unsigned long sleeptimer_endtick;
+static long sleeptimer_endtick;
+
+static long last_event_tick;
+
+static void battery_level_update(void);     /* forward declaration */
+
+void reset_poweroff_timer(void)
+{
+    last_event_tick = current_tick;
+}
 
 #if BATTERY_TYPES_COUNT > 1
 void set_battery_type(int type)
 {
     if (type != battery_type) {
         battery_type = type;
-        battery_percent = -1; /* reset on type change */
+        battery_level_update();     /* recalculate the battery level */
     }
 }
 #endif
@@ -219,79 +231,6 @@ int battery_time(void)
     return powermgmt_est_runningtime_min;
 }
 
-/* look into the percent_to_volt_* table and get a realistic battery level
-       percentage */
-int voltage_to_percent(int voltage, const short* table)
-{
-    if (voltage <= table[0])
-        return 0;
-    else
-        if (voltage >= table[10])
-            return 100;
-        else {    
-            /* search nearest value */
-            int i = 0;
-            while ((i < 10) && (table[i+1] < voltage))
-                i++;
-            /* interpolate linear between the smaller and greater value */
-            return i * 10 /* 10th */
-                + (voltage - table[i]) *
-                10 / (table[i+1] - table[i]); /* 1th */
-        }
-}
-
-/* update battery level, called only once per minute */
-void battery_level_update(void)
-{
-    int level;
-
-    level = battery_centivolts;
-    if(level > BATTERY_LEVEL_FULL)
-        level = BATTERY_LEVEL_FULL;
-
-    if(level < BATTERY_LEVEL_EMPTY)
-        level = BATTERY_LEVEL_EMPTY;
-
-#ifdef HAVE_CHARGE_CTRL
-    if (charge_state == DISCHARGING) {
-        level = voltage_to_percent(level, 
-                percent_to_volt_discharge[battery_type]);
-    }
-    else if (charge_state == CHARGING) {
-        level = voltage_to_percent(level, percent_to_volt_charge);
-    }
-    else {/* in trickle charge, the battery is by definition 100% full */
-        battery_percent = level = 100;
-    }
-#else
-    /* always use the discharge table */
-    level = voltage_to_percent(level,
-            percent_to_volt_discharge[battery_type]);
-#endif
-
-#ifndef HAVE_MMC  /* this adjustment is only needed for HD based */
-    if (battery_percent == -1) { /* first run of this procedure */
-        /* The battery voltage is usually a little lower directly after
-           turning on, because the disk was used heavily. Raise it by 5. % */
-        battery_percent = (level > 95) ? 100 : level + 5;
-    }
-    else 
-#endif
-    {
-        /* the level is allowed to be -1 of the last value when usb not
-           connected and to be -3 of the last value when usb is connected */
-        if (usb_inserted()) {
-            if (level < battery_percent - 3)
-                level = battery_percent - 3;
-        }
-        else {
-            if (level < battery_percent - 1)
-                level = battery_percent - 1;
-        }
-        battery_percent = level;
-    }
-}
-
 /* Returns battery level in percent */
 int battery_level(void)
 {
@@ -316,11 +255,11 @@ void set_poweroff_timeout(int timeout)
 void set_sleep_timer(int seconds)
 {
     if(seconds) {
-        sleeptimer_active = true;
+        sleeptimer_active  = true;
         sleeptimer_endtick = current_tick + seconds * HZ;
     }
     else {
-        sleeptimer_active = false;
+        sleeptimer_active  = false;
         sleeptimer_endtick = 0;
     }
 }
@@ -333,31 +272,82 @@ int get_sleep_timer(void)
         return 0;
 }
 
-/* We shut off in the following cases:
-   1) The unit is idle, not playing music
-   2) The unit is playing music, but is paused
+/* look into the percent_to_volt_* table and get a realistic battery level
+       percentage */
+static int voltage_to_percent(int voltage, const short* table)
+{
+    if (voltage <= table[0])
+        return 0;
+    else
+        if (voltage >= table[10])
+            return 100;
+        else {    
+            /* search nearest value */
+            int i = 0;
+            while ((i < 10) && (table[i+1] < voltage))
+                i++;
+            /* interpolate linear between the smaller and greater value */
+            return (i * 10) /* Tens digit, 10% per entry */
+                + (((voltage - table[i]) * 10)
+                   / (table[i+1] - table[i])); /* Ones digit: interpolated */
+        }
+}
 
-   We do not shut off in the following cases:
-   1) The USB is connected
-   2) The charger is connected
-   3) We are recording, or recording with pause
-*/
+/* update battery level, called once per minute */
+static void battery_level_update(void)
+{
+    int level;
+
+#ifdef HAVE_CHARGE_CTRL
+    if (charge_state == DISCHARGING) {
+        level = voltage_to_percent(battery_centivolts, 
+                percent_to_volt_discharge[battery_type]);
+    }
+    else if (charge_state == CHARGING) {
+        level = voltage_to_percent(battery_centivolts, percent_to_volt_charge);
+    }
+    else { /* in trickle charge, the battery is by definition 100% full */
+        level = 100;
+    }
+#else
+    /* always use the discharge table */
+    level = voltage_to_percent(battery_centivolts,
+            percent_to_volt_discharge[battery_type]);
+#endif
+
+#ifndef HAVE_MMC  /* this adjustment is only needed for HD based */
+    if (battery_percent == -1) { /* first run of this procedure */
+        /* The battery voltage is usually a little lower directly after
+           turning on, because the disk was used heavily. Raise it by 5. % */
+        level = (level > 95) ? 100 : level + 5;
+    }
+#endif
+    battery_percent = level;
+}
+
+/*
+ * We shut off in the following cases:
+ * 1) The unit is idle, not playing music
+ * 2) The unit is playing music, but is paused
+ *
+ * We do not shut off in the following cases:
+ * 1) The USB is connected
+ * 2) The charger is connected
+ * 3) We are recording, or recording with pause
+ */
 static void handle_auto_poweroff(void)
 {
     long timeout = poweroff_idle_timeout_value[poweroff_timeout]*60*HZ;
     int  mpeg_stat = mpeg_status();
-#ifdef HAVE_CHARGING
-    bool charger_is_inserted = charger_inserted();
-#endif
 
 #ifdef HAVE_CHARGING
-    /* The was_inserted thing prevents the unit to shut down immediately
-       when the charger is extracted */
-    if(charger_is_inserted || charger_was_inserted)
-    {
-        last_charge_time = current_tick;
+    /*
+     * Inhibit shutdown as long as the charger is plugged in.  If it is
+     * unplugged, wait for a timeout period and then shut down.
+     */
+    if(charger_input_state == CHARGER) {
+        last_event_tick = current_tick;
     }
-    charger_was_inserted = charger_is_inserted;
 #endif
     
     if(timeout &&
@@ -369,9 +359,8 @@ static void handle_auto_poweroff(void)
         ((mpeg_stat == (MPEG_STATUS_PLAY | MPEG_STATUS_PAUSE)) &&
          !sleeptimer_active)))
     {
-        if(TIME_AFTER(current_tick, last_event_tick + timeout) &&
-           TIME_AFTER(current_tick, last_disk_activity + timeout) &&
-           TIME_AFTER(current_tick, last_charge_time + timeout))
+        if(TIME_AFTER(current_tick, last_event_tick    + timeout) &&
+           TIME_AFTER(current_tick, last_disk_activity + timeout))
         {
             shutdown_hw();
         }
@@ -385,7 +374,8 @@ static void handle_auto_poweroff(void)
             {
                 mpeg_stop();
 #ifdef HAVE_CHARGING
-                if(charger_is_inserted)
+                if((charger_input_state == CHARGER) ||
+                   (charger_input_state == CHARGER_PLUGGED))
                 {
                     DEBUGF("Sleep timer timeout. Stopping...\n");
                     set_sleep_timer(0);
@@ -413,10 +403,7 @@ void set_car_adapter_mode(bool setting)
 
 #ifdef HAVE_CHARGING
 static void car_adapter_mode_processing(void)
-{
-    static bool waiting_to_resume_play = false;
-    static long play_resume_time;
-    
+{    
     if (car_adapter_mode_enabled) {
 
         if (waiting_to_resume_play) {
@@ -426,34 +413,23 @@ static void car_adapter_mode_processing(void)
                 }
                 waiting_to_resume_play = false;
             }
-        }
-        else {
-            if (charger_power_is_on) {
-
-                /* if external power was turned off */
-                if (!charger_inserted()) { 
-
-                    charger_power_is_on = false;
-
-                    /* if playing */
-                    if ((mpeg_status() & MPEG_STATUS_PLAY) &&
-                        !(mpeg_status() & MPEG_STATUS_PAUSE)) {
-                        mpeg_pause(); 
-                    }
+        } else {
+            if (charger_input_state == CHARGER_UNPLUGGED) {
+                /*
+                 * Just got unplugged, pause if playing
+                 */
+                if ((mpeg_status() & MPEG_STATUS_PLAY) &&
+                    !(mpeg_status() & MPEG_STATUS_PAUSE)) {
+                    mpeg_pause(); 
                 }
-            }
-            else { 
-                /* if external power was turned on */
-                if (charger_inserted()) { 
-
-                    charger_power_is_on = true;
-
-                    /* if paused */
-                    if (mpeg_status() & MPEG_STATUS_PAUSE) {
-                        /* delay resume a bit while the engine is cranking */
-                        play_resume_time = current_tick + HZ*5;
-                        waiting_to_resume_play = true;
-                    }
+            } else if(charger_input_state == CHARGER_PLUGGED) { 
+                /*
+                 * Just got plugged in, delay & resume if we were playing
+                 */
+                if (mpeg_status() & MPEG_STATUS_PAUSE) {
+                    /* delay resume a bit while the engine is cranking */
+                    play_resume_time = current_tick + HZ*5;
+                    waiting_to_resume_play = true;
                 }
             }
         }
@@ -509,14 +485,43 @@ static void power_thread_rtc_process(void)
 static void power_thread_sleep(int ticks)
 {
     int small_ticks;
-#ifdef HAVE_CHARGING
-    bool charger_plugged;
-#endif
+
+    while (ticks > 0) {
 
 #ifdef HAVE_CHARGING
-    charger_plugged = charger_inserted();
+        /*
+         * Detect charger plugged/unplugged transitions.  On a plugged or
+         * unplugged event, we return immediately, run once through the main
+         * loop (including the subroutines), and end up back here where we
+         * transition to the appropriate steady state charger on/off state.
+         */
+        if(charger_inserted()) {
+            switch(charger_input_state) {
+                case NO_CHARGER:
+                case CHARGER_UNPLUGGED:
+                    charger_input_state = CHARGER_PLUGGED;
+                    return;
+                case CHARGER_PLUGGED:
+                    charger_input_state = CHARGER;
+                    break;
+                case CHARGER:
+                    break;
+            }
+        } else {    /* charger not inserted */
+            switch(charger_input_state) {
+                case NO_CHARGER:
+                    break;
+                case CHARGER_UNPLUGGED:
+                    charger_input_state = NO_CHARGER;
+                    break;
+                case CHARGER_PLUGGED:
+                case CHARGER:
+                    charger_input_state = CHARGER_UNPLUGGED;
+                    return;
+            }
+        }
 #endif
-    while (ticks > 0) {
+
         small_ticks = MIN(HZ/2, ticks);
         sleep(small_ticks);
         ticks -= small_ticks;
@@ -532,8 +537,6 @@ static void power_thread_sleep(int ticks)
          * Do a digital exponential filter.  We don't sample the battery if
          * the disk is spinning unless we are in USB mode (the disk will most
          * likely always be spinning in USB mode).
-         * If the charging voltage is greater than 0x3F0 charging isn't active
-         * and that voltage isn't valid.
          */
         if (!ata_disk_is_active() || usb_inserted()) {
             avgbat = avgbat - (avgbat / BATT_AVE_SAMPLES) +
@@ -541,13 +544,19 @@ static void power_thread_sleep(int ticks)
             /*
              * battery_centivolts is the centivolt-scaled filtered battery value.
              */
-            battery_centivolts = ((avgbat / BATT_AVE_SAMPLES) * BATTERY_SCALE_FACTOR) / 10000;
+            battery_centivolts = ((avgbat / BATT_AVE_SAMPLES) *
+                                   BATTERY_SCALE_FACTOR) / 10000;
+        }
+#if defined(DEBUG_FILE) && defined(HAVE_CHARGE_CTRL)
+        /*
+         * If we have a lot of pending writes or if the disk is spining,
+         * fsync the debug log file.
+         */
+        if((wrcount > 10) ||
+           ((wrcount > 0) && ata_disk_is_active())) {
+            fsync(fd);
+            wrcount = 0;
         }
-
-#ifdef HAVE_CHARGING
-        /* If the charger was plugged in, exit now so we can start charging */
-        if(!charger_plugged && charger_inserted())
-            return;
 #endif
     }
 }
@@ -568,17 +577,19 @@ static void power_thread(void)
     int charging_current;
 #endif
 #ifdef HAVE_CHARGE_CTRL
-    int charge_max_time_now = 0;  /* max. charging duration, calculated at
-                                     beginning of charging */
+    unsigned int target_voltage;    /* desired topoff/trickle voltage level */
+    int charge_max_time_now = 0;    /* max. charging duration, calculated at
+                                       beginning of charging */
 #endif
 
     /* initialize the voltages for the exponential filter */
-    
+
     avgbat = adc_read(ADC_UNREG_POWER) * BATT_AVE_SAMPLES;
-    battery_centivolts    = ((avgbat / BATT_AVE_SAMPLES) * BATTERY_SCALE_FACTOR) / 10000;
+    battery_centivolts = ((avgbat / BATT_AVE_SAMPLES) * BATTERY_SCALE_FACTOR) / 10000;
 
 #if defined(DEBUG_FILE) && defined(HAVE_CHARGE_CTRL)
-    fd = -1;
+    fd      = -1;
+    wrcount = 0;
 #endif
             
     while (1)
@@ -611,7 +622,6 @@ static void power_thread(void)
          * If we are charging, the "runtime" is estimated time till the battery
          * is recharged.
          */
-        // TBD: use real charging current estimate
         if (charge_state == CHARGING) {
             powermgmt_est_runningtime_min = (100 - battery_level()) *
                  battery_capacity / 100 * 60 / (CURRENT_MAX_CHG - runcurrent());
@@ -638,185 +648,200 @@ static void power_thread(void)
 #endif /* # if CONFIG_BATTERY == BATT_LIION2200 */
 
 #ifdef HAVE_CHARGE_CTRL
-
-        if (charger_inserted()) {
+        if (charger_input_state == CHARGER_PLUGGED) {
+            pid_p = 0;
+            pid_i = 0;
+            snprintf(power_message, POWER_MESSAGE_LEN, "Charger plugged in");
             /*
-             * Time to start charging again?
-             * 1) If we are currently discharging but trickle is enabled,
-             *    the charger must have just been plugged in.
-             * 2) If our battery level falls below the restart level, charge!
+             * The charger was just plugged in.  If the battery level is
+             * nearly charged, just trickle.  If the battery is low, start
+             * a full charge cycle.  If the battery level is in between,
+             * top-off and then trickle.
              */
-            if ((charge_state == DISCHARGING) ||
-                (battery_level() < CHARGE_RESTART)) {
-
+            if(battery_percent > START_TOPOFF_CHG) {
+                powermgmt_last_cycle_level = battery_percent;
+                powermgmt_last_cycle_startstop_min = 0;
+                if(battery_percent >= START_TRICKLE_CHG) {
+                    charge_state = TRICKLE;
+                } else {
+                    charge_state = TOPOFF;
+                } 
+            } else {
                 /*
-                 * If the battery level is nearly charged, just trickle.
-                 * If the battery is in between, top-off and then trickle.
+                 * Start the charger full strength
                  */
-                if(battery_percent > CHARGE_RESTART) {
-                    powermgmt_last_cycle_level = battery_percent;
-                    powermgmt_last_cycle_startstop_min = 0;
-                    if(battery_percent >= 95) {
-                        trickle_sec  = START_TRICKLE_SEC;
-                        charge_state = TRICKLE;
-                    } else {
-                        trickle_sec  = START_TOPOFF_SEC;
-                        charge_state = TOPOFF;
-                    }
-                } else {
-                    /*
-                     * Start the charger full strength
-                     */
-                    i = CHARGE_MAX_TIME_1500 * battery_capacity / 1500;
-                    charge_max_time_now =
-                        i * (100 + 35 - battery_percent) / 100;
-                    if (charge_max_time_now > i) {
-                        charge_max_time_now = i;
-                    }
-                    snprintf(power_message, POWER_MESSAGE_LEN,
-                             "ChgAt %d%% max %dm", battery_level(),
-                             charge_max_time_now);
-    
-                    /* enable the charger after the max time calc is done,
-                       because battery_level depends on if the charger is
-                       on */
-                    DEBUGF("power: charger inserted and battery"
-                           " not full, enabling\n");
-                    powermgmt_last_cycle_level = battery_percent;
-                    powermgmt_last_cycle_startstop_min = 0;
-                    trickle_sec  = 60;
-                    charge_state = CHARGING;
+                i = CHARGE_MAX_TIME_1500 * battery_capacity / 1500;
+                charge_max_time_now =
+                    i * (100 + 35 - battery_percent) / 100;
+                if (charge_max_time_now > i) {
+                    charge_max_time_now = i;
                 }
-            }
-            if (charge_state == CHARGING) {
-                /* charger inserted and enabled 100% of the time */
-                trickle_sec = 60; /* 100% on */
-
                 snprintf(power_message, POWER_MESSAGE_LEN,
-                         "Chg %dm, max %dm", powermgmt_last_cycle_startstop_min,
+                         "ChgAt %d%% max %dm", battery_level(),
                          charge_max_time_now);
-                /*
-                 * Sum the deltas over the last X minutes so we can do our
-                 *   end-of-charge logic based on the battery level change.
-                 */
-                long_delta = short_delta = 999999;
-                if (powermgmt_last_cycle_startstop_min > CHARGE_MIN_TIME) {
-                    short_delta = power_history[0] -
-                                  power_history[CHARGE_END_NEGD - 1];
-                }
-                if (powermgmt_last_cycle_startstop_min > CHARGE_END_ZEROD) {
-                /*
-                 * Scan the history: if we have a big delta in the middle of
-                 * our history, the long term delta isn't a valid end-of-charge
-                 * indicator.
-                 */
-                    long_delta = power_history[0] -
-                                 power_history[CHARGE_END_ZEROD - 1];
-                    for(i = 0; i < CHARGE_END_ZEROD; i++) {
-                        if(((power_history[i] - power_history[i+1]) >  5) ||
-                           ((power_history[i] - power_history[i+1]) < -5)) {
-                            long_delta = 888888;
-                            break;
-                        }
+
+                /* enable the charger after the max time calc is done,
+                   because battery_level depends on if the charger is
+                   on */
+                DEBUGF("power: charger inserted and battery"
+                       " not full, charging\n");
+                powermgmt_last_cycle_level = battery_percent;
+                powermgmt_last_cycle_startstop_min = 0;
+                trickle_sec  = 60;
+                long_delta   = short_delta = 999999;
+                charge_state = CHARGING;
+            }
+        }
+        if (charge_state == CHARGING) {
+            snprintf(power_message, POWER_MESSAGE_LEN,
+                     "Chg %dm, max %dm", powermgmt_last_cycle_startstop_min,
+                     charge_max_time_now);
+            /*
+             * Check the delta voltage over the last X minutes so we can do
+             *   our end-of-charge logic based on the battery level change.
+             */
+            if (powermgmt_last_cycle_startstop_min > CHARGE_MIN_TIME) {
+                short_delta = power_history[0] -
+                              power_history[CHARGE_END_NEGD - 1];
+            }
+            if (powermgmt_last_cycle_startstop_min > CHARGE_END_ZEROD) {
+            /*
+             * Scan the history: if we have a big delta in the middle of
+             * our history, the long term delta isn't a valid end-of-charge
+             * indicator.
+             */
+                long_delta = power_history[0] -
+                             power_history[CHARGE_END_ZEROD - 1];
+                for(i = 0; i < CHARGE_END_ZEROD; i++) {
+                    if(((power_history[i] - power_history[i+1]) >  5) ||
+                       ((power_history[i] - power_history[i+1]) < -5)) {
+                        long_delta = 888888;
+                        break;
                     }
                 }
+            }
 
-                /*
-                 * End of charge criteria (any qualify):
-                 * 1) Charged a long time
-                 * 2) DeltaV went negative for a short time
-                 * 3) DeltaV was close to zero for a long time
-                 * Note: short_delta and long_delta are centivolts
-                 */
-                if ((powermgmt_last_cycle_startstop_min > charge_max_time_now) ||
-                    (short_delta < -5) || (long_delta  <  5))
-                {
-                    if (powermgmt_last_cycle_startstop_min > charge_max_time_now) {
-                        DEBUGF("power: powermgmt_last_cycle_startstop_min > charge_max_time_now, "
-                               "enough!\n");
-                        /* have charged too long and deltaV detection did not
-                           work! */
+            /*
+             * End of charge criteria (any qualify):
+             * 1) Charged a long time
+             * 2) DeltaV went negative for a short time
+             * 3) DeltaV was close to zero for a long time
+             * Note: short_delta and long_delta are centivolts
+             */
+            if ((powermgmt_last_cycle_startstop_min > charge_max_time_now) ||
+                (short_delta <= -5) || (long_delta  <  5))
+            {
+                if (powermgmt_last_cycle_startstop_min > charge_max_time_now) {
+                    DEBUGF("power: powermgmt_last_cycle_startstop_min > charge_max_time_now, "
+                           "enough!\n");
+                    /* have charged too long and deltaV detection did not
+                       work! */
+                    snprintf(power_message, POWER_MESSAGE_LEN,
+                             "Chg tmout %d min", charge_max_time_now);
+                    /*
+                     * Switch to trickle charging.  We skip the top-off
+                     * since we've effectively done the top-off operation
+                     * already since we charged for the maximum full
+                     * charge time.
+                     */
+                    powermgmt_last_cycle_level = battery_percent;
+                    powermgmt_last_cycle_startstop_min = 0;
+                    charge_state = TRICKLE;
+                } else {
+                    if(short_delta <= -5) {
+                        DEBUGF("power: short-term negative"
+                               " delta, enough!\n");
                         snprintf(power_message, POWER_MESSAGE_LEN,
-                                 "Chg tmout %d min", charge_max_time_now);
+                                 "end negd %d %dmin", short_delta,
+                                 powermgmt_last_cycle_startstop_min);
                     } else {
-                        if(short_delta < -5) {
-                            DEBUGF("power: short-term negative"
-                                   " delta, enough!\n");
-                            snprintf(power_message, POWER_MESSAGE_LEN,
-                                     "end negd %d %dmin", short_delta,
-                                     powermgmt_last_cycle_startstop_min);
-                        } else {
-                            DEBUGF("power: long-term small "
-                                   "positive delta, enough!\n");
-                            snprintf(power_message, POWER_MESSAGE_LEN,
-                                     "end lowd %d %dmin", long_delta,
-                                     powermgmt_last_cycle_startstop_min);
-                        }
+                        DEBUGF("power: long-term small "
+                               "positive delta, enough!\n");
+                        snprintf(power_message, POWER_MESSAGE_LEN,
+                                 "end lowd %d %dmin", long_delta,
+                                 powermgmt_last_cycle_startstop_min);
                     }
-                    /* Switch to trickle charging.  We skip the top-off
-                       since we've effectively done the top-off operation
-                       already since we charged for the maximum full
-                       charge time.  For trickle charging, we use 0.05C */
+                    /*
+                     * Switch to top-off charging.
+                     */
                     powermgmt_last_cycle_level = battery_percent;
                     powermgmt_last_cycle_startstop_min = 0;
-
-                    trickle_sec  = START_TRICKLE_SEC;
-                    charge_state = TRICKLE;
+                    charge_state = TOPOFF;
                 }
             }
-            else if (charge_state > CHARGING)  /* top off or trickle */
+        }
+        else if (charge_state > CHARGING)  /* top off or trickle */
+        {
+            /* Time to switch from topoff to trickle?
+             */
+            if ((charge_state == TOPOFF) &&
+                (powermgmt_last_cycle_startstop_min > TOPOFF_MAX_TIME))
             {
-                /* Time to switch from topoff to trickle? Note that we don't
-                 * adjust trickle_sec: it will get adjusted down by the
-                 * charge level adjustment in the loop and will drift down
-                 * from the topoff level to the trickle level.
-                 */
-                if ((charge_state == TOPOFF) &&
-                    (powermgmt_last_cycle_startstop_min > TOPOFF_MAX_TIME))
-                {
-                    powermgmt_last_cycle_level = battery_percent;
-                    powermgmt_last_cycle_startstop_min = 0;
-                    charge_state = TRICKLE;
-                }
-
-                /* Adjust trickle charge time.  I considered setting the level
-                 * higher if the USB is plugged in, but it doesn't appear to
-                 * be necessary and will generate more heat [gvb].
-                 */
-                if(((charge_state == TOPOFF)  && (battery_centivolts > TOPOFF_VOLTAGE)) ||
-                   ((charge_state == TRICKLE) && (battery_centivolts > TRICKLE_VOLTAGE)))
-                { /* charging too much */
-                    if(trickle_sec > 0)
-                        trickle_sec--;
+                powermgmt_last_cycle_level = battery_percent;
+                powermgmt_last_cycle_startstop_min = 0;
+                charge_state = TRICKLE;
+            }
+            /*
+             * Adjust trickle charge time (proportional and integral terms).
+             * Note: I considered setting the level higher if the USB is
+             * plugged in, but it doesn't appear to be necessary and will
+             * generate more heat [gvb].
+             */
+            if(charge_state == TOPOFF)
+                target_voltage = TOPOFF_VOLTAGE;
+            else
+                target_voltage = TRICKLE_VOLTAGE;
+            
+            pid_p = target_voltage - battery_centivolts;
+            if((pid_p > PID_DEADZONE) || (pid_p < -PID_DEADZONE))
+                pid_p = pid_p * PID_PCONST;
+            else
+                pid_p = 0;
+            if(battery_centivolts < target_voltage) {
+                if(pid_i < 60) {
+                    pid_i++;        /* limit so it doesn't "wind up" */
                 }
-                else { /* charging too little */
-                    if(trickle_sec < 60)
-                        trickle_sec++;
+            } else {
+                if(pid_i > 0) {
+                    pid_i--;        /* limit so it doesn't "wind up" */
                 }
+            }
+
+            trickle_sec = pid_p + pid_i;
 
-            } else if (charge_state == DISCHARGING) {
+            if(trickle_sec > 60) {
+                trickle_sec = 60;
+            }
+            if(trickle_sec < 0) {
                 trickle_sec = 0;
-                /* the charger is enabled here only in one case: if it was
-                   turned on at boot time (power_init) */
-                /* turn it off now */
-                if (charger_enabled)
-                    charger_enable(false);
             }
-        } else {
-            if (charge_state != DISCHARGING) {
-                /* charger not inserted but was enabled */
-                DEBUGF("power: charger disconnected, disabling\n");
 
+        } else if (charge_state == DISCHARGING) {
+            trickle_sec = 0;
+            /*
+             * The charger is enabled here only in one case: if it was
+             * turned on at boot time (power_init).  Turn it off now.
+             */
+            if (charger_enabled)
                 charger_enable(false);
-                powermgmt_last_cycle_level = battery_percent;
-                powermgmt_last_cycle_startstop_min = 0;
-                trickle_sec = 0;
-                charge_state = DISCHARGING;
-                snprintf(power_message, POWER_MESSAGE_LEN, "Charger: discharge");
-            }
         }
-        powermgmt_last_cycle_startstop_min++;
-        
+
+        if (charger_input_state == CHARGER_UNPLUGGED) {
+            /*
+             * The charger was just unplugged.
+             */
+            DEBUGF("power: charger disconnected, disabling\n");
+
+            charger_enable(false);
+            powermgmt_last_cycle_level = battery_percent;
+            powermgmt_last_cycle_startstop_min = 0;
+            trickle_sec  = 0;
+            pid_p        = 0;
+            pid_i        = 0;
+            charge_state = DISCHARGING;
+            snprintf(power_message, POWER_MESSAGE_LEN, "Charger: discharge");
+        }
+
 #endif /* HAVE_CHARGE_CTRL*/
 
         /* sleep for a minute */
@@ -834,25 +859,36 @@ static void power_thread(void)
 #endif
 
 #if defined(DEBUG_FILE) && defined(HAVE_CHARGE_CTRL)
-        if((fd < 0) && !usb_inserted()) {
-            fd = open(DEBUG_FILE_NAME, O_WRONLY | O_APPEND | O_CREAT);
-            snprintf(debug_message, DEBUG_MESSAGE_LEN,
-            "cycle_min, bat_centivolts, bat_percent, chgr, chg_state, trickle_sec\n");
-            write(fd, debug_message, strlen(debug_message));
-            fsync(fd);
-        } else if((fd >= 0) && !usb_inserted()) {
-            snprintf(debug_message, DEBUG_MESSAGE_LEN, "%d, %d, %d, %d, %d, %d\n",
-                powermgmt_last_cycle_startstop_min, battery_centivolts,
-                battery_percent, charger_inserted(), charge_state, trickle_sec);
-            write(fd, debug_message, strlen(debug_message));
-            fsync(fd);
-        } else if((fd >= 0) && usb_inserted()) {
-            /* NOTE: It is probably already TOO LATE to close the file */
-            close(fd);
-            fd = -1;
+        if(usb_inserted()) {
+            if(fd >= 0) {
+                /* It is probably too late to close the file but we can try... */
+                close(fd);
+                fd = -1;
+            }
+        } else {
+            if(fd < 0) {
+                fd = open(DEBUG_FILE_NAME, O_WRONLY | O_APPEND | O_CREAT);
+                if(fd >= 0) {
+                    snprintf(debug_message, DEBUG_MESSAGE_LEN,
+                    "cycle_min, bat_centivolts, bat_percent, chgr_state, charge_state, pid_p, pid_i, trickle_sec\n");
+                    write(fd, debug_message, strlen(debug_message));
+                    wrcount = 99;   /* force a flush */
+                }
+            }
+            if(fd >= 0) {
+                snprintf(debug_message, DEBUG_MESSAGE_LEN, "%d, %d, %d, %d, %d, %d, %d, %d\n",
+                    powermgmt_last_cycle_startstop_min, battery_centivolts,
+                    battery_percent, charger_input_state, charge_state, pid_p, pid_i, trickle_sec);
+                write(fd, debug_message, strlen(debug_message));
+                wrcount++;
+            }
         }
 #endif
         handle_auto_poweroff();
+
+#ifdef HAVE_CHARGE_CTRL
+        powermgmt_last_cycle_startstop_min++;
+#endif
     }
 }
 
@@ -862,10 +898,6 @@ void powermgmt_init(void)
     
     /* init history to 0 */
     memset(power_history, 0x00, sizeof(power_history));
-
-#ifdef HAVE_CHARGING
-    charger_power_is_on = charger_inserted();
-#endif
     
     create_thread(power_thread, power_stack, sizeof(power_stack),
                   power_thread_name);
@@ -876,11 +908,10 @@ void powermgmt_init(void)
 /* Various hardware housekeeping tasks relating to shutting down the jukebox */
 void shutdown_hw(void) 
 {
-#ifndef SIMULATOR
 #if defined(DEBUG_FILE) && defined(HAVE_CHARGE_CTRL)
-    if(fd > 0) {
+    if(fd >= 0) {
         close(fd);
-        fd = 0;
+        fd = -1;
     }
 #endif
     mpeg_stop();
@@ -899,5 +930,4 @@ void shutdown_hw(void)
     lcd_set_contrast(0);
 #endif
     power_off();
-#endif
 }