Merge branch 'master' of https://github.com/ChibiOS/ChibiOS-Contrib

5 files changed, 285 insertions, 12 deletions

diff --git a/os/various/fatfs_bindings/fatfs.mk b/os/various/fatfs_bindings/fatfs.mk
index e3ab0c7..02ebed5 100644
--- a/os/various/fatfs_bindings/fatfs.mk
+++ b/os/various/fatfs_bindings/fatfs.mk
@@ -2,6 +2,6 @@
 FATFSSRC = ${CHIBIOS_CONTRIB}/os/various/fatfs_bindings/fatfs_diskio.c \
            ${CHIBIOS}/os/various/fatfs_bindings/fatfs_syscall.c \
            ${CHIBIOS}/ext/fatfs/src/ff.c \
-           ${CHIBIOS}/ext/fatfs/src/option/unicode.c
+           $(CHIBIOS)/ext/fatfs/src/ffunicode.c
 
 FATFSINC = ${CHIBIOS}/ext/fatfs/src ${CHIBIOS_CONTRIB}/os/various/fatfs_bindings
diff --git a/os/various/fatfs_bindings/fatfs_diskio.c b/os/various/fatfs_bindings/fatfs_diskio.c
index 85b07df..3d068f3 100644
--- a/os/various/fatfs_bindings/fatfs_diskio.c
+++ b/os/various/fatfs_bindings/fatfs_diskio.c
@@ -58,7 +58,7 @@ DSTATUS disk_initialize (
 /* Return Disk Status                                                    */
 
 DSTATUS disk_status (
-    BYTE pdrv        /* Physical drive nmuber (0..) */
+    BYTE pdrv         /* Physical drive number (0..) */
 )
 {
   DSTATUS stat;
@@ -101,9 +101,9 @@ DSTATUS disk_status (
 /* Read Sector(s)                                                        */
 
 DRESULT disk_read (
-    BYTE pdrv,        /* Physical drive nmuber (0..) */
-    BYTE *buff,        /* Data buffer to store read data */
-    DWORD sector,    /* Sector address (LBA) */
+    BYTE pdrv,        /* Physical drive number (0..) */
+    BYTE *buff,       /* Data buffer to store read data */
+    DWORD sector,     /* Sector address (LBA) */
     UINT count        /* Number of sectors to read (1..255) */
 )
 {
@@ -150,10 +150,10 @@ DRESULT disk_read (
 /* Write Sector(s)                                                       */
 
 DRESULT disk_write (
-    BYTE pdrv,            /* Physical drive nmuber (0..) */
-    const BYTE *buff,    /* Data to be written */
-    DWORD sector,        /* Sector address (LBA) */
-    UINT count            /* Number of sectors to write (1..255) */
+    BYTE pdrv,        /* Physical drive number (0..) */
+    const BYTE *buff, /* Data to be written */
+    DWORD sector,     /* Sector address (LBA) */
+    UINT count        /* Number of sectors to write (1..255) */
 )
 {
   switch (pdrv) {
@@ -201,8 +201,8 @@ DRESULT disk_write (
 /* Miscellaneous Functions                                               */
 
 DRESULT disk_ioctl (
-    BYTE pdrv,        /* Physical drive nmuber (0..) */
-    BYTE cmd,        /* Control code */
+    BYTE pdrv,        /* Physical drive number (0..) */
+    BYTE cmd,         /* Control code */
     void *buff        /* Buffer to send/receive control data */
 )
 {
diff --git a/os/various/lib_scsi.c b/os/various/lib_scsi.c
index ea2adda..720a90f 100644
--- a/os/various/lib_scsi.c
+++ b/os/various/lib_scsi.c
@@ -364,7 +364,8 @@ static bool data_read_write10(SCSITarget *scsip, const uint8_t *cmd) {
     size_t bs = bdi.blk_size;
     uint8_t *buf = scsip->config->blkbuf;
 
-    for (size_t i=0; i<req.blk_cnt; i++) {
+    size_t i = 0;
+    for (i=0; i<req.blk_cnt; i++) {
       if (cmd[0] == SCSI_CMD_READ_10) {
         // TODO: block error handling
         blkRead(blkdev, req.first_lba + i, buf, 1);
diff --git a/os/various/pid.c b/os/various/pid.c
new file mode 100644
index 0000000..fee9608
--- /dev/null
+++ b/os/various/pid.c
@@ -0,0 +1,194 @@
+/**********************************************************************************************
+* Arduino PID Library - Version 1.2.1
+* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
+* Modified by Fabien Poussin <fabien.poussin@gmail.com> for ChibiOS.
+*
+* This Library is licensed under the MIT License
+**********************************************************************************************/
+
+#include "pid.h"
+#include "osal.h"
+
+#define TIME_MS ((osalOsGetSystemTimeX() * 1000) / OSAL_ST_FREQUENCY )
+
+/*Constructor (...)*********************************************************
+*    The parameters specified here are those for for which we can't set up
+*    reliable defaults, so we need to have the user set them.
+***************************************************************************/
+void pid_create(pidc_t* p, float* Input, float* Output, float* Setpoint,
+        float Kp, float Ki, float Kd, int POn, int Direction)
+{
+    p->output = Output;
+    p->input = Input;
+    p->setPoint = Setpoint;
+    p->inAuto = false;
+
+    pid_setOutputLimits(p, 0, 4095); // default output limit corresponds to
+                                     // the 12 bit dac limit
+
+    p->sampleTime = 100; // default Controller Sample Time is 100ms
+
+    pid_setDirection(p, Direction);
+    pid_setTunings(p, Kp, Ki, Kd, POn);
+    pid_initialize(p);
+
+    p->lastTime = TIME_MS - p->sampleTime;
+}
+
+
+/* Compute() **********************************************************************
+*   This, as they say, is where the magic happens.  this function should be called
+*   every time "void loop()" executes.  the function will decide for itself whether a new
+*   pid Output needs to be computed.  returns true when the output is computed,
+*   false when nothing has been done.
+**********************************************************************************/
+bool pid_compute(pidc_t* p)
+{
+    if(!p->inAuto) return false;
+    unsigned long now = TIME_MS;
+    unsigned long timeChange = (now - p->lastTime);
+    if(timeChange >= p->sampleTime)
+    {
+        /* Compute all the working error variables */
+        float input = *p->input;
+        float error = *p->setPoint - input;
+        float dInput = (input - p->lastInput);
+        p->outputSum += (p->ki * error);
+
+        /* Add Proportional on Measurement, if PID_ON_M is specified */
+        if(!p->pOnE) p->outputSum -= p->kp * dInput;
+
+        if(p->outputSum > p->outMax) p->outputSum = p->outMax;
+        else if(p->outputSum < p->outMin) p->outputSum = p->outMin;
+
+        /* Add Proportional on Error, if P_ON_E is specified */
+        float output;
+        if(p->pOnE) output = p->kp * error;
+        else output = 0;
+
+        /* Compute Rest of PID Output */
+        output += p->outputSum - p->kd * dInput;
+
+        if(output > p->outMax) output = p->outMax;
+        else if(output < p->outMin) output = p->outMin;
+            *p->output = output;
+
+        /* Remember some variables for next time */
+        p->lastInput = input;
+        p->lastTime = now;
+        return true;
+    }
+    else return false;
+}
+
+/* SetTunings(...)*************************************************************
+* This function allows the controller's dynamic performance to be adjusted.
+* it's called automatically from the constructor, but tunings can also
+* be adjusted on the fly during normal operation
+******************************************************************************/
+void pid_setTunings(pidc_t* p, float Kp, float Ki, float Kd, int POn)
+{
+    if (Kp < 0 || Ki < 0 || Kd < 0) return;
+
+    p->pOn = POn;
+    p->pOnE = POn == PID_ON_E;
+
+    p->dispKp = Kp;
+    p->dispKi = Ki;
+    p->dispKd = Kd;
+
+    float SampleTimeInSec = ((float)p->sampleTime) / 1000.0;
+    p->kp = Kp;
+    p->ki = Ki * SampleTimeInSec;
+    p->kd = Kd / SampleTimeInSec;
+
+    if(p->direction == PID_REVERSE)
+    {
+        p->kp = (0 - p->kp);
+        p->ki = (0 - p->ki);
+        p->kd = (0 - p->kd);
+    }
+}
+
+/* SetSampleTime(...) *********************************************************
+* sets the period, in Milliseconds, at which the calculation is performed
+******************************************************************************/
+void pid_setSampleTime(pidc_t* p, int NewSampleTime)
+{
+    if (NewSampleTime > 0)
+    {
+        float ratio  = (float)NewSampleTime / (float)p->sampleTime;
+        p->ki *= ratio;
+        p->kd /= ratio;
+        p->sampleTime = (unsigned long)NewSampleTime;
+    }
+}
+
+/* SetOutputLimits(...)****************************************************
+*     This function will be used far more often than SetInputLimits.  while
+*  the input to the controller will generally be in the 0-1023 range (which is
+*  the default already,)  the output will be a little different.  maybe they'll
+*  be doing a time window and will need 0-8000 or something.  or maybe they'll
+*  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
+*  here.
+**************************************************************************/
+void pid_setOutputLimits(pidc_t* p, float Min, float Max)
+{
+    if(Min >= Max) return;
+    p->outMin = Min;
+    p->outMax = Max;
+
+    if(p->inAuto)
+    {
+        if(*p->output > p->outMax) *p->output = p->outMax;
+        else if(*p->output < p->outMin) *p->output = p->outMin;
+
+        if(p->outputSum > p->outMax) p->outputSum = p->outMax;
+        else if(p->outputSum < p->outMin) p->outputSum = p->outMin;
+    }
+}
+
+/* SetMode(...)****************************************************************
+* Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
+* when the transition from manual to auto occurs, the controller is
+* automatically initialized
+******************************************************************************/
+void pid_setMode(pidc_t* p, int Mode)
+{
+    bool newAuto = (Mode == PID_AUTOMATIC);
+    if(newAuto && !p->inAuto)
+    {  /* we just went from manual to auto */
+        pid_initialize(p);
+    }
+    p->inAuto = newAuto;
+}
+
+/* Initialize()****************************************************************
+*	does all the things that need to happen to ensure a bumpless transfer
+*  from manual to automatic mode.
+******************************************************************************/
+void pid_initialize(pidc_t* p)
+{
+    p->outputSum = *p->output;
+    p->lastInput = *p->input;
+    if(p->outputSum > p->outMax) p->outputSum = p->outMax;
+    else if(p->outputSum < p->outMin) p->outputSum = p->outMin;
+}
+
+/* SetControllerDirection(...)*************************************************
+* The PID will either be connected to a DIRECT acting process (+Output leads
+* to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
+* know which one, because otherwise we may increase the output when we should
+* be decreasing.  This is called from the constructor.
+******************************************************************************/
+void pid_setDirection(pidc_t* p, int Direction)
+{
+    if(p->inAuto && Direction != p->direction)
+    {
+        p->kp = (0 - p->kp);
+        p->ki = (0 - p->ki);
+        p->kd = (0 - p->kd);
+    }
+    p->direction = Direction;
+}
+
diff --git a/os/various/pid.h b/os/various/pid.h
new file mode 100644
index 0000000..49ccd6f
--- /dev/null
+++ b/os/various/pid.h
@@ -0,0 +1,78 @@
+#ifndef PID_h
+#define PID_h
+
+#include "chtypes.h"
+
+//Constants used in some of the functions below
+#define PID_AUTOMATIC 1
+#define PID_MANUAL 0
+#define PID_DIRECT 0
+#define PID_REVERSE 1
+#define PID_ON_M 0
+#define PID_ON_E 1
+
+
+typedef struct {
+
+    float kp;     // * (P)roportional Tuning Parameter
+    float ki;     // * (I)ntegral Tuning Parameter
+    float kd;     // * (D)erivative Tuning Parameter
+
+    float dispKp; // * we'll hold on to the tuning parameters in user-entered 
+    float dispKi; //   format for display purposes
+    float dispKd; //
+
+    int direction;
+    int pOn;
+
+    float *input;              // * Pointers to the Input, Output, and Setpoint variables
+    float *output;             //   This creates a hard link between the variables and the 
+    float *setPoint;           //   PID, freeing the user from having to constantly tell us
+                               //   what these values are.  with pointers we'll just know.
+    unsigned long lastTime;
+    float outputSum;
+    float lastInput;
+
+    unsigned long sampleTime;
+    float outMin;
+    float outMax;
+
+    bool inAuto;
+    bool pOnE;
+
+} pidc_t;
+
+
+//commonly used functions **************************************************************************
+void pid_create(pidc_t* p, float* Input, float* Output, float* Setpoint,  // * constructor.  links the PID to the Input, Output, and 
+                float Kp, float Ki, float Kd, int POn, int Direction);    //   Setpoint.  Initial tuning parameters are also set here.
+                                                                          //   (overload for specifying proportional mode)
+
+void pid_setMode(pidc_t* p, int mode);                 // * sets PID to either Manual (0) or Auto (non-0)
+
+bool pid_compute(pidc_t* p);               // * performs the PID calculation.  it should be
+                                           //   called every time loop() cycles. ON/OFF and
+                                           //   calculation frequency can be set using SetMode
+                                           //   SetsampleTime respectively
+
+void pid_setOutputLimits(pidc_t* p, float Min, float Max); // * clamps the output to a specific range. 0-255 by default, but
+                                                           //   it's likely the user will want to change this depending on
+                                                           //   the application
+
+
+
+//available but not commonly used functions ********************************************************
+void pid_setTunings(pidc_t* p, float Kp, float Ki, float Kd, int POn);  // * While most users will set the tunings once in the 
+                                                                        //   constructor, this function gives the user the option
+                                                                        //   of changing tunings during runtime for Adaptive control
+
+void pid_setDirection(pidc_t* p, int Direction);  // * Sets the Direction, or "Action" of the controller. DIRECT
+                                                  //   means the output will increase when error is positive. REVERSE
+                                                  //   means the opposite.  it's very unlikely that this will be needed
+                                                  //   once it is set in the constructor.
+void pid_setSampleTime(pidc_t* p, int NewSampleTime);  // * sets the frequency, in Milliseconds, with which 
+                                                       //   the PID calculation is performed.  default is 100
+
+void pid_initialize(pidc_t* p);
+
+#endif