void flying_status_return() { uint8 *fa = (uint8*)(get_flying_attitude()); uint8 buf[145]; uint16 crc_value; buf[0] = CTRL_FRAME_START1; buf[1] = CTRL_FRAME_START2; buf[2] = get_aircraft_no()&0xFF; buf[3] = get_aircraft_no()>>8; *(uint32*)(buf+4) = 0x91; buf[8] = 1; buf[9] = 1; buf[10] = CTRL_FRAME_TYPE_FLY_STATUS; // compiler add 4bytes between float data and double data for flying_attitude struct // so we have to copy separately memcpy(buf+11,fa,60); memcpy(buf+71,fa+64,36); *(uint32*)(buf+107) = get_sonar_data(); buf[111] = 0; buf[112] = 0; // next waypoint memcpy(buf+113,(uint8 *)(&ppwm),20);//pwm output buf[133] = get_flying_status()&0xFF; buf[134] = get_flying_status()>>8; buf[135] = 0; // gps status buf[136] = 0; // imu status buf[137] = 0; // AP status :cpu1 or cpu2 buf[138] = get_input_voltage()&0xFF; buf[139] = 0; buf[140] = get_cpu_temperature()/1000; buf[141] = 0; crc_value=crc_checksum16(buf, 142); buf[142] = crc_value&0xFF; buf[143] = crc_value>>8; buf[144] = CTRL_FRAME_END; control_cmd_send(buf, 145); }
/** * \brief ACC example application entry point. * * \return Unused (ANSI-C compatibility). */ int main(void) { uint32_t uc_key; int16_t s_volt = 0; uint32_t ul_value = 0; volatile uint32_t ul_status = 0x0; int32_t l_volt_dac0 = 0; /* Initialize the system */ sysclk_init(); board_init(); /* Initialize debug console */ configure_console(); /* Output example information */ puts(STRING_HEADER); /* Initialize DACC */ /* Enable clock for DACC */ pmc_enable_periph_clk(ID_DACC); /* Reset DACC registers */ dacc_reset(DACC); /* External trigger mode disabled. DACC in free running mode. */ dacc_disable_trigger(DACC, DACC_CHANNEL_0); /* Half word transfer mode */ dacc_set_transfer_mode(DACC, 0); #if (SAM3S) || (SAM3XA) /* Power save: * sleep mode - 0 (disabled) * fast wakeup - 0 (disabled) */ dacc_set_power_save(DACC, 0, 0); #endif /* Enable output channel DACC_CHANNEL */ dacc_enable_channel(DACC, DACC_CHANNEL_0); /* Setup analog current */ dacc_set_analog_control(DACC, DACC_ANALOG_CONTROL); /* Set DAC0 output at ADVREF/2. The DAC formula is: * * (5/6 * VOLT_REF) - (1/6 * VOLT_REF) volt - (1/6 * VOLT_REF) * ----------------------------------- = -------------------------- * MAX_DIGITAL digit * * Here, digit = MAX_DIGITAL/2 */ dacc_write_conversion_data(DACC, MAX_DIGITAL / 2, DACC_CHANNEL_0); l_volt_dac0 = (MAX_DIGITAL / 2) * (2 * VOLT_REF / 3) / MAX_DIGITAL + VOLT_REF / 6; /* Enable clock for AFEC */ afec_enable(AFEC0); struct afec_config afec_cfg; afec_get_config_defaults(&afec_cfg); /* Initialize AFEC */ afec_init(AFEC0, &afec_cfg); struct afec_ch_config afec_ch_cfg; afec_ch_get_config_defaults(&afec_ch_cfg); afec_ch_cfg.gain = AFEC_GAINVALUE_0; afec_ch_set_config(AFEC0, AFEC_CHANNEL_POTENTIOMETER, &afec_ch_cfg); /* * Because the internal ADC offset is 0x200, it should cancel it and shift * down to 0. */ afec_channel_set_analog_offset(AFEC0, AFEC_CHANNEL_POTENTIOMETER, 0x200); afec_set_trigger(AFEC0, AFEC_TRIG_SW); /* Enable channel for potentiometer. */ afec_channel_enable(AFEC0, AFEC_CHANNEL_POTENTIOMETER); /* Enable clock for ACC */ pmc_enable_periph_clk(ID_ACC); /* Initialize ACC */ acc_init(ACC, ACC_MR_SELPLUS_AFE0_AD0, ACC_MR_SELMINUS_DAC0, ACC_MR_EDGETYP_ANY, ACC_MR_INV_DIS); /* Enable ACC interrupt */ NVIC_EnableIRQ(ACC_IRQn); /* Enable */ acc_enable_interrupt(ACC); dsplay_menu(); while (1) { while (usart_read(CONSOLE_UART, &uc_key)) { } printf("input: %c\r\n", uc_key); switch (uc_key) { case 's': case 'S': printf("Input DAC0 output voltage (%d~%d mv): ", (VOLT_REF / 6), (VOLT_REF * 5 / 6)); s_volt = get_input_voltage(); puts("\r"); if (s_volt > 0) { l_volt_dac0 = s_volt; /* The DAC formula is: * * (5/6 * VOLT_REF) - (1/6 * VOLT_REF) volt - (1/6 * VOLT_REF) * ----------------------------------- = -------------------------- * MAX_DIGITAL digit * */ ul_value = ((s_volt - (VOLT_REF / 6)) * (MAX_DIGITAL * 6) / 4) / VOLT_REF; dacc_write_conversion_data(DACC, ul_value, DACC_CHANNEL_0); puts("-I- Set ok\r"); } else { puts("-I- Input voltage is invalid\r"); } break; case 'v': case 'V': /* Start conversion */ afec_start_software_conversion(AFEC0); ul_status = afec_get_interrupt_status(AFEC0); while ((ul_status & AFEC_ISR_EOC0) != AFEC_ISR_EOC0) { ul_status = afec_get_interrupt_status(AFEC0); } /* Conversion is done */ ul_value = afec_channel_get_value(AFEC0, AFEC_CHANNEL_POTENTIOMETER); /* * Convert AFEC sample data to voltage value: * voltage value = (sample data / max. resolution) * reference voltage */ s_volt = (ul_value * VOLT_REF) / MAX_DIGITAL; printf("-I- Voltage on potentiometer(AD0) is %d mv\n\r", s_volt); printf("-I- Voltage on DAC0 is %ld mv \n\r", (long)l_volt_dac0); break; case 'm': case 'M': dsplay_menu(); break; } } }
/** * \brief ACC example application entry point. * * \return Unused (ANSI-C compatibility). */ int main(void) { uint8_t uc_key; int16_t s_volt = 0; uint32_t ul_value = 0; volatile uint32_t ul_status = 0x0; int32_t l_volt_dac0 = 0; /* Initialize the system */ sysclk_init(); board_init(); /* Initialize debug console */ configure_console(); /* Output example information */ puts(STRING_HEADER); /* Initialize DACC */ /* Enable clock for DACC */ pmc_enable_periph_clk(ID_DACC); /* Reset DACC registers */ dacc_reset(DACC); /* External trigger mode disabled. DACC in free running mode. */ dacc_disable_trigger(DACC); /* Half word transfer mode */ dacc_set_transfer_mode(DACC, 0); /* Power save: * sleep mode - 0 (disabled) * fast wake-up - 0 (disabled) */ dacc_set_power_save(DACC, 0, 0); /* Timing: * refresh - 0x08 (1024*8 dacc clocks) * max speed mode - 0 (disabled) * startup time - 0xf (960 dacc clocks) */ dacc_set_timing(DACC, 0x08, 0, 0xf); /* Disable TAG and select output channel DACC_CHANNEL */ dacc_set_channel_selection(DACC, DACC_CHANNEL_0); /* Enable output channel DACC_CHANNEL */ dacc_enable_channel(DACC, DACC_CHANNEL_0); /* Setup analog current */ dacc_set_analog_control(DACC, DACC_ANALOG_CONTROL); /* Set DAC0 output at ADVREF/2. The DAC formula is: * * (5/6 * VOLT_REF) - (1/6 * VOLT_REF) volt - (1/6 * VOLT_REF) * ----------------------------------- = -------------------------- * MAX_DIGITAL digit * * Here, digit = MAX_DIGITAL/2 */ dacc_write_conversion_data(DACC, MAX_DIGITAL / 2); l_volt_dac0 = (MAX_DIGITAL / 2) * (2 * VOLT_REF / 3) / MAX_DIGITAL + VOLT_REF / 6; /* Initialize ADC */ /* Enable clock for ADC */ pmc_enable_periph_clk(ID_ADC); /* * Formula: ADCClock = MCK / ( (PRESCAL+1) * 2 ) * For example, MCK = 64MHZ, PRESCAL = 4, then: * ADCClock = 64 / ((4+1) * 2) = 6.4MHz; */ adc_init(ADC, sysclk_get_cpu_hz(), ADC_CLOCK, ADC_STARTUP_TIME_SETTING); /* Formula: * Startup Time = startup value / ADCClock * Transfer Time = (TRANSFER * 2 + 3) / ADCClock * Tracking Time = (TRACKTIM + 1) / ADCClock * Settling Time = settling value / ADCClock * For example, ADC clock = 6MHz (166.7 ns) * Startup time = 512 / 6MHz = 85.3 us * Transfer Time = (1 * 2 + 3) / 6MHz = 833.3 ns * Tracking Time = (0 + 1) / 6MHz = 166.7 ns * Settling Time = 3 / 6MHz = 500 ns */ /* Set ADC timing */ adc_configure_timing(ADC, ADC_TRACK_SETTING, ADC_SETTLING_TIME_3, ADC_TRANSFER_SETTING); /* Channel 5 has to be compared */ adc_enable_channel(ADC, ADC_CHANNEL_5); //! [acc_enable_clock] /** Enable clock for ACC */ pmc_enable_periph_clk(ID_ACC); //! [acc_enable_clock] //! [acc_init] /** Initialize ACC */ acc_init(ACC, ACC_MR_SELPLUS_AD5, ACC_MR_SELMINUS_DAC0, ACC_MR_EDGETYP_ANY, ACC_MR_INV_DIS); //! [acc_init] //! [acc_irq_enable] /** Enable ACC interrupt */ NVIC_EnableIRQ(ACC_IRQn); /** Enable */ acc_enable_interrupt(ACC); //! [acc_irq_enable] dsplay_menu(); while (1) { while (uart_read(CONSOLE_UART, &uc_key)) { } printf("input: %c\r\n", uc_key); switch (uc_key) { case 's': case 'S': printf("Input DAC0 output voltage (%d~%d mv): ", (VOLT_REF / 6), (VOLT_REF * 5 / 6)); s_volt = get_input_voltage(); puts("\r"); if (s_volt > 0) { l_volt_dac0 = s_volt; /* The DAC formula is: * * (5/6 * VOLT_REF) - (1/6 * VOLT_REF) volt - (1/6 * VOLT_REF) * ----------------------------------- = -------------------------- * MAX_DIGITAL digit * */ ul_value = ((s_volt - (VOLT_REF / 6)) * (MAX_DIGITAL * 6) / 4) / VOLT_REF; dacc_write_conversion_data(DACC, ul_value); puts("-I- Set ok\r"); } else { puts("-I- Input voltage is invalid\r"); } break; case 'v': case 'V': /* Start conversion */ adc_start(ADC); ul_status = adc_get_status(ADC); while ((ul_status & ADC_ISR_EOC5) != ADC_ISR_EOC5) { ul_status = adc_get_status(ADC); } /* Conversion is done */ ul_value = adc_get_channel_value(ADC, ADC_CHANNEL_5); /* * Convert ADC sample data to voltage value: * voltage value = (sample data / max. resolution) * reference voltage */ s_volt = (ul_value * VOLT_REF) / MAX_DIGITAL; printf("-I- Voltage on potentiometer(AD5) is %d mv\n\r", s_volt); printf("-I- Voltage on DAC0 is %ld mv \n\r", (long)l_volt_dac0); break; case 'm': case 'M': dsplay_menu(); break; } } }
void flying_status_return(int transmit_data) { flying_attitude_s *fa = get_flying_attitude(); uint8 buf[256]; uint16 crc_value; int pressure; buf[0] = CTRL_FRAME_START1; buf[1] = CTRL_FRAME_START2; buf[2] = get_aircraft_no()&0xFF; buf[3] = get_aircraft_no()>>8; *(uint16*)(buf+4) = 0x91; buf[6] = 1; buf[7] = 0; buf[8] = 1; buf[9] = 0; buf[10] = CTRL_FRAME_TYPE_FLY_STATUS; *((float *)(buf+11))=fa->roll; *((float *)(buf+15))=fa->pitch; *((float *)(buf+19))=fa->yaw; *((float *)(buf+23))=fa->gx; *((float *)(buf+27))=fa->gy; *((float *)(buf+31))=fa->gz; *((float *)(buf+35)) =fa->ax; *((float *)(buf+39))=fa->ay; *((float *)(buf+43))=fa->az; *((uint32 *)(buf+47))=fa->g_time; *((int *)(buf+51))=fa->vn; *((int *)(buf+55))=fa->ve; *((int *)(buf+59))=fa->vd; *(( int *)(buf+63))=fa->heading; *(( int *)(buf+67))=fa->b_h; *((double *)(buf+71))=fa->lat; *((double *)(buf+79))=fa->Long; *((double *)(buf+87))=fa->g_h; *((float *)(buf+95))=fa->vx; *((float *)(buf+99))=fa->vy; *((float *)(buf+103))=fa->vz; sonar_data=get_sonar_data(); *(uint32*)(buf+107) = sonar_data; buf[111] = gepoint.id & 0xFF ; buf[112] = gepoint.id >> 8 ;// next waypoint read_rc_data(rc_data); if(get_flying_status() == AIRCRAFT_MANUAL_MODE){ memcpy(buf+113,rc_data,14); }else memcpy(buf+113,(uint8 *)(&ppwm),20);//pwm output *(uint16*)(buf+131) = ppwm.c[9]; buf[133] = get_flying_status()&0xFF; buf[134] = get_flying_status()>>8; buf[135] = 0; // gps status buf[136] = 0; // imu status buf[137] = 0; // AP status :cpu1 or cpu2 working_status.input_voltage = get_input_voltage(); working_status.engine_voltage = get_monitor_voltage(); working_status.cpu_temprature = get_cpu_temperature(); buf[138] = working_status.input_voltage&0xFF;//AP power buf[139] = working_status.engine_voltage&0xFF;//UAV power buf[140] = working_status.cpu_temprature/1000; buf[141] = 0; if(transmit_data){ crc_value=crc_checksum16(buf, 142); buf[142] = crc_value&0xFF; buf[143] = crc_value>>8; buf[144] = CTRL_FRAME_END; control_cmd_send(buf, 145); } *(uint16*)(buf+4) = 0xAF; memcpy(buf+142,rc_data,14); pressure = get_altimeter(); *(uint16*)(buf+154)= pressure>>16; *(uint16*)(buf+156)= pressure&0xffff; *(uint16*)(buf+158)=time_estimation.data_return; *(uint16*)(buf+160)=time_estimation.algorithm; crc_value=crc_checksum16(buf, 172); buf[172] = crc_value&0xFF; buf[173] = crc_value>>8; buf[174] = CTRL_FRAME_END; fwrite(buf,175,1,fp_fly_status); }