static void Draw(void)
{
int i;
systime_t current = chVTGetSystemTime();
/* clear buffer */
memset(display.pixels, 0, sizeof(struct Color) * LEDCOUNT);
if (effects.p_next != NULL)
{
EffectUpdate(effects.p_next, 0, 0, current, &display);
}
/* render volume for a 5s */
if (showVolume == true)
{
startVolumeEffect = current;
showVolume = false;
effVolume_cfg.volume = newVolume;
showVolumeEffect = true;
if (chVTTimeElapsedSinceX(startVolumeEffect) > MS2ST(15000))
{
EffectReset(&effVolume, 0, 0, current);
}
}
if (showVolumeEffect == true)
{
if (chVTTimeElapsedSinceX(startVolumeEffect) <= MS2ST(5000))
{
EffectUpdate(&effVolume, 0, 0, current, &display);
}
else
{
showVolumeEffect = false;
}
}
//draw buttons
if (showControls == true)
{
EffectUpdate(&effButtons, 0, 0, current, &display);
}
for (i = 0; i < LEDCOUNT; i++)
{
SetLedColor(i, &display.pixels[i]);
}
SetUpdateLed();
}
uint8_t EE_t::Write(uint32_t Addr, void *ptr, uint32_t Sz) {
uint8_t DevAddr = EE_BLOCK1_DEV_ADDR;
uint8_t ByteAddr;
while(Sz != 0) {
if(Addr >= EE_BLOCK_SZ) {
ByteAddr = (uint8_t)(Addr - EE_BLOCK_SZ);
DevAddr = EE_BLOCK2_DEV_ADDR;
}
else ByteAddr = (uint8_t)Addr;
uint32_t SzToW;
if(Sz < 8) SzToW = Sz;
else SzToW = 8;
if(Addr + SzToW >= EE_BLOCK_SZ) SzToW = EE_BLOCK_SZ - Addr;
Uart.Printf("DA=%X; BA=%u; SzToW=%u\r", DevAddr, ByteAddr, SzToW);
uint8_t Rslt = pi2c->WriteWrite(DevAddr, &ByteAddr, 1, (uint8_t*)ptr, SzToW);
if(Rslt != OK) return Rslt;
// Wait write cycle completion
systime_t t = chVTGetSystemTime();
do {
chThdSleepMilliseconds(2);
Rslt = pi2c->Write(DevAddr, &ByteAddr, 1);
if(chVTTimeElapsedSinceX(t) > 999) return TIMEOUT;
} while(Rslt != OK);
Addr += SzToW;
Sz -= SzToW;
}
Uart.Printf("Done\r");
return OK;
}
/** Check the state of a tracker channel and generate events as required.
* \param tracker_channel Tracker channel to use.
* \param update_required True when correlations are pending for the
* tracking channel.
*/
static void tracker_channel_process(tracker_channel_t *tracker_channel,
bool update_required)
{
switch (tracker_channel_state_get(tracker_channel)) {
case STATE_ENABLED: {
if (update_required) {
tracker_channel_lock(tracker_channel);
{
interface_function(tracker_channel,
tracker_channel->interface->update);
}
tracker_channel_unlock(tracker_channel);
}
}
break;
case STATE_DISABLE_REQUESTED: {
nap_channel_disable(tracker_channel);
tracker_channel_lock(tracker_channel);
{
interface_function(tracker_channel,
tracker_channel->interface->disable);
tracker_channel->disable_time = chVTGetSystemTimeX();
event(tracker_channel, EVENT_DISABLE);
}
tracker_channel_unlock(tracker_channel);
}
break;
case STATE_DISABLE_WAIT: {
nap_channel_disable(tracker_channel);
if (chVTTimeElapsedSinceX(tracker_channel->disable_time) >=
MS2ST(CHANNEL_DISABLE_WAIT_TIME_ms)) {
event(tracker_channel, EVENT_DISABLE_WAIT_COMPLETE);
}
}
break;
case STATE_DISABLED: {
if (update_required) {
/* Tracking channel is not owned by the update thread, but the update
* register must be written to clear the interrupt flag. Set error
* flag to indicate that NAP is in an unknown state. */
nap_channel_disable(tracker_channel);
error_flags_add(tracker_channel, ERROR_FLAG_INTERRUPT_WHILE_DISABLED);
}
}
break;
default: {
assert(!"Invalid tracking channel state");
}
break;
}
}
/**
* Returns the total bytes divided by the total elapsed seconds since the
* previous call of this function.
*
* \param s The USART stats structure
*/
float usart_throughput(struct usart_stats* s)
{
float elapsed = ((float)chVTTimeElapsedSinceX(s->last_byte_ticks) /
(double)CH_CFG_ST_FREQUENCY*1000.0);
float kbps = s->byte_counter / elapsed;
s->byte_counter = 0;
s->last_byte_ticks = chVTGetSystemTime();
return kbps;
}
void rotenc_push_cb(EXTDriver *extp, expchannel_t channel)
{
(void)extp;
(void)channel;
if(ST2MS(chVTTimeElapsedSinceX(rotenc_press_time)) > 500) {
chSysLockFromISR();
chEvtBroadcastFlagsI(&rotenc_es, ROTENC_PRESS_FLAG);
rotenc_press_time = chVTGetSystemTimeX();
chSysUnlockFromISR();
}
}
/** Clear unhealthy flags after some time, so we eventually retry
those sats in case they recover from their sickness. Call this
function regularly, and once per day it will reset the flags. */
static void check_clear_unhealthy(void)
{
static systime_t ticks;
if (chVTTimeElapsedSinceX(ticks) < S2ST(24*60*60))
return;
ticks = chVTGetSystemTime();
for (u32 i=0; i<PLATFORM_SIGNAL_COUNT; i++) {
if (acq_status[i].state == ACQ_PRN_UNHEALTHY)
acq_status[i].state = ACQ_PRN_ACQUIRING;
}
}
void solution_send_sbp(gnss_solution *soln, dops_t *dops, bool clock_jump)
{
if (soln) {
/* Send GPS_TIME message first. */
msg_gps_time_t gps_time;
sbp_make_gps_time(&gps_time, &soln->time, 0);
sbp_send_msg(SBP_MSG_GPS_TIME, sizeof(gps_time), (u8 *) &gps_time);
if (chVTTimeElapsedSinceX(last_dgnss) > DGNSS_TIMEOUT(soln_freq)) {
/* Position in LLH. */
msg_pos_llh_t pos_llh;
sbp_make_pos_llh(&pos_llh, soln, 0);
sbp_send_msg(SBP_MSG_POS_LLH, sizeof(pos_llh), (u8 *) &pos_llh);
/* Position in ECEF. */
msg_pos_ecef_t pos_ecef;
sbp_make_pos_ecef(&pos_ecef, soln, 0);
sbp_send_msg(SBP_MSG_POS_ECEF, sizeof(pos_ecef), (u8 *) &pos_ecef);
}
/* Velocity in NED. */
/* Do not send if there has been a clock jump. Velocity may be unreliable.*/
if (!clock_jump) {
msg_vel_ned_t vel_ned;
sbp_make_vel_ned(&vel_ned, soln, 0);
sbp_send_msg(SBP_MSG_VEL_NED, sizeof(vel_ned), (u8 *) &vel_ned);
/* Velocity in ECEF. */
msg_vel_ecef_t vel_ecef;
sbp_make_vel_ecef(&vel_ecef, soln, 0);
sbp_send_msg(SBP_MSG_VEL_ECEF, sizeof(vel_ecef), (u8 *) &vel_ecef);
}
}
if (dops) {
DO_EVERY(10,
msg_dops_t sbp_dops;
sbp_make_dops(&sbp_dops, dops, &(soln->time));
sbp_send_msg(SBP_MSG_DOPS, sizeof(msg_dops_t), (u8 *) &sbp_dops);
);
}
systime_t mr_control_time_since_input_update(void) {
return ST2MS(chVTTimeElapsedSinceX(m_rc.last_update_time));
}
/**
* Get the amount of milliseconds that have passed since
* altitude values were received the last time.
*/
uint32_t packet_handler_int_get_time_since_update_altitude(void) {
return (systime_t)((float)chVTTimeElapsedSinceX(last_update_time_altitude) /
((float)CH_CFG_ST_FREQUENCY / 1000.0));
}
/**
* Get the amount of milliseconds that has passed since
* the last time servo positions were received.
*
* @return
* The amount of milliseconds that have passed since an update.
*/
uint32_t servodec_get_time_since_update(void) {
return chVTTimeElapsedSinceX(last_update_time) / (CH_CFG_ST_FREQUENCY / 1000);
}
/**
* Get the amount of milliseconds that have passed since
* servo values were received the last time.
*/
uint32_t transmitter_receiver_get_time_since_update(void) {
return (systime_t)((float)chVTTimeElapsedSinceX(last_update_time) /
((float)CH_CFG_ST_FREQUENCY / 1000.0));
}
// ========================= Postprocessor for PinSns ==========================
void ProcessButtons(PinSnsState_t *BtnState, uint32_t Len) {
// Uart.Printf("\r%A", BtnState, Len, ' ');
for(uint8_t i=0; i<BUTTONS_CNT; i++) {
// ==== Button Press ====
if(BtnState[i] == BTN_PRESS_STATE) {
#if BTN_LONGPRESS
IsLongPress[i] = false;
#endif
#if BTN_REPEAT
IsRepeating[i] = false;
#endif
#if BTN_COMBO // Check if combo
BtnEvtInfo_t IEvt;
IEvt.BtnCnt = 0;
for(uint8_t j=0; j<BUTTONS_CNT; j++) {
if(BtnState[j] == BTN_HOLDDOWN_STATE or BtnState[j] == BTN_PRESS_STATE) {
IEvt.BtnID[IEvt.BtnCnt] = j;
IEvt.BtnCnt++;
if((j != i) and !IsCombo) {
IsCombo = true;
AddEvtToQueue(beCancel, j);
}
}
} // for j
if(IEvt.BtnCnt > 1) { // Combo
IEvt.Type = beCombo;
AddEvtToQueue(IEvt);
continue;
}
else IsCombo = false;
#endif // combo
// Single key pressed, no combo
AddEvtToQueue(bePress, i); // Add single keypress
#if BTN_LONGPRESS
LongPressTimer = chVTGetSystemTimeX();
#endif
#if BTN_REPEAT
RepeatTimer = chVTGetSystemTimeX();
#endif
} // if press
// ==== Button Release ====
#if BTN_COMBO || BTN_RELEASE
else if(BtnState[i] == BTN_RELEASE_STATE) {
#if BTN_COMBO // Check if combo completely released
if(IsCombo) {
IsCombo = false;
for(uint8_t j=0; j<BUTTONS_CNT; j++) {
if(BtnState[j] == BTN_HOLDDOWN_STATE) {
IsCombo = true;
break;
}
}
continue; // do not send release evt (if enabled)
} // if combo
#endif
#if BTN_RELEASE // Send evt if not combo
AddEvtToQueue(beRelease, i);
#endif
}
#endif // if combo or release
// ==== Long Press ====
#if BTN_LONGPRESS || BTN_REPEAT
else if(BtnState[i] == BTN_HOLDDOWN_STATE
#if BTN_COMBO
and !IsCombo
#endif
) {
#if BTN_LONGPRESS // Check if long press
if(!IsLongPress[i]) {
// Uart.Printf("Elapsed %u\r", chVTTimeElapsedSinceX(LongPressTimer));
if(chVTTimeElapsedSinceX(LongPressTimer) >= MS2ST(BTN_LONGPRESS_DELAY_MS)) {
IsLongPress[i] = true;
AddEvtToQueue(beLongPress, i);
}
}
#endif
#if BTN_REPEAT // Check if repeat
if(!IsRepeating[i]) {
if(TimeElapsed(&RepeatTimer, BTN_DELAY_BEFORE_REPEAT_MS)) {
IsRepeating[i] = true;
AddEvtToQueue(beRepeat, i);
}
}
else {
if(TimeElapsed(&RepeatTimer, BTN_REPEAT_PERIOD_MS)) {
AddEvtToQueue(beRepeat, i);
}
}
#endif
} // if still pressed
#endif
} // for i
}
bool rpm_is_engine_running(void)
{
return chVTTimeElapsedSinceX(m_last_update) < US2ST(UPDATE_TIMEOUT_US)
&& m_curr_rpm > gp_rpm_min_idle;
}
/**
* @brief Requests a DMA transfer operation from the DMA engine.
* @note The DMA engine uses unclaimed DMA channels to provide DMA services
* for one-off or infrequent uses. If all channels are busy, and
* semaphores are enabled, the calling thread will sleep until a
* channel is available or the request times out. If semaphores are
* disabled, the calling thread will busy-wait instead of sleeping.
*/
bool dmaRequest(msp430x_dma_req_t * request, systime_t timeout) {
/* Check if a DMA channel is available */
#if CH_CFG_USE_SEMAPHORES
msg_t semresult = chSemWaitTimeout(&dma_lock, timeout);
if (semresult != MSG_OK)
return true;
#endif
#if !(CH_CFG_USE_SEMAPHORES)
systime_t start = chVTGetSystemTimeX();
do {
#endif
/* Grab the correct DMA channel to use */
int i = 0;
for (i = 0; i < MSP430X_DMA_CHANNELS; i++) {
if (!(dma_channels[i].ctl & DMAEN)) {
break;
}
}
#if !(CH_CFG_USE_SEMAPHORES)
while (chVTTimeElapsedSinceX(start) < timeout)
;
#endif
#if !(CH_CFG_USE_SEMAPHORES)
if (i == MSP430X_DMA_CHANNELS) {
return true;
}
#endif
/* Make the request */
init_request(request, i);
return false;
}
/**
* @brief Acquires exclusive control of a DMA channel.
* @pre The channel must not be already acquired or an error is returned.
* @note If the channel is in use by the DMA engine, blocks until acquired.
* @post This channel must be interacted with using only the functions
* defined in this module.
*
* @param[out] channel The channel handle. Must be pre-allocated.
* @param[in] index The index of the channel (< MSP430X_DMA_CHANNELS).
* @return The operation status.
* @retval false no error, channel acquired.
* @retval true error, channel already acquired.
*/
bool dmaAcquire(msp430x_dma_ch_t * channel, uint8_t index) {
/* Acquire the channel in an idle mode */
/* Is the channel already acquired? */
osalDbgAssert(index < MSP430X_DMA_CHANNELS, "invalid channel index");
if (dma_channels[index].ctl & DMADT_4) {
return true;
}
/* Increment the DMA counter */
#if CH_CFG_USE_SEMAPHORES
msg_t semresult = chSemWait(&dma_lock);
if (semresult != MSG_OK)
return true;
#endif
while (dma_channels[index].ctl & DMAEN)
;
dma_trigger_set(index, DMA_TRIGGER_MNEM(DMAREQ));
dma_channels[index].sz = 0;
dma_channels[index].ctl = DMAEN | DMAABORT | DMADT_4;
channel->registers = dma_channels + index;
channel->index = index;
channel->cb = callbacks + index;
return false;
}
/**
* Get the amount of milliseconds that have passed since
* IMU values were received the last time.
*/
uint32_t mpu9150_get_time_since_update(void) {
return (systime_t)((float)chVTTimeElapsedSinceX(last_update_time) /
((float)CH_CFG_ST_FREQUENCY / 1000.0));
}
