Repo unification
This commit is contained in:
67
firmware/src/hw/board/utils/faults.c
Normal file
67
firmware/src/hw/board/utils/faults.c
Normal file
@@ -0,0 +1,67 @@
|
||||
/**** Includes ****/
|
||||
#include "faults.h"
|
||||
#include "utils.h"
|
||||
|
||||
/**** Private definitions ****/
|
||||
/**** Private constants ****/
|
||||
/**** Private variables ****/
|
||||
/**** Mapping function declarations ****/
|
||||
/**** Private function declarations ****/
|
||||
/**** Public function definitions ****/
|
||||
#ifdef TESTING
|
||||
#endif
|
||||
|
||||
uint8_t fault_process(fault_t* fault, uint8_t w_trig, uint8_t f_trig, fault_cfg_t* cfg)
|
||||
{
|
||||
// Override warning trigger in case of fault trigger
|
||||
if(f_trig) w_trig = 1;
|
||||
|
||||
// Increase warning time, if reoccurring
|
||||
if((w_trig)&&(fault->severity != FAULT_LVL_OK)) fault->w_time = util_sat_add_16b(fault->w_time, 1);
|
||||
else fault->w_time = 0;
|
||||
|
||||
// Check if waring can be considered fault
|
||||
if((cfg->wtof > 0)&&(fault->w_time > cfg->wtof)) f_trig = 1;
|
||||
|
||||
// Increase fault time, if reoccurring
|
||||
if((f_trig)&&(fault->severity != FAULT_LVL_OK)) fault->f_time = util_sat_add_16b(fault->f_time, 1);
|
||||
else fault->f_time = 0;
|
||||
|
||||
// Modify fault trigger
|
||||
if((cfg->delay)&&(fault->f_time < cfg->delay)) f_trig = 0;
|
||||
|
||||
// Process fault level
|
||||
if(f_trig)
|
||||
{
|
||||
fault->severity = FAULT_LVL_FAULT;
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(w_trig) fault->severity = FAULT_LVL_WARNING;
|
||||
else fault->severity = FAULT_LVL_OK;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint8_t fault_is_active(fault_t* fault)
|
||||
{
|
||||
if(fault->severity == FAULT_LVL_FAULT) return 1;
|
||||
else return 0;
|
||||
}
|
||||
|
||||
uint8_t fault_is_warning(fault_t* fault)
|
||||
{
|
||||
if(fault->severity != FAULT_LVL_OK) return 1;
|
||||
else return 0;
|
||||
}
|
||||
|
||||
void fault_reset(fault_t* fault)
|
||||
{
|
||||
fault->severity = FAULT_LVL_OK;
|
||||
fault->w_time = 0;
|
||||
fault->f_time = 0;
|
||||
}
|
||||
|
||||
/**** Private function definitions ****/
|
||||
37
firmware/src/hw/board/utils/faults.h
Normal file
37
firmware/src/hw/board/utils/faults.h
Normal file
@@ -0,0 +1,37 @@
|
||||
#ifndef FAULTS_H_
|
||||
#define FAULTS_H_
|
||||
|
||||
/*
|
||||
*/
|
||||
|
||||
/**** Includes ****/
|
||||
#include <stdint.h>
|
||||
|
||||
/**** Public definitions ****/
|
||||
typedef enum {
|
||||
FAULT_LVL_OK,
|
||||
FAULT_LVL_WARNING,
|
||||
FAULT_LVL_FAULT
|
||||
} fault_lvl_t;
|
||||
|
||||
typedef struct {
|
||||
fault_lvl_t severity;
|
||||
uint16_t w_time;
|
||||
uint16_t f_time;
|
||||
} fault_t;
|
||||
|
||||
typedef struct {
|
||||
uint16_t delay;
|
||||
uint16_t wtof;
|
||||
} fault_cfg_t;
|
||||
|
||||
/**** Public function declarations ****/
|
||||
uint8_t fault_process(fault_t* fault, uint8_t w_trig, uint8_t f_trig, fault_cfg_t* cfg);
|
||||
uint8_t fault_is_active(fault_t* fault);
|
||||
uint8_t fault_is_warning(fault_t* fault);
|
||||
void fault_reset(fault_t* fault);
|
||||
|
||||
#ifdef TESTING
|
||||
#endif
|
||||
|
||||
#endif /* FAULTS_H_ */
|
||||
81
firmware/src/hw/board/utils/fuses.c
Normal file
81
firmware/src/hw/board/utils/fuses.c
Normal file
@@ -0,0 +1,81 @@
|
||||
/**** Includes ****/
|
||||
#include "fuses.h"
|
||||
#include "utils.h"
|
||||
|
||||
/**** Private definitions ****/
|
||||
/**** Private constants ****/
|
||||
/**** Private variables ****/
|
||||
/**** Mapping function declarations ****/
|
||||
/**** Private function declarations ****/
|
||||
/**** Public function definitions ****/
|
||||
#ifdef TESTING
|
||||
#endif
|
||||
|
||||
void fuse_reset(fuse_t* fuse)
|
||||
{
|
||||
fuse->state = FUSE_OFF;
|
||||
fuse->timer = 0;
|
||||
fuse->count = 0;
|
||||
}
|
||||
|
||||
uint8_t fuse_process(fuse_t* fuse, uint8_t fault, fuse_cfg_t* cfg)
|
||||
{
|
||||
// Active fault condition
|
||||
if(fault)
|
||||
{
|
||||
// Note fuse time count
|
||||
if((fuse->state == FUSE_OFF)||(fuse->state == FUSE_RETRY)) fuse->count++;
|
||||
|
||||
// Go to fused state in any case
|
||||
fuse->state = FUSE_ACTIVE;
|
||||
fuse->timer = 0;
|
||||
return 1;
|
||||
};
|
||||
|
||||
// No active fault condition
|
||||
if(fuse->state==FUSE_ACTIVE)
|
||||
{
|
||||
// Go to cooldown
|
||||
fuse->state = FUSE_COOLDOWN;
|
||||
fuse->timer = cfg->cooldown_time;
|
||||
};
|
||||
|
||||
// Wait for timeout
|
||||
if(fuse->timer)
|
||||
{
|
||||
fuse->timer--;
|
||||
if(fuse->state == FUSE_RETRY) return 0;
|
||||
else return 1;
|
||||
};
|
||||
|
||||
// Timeout end, transition logic
|
||||
switch(fuse->state)
|
||||
{
|
||||
case FUSE_COOLDOWN:
|
||||
// Cooldown end
|
||||
if(cfg->retry_time)
|
||||
{
|
||||
fuse->state = FUSE_RETRY;
|
||||
fuse->timer = cfg->retry_time;
|
||||
}
|
||||
else
|
||||
{
|
||||
fuse->state = FUSE_OFF;
|
||||
fuse->timer = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case FUSE_RETRY:
|
||||
// Go back to normal
|
||||
fuse->state = FUSE_OFF;
|
||||
break;
|
||||
|
||||
default:
|
||||
// Nothing to do
|
||||
break;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**** Private function definitions ****/
|
||||
36
firmware/src/hw/board/utils/fuses.h
Normal file
36
firmware/src/hw/board/utils/fuses.h
Normal file
@@ -0,0 +1,36 @@
|
||||
#ifndef FUSES_H_
|
||||
#define FUSES_H_
|
||||
|
||||
/*
|
||||
*/
|
||||
|
||||
/**** Includes ****/
|
||||
#include <stdint.h>
|
||||
|
||||
/**** Public definitions ****/
|
||||
typedef enum {
|
||||
FUSE_OFF,
|
||||
FUSE_ACTIVE,
|
||||
FUSE_COOLDOWN,
|
||||
FUSE_RETRY
|
||||
} fuse_state_t;
|
||||
|
||||
typedef struct {
|
||||
fuse_state_t state;
|
||||
uint8_t count;
|
||||
uint16_t timer;
|
||||
} fuse_t;
|
||||
|
||||
typedef struct {
|
||||
uint16_t cooldown_time;
|
||||
uint16_t retry_time;
|
||||
} fuse_cfg_t;
|
||||
|
||||
/**** Public function declarations ****/
|
||||
void fuse_reset(fuse_t* fuse);
|
||||
uint8_t fuse_process(fuse_t* fuse, uint8_t fault, fuse_cfg_t* cfg);
|
||||
|
||||
#ifdef TESTING
|
||||
#endif
|
||||
|
||||
#endif /* FUSES_H_ */
|
||||
272
firmware/src/hw/board/utils/utils.c
Normal file
272
firmware/src/hw/board/utils/utils.c
Normal file
@@ -0,0 +1,272 @@
|
||||
/**** Includes ****/
|
||||
#include "utils.h"
|
||||
|
||||
/**** Private definitions ****/
|
||||
/**** Private constants ****/
|
||||
/**** Private variables ****/
|
||||
/**** Private function declarations ****/
|
||||
#ifndef TESTING
|
||||
static uint8_t find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis);
|
||||
static uint16_t interpolate_u16b(uint16_t x, uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1);
|
||||
static uint16_t index2d_to_index1d(uint8_t ix, uint8_t iy, uint8_t len_x);
|
||||
#endif
|
||||
|
||||
/**** Public function definitions ****/
|
||||
uint8_t util_invert_8b(uint8_t x)
|
||||
{
|
||||
if(x!=0) return 0;
|
||||
else return 1;
|
||||
}
|
||||
|
||||
uint8_t util_sat_add_8b(uint8_t x, uint8_t y)
|
||||
{
|
||||
uint8_t z = x + y;
|
||||
// Check for overflow
|
||||
if((z < x)||(z < y)) return 0xFF;
|
||||
else return z;
|
||||
}
|
||||
|
||||
uint8_t util_sat_subtract_8b(uint8_t x, uint8_t y)
|
||||
{
|
||||
uint8_t z = x - y;
|
||||
// Check for underflow
|
||||
if(z > x) return 0;
|
||||
else return z;
|
||||
}
|
||||
|
||||
uint16_t util_sat_add_16b(uint16_t x, uint16_t y)
|
||||
{
|
||||
uint16_t z = x + y;
|
||||
// Check for overflow
|
||||
if((z < x)||(z < y)) return 0xFFFF;
|
||||
else return z;
|
||||
}
|
||||
|
||||
uint16_t util_sat_subtract_16b(uint16_t x, uint16_t y)
|
||||
{
|
||||
uint16_t z = x - y;
|
||||
// Check for underflow
|
||||
if(z > x) return 0;
|
||||
else return z;
|
||||
}
|
||||
|
||||
uint16_t util_limit_u32b_to_u16b(uint32_t in)
|
||||
{
|
||||
if(in == 0) return 0;
|
||||
else if(in >= 0x0000FFFF) return 0xFFFF;
|
||||
else return (uint16_t)in;
|
||||
}
|
||||
|
||||
uint16_t util_limit_s32b_to_u16b(int32_t in)
|
||||
{
|
||||
if(in <= 0) return 0;
|
||||
else if(in >= 0x0000FFFF) return 0xFFFF;
|
||||
else return (uint16_t)in;
|
||||
}
|
||||
|
||||
uint16_t util_convert_muldivoff(uint16_t raw, uint8_t mul, uint8_t div, int16_t offset)
|
||||
{
|
||||
int32_t temp = (int32_t)raw;
|
||||
|
||||
temp = temp * mul;
|
||||
if(div>1) temp /= div;
|
||||
temp += offset;
|
||||
|
||||
return util_limit_s32b_to_u16b(temp);
|
||||
}
|
||||
|
||||
uint16_t util_sat_mul_kilo(uint16_t xk, uint16_t yk)
|
||||
{
|
||||
uint32_t temp = (uint32_t)xk * (uint32_t)yk;
|
||||
temp /= 1000;
|
||||
|
||||
return util_limit_u32b_to_u16b(temp);
|
||||
}
|
||||
|
||||
uint16_t util_sat_div_kilo(uint16_t top, uint16_t bot)
|
||||
{
|
||||
//Sanity check bot
|
||||
if(bot==0) return 0xFFFF; //aka infinity
|
||||
|
||||
uint32_t temp = (uint32_t)top * 1000;
|
||||
temp /= (uint32_t)bot;
|
||||
|
||||
return util_limit_u32b_to_u16b(temp);
|
||||
}
|
||||
|
||||
uint16_t util_sat_ratio_16b(uint16_t top, uint16_t bot)
|
||||
{
|
||||
//Sanity check bot
|
||||
if(bot==0) return 0xFFFF; //aka infinity
|
||||
|
||||
//Easy option
|
||||
if(top>=bot) return 0xFFFF;
|
||||
|
||||
uint32_t temp = (uint32_t)top * 0x0000FFFF;
|
||||
temp /= (uint32_t)bot;
|
||||
|
||||
return util_limit_u32b_to_u16b(temp);
|
||||
}
|
||||
|
||||
uint16_t util_percent_to_16b(uint8_t percent)
|
||||
{
|
||||
uint32_t temp = (uint32_t)percent * 0x0000FFFF;
|
||||
temp /= 100;
|
||||
|
||||
// Limit to 16 bits
|
||||
uint16_t pwm = util_limit_u32b_to_u16b(temp);
|
||||
|
||||
return pwm;
|
||||
}
|
||||
|
||||
uint16_t util_interpolate_1d_u16b(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis)
|
||||
{
|
||||
// validate axis length
|
||||
if(len_axis==0) return 0; // Empty data set
|
||||
if(len_axis==1) return y_values[0]; // Only one data point
|
||||
|
||||
uint16_t y;
|
||||
|
||||
uint8_t i = find_interval_end_index(x, x_axis, len_axis);
|
||||
if(i==0)
|
||||
{
|
||||
//Less then start
|
||||
y = y_values[0];
|
||||
}
|
||||
else if(i==len_axis)
|
||||
{
|
||||
//More than end
|
||||
y = y_values[len_axis-1];
|
||||
}
|
||||
else
|
||||
{
|
||||
// Do interpolate
|
||||
y = interpolate_u16b(x, x_axis[i-1], x_axis[i], y_values[i-1], y_values[i]);
|
||||
}
|
||||
|
||||
return y;
|
||||
}
|
||||
|
||||
uint16_t util_interpolate_2d_u16b(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t len_x_axis, uint16_t* y_axis, uint8_t len_y_axis, uint16_t* z_values)
|
||||
{
|
||||
// validate axis length
|
||||
if((len_x_axis==0)&&(len_y_axis==0)) return 0; // Empty data set
|
||||
if((len_x_axis==1)&&(len_y_axis==1)) return z_values[0]; // Only one data point
|
||||
|
||||
uint8_t ix = find_interval_end_index(x, x_axis, len_x_axis);
|
||||
uint8_t iy = find_interval_end_index(y, y_axis, len_y_axis);
|
||||
|
||||
// Check corners - easy answers
|
||||
if((ix==0)&&(iy==0))
|
||||
{
|
||||
return z_values[0]; //[0][0] [Y][X]
|
||||
}
|
||||
else if((ix==len_x_axis)&&(iy==0))
|
||||
{
|
||||
return z_values[len_x_axis-1]; //[0][end]
|
||||
}
|
||||
else if((ix==0)&&(iy==len_y_axis))
|
||||
{
|
||||
uint16_t i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
|
||||
return z_values[i]; //[end][0]
|
||||
}
|
||||
else if((ix==len_x_axis)&&(iy==len_y_axis))
|
||||
{
|
||||
uint16_t i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
|
||||
return z_values[i]; //[end][end]
|
||||
};
|
||||
|
||||
// Check boundaries - 1D interpolation
|
||||
if(ix==0)
|
||||
{
|
||||
// On ix=0 line
|
||||
uint16_t i = 0;
|
||||
uint16_t z0 = z_values[i];
|
||||
i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
|
||||
uint16_t z1 = z_values[i];
|
||||
return interpolate_u16b(y, y_axis[0], y_axis[len_y_axis-1], z0, z1);
|
||||
}
|
||||
else if(ix==len_x_axis)
|
||||
{
|
||||
// On ix=END line
|
||||
uint16_t i = len_x_axis-1;
|
||||
uint16_t z0 = z_values[i];
|
||||
i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
|
||||
uint16_t z1 = z_values[i];
|
||||
return interpolate_u16b(y, y_axis[0], y_axis[len_y_axis-1], z0, z1);
|
||||
}
|
||||
else if(iy==0)
|
||||
{
|
||||
// On iy=0 line
|
||||
uint16_t i = 0;
|
||||
uint16_t z0 = z_values[i];
|
||||
i = len_x_axis-1;
|
||||
uint16_t z1 = z_values[i];
|
||||
return interpolate_u16b(x, x_axis[0], x_axis[len_x_axis-1], z0, z1);
|
||||
}
|
||||
else if(iy==len_y_axis)
|
||||
{
|
||||
// On iy=END line
|
||||
uint16_t i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
|
||||
uint16_t z0 = z_values[i];
|
||||
i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
|
||||
uint16_t z1 = z_values[i];
|
||||
return interpolate_u16b(x, x_axis[0], x_axis[len_x_axis-1], z0, z1);
|
||||
}
|
||||
|
||||
// Do interpolation
|
||||
// Get axis values
|
||||
uint16_t x0 = x_axis[ix-1];
|
||||
uint16_t x1 = x_axis[ix];
|
||||
uint16_t y0 = y_axis[iy-1];
|
||||
uint16_t y1 = y_axis[iy];
|
||||
|
||||
// Do y0 line calculation
|
||||
// Get z values at x0 and x1 points on y0 line
|
||||
uint16_t i = index2d_to_index1d(ix-1, iy-1, len_x_axis);
|
||||
uint16_t z0 = z_values[i];
|
||||
uint16_t z1 = z_values[i+1];
|
||||
// Interpolate z value on y0 line
|
||||
uint16_t zy0 = interpolate_u16b(x, x0, x1, z0, z1);
|
||||
|
||||
// Do y1 line calculation
|
||||
// Get z values at x0 and x1 points on y1 line
|
||||
i = index2d_to_index1d(ix-1, iy, len_x_axis);
|
||||
z0 = z_values[i];
|
||||
z1 = z_values[i+1];
|
||||
// Interpolate z value on y0 line
|
||||
uint16_t zy1 = interpolate_u16b(x, x0, x1, z0, z1);
|
||||
|
||||
// Do calculation in y axis on xz line
|
||||
return interpolate_u16b(y, y0, y1, zy0, zy1);
|
||||
}
|
||||
|
||||
/**** Private function definitions ****/
|
||||
static uint16_t interpolate_u16b(uint16_t x, uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1)
|
||||
{
|
||||
int32_t dy = (int32_t)y1 - (int32_t)y0;
|
||||
int32_t dx = (int32_t)x1 - (int32_t)x0;
|
||||
int32_t d = (int32_t)x - (int32_t)x0;
|
||||
|
||||
int32_t y = dy * d;
|
||||
y /= dx;
|
||||
y += y0;
|
||||
|
||||
return util_limit_s32b_to_u16b(y);
|
||||
}
|
||||
|
||||
static uint8_t find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis)
|
||||
{
|
||||
for(uint8_t i=0; i<len_axis; i++)
|
||||
{
|
||||
if(val < axis_values[i]) return i;
|
||||
continue;
|
||||
}
|
||||
|
||||
return len_axis;
|
||||
}
|
||||
|
||||
static uint16_t index2d_to_index1d(uint8_t ix, uint8_t iy, uint8_t len_x)
|
||||
{
|
||||
return ((uint16_t)len_x * iy) + ix;
|
||||
}
|
||||
37
firmware/src/hw/board/utils/utils.h
Normal file
37
firmware/src/hw/board/utils/utils.h
Normal file
@@ -0,0 +1,37 @@
|
||||
#ifndef UTILS_H_
|
||||
#define UTILS_H_
|
||||
|
||||
/**** Includes ****/
|
||||
#include <stdint.h>
|
||||
|
||||
/**** Public definitions ****/
|
||||
|
||||
/**** Public function declarations ****/
|
||||
uint8_t util_invert_8b(uint8_t x);
|
||||
|
||||
uint16_t util_limit_u32b_to_u16b(uint32_t in);
|
||||
uint16_t util_limit_s32b_to_u16b(int32_t in);
|
||||
|
||||
uint16_t util_convert_muldivoff(uint16_t raw, uint8_t mul, uint8_t div, int16_t offset);
|
||||
uint16_t util_sat_mul_kilo(uint16_t xk, uint16_t yk);
|
||||
uint16_t util_sat_div_kilo(uint16_t top, uint16_t bot);
|
||||
uint16_t util_sat_ratio_16b(uint16_t top, uint16_t bot);
|
||||
uint16_t util_percent_to_16b(uint8_t percent);
|
||||
|
||||
uint8_t util_sat_add_8b(uint8_t x, uint8_t y);
|
||||
uint8_t util_sat_subtract_8b(uint8_t x, uint8_t y);
|
||||
|
||||
uint16_t util_sat_add_16b(uint16_t x, uint16_t y);
|
||||
uint16_t util_sat_subtract_16b(uint16_t x, uint16_t y);
|
||||
|
||||
uint16_t util_interpolate_1d_u16b(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis);
|
||||
uint16_t util_interpolate_2d_u16b(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t len_x_axis, uint16_t* y_axis, uint8_t len_y_axis, uint16_t* z_values);
|
||||
|
||||
#ifdef TESTING
|
||||
// Access to private functions for unit testing
|
||||
static uint8_t find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis);
|
||||
static uint16_t interpolate_u16b(uint16_t x, uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1);
|
||||
static uint16_t index2d_to_index1d(uint8_t ix, uint8_t iy, uint8_t len_x);
|
||||
#endif
|
||||
|
||||
#endif /* UTILS_H_ */
|
||||
Reference in New Issue
Block a user