double pidUpdate(pidcontroller_t *pid, double setPoint, double measured) {
double error = setPoint - measured;
double dt = ((double) (halGetCounterValue() - pid->lastTime)) / halGetCounterFrequency();
double derivitave = (error - pid->previousError) / dt;
pid->previousError = error;
pid->lastTime = halGetCounterValue();
pid->integral += error * dt;
return pid->p * error + pid->i * pid->integral + pid->d * derivitave;
}
/* This function will start the PWM generator.
*/
extern void bldcInit(void) {
bldc.scheme = &pwmScheme;
bldc.state = 0; //Default to first state
bldc.nextState = 0;
bldc.directionFwd = TRUE;
bldc.stateChangeInterval = MS2RTT(20);
bldc.prevStateChange = halGetCounterValue();
bldc.nextStateChange = bldc.prevStateChange + bldc.stateChangeInterval;
bldc.pwmOutT0 = 0;
bldc.pwmOutT1 = 0;
bldc.stateCount = sizeof(pwmScheme)/3;
bldc.dutyCycle = 1800;
palWriteGroup (PWM_OUT_PORT, PWM_OUT_PORT_MASK, PWM_OUT_OFFSET, PWM_OFF);
palSetGroupMode (PWM_OUT_PORT, PWM_OUT_PORT_MASK, PWM_OUT_OFFSET, PAL_MODE_OUTPUT_PUSHPULL);
palSetPadMode (GPIOA, GPIOA_PIN1, PAL_MODE_INPUT_ANALOG);
pwmStart(&PWMD1, &pwmcfg);
// ADC trigger channel. This will trigger the ADC read at 95% of the cycle,
// when all the PWM outputs are set to 0
pwmEnableChannel (&PWMD1, PWM_ADCTRIG_CH, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, PWM_MAX_DUTY_CYCLE));
// Start the ADC
adcStart(&ADCD1, NULL);
}
/*Callback function for the interrupt on RC Channel 4.
*/
static void extcb4(EXTDriver *extp, expchannel_t channel) {
(void)extp ;
(void)channel ;
chSysLockFromIsr() ;
if(palReadPad(RC4_PORT, RC4_PIN) == PAL_HIGH) {
start[3] = halGetCounterValue() ;
}
else if(start[3] && (palReadPad(RC4_PORT, RC4_PIN) == PAL_LOW)) {
float tmp = convertCounterToMilliseconds(start[3], halGetCounterValue()) ;
if(RC_IN_RANGE(tmp))
RCInput[3] = tmp ;
start[3] = 0 ;
}
chSysUnlockFromIsr() ;
}
void pidInit(pidcontroller_t *pid, double p, double i, double d) {
pid->integral = 0;
pid->previousError = 0;
pid->p = p;
pid->i = i;
pid->d = d;
pid->lastTime = halGetCounterValue();
}
/**
* @brief Stops a measurement.
*
* @param[in,out] tmp pointer to a @p TimeMeasurement structure
*
* @notapi
*/
static void tm_stop(TimeMeasurement *tmp) {
halrtcnt_t now = halGetCounterValue();
tmp->last = now - tmp->last - measurement_offset;
if (tmp->last > tmp->worst)
tmp->worst = tmp->last;
else if (tmp->last < tmp->best)
tmp->best = tmp->last;
}
void rfm69_log_s64(uint8_t channel, int64_t data)
{
char *msg;
msg = (void*)chPoolAlloc(&rfm69_mp);
msg[4] = (char)(1 | conf.location << 4);
msg[5] = (char)channel;
memcpy(msg, (void*)&halGetCounterValue(), 4);
memcpy(&msg[8], &data, 8);
chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE);
}
void rfm69_log_c(uint8_t channel, const char* data)
{
volatile char *msg;
msg = (char*)chPoolAlloc(&rfm69_mp);
msg[4] = (char)(0 | conf.location << 4);
msg[5] = (char)channel;
memcpy((void*)msg, (void*)&halGetCounterValue(), 4);
memcpy((void*)&msg[8], data, 8);
chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE);
}
void rfm69_log_f(uint8_t channel, float data_a, float data_b)
{
char *msg;
msg = (void*)chPoolAlloc(&rfm69_mp);
msg[4] = (char)(9 | conf.location << 4);
msg[5] = (char)channel;
memcpy(msg, (void*)&halGetCounterValue(), 4);
memcpy(&msg[8], &data_a, 4);
memcpy(&msg[12], &data_b, 4);
chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE);
}
static void cmd_rt(BaseSequentialStream *chp, int argc, char *argv[]) {
(void)argv;
if(argc > 0) {
chprintf(chp, "Usage: rt\r\n");
chprintf(chp, "Prints current realtime clock ticks and frequency\r\n");
}
halrtcnt_t t = halGetCounterValue();
halclock_t f = halGetCounterFrequency();
chprintf(chp, "Current real time clock ticks: %u\r\n", t);
chprintf(chp, "Real time clock frequency: %u\r\n", f);
}
uint32_t time_ticks_since(uint32_t *t0)
{
uint32_t t1 = halGetCounterValue();
uint32_t tp = *t0;
if(t1 < tp) {
*t0 = t1;
return t1 + ((0xffffffff - tp) + 1);
} else {
*t0 = t1;
return t1 - tp;
}
}
//-----------------------------------------------------------------------------
static void
one_sec_cb(GPTDriver * gptp)
{
(void)gptp;
int32_t b = halGetCounterValue();
one_sec_pps_diff_diff = (b-pps_time)-one_sec_pps_diff;
one_sec_pps_diff = b-pps_time;
one_sec_pps_changed = TRUE;
one_sec_pps++;
kbg_toggleLED2();
}
//-----------------------------------------------------------------------------
static void
gps_timepulse(EXTDriver *extp, expchannel_t channel)
{
(void)extp;
(void)channel;
int32_t b = halGetCounterValue();
pps_diff = b-pps_time;
pps_time = b;
pps_changed = TRUE;
pps++;
kbg_toggleLED1();
}
void rfm69_log_u16(uint8_t channel, uint16_t data_a, uint16_t data_b,
uint16_t data_c, uint16_t data_d)
{
char *msg;
msg = (void*)chPoolAlloc(&rfm69_mp);
msg[4] = (char)(6 | conf.location << 4);
msg[5] = (char)channel;
memcpy(msg, (void*)&halGetCounterValue(), 4);
memcpy(&msg[8], &data_a, 2);
memcpy(&msg[10], &data_b, 2);
memcpy(&msg[12], &data_c, 2);
memcpy(&msg[14], &data_d, 2);
chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE);
}
/**
* Initialize library
*/
int32_t TaskMonitorInitialize(void)
{
lock = PIOS_Mutex_Create();
PIOS_Assert(lock != NULL);
memset(handles, 0, sizeof(struct pios_thread) * TASKINFO_RUNNING_NUMELEM);
lastMonitorTime = 0;
#if defined(DIAG_TASKS)
#if defined(PIOS_INCLUDE_FREERTOS)
lastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
#elif defined(PIOS_INCLUDE_CHIBIOS)
lastMonitorTime = halGetCounterValue();
#endif /* defined(PIOS_INCLUDE_CHIBIOS) */
#endif
return 0;
}
/**
* @brief Realtime window test.
* @details This function verifies if the current realtime counter value
* lies within the specified range or not. The test takes care
* of the realtime counter wrapping to zero on overflow.
* @note When start==end then the function returns always true because the
* whole time range is specified.
* @note This is an optional service that could not be implemented in
* all HAL implementations.
* @note This function can be called from any context.
*
* @par Example 1
* Example of a guarded loop using the realtime counter. The loop implements
* a timeout after one second.
* @code
* halrtcnt_t start = halGetCounterValue();
* halrtcnt_t timeout = start + S2RTT(1);
* while (my_condition) {
* if (!halIsCounterWithin(start, timeout)
* return TIMEOUT;
* // Do something.
* }
* // Continue.
* @endcode
*
* @par Example 2
* Example of a loop that lasts exactly 50 microseconds.
* @code
* halrtcnt_t start = halGetCounterValue();
* halrtcnt_t timeout = start + US2RTT(50);
* while (halIsCounterWithin(start, timeout)) {
* // Do something.
* }
* // Continue.
* @endcode
*
* @param[in] start the start of the time window (inclusive)
* @param[in] end the end of the time window (non inclusive)
* @retval TRUE current time within the specified time window.
* @retval FALSE current time not within the specified time window.
*
* @special
*/
bool_t halIsCounterWithin(halrtcnt_t start, halrtcnt_t end) {
halrtcnt_t now = halGetCounterValue();
return end > start ? (now >= start) && (now < end) :
(now >= start) || (now < end);
}
/**
* @brief Polled delay.
* @note The real delays is always few cycles in excess of the specified
* value.
* @note This is an optional service that could not be implemented in
* all HAL implementations.
* @note This function can be called from any context.
*
* @param[in] ticks number of ticks
*
* @special
*/
void halPolledDelay(halrtcnt_t ticks) {
halrtcnt_t start = halGetCounterValue();
halrtcnt_t timeout = start + (ticks);
while (halIsCounterWithin(start, timeout))
;
}
/**
* @brief Starts a measurement.
*
* @param[in,out] tmp pointer to a @p TimeMeasurement structure
*
* @notapi
*/
static void tm_start(TimeMeasurement *tmp) {
tmp->last = halGetCounterValue();
}
