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);
}
