static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up)
{
if (msg_up->valid.vane && vane_callback)
vane_callback(0, msg_up->vane_angle1, msg_up->vane_angle2);
// Fill pressure data
if (msg_up->valid.pressure_absolute && pressure_absolute_callback)
pressure_absolute_callback(0, msg_up->pressure_absolute);
if (msg_up->valid.pressure_differential && pressure_differential_callback)
pressure_differential_callback(0, (32768 + msg_up->pressure_differential));
if (msg_up->valid.adc) {
if(adc_callback) {
adc_callback(msg_up->adc.channels);
}
}
// Fill radio data
if (msg_up->valid.rc && radio_control_callback) {
radio_control.values[RADIO_ROLL] = msg_up->rc_roll;
radio_control.values[RADIO_PITCH] = msg_up->rc_pitch;
radio_control.values[RADIO_YAW] = msg_up->rc_yaw;
radio_control.values[RADIO_THROTTLE] = msg_up->rc_thrust;
radio_control.values[RADIO_MODE] = msg_up->rc_mode;
radio_control.values[RADIO_KILL] = msg_up->rc_kill;
radio_control.values[RADIO_GEAR] = msg_up->rc_gear;
radio_control.values[RADIO_AUX2] = msg_up->rc_aux2;
radio_control.values[RADIO_AUX3] = msg_up->rc_aux3;
radio_control_callback();
}
// always fill status, it may change even when in the case when there is no new data
radio_control.status = msg_up->rc_status;
// Fill IMU data
imuFloat.gyro.p = RATE_FLOAT_OF_BFP(msg_up->gyro.p);
imuFloat.gyro.q = RATE_FLOAT_OF_BFP(msg_up->gyro.q);
imuFloat.gyro.r = RATE_FLOAT_OF_BFP(msg_up->gyro.r);
imuFloat.accel.x = ACCEL_FLOAT_OF_BFP(msg_up->accel.x);
imuFloat.accel.y = ACCEL_FLOAT_OF_BFP(msg_up->accel.y);
imuFloat.accel.z = ACCEL_FLOAT_OF_BFP(msg_up->accel.z);
imuFloat.mag.x = MAG_FLOAT_OF_BFP(msg_up->mag.x);
imuFloat.mag.y = MAG_FLOAT_OF_BFP(msg_up->mag.y);
imuFloat.mag.z = MAG_FLOAT_OF_BFP(msg_up->mag.z);
imuFloat.sample_count = msg_up->imu_tick;
if (msg_up->valid.imu)
rdyb_booz_imu_update(&imuFloat);
}
/**
* @brief Main program.
* @param None
* @retval None
*/
int main(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
// Initialize library
ge_init();
// Initialize LEDs
setup_led_gpio();
led_state = false;
led_speed = false;
// Initialize the USER button as an input
gpio_setup_pin(GE_PBTN2, GPIO_INPUT, false, false);
// Initialize PBTN1
gpio_setup_pin(GE_PBTN1, GPIO_INPUT, false, false);
// Print to serial port
printf("Hello, World!\n");
// Print Hello World
lcd_clear();
lcd_goto(0, 0);
lcd_puts("Hello, World!");
// Setup timer library
// Set minimum timestep to 1ms (number of counts referecned to
// a 72MHz clock)
timer_set_timestep(72000);
// register callback for toggling LEDs every 500ms
led_timer = timer_register(500, &toggle_led, GE_PERIODIC);
timer_start(led_timer);
// set mode to the LED demo
ui_state = LED_DEMO;
// set pwm level for PWM demo
float pwm_level = 0.0;
// setup PWM library
pwm_freq(10000.0);
// setup ADC library
// set sampling rate to 10kHz
adc_set_fs(10000);
// register callback method
adc_callback(&my_adc_callback);
// enable ADC channels
adc_enable_channels(chan_to_conv, NUM_ADC);
adc_initialize_channels();
adc_start();
// keep track of how many times the UI has looped
num_refresh = 0;
/* Infinite loop */
/**
* Handles the user interface state machine
*/
while (1) {
switch (ui_state) {
/**
* User can toggle the flashing speed of the Discovery board
* LEDs. This demos how the timer library can be used.
*/
case LED_DEMO:
//check if button depressed
if (!gpio_read_pin(GE_PBTN2)) {
if (led_speed) {
timer_set_period(led_timer, 500);
led_speed = false;
} else {
timer_set_period(led_timer, 100);
led_speed = true;
}
// wait for button to be released
while (!gpio_read_pin(GE_PBTN2));
}
break;
case PWM_DEMO:
pwm_level = pwm_level + .05;
if (pwm_level > 1.0) pwm_level = 0.0;
pwm_set(PWM_CHAN1, pwm_level);
pwm_set(PWM_CHAN2, pwm_level);
pwm_set(PWM_CHAN3, pwm_level);
pwm_set(PWM_CHAN4, pwm_level);
break;
case ADC_DEMO:
// print results every 10 refreshes
num_refresh++;
if (num_refresh >= 10) {
num_refresh = 0;
printf("%u\t%u\t%u\t%u\n", val[0], val[1], val[2], val[3]);
// printf("%u\t%u\n", val[0], val[1]);
}
break;
case USART_DEMO: ;
if (ge_uart_available()) {
uint8_t c = ge_uart_get();
printf("%c\n", c);
lcd_putc(c);
}
break;
default:
break;
}
// check whether to change state
if (!gpio_read_pin(GE_PBTN1)) {
// stop LED timer if necessary
if (ui_state == LED_DEMO) timer_stop(led_timer);
ui_state++;
if (ui_state >= NUM_STATES) ui_state = LED_DEMO;
change_state();
// wait for button to be released
while (!gpio_read_pin(GE_PBTN1));
}
delay_ms(50);
}
}
