void
ble_hs_unlock(void)
{
int rc;
rc = os_mutex_release(&ble_hs_mutex);
BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED);
}
static void
nffs_unlock(void)
{
int rc;
rc = os_mutex_release(&nffs_mutex);
assert(rc == 0 || rc == OS_NOT_STARTED);
}
uint8_t kalman_update(
kalman_robot_handle_t * handle,
const position_t * measurement,
float delta_t,
robot_pos_t * dest)
{
if(handle == NULL || dest == NULL || delta_t < 0.0f) {
return 0;
}
os_mutex_take(&(handle->_mutex));
// predict new state
predict_state(&(handle->_state), delta_t, &(handle->_state));
// predict new covariance
covariance_t proc_noise_cov;
process_noise_covariance(handle, delta_t, &proc_noise_cov);
predict_covariance(
&(handle->_state_covariance),
&proc_noise_cov,
delta_t,
&(handle->_state_covariance));
// if there is a measurement make kalman update
if(measurement != NULL) {
// compute kalman gain
kalman_gain_t gain;
kalman_gain(
&(handle->_state_covariance),
&(handle->_measurement_covariance),
&gain);
// compute difference between prediction and measurement
vec2d_t residual;
residual._x = measurement->x - handle->_state._x;
residual._y = measurement->y - handle->_state._y;
// estimate new state considering measurement
update_state(&(handle->_state), &gain, &residual, &(handle->_state));
update_covariance(
&(handle->_state_covariance),
&gain,
&(handle->_state_covariance));
}
// write resulting position (and associated variances) to dest
dest->x = handle->_state._x;
dest->y = handle->_state._y;
dest->var_x = handle->_state_covariance._cov_a._a;
dest->var_y = handle->_state_covariance._cov_a._d;
dest->cov_xy = handle->_state_covariance._cov_a._b;
os_mutex_release(&(handle->_mutex));
return 1;
}
uint8_t kalman_init(
kalman_robot_handle_t * handle,
const robot_pos_t * initial_config)
{
// verify input
if(handle == NULL || initial_config == NULL) {
return 0;
}
// initialize mutex
os_mutex_init(&(handle->_mutex));
os_mutex_take(&(handle->_mutex));
// set initial state of robot
handle->_state._x = initial_config->x;
handle->_state._y = initial_config->y;
// assume that the robot is standing still initially
handle->_state._v_x = 0.0f;
handle->_state._v_y = 0.0f;
// set initial state covariance
handle->_state_covariance._cov_a._a = initial_config->var_x;
handle->_state_covariance._cov_a._b = initial_config->cov_xy;
handle->_state_covariance._cov_a._c = initial_config->cov_xy;
handle->_state_covariance._cov_a._d = initial_config->var_y;
handle->_state_covariance._cov_b._a = 0.0f;
handle->_state_covariance._cov_b._b = 0.0f;
handle->_state_covariance._cov_b._c = 0.0f;
handle->_state_covariance._cov_b._d = 0.0f;
handle->_state_covariance._cov_c._a = 0.0f;
handle->_state_covariance._cov_c._b = 0.0f;
handle->_state_covariance._cov_c._c = 0.0f;
handle->_state_covariance._cov_c._d = 0.0f;
handle->_state_covariance._cov_d._a = 0.0f;
handle->_state_covariance._cov_d._b = 0.0f;
handle->_state_covariance._cov_d._c = 0.0f;
handle->_state_covariance._cov_d._d = 0.0f;
// set default measurment covariance
handle->_measurement_covariance._a = MEAS_VAR_X;
handle->_measurement_covariance._b = MEAS_COV_XY;
handle->_measurement_covariance._c = MEAS_COV_XY;
handle->_measurement_covariance._d = MEAS_VAR_Y;
// default max acc of robot
handle->_max_acc = MAX_ACC;
// default proportionality constant for process noise covariance
handle->_process_noise_proportionality = PROC_NOISE_PROP;
os_mutex_release(&(handle->_mutex));
return 1;
}
/**
* Unlock access to the charge_control_itf specified by bi.
*
* @param The charge_control_itf to unlock access to
*
* @return 0 on success, non-zero on failure.
*/
static void
adp5061_itf_unlock(struct charge_control_itf *cci)
{
if (!cci->cci_lock) {
return;
}
os_mutex_release(cci->cci_lock);
}
static void
os_malloc_unlock(void)
{
int rc;
if (g_os_started) {
rc = os_mutex_release(&os_malloc_mutex);
assert(rc == 0);
}
}
int
fcb_getnext(struct fcb *fcb, struct fcb_entry *loc)
{
int rc;
rc = os_mutex_pend(&fcb->f_mtx, OS_WAIT_FOREVER);
if (rc && rc != OS_NOT_STARTED) {
return FCB_ERR_ARGS;
}
rc = fcb_getnext_nolock(fcb, loc);
os_mutex_release(&fcb->f_mtx);
return rc;
}
/**
* Close the STM32F4 ADC device.
*
* This function unlocks the device.
*
* @param odev The device to close.
*/
static int
stm32f4_adc_close(struct os_dev *odev)
{
struct adc_dev *dev;
dev = (struct adc_dev *) odev;
stm32f4_adc_uninit(dev);
if (os_started()) {
os_mutex_release(&dev->ad_lock);
}
return (OS_OK);
}
void
ble_hs_unlock(void)
{
int rc;
#if BLE_HS_DEBUG
if (!os_started()) {
BLE_HS_DBG_ASSERT(ble_hs_dbg_mutex_locked);
ble_hs_dbg_mutex_locked = 0;
return;
}
#endif
rc = os_mutex_release(&ble_hs_mutex);
BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED);
}
/**
* Close the NRF52 ADC device.
*
* This function unlocks the device.
*
* @param odev The device to close.
*/
static int
nrf52_adc_close(struct os_dev *odev)
{
struct adc_dev *dev;
dev = (struct adc_dev *) odev;
nrfx_saadc_uninit();
global_adc_dev = NULL;
global_adc_config = NULL;
if (os_started()) {
os_mutex_release(&dev->ad_lock);
}
return (0);
}
int
cbmem_lock_release(struct cbmem *cbmem)
{
int rc;
if (!os_started()) {
return (0);
}
rc = os_mutex_release(&cbmem->c_lock);
if (rc != 0) {
goto err;
}
return (0);
err:
return (rc);
}
uint8_t kalman_update_measurement_covariance(
kalman_robot_handle_t * handle,
float var_x,
float var_y,
float cov_xy)
{
if(handle == NULL) {
return 0;
}
os_mutex_take(&(handle->_mutex));
handle->_measurement_covariance._a = var_x;
handle->_measurement_covariance._b = cov_xy;
handle->_measurement_covariance._c = cov_xy;
handle->_measurement_covariance._d = var_y;
os_mutex_release(&(handle->_mutex));
return 1;
}
/**
* Open the NRF52 ADC device
*
* This function locks the device for access from other tasks.
*
* @param odev The OS device to open
* @param wait The time in MS to wait. If 0 specified, returns immediately
* if resource unavailable. If OS_WAIT_FOREVER specified, blocks
* until resource is available.
* @param arg Argument provided by higher layer to open, in this case
* it can be a nrfx_saadc_config_t, to override the default
* configuration.
*
* @return 0 on success, non-zero on failure.
*/
static int
nrf52_adc_open(struct os_dev *odev, uint32_t wait, void *arg)
{
struct adc_dev *dev;
struct nrf52_adc_dev_cfg *cfg = arg;
int rc;
dev = (struct adc_dev *) odev;
if (os_started()) {
rc = os_mutex_pend(&dev->ad_lock, wait);
if (rc != OS_OK) {
goto err;
}
}
if (odev->od_flags & OS_DEV_F_STATUS_OPEN) {
os_mutex_release(&dev->ad_lock);
rc = OS_EBUSY;
goto err;
}
/* If user did not provide config let us use init */
if (!cfg) {
cfg = init_adc_config;
}
/* Initialize the device */
rc = nrfx_saadc_init(&cfg->saadc_cfg, nrf52_saadc_event_handler);
if (rc != NRFX_SUCCESS) {
goto err;
}
global_adc_dev = dev;
global_adc_config = arg;
return (0);
err:
return (rc);
}
/**
* Open the STM32F4 ADC device
*
* This function locks the device for access from other tasks.
*
* @param odev The OS device to open
* @param wait The time in MS to wait. If 0 specified, returns immediately
* if resource unavailable. If OS_WAIT_FOREVER specified, blocks
* until resource is available.
* @param arg Argument provided by higher layer to open.
*
* @return 0 on success, non-zero on failure.
*/
static int
stm32f4_adc_open(struct os_dev *odev, uint32_t wait, void *arg)
{
DMA_HandleTypeDef *hdma;
ADC_HandleTypeDef *hadc;
struct stm32f4_adc_dev_cfg *cfg;
struct adc_dev *dev;
int rc;
assert(odev);
rc = OS_OK;
dev = (struct adc_dev *) odev;
if (os_started()) {
rc = os_mutex_pend(&dev->ad_lock, wait);
if (rc != OS_OK) {
goto err;
}
}
if (odev->od_flags & OS_DEV_F_STATUS_OPEN) {
os_mutex_release(&dev->ad_lock);
rc = OS_EBUSY;
goto err;
}
stm32f4_adc_init(dev);
cfg = (struct stm32f4_adc_dev_cfg *)dev->ad_dev.od_init_arg;
hadc = cfg->sac_adc_handle;
hdma = hadc->DMA_Handle;
adc_dma[stm32f4_resolve_dma_handle_idx(hdma)] = dev;
return (OS_OK);
err:
return (rc);
}
