/**
* Check for some event triggering the shutdown.
*
* It can be either a long power button press or a shutdown triggered from the
* AP and detected by reading POWER_GOOD.
*
* @return non-zero if a shutdown should happen, 0 if not
*/
static int check_for_power_off_event(void)
{
timestamp_t now;
int pressed = 0;
int ret = 0;
/*
* Check for power button press.
*/
if (power_button_is_pressed()) {
pressed = 1;
} else if (power_request == POWER_REQ_OFF) {
power_request = POWER_REQ_NONE;
return 4; /* return non-zero for shudown down */
}
#ifdef HAS_TASK_KEYSCAN
/* Dis/Enable keyboard scanning when the power button state changes */
if (!pressed || pressed != power_button_was_pressed)
keyboard_scan_enable(!pressed, KB_SCAN_DISABLE_POWER_BUTTON);
#endif
now = get_time();
if (pressed) {
if (!power_button_was_pressed) {
power_off_deadline.val = now.val + DELAY_FORCE_SHUTDOWN;
CPRINTS("power waiting for long press %u",
power_off_deadline.le.lo);
/* Ensure we will wake up to check the power key */
timer_arm(power_off_deadline, TASK_ID_CHIPSET);
} else if (timestamp_expired(power_off_deadline, &now)) {
power_off_deadline.val = 0;
CPRINTS("power off after long press now=%u, %u",
now.le.lo, power_off_deadline.le.lo);
return 2;
}
} else if (power_button_was_pressed) {
CPRINTS("power off cancel");
timer_cancel(TASK_ID_CHIPSET);
}
/* POWER_GOOD released by AP : shutdown immediately */
if (!power_has_signals(IN_POWER_GOOD)) {
if (power_button_was_pressed)
timer_cancel(TASK_ID_CHIPSET);
ret = 3;
}
power_button_was_pressed = pressed;
return ret;
}
static int wait_until_stop_sent(int port)
{
timestamp_t deadline;
timestamp_t slow_cutoff;
uint8_t is_slow;
deadline = slow_cutoff = get_time();
deadline.val += TIMEOUT_STOP_SENT_US;
slow_cutoff.val += SLOW_STOP_SENT_US;
while (STM32_I2C_CR1(port) & (1 << 9)) {
if (timestamp_expired(deadline, NULL)) {
ccprintf("Stop event deadline passed:\ttask=%d"
"\tCR1=%016b\n",
(int)task_get_current(), STM32_I2C_CR1(port));
return EC_ERROR_TIMEOUT;
}
if (is_slow) {
/* If we haven't gotten a fast response, sleep */
usleep(STOP_SENT_RETRY_US);
} else {
/* Check to see if this request is taking a while */
if (timestamp_expired(slow_cutoff, NULL)) {
ccprintf("Stop event taking a while: task=%d",
(int)task_get_current());
is_slow = 1;
}
}
}
return EC_SUCCESS;
}
void LedIndicator::Update(){
if(led_value && blink_low_delay > 0 &&
timestamp_expired(blink_timeout)){
led_value = 0;
fc_led_off();
blink_timeout = timestamp_from_now_us(blink_low_delay * 1000L);
} else if(!led_value && blink_high_delay > 0 &&
timestamp_expired(blink_timeout)){
led_value = 1;
fc_led_on();
blink_timeout = timestamp_from_now_us(blink_high_delay * 1000);
}
}
int16_t hcsr04_read_distance_in_cm(struct hcsr04 *self)
{
if(hcsr04_check_response(self) || self->state == ST_IDLE || timestamp_expired(self->pulse_timeout)){
hcsr04_send_pulse(self);
}
return self->distance;
}
int main(void){
cc3d_init();
printf("SystemCoreClock: %d\n", (int)SystemCoreClock);
// test config read/write (to eeprom)
const char str[] = "Hello World!";
uint8_t buf[13] = {0};
printf("Writing string to config: %s\n", str);
cc3d_write_config((const uint8_t*)str, sizeof(str));
printf("Reading string from config: ");
cc3d_read_config(buf, sizeof(str));
printf("%s\n", buf);
uint8_t led_state = 0;
timestamp_t ts = timestamp_from_now_us(500000);
serial_dev_t flexiport = cc3d_get_flexiport_serial_interface();
unsigned int loop = 0;
while(1){
printf("RC1: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM1));
printf("RC2: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM2));
printf("RC3: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM3));
printf("RC4: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM4));
printf("RC5: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM5));
printf("RC6: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM6));
cc3d_write_pwm(CC3D_OUT_PWM1, 1000 + loop % 1000);
cc3d_write_pwm(CC3D_OUT_PWM2, 1000 + loop % 1000);
cc3d_write_pwm(CC3D_OUT_PWM3, 1000 + loop % 1000);
cc3d_write_pwm(CC3D_OUT_PWM4, 1000 + loop % 1000);
cc3d_write_pwm(CC3D_OUT_PWM5, 1000 + loop % 1000);
cc3d_write_pwm(CC3D_OUT_PWM6, 1000 + loop % 1000);
float ax, ay, az, gx, gy, gz;
cc3d_read_acceleration_g(&ax, &ay, &az);
cc3d_read_angular_velocity_dps(&gx, &gy, &gz);
printf("Time: %ld ", (long int)timestamp_now());
printf("Gyro: %5d %5d %5d, Acc: %5d %5d %5d\n",
(int)(ax * 1000), (int)(ay * 1000), (int)(az * 1000),
(int)(gx * 1000), (int)(gy * 1000), (int)(gz * 1000));
serial_printf(flexiport, "Hello World!\n");
if(timestamp_expired(ts)){
led_state = ~led_state;
if(led_state) cc3d_led_on();
else cc3d_led_off();
ts = timestamp_from_now_us(500000);
loop += 100;
}
}
}
void mwii_process_events(void){
compute_rc_values();
//TIMSK0 |= _BV(TOIE0);
if(timestamp_expired(brd->rc_reset_timeout)){
reset_rc();
}
}
static int interrupt_test(void)
{
timestamp_t deadline = get_time();
deadline.val += SECOND / 2;
while (!timestamp_expired(deadline, NULL))
++main_count;
ccprintf("Interrupt count: %d\n", interrupt_count);
ccprintf("Main thread tick: %d\n", main_count);
TEST_ASSERT(!has_error);
TEST_ASSERT(!in_interrupt_context());
return EC_SUCCESS;
}
static int interrupt_disable_test(void)
{
timestamp_t deadline = get_time();
int start_int_cnt, end_int_cnt;
deadline.val += SECOND / 2;
interrupt_disable();
start_int_cnt = interrupt_count;
while (!timestamp_expired(deadline, NULL))
;
end_int_cnt = interrupt_count;
interrupt_enable();
TEST_ASSERT(start_int_cnt == end_int_cnt);
return EC_SUCCESS;
}
int adc_read_channel(enum adc_channel ch)
{
const struct adc_t *adc = adc_channels + ch;
int value;
int restore_watchdog = 0;
timestamp_t deadline;
if (!adc_powered())
return EC_ERROR_UNKNOWN;
mutex_lock(&adc_lock);
if (adc_watchdog_enabled()) {
restore_watchdog = 1;
adc_disable_watchdog_no_lock();
}
adc_configure(adc->channel);
/* Clear EOC bit */
STM32_ADC_SR &= ~(1 << 1);
/* Start conversion */
STM32_ADC_CR2 |= (1 << 0); /* ADON */
/* Wait for EOC bit set */
deadline.val = get_time().val + ADC_SINGLE_READ_TIMEOUT;
value = ADC_READ_ERROR;
do {
if (adc_conversion_ended()) {
value = STM32_ADC_DR & ADC_READ_MAX;
break;
}
} while (!timestamp_expired(deadline, NULL));
if (restore_watchdog)
adc_enable_watchdog_no_lock();
mutex_unlock(&adc_lock);
return (value == ADC_READ_ERROR) ? ADC_READ_ERROR :
value * adc->factor_mul / adc->factor_div + adc->shift;
}
/**
* Wait until we have received a certain number of bytes
*
* Watch the DMA receive channel until it has the required number of bytes,
* or a timeout occurs
*
* We keep an eye on the NSS line - if this goes high then the transaction is
* over so there is no point in trying to receive the bytes.
*
* @param rxdma RX DMA channel to watch
* @param needed Number of bytes that are needed
* @param nss_regs GPIO register for NSS control line
* @param nss_mask Bit to check in GPIO register (when high, we abort)
* @return 0 if bytes received, -1 if we hit a timeout or NSS went high
*/
static int wait_for_bytes(stm32_dma_chan_t *rxdma, int needed,
uint16_t *nss_reg, uint32_t nss_mask)
{
timestamp_t deadline;
ASSERT(needed <= sizeof(in_msg));
deadline.val = 0;
while (1) {
if (dma_bytes_done(rxdma, sizeof(in_msg)) >= needed)
return 0;
if (REG16(nss_reg) & nss_mask)
return -1;
if (!deadline.val) {
deadline = get_time();
deadline.val += SPI_CMD_RX_TIMEOUT_US;
}
if (timestamp_expired(deadline, NULL))
return -1;
}
}
/**
* Wait for the power button to be released
*
* @param timeout_us Timeout in microseconds, or -1 to wait forever
* @return EC_SUCCESS if ok, or
* EC_ERROR_TIMEOUT if power button failed to release
*/
int power_button_wait_for_release(int timeout_us)
{
timestamp_t deadline;
timestamp_t now = get_time();
deadline.val = now.val + timeout_us;
while (!power_button_is_stable || power_button_is_pressed()) {
now = get_time();
if (timeout_us < 0) {
task_wait_event(-1);
} else if (timestamp_expired(deadline, &now) ||
(task_wait_event(deadline.val - now.val) ==
TASK_EVENT_TIMER)) {
CPRINTS("power button not released in time");
return EC_ERROR_TIMEOUT;
}
}
CPRINTS("power button released in time");
return EC_SUCCESS;
}
static int usb_wait_console(void)
{
timestamp_t deadline = get_time();
int wait_time_us = 1;
if (!is_enabled || !tx_fifo_is_ready())
return EC_SUCCESS;
deadline.val += USB_CONSOLE_TIMEOUT_US;
/*
* If the USB console is not used, Tx buffer would never free up.
* In this case, let's drop characters immediately instead of sitting
* for some time just to time out. On the other hand, if the last
* Tx is good, it's likely the host is there to receive data, and
* we should wait so that we don't clobber the buffer.
*/
if (last_tx_ok) {
while (queue_space(&tx_q) < USB_MAX_PACKET_SIZE || !is_reset) {
if (timestamp_expired(deadline, NULL) ||
in_interrupt_context()) {
last_tx_ok = 0;
return EC_ERROR_TIMEOUT;
}
if (wait_time_us < MSEC)
udelay(wait_time_us);
else
usleep(wait_time_us);
wait_time_us *= 2;
}
return EC_SUCCESS;
}
last_tx_ok = queue_space(&tx_q);
return EC_SUCCESS;
}
/**
* Charge state handler
*
* - detect battery status change
* - new state: INIT
*/
static enum charge_state state_charge(struct charge_state_context *ctx)
{
struct charge_state_data *curr = &ctx->curr;
struct batt_params *batt = &ctx->curr.batt;
int debounce = 0;
int want_current;
int want_voltage;
timestamp_t now;
if (curr->error)
return PWR_STATE_ERROR;
/*
* Some chargers will reset out from underneath us. If this happens,
* reinitialize charging.
*/
if (curr->charging_voltage == 0 ||
curr->charging_current == 0)
return PWR_STATE_REINIT;
if (!curr->ac)
return PWR_STATE_REINIT;
/* Stop charging if charging is no longer allowed */
if (!(batt->flags & BATT_FLAG_WANT_CHARGE)) {
if (charge_request(0, 0))
return PWR_STATE_ERROR;
return PWR_STATE_IDLE;
}
now = get_time();
/*
* Adjust desired voltage to one the charger can actually supply
* or else we'll keep asking for a voltage the charger can't actually
* supply.
*/
want_voltage = charger_closest_voltage(batt->desired_voltage);
if (want_voltage != curr->charging_voltage) {
CPRINTS("Charge voltage %dmV", want_voltage);
if (charge_request(want_voltage, -1))
return PWR_STATE_ERROR;
update_charger_time(ctx, now);
}
/*
* Adjust desired current to one the charger can actually supply before
* we do debouncing, or else we'll keep asking for a current the
* charger can't actually supply.
*/
want_current = charger_closest_current(batt->desired_current);
if (want_current == curr->charging_current) {
/* Tick charger watchdog */
if (!is_charger_expired(ctx, now))
return PWR_STATE_UNCHANGE;
} else if (want_current > curr->charging_current) {
if (!timestamp_expired(ctx->voltage_debounce_time, &now))
return PWR_STATE_UNCHANGE;
} else {
debounce = 1;
}
if (want_current != curr->charging_current) {
CPRINTS("Charge current %dmA @ %dmV",
want_current, batt->desired_voltage);
}
if (charge_request(-1, want_current))
return PWR_STATE_ERROR;
/* Update charger watchdog timer and debounce timer */
update_charger_time(ctx, now);
if (debounce)
ctx->voltage_debounce_time.val = now.val + DEBOUNCE_TIME;
return PWR_STATE_UNCHANGE;
}
void timestamp_delay_us(timestamp_t usec) {
volatile timestamp_t t = timestamp_from_now_us(usec);
//printf("waiting.. %d %d\n", (int)t, (int)timestamp_now());
while(!timestamp_expired(t)); // printf("waiting.. %d %d\n", (int)t, (int)timestamp_now());
}
