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 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);
}
}
static void compute_rc_values(void){
timestamp_t t_up, t_down;
#define COMPUTE_RC_CHAN(ch) {\
timestamp_t ticks = timestamp_ticks_to_us(t_down - t_up);\
if(abs(brd->rc_values[ch] - ticks) > 10 && ticks > RC_MIN && ticks < RC_MAX)\
brd->rc_values[ch] = constrain(ticks, 1000, 2000);\
}
uint8_t active = 0;
if(gpio_get_status(GPIO_RC0, &t_up, &t_down) & GP_WENT_LOW){
active = 1;
COMPUTE_RC_CHAN(0);
}
if(gpio_get_status(GPIO_RC1, &t_up, &t_down) & GP_WENT_LOW){
active = 1;
COMPUTE_RC_CHAN(1);
}
if(gpio_get_status(GPIO_RC2, &t_up, &t_down) & GP_WENT_LOW){
active = 1;
COMPUTE_RC_CHAN(2);
}
if(gpio_get_status(GPIO_RC3, &t_up, &t_down) & GP_WENT_LOW){
active = 1;
COMPUTE_RC_CHAN(3);
}
if(active){
brd->rc_reset_timeout = timestamp_from_now_us(1000000);
}
}
static uint8_t hcsr04_send_pulse(struct hcsr04 *self)
{
pio_write_pin(self->gpio, self->trigger_pin, 1);
delay_us(10);
pio_write_pin(self->gpio, self->trigger_pin, 0);
self->state = ST_PULSE_SENT;
self->pulse_timeout = timestamp_from_now_us(HCSR04_PULSE_TIMEOUT);
return 1;
}
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());
}
