void PerpetuatePacket( struct usb_internal_state_struct * ist )
{
if( ist->usb_bufferret )
{
int tosend = ist->usb_bufferret_len;
if( tosend > 8 ) tosend = 8;
uint8_t sendnow[12];
sendnow[0] = 0x80;
if( ist->usb_buffer_status & 0x02 )
{
sendnow[1] = 0b01001011; //DATA1
}
else
{
sendnow[1] = 0b11000011; //DATA0
}
if( ist->usb_bufferret == (uint8_t*)1 ) //Tricky: Empty packet.
{
usb_send_data( sendnow, 2, 3 ); //Force a CRC
ist->usb_bufferret = 0;
ist->usb_bufferret_len = 0;
}
else
{
if( tosend )
ets_memcpy( sendnow+2, ist->usb_bufferret, tosend );
usb_send_data( sendnow, tosend+2, 0 );
ist->last_sent_qty = tosend;
}
}
}
void main(void)
{
char x=10;
char y=10;
CLK->CKDIVR = 0;
disableInterrupts();
Init_GPIO();
Init_TIM1();
Init_Clock();
usb_init();
enableInterrupts();
while(usb_ready == 0)
{
usb_process();
}
while(1)
{
delay(100);
if(get_random_byte()>127)
{
x=-x;
y=-y;
}
data_buffer[0] = 0x00;
data_buffer[1] = x;
data_buffer[2] = y;
data_buffer[3] = 0x00;
usb_send_data(&data_buffer[0], 4, 0);
}
}
void SOF_Callback(void) {
static uint32_t FrameCount = 0;
if (g_usb_deviceState == CONFIGURED) {
if (FrameCount++ == VCOMPORT_IN_FRAME_INTERVAL) {
/* Reset the frame counter */
FrameCount = 0;
/* Check the data to be sent through IN pipe */
usb_send_data();
}
}
}
void usb_handle_class_request_callback(setup_data_packet sdp) {
switch (sdp.bRequest) {
case req_SET_LINE_CODING:
// we now expect the line coding to arrive in the data stage
#ifdef CDC_DEBUG
serial_print_str("SET_LINE ");
#endif
control_mode = cm_CTRL_WRITE_DATA_STAGE_CLASS;
break;
case req_GET_LINE_CODING:
#ifdef CDC_DEBUG
serial_print_str("GET_LINE ");
serial_print_str(" len=");
serial_print_int(sdp.wLength);
serial_putc(' ');
#endif
//control_mode = cm_CTRL_READ_DATA_STAGE_CLASS;
control_mode = cm_CTRL_READ_DATA_STAGE_CLASS;
// need to prime ep0 IN with some funky data here
usb_send_data(/*ep*/ 0, /*data*/ &class_data, /*count*/ 7, /*first*/ 1);
// actually we know this will be the last packet, so go straight to waiting for the status ack
control_mode = cm_CTRL_READ_AWAITING_STATUS;
break;
case req_SET_CONTROL_LINE_STATE:
#ifdef CDC_DEBUG
serial_print_str("scls=");//dtr = bit 0, rts = bit 1
serial_print_int_hex(sdp.wValue);
#endif
// no data, so just ack the status
control_mode = cm_CTRL_WRITE_SENDING_STATUS;
usb_send_status_ack();
// Could put a callback here for your own code when DTR or RTS change
break;
default:
#ifdef CDC_DEBUG
serial_print_str("??r=");
serial_print_int(sdp.bRequest);
#endif
}
}
int main(void)
{
uint32_t currentTime;
// High Speed Telemetry Test Code Begin
char numberString[12];
// High Speed Telemetry Test Code End
RCC_GetClocksFreq(&clocks);
USB_Interrupts_Config();
Set_USBClock();
USB_Init();
// Wait until device configured
//while(bDeviceState != CONFIGURED);
testInit();
LED0_ON;
systemReady = true;
//nrf_tx_mode_no_aa(addr,5,40);
nrf_rx_mode_dual(addr,5,40);
{
uint8_t status = nrf_read_reg(NRF_STATUS);
nrf_write_reg(NRF_WRITE_REG|NRF_STATUS,status); // clear IRQ flags
nrf_write_reg(NRF_FLUSH_RX, 0xff);
nrf_write_reg(NRF_FLUSH_TX, 0xff);
}
while (1)
{
uint8_t buf[64];
static uint8_t last_tx_done = 0;
if(ring_buf_pop(nrf_rx_buffer,buf,32)){
// get data from the adapter
switch(buf[0]){
case SET_ATT:
break;
case SET_MOTOR:
break;
case SET_MODE:
report_mode = buf[1];
break;
}
last_tx_done = 1;
}
if(tx_done){
tx_done = 0;
// report ACK success
last_tx_done = 1;
}
if(ring_buf_pop(nrf_tx_buffer,buf,32)){
if(last_tx_done){
last_tx_done = 0;
nrf_ack_packet(0, buf, 32);
}
}
if (frame_50Hz)
{
int16_t motor_val[4];
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
//memcpy(buf, accelSummedSamples100Hz, 12);
//memcpy(buf+12, gyroSummedSamples100Hz, 12);
//memcpy(buf+24, magSumed, 6);
if(report_mode == DT_ATT){
buf[0] = DT_ATT;
memcpy(buf + 1, &sensors.attitude200Hz[0], 12);
memcpy(buf + 13, &executionTime200Hz, 4);
motor_val[0] = motor[0];
motor_val[1] = motor[1];
motor_val[2] = motor[2];
motor_val[3] = motor[3];
memcpy(buf + 17, motor_val, 8);
usb_send_data(buf , 64);
executionTime50Hz = micros() - currentTime;
}else if(report_mode == DT_SENSOR){
buf[0] = DT_SENSOR;
memcpy(buf + 1, gyroSummedSamples100Hz, 12);
memcpy(buf + 13, accelSummedSamples100Hz, 12);
memcpy(buf + 25, magSumed, 6);
}
//nrf_tx_packet(buf,16);
//if(nrf_rx_packet(buf,16) == NRF_RX_OK)
//{
// LED0_TOGGLE;
//}
ring_buf_push(nrf_tx_buffer, buf, 32);
}
if(frame_10Hz)
{
frame_10Hz = false;
magSumed[XAXIS] = magSum[XAXIS];
magSumed[YAXIS] = magSum[YAXIS];
magSumed[ZAXIS] = magSum[ZAXIS];
magSum[XAXIS] = 0;
magSum[YAXIS] = 0;
magSum[ZAXIS] = 0;
newMagData = true;
}
if (frame_100Hz)
{
frame_100Hz = false;
computeAxisCommands(dt100Hz);
mixTable();
writeServos();
writeMotors();
}
if (frame_200Hz)
{
frame_200Hz = false;
currentTime = micros();
deltaTime200Hz = currentTime - previous200HzTime;
previous200HzTime = currentTime;
dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#endif
#if defined(USE_MADGWICK_AHRS)
MadgwickAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
#if defined(USE_MARG_AHRS)
MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
executionTime200Hz = micros() - currentTime;
}
}
systemInit();
systemReady = true;
while (1)
{
///////////////////////////////
if (frame_50Hz)
{
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
executionTime50Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_10Hz)
{
LED0_TOGGLE;
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
sensors.mag10Hz[XAXIS] = -((float)magSum[XAXIS] / 5.0f * magScaleFactor[XAXIS] - sensorConfig.magBias[XAXIS]);
sensors.mag10Hz[YAXIS] = (float)magSum[YAXIS] / 5.0f * magScaleFactor[YAXIS] - sensorConfig.magBias[YAXIS];
sensors.mag10Hz[ZAXIS] = -((float)magSum[ZAXIS] / 5.0f * magScaleFactor[ZAXIS] - sensorConfig.magBias[ZAXIS]);
magSum[XAXIS] = 0;
magSum[YAXIS] = 0;
magSum[ZAXIS] = 0;
newMagData = true;
pressureAverage = pressureSum / 10;
pressureSum = 0;
calculateTemperature();
calculatePressureAltitude();
sensors.pressureAltitude10Hz = pressureAlt;
serialCom();
if ( EKF_Initialized == false ) EKF_Init( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS] );
executionTime10Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_500Hz)
{
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
dt500Hz = (float)deltaTime500Hz * 0.000001f; // For integrations in 500 Hz loop
computeGyroTCBias();
sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#if defined(USE_CHR6DM_AHRS)
if ( EKF_Initialized == true ) EKF_Predict( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
dt500Hz );
sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
sensors.attitude200Hz[YAW ] = gEstimatedStates.psi;
#endif
executionTime500Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_200Hz)
{
frame_200Hz = false;
currentTime = micros();
deltaTime200Hz = currentTime - previous200HzTime;
previous200HzTime = currentTime;
dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#endif
#if defined(USE_MADGWICK_AHRS)
MadgwickAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
#if defined(USE_MARG_AHRS)
MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
executionTime200Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_100Hz)
{
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
dt100Hz = (float)deltaTime100Hz * 0.000001f; // For integrations in 100 Hz loop
sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel100Hz[XAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[XAXIS], &fourthOrder100Hz[AX_FILTER]);
sensors.accel100Hz[YAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[YAXIS], &fourthOrder100Hz[AY_FILTER]);
sensors.accel100Hz[ZAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[ZAXIS], &fourthOrder100Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro100Hz[ROLL ] = ((float)gyroSummedSamples100Hz[ROLL] / 10.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro100Hz[PITCH] = -((float)gyroSummedSamples100Hz[PITCH] / 10.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro100Hz[YAW ] = -((float)gyroSummedSamples100Hz[YAW] / 10.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#if defined(USE_CHR6DM_AHRS)
if ( EKF_Initialized == true ) EKF_Update( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData );
newMagData = false;
sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
sensors.attitude200Hz[YAW ] = gEstimatedStates.psi;
#endif
computeAxisCommands(dt100Hz);
mixTable();
writeServos();
writeMotors();
// High Speed Telemetry Test Code Begin
if ( highSpeedAccelTelemEnabled == true )
{
// 100 Hz Accels
ftoa(sensors.accel100Hz[XAXIS], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.accel100Hz[YAXIS], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.accel100Hz[ZAXIS], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedGyroTelemEnabled == true )
{
// 100 Hz Gyros
ftoa(sensors.gyro100Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.gyro100Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedRollRateTelemEnabled == true )
{
// Roll Rate, Roll Rate Command
ftoa(sensors.gyro100Hz[ROLL], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[ROLL], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedPitchRateTelemEnabled == true )
{
// Pitch Rate, Pitch Rate Command
ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[PITCH], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedYawRateTelemEnabled == true )
{
// Yaw Rate, Yaw Rate Command
ftoa(sensors.gyro100Hz[YAW], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[YAW], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedAttitudeTelemEnabled == true )
{
// 200 Hz Attitudes
ftoa(sensors.attitude200Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.attitude200Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.attitude200Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n");
}
// High Speed Telemetry Test Code End
executionTime100Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
}
void EP1_IN_Callback(void) {
usb_send_data();
}
/*
* Our main
*/
int main(void)
{
/* Check if the firmware asked to start the bootloader */
if (bootloader_start_var == 0xBEEF)
{
wdt_disable();
start_bootloader();
}
/* Pull PC3 low to enable boot loader */
PORTC_DIRCLR = PIN3_bm; // PC3 input
PORTC.PIN3CTRL = PORT_OPC_PULLUP_gc; // Pullup PC3
_delay_ms(10); // Small delay
if((PORTC_IN & PIN3_bm) == 0) // Check PC3
{
start_bootloader();
}
/* Normal boot */
switch_to_32MHz_clock(); // Switch to 32MHz
_delay_ms(1000); // Wait for power to settle
init_serial_port(); // Initialize serial port
init_dac(); // Init DAC
init_adc(); // Init ADC
init_ios(); // Init IOs
init_calibration(); // Init calibration
enable_interrupts(); // Enable interrupts
init_usb(); // Init USB comms
functional_test(); // Functional test if started for the first time
uint8_t current_fw_mode = MODE_IDLE;
while(1)
{
if (current_fw_mode == MODE_CAP_MES)
{
// If we are in cap measurement mode and have a report to send
if(cap_measurement_loop(&cap_report) == TRUE)
{
maindprintf_P(PSTR("*"));
usb_packet.length = sizeof(cap_report);
usb_packet.command_id = CMD_CAP_MES_REPORT;
memcpy((void*)usb_packet.payload, (void*)&cap_report, sizeof(cap_report));
usb_send_data((uint8_t*)&usb_packet);
}
}
// USB command parser
if (usb_receive_data((uint8_t*)&usb_packet) == TRUE)
{
switch(usb_packet.command_id)
{
case CMD_BOOTLOADER_START:
{
maindprintf_P(PSTR("USB- Bootloader start"));
bootloader_start_var = 0xBEEF;
wdt_enable(WDTO_1S);
while(1);
}
case CMD_PING:
{
maindprintf_P(PSTR("."));
// Ping packet, resend the same one
usb_send_data((uint8_t*)&usb_packet);
break;
}
case CMD_VERSION:
{
maindprintf_P(PSTR("USB- Version\r\n"));
// Version request packet
strcpy((char*)usb_packet.payload, CAPMETER_VER);
usb_packet.length = sizeof(CAPMETER_VER);
usb_send_data((uint8_t*)&usb_packet);
break;
}
case CMD_OE_CALIB_STATE:
{
maindprintf_P(PSTR("USB- Calib state\r\n"));
// Get open ended calibration state.
if (is_platform_calibrated() == TRUE)
{
// Calibrated, return calibration data
usb_packet.length = get_openended_calibration_data(usb_packet.payload);
}
else
{
// Not calibrated, return 0
usb_packet.length = 1;
usb_packet.payload[0] = 0;
}
usb_send_data((uint8_t*)&usb_packet);
break;
}
case CMD_OE_CALIB_START:
{
maindprintf_P(PSTR("USB- Calib start\r\n"));
// Check if we are in idle mode
if (current_fw_mode == MODE_IDLE)
{
// Calibration start
start_openended_calibration(usb_packet.payload[0], usb_packet.payload[1], usb_packet.payload[2]);
usb_packet.length = get_openended_calibration_data(usb_packet.payload);
}
else
{
usb_packet.length = 1;
usb_packet.payload[0] = USB_RETURN_ERROR;
}
usb_send_data((uint8_t*)&usb_packet);
break;
}
case CMD_GET_OE_CALIB:
{
maindprintf_P(PSTR("USB- Calib data\r\n"));
// Get calibration data
usb_packet.length = get_openended_calibration_data(usb_packet.payload);
usb_send_data((uint8_t*)&usb_packet);
break;
}
case CMD_SET_VBIAS:
{
// Check that we are not measuring anything and if so, skip samples and stop oscillation
if (current_fw_mode == MODE_CAP_MES)
{
pause_capacitance_measurement_mode();
}
// Enable and set vbias... can also be called to update it
uint16_t* temp_vbias = (uint16_t*)usb_packet.payload;
uint16_t set_vbias = enable_bias_voltage(*temp_vbias);
uint16_t cur_dacv = get_current_vbias_dac_value();
usb_packet.length = 4;
memcpy((void*)usb_packet.payload, (void*)&set_vbias, sizeof(set_vbias));
memcpy((void*)&usb_packet.payload[2], (void*)&cur_dacv, sizeof(cur_dacv));
usb_send_data((uint8_t*)&usb_packet);
// If we are measuring anything, resume measurements
if (current_fw_mode == MODE_CAP_MES)
{
resume_capacitance_measurement_mode();
}
break;
}
case CMD_DISABLE_VBIAS:
{
// Disable vbias
usb_packet.length = 0;
disable_bias_voltage();
usb_send_data((uint8_t*)&usb_packet);
break;
}
case CMD_CUR_MES_MODE:
{
// Enable current measurement or start another measurement
if (current_fw_mode == MODE_IDLE)
{
set_current_measurement_mode(usb_packet.payload[0]);
current_fw_mode = MODE_CURRENT_MES;
}
// Check if we are in the right mode to start a measurement
if (current_fw_mode == MODE_CURRENT_MES)
{
// We either just set current measurement mode or another measurement was requested
if (get_configured_adc_ampl() != usb_packet.payload[0])
{
// If the amplification isn't the same one as requested
set_current_measurement_mode(usb_packet.payload[0]);
}
// Start measurement
usb_packet.length = 2;
uint16_t return_value = cur_measurement_loop(usb_packet.payload[1]);
memcpy((void*)usb_packet.payload, (void*)&return_value, sizeof(return_value));
usb_send_data((uint8_t*)&usb_packet);
}
else
{
usb_packet.length = 1;
usb_packet.payload[0] = USB_RETURN_ERROR;
usb_send_data((uint8_t*)&usb_packet);
}
break;
}
case CMD_CUR_MES_MODE_EXIT:
{
if (current_fw_mode == MODE_CURRENT_MES)
{
usb_packet.payload[0] = USB_RETURN_OK;
disable_current_measurement_mode();
current_fw_mode = MODE_IDLE;
}
else
{
usb_packet.payload[0] = USB_RETURN_ERROR;
}
usb_packet.length = 1;
usb_send_data((uint8_t*)&usb_packet);
break;
}
case CMD_CAP_REPORT_FREQ:
{
if (current_fw_mode == MODE_IDLE)
{
set_capacitance_report_frequency(usb_packet.payload[0]);
usb_packet.payload[0] = USB_RETURN_OK;
}
else
{
usb_packet.payload[0] = USB_RETURN_ERROR;
}
usb_packet.length = 1;
usb_send_data((uint8_t*)&usb_packet);
break;
}
case CMD_CAP_MES_START:
{
if (current_fw_mode == MODE_IDLE)
{
current_fw_mode = MODE_CAP_MES;
set_capacitance_measurement_mode();
usb_packet.payload[0] = USB_RETURN_OK;
}
else
{
usb_packet.payload[0] = USB_RETURN_ERROR;
}
usb_packet.length = 1;
usb_send_data((uint8_t*)&usb_packet);
break;
}
case CMD_CAP_MES_EXIT:
{
if (current_fw_mode == MODE_CAP_MES)
{
current_fw_mode = MODE_IDLE;
disable_capacitance_measurement_mode();
usb_packet.payload[0] = USB_RETURN_OK;
}
else
{
usb_packet.payload[0] = USB_RETURN_ERROR;
}
usb_packet.length = 1;
usb_send_data((uint8_t*)&usb_packet);
break;
}
case CMD_SET_VBIAS_DAC:
{
uint16_t* requested_dac_val = (uint16_t*)usb_packet.payload;
uint16_t* requested_wait = (uint16_t*)&usb_packet.payload[2];
usb_packet.length = 2;
if (is_ldo_enabled() == TRUE)
{
uint16_t set_vbias = force_vbias_dac_change(*requested_dac_val, *requested_wait);
memcpy((void*)usb_packet.payload, (void*)&set_vbias, sizeof(set_vbias));
}
else
{
usb_packet.payload[0] = 0;
usb_packet.payload[1] = 0;
}
usb_send_data((uint8_t*)&usb_packet);
break;
}
case CMD_RESET_STATE:
{
maindprintf_P(PSTR("USB- Reset\r\n"));
usb_packet.length = 1;
current_fw_mode = MODE_IDLE;
if(is_platform_calibrated() == TRUE)
{
disable_bias_voltage();
disable_current_measurement_mode();
disable_capacitance_measurement_mode();
usb_packet.payload[0] = USB_RETURN_OK;
}
else
{
usb_packet.payload[0] = USB_RETURN_ERROR;
}
usb_send_data((uint8_t*)&usb_packet);
break;
}
case CMD_SET_EEPROM_VALS:
{
uint16_t* addr = (uint16_t*)usb_packet.payload;
uint16_t size = usb_packet.payload[2];
if(((*addr) + size > APP_STORED_DATA_MAX_SIZE) || (size > (RAWHID_RX_SIZE-5)))
{
usb_packet.payload[0] = USB_RETURN_ERROR;
}
else
{
eeprom_write_block((void*)&usb_packet.payload[3], (void*)(EEP_APP_STORED_DATA + (*addr)), size);
usb_packet.payload[0] = USB_RETURN_OK;
}
usb_packet.length = 1;
usb_send_data((uint8_t*)&usb_packet);
break;
}
case CMD_READ_EEPROM_VALS:
{
uint16_t* addr = (uint16_t*)usb_packet.payload;
uint16_t size = usb_packet.payload[2];
if(((*addr) + size > APP_STORED_DATA_MAX_SIZE) || (size > (RAWHID_TX_SIZE-2)))
{
usb_packet.length = 1;
usb_packet.payload[0] = USB_RETURN_ERROR;
}
else
{
usb_packet.length = size;
eeprom_read_block(usb_packet.payload, (void*)(EEP_APP_STORED_DATA + (*addr)), size);
}
usb_send_data((uint8_t*)&usb_packet);
break;
}
default: break;
}
}
}
// Left here for reference
//enable_bias_voltage(850);while(1);
//automated_current_testing();
//ramp_bias_voltage_test();
//voltage_settling_test();
//automated_vbias_testing();
//automated_current_testing();
//peak_to_peak_adc_noise_measurement_test();
//ramp_bias_voltage_test();
//printf("blah\r\n");_delay_ms(3333);
//ramp_current_test();
//functional_test();
//while(1);
//calibrate_thresholds(); // Calibrate vup vlow & thresholds
//calibrate_cur_mos_0nA(); // Calibrate 0nA point and store values in eeprom
//calibrate_current_measurement(); // Calibrate the ADC for current measurements
}
int main(void)
{
/* 2 bit for pre-emption priority, 2 bits for subpriority */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
setup_systick();
enable_tick_count();
setup_io_leds();
setup_io_usb();
init_sensor_config();
GYRO_INIT();
ACC_INIT();
MAG_INIT();
nrf_init();
nrf_detect();
nrf_rx_mode_dual(nrf_addr, 5, 40);
{
uint8_t status = nrf_read_reg(NRF_STATUS);
nrf_write_reg(NRF_FLUSH_RX, 0xff);
nrf_write_reg(NRF_FLUSH_TX, 0xff);
nrf_write_reg(NRF_WRITE_REG|NRF_STATUS,status); // clear IRQ flags
}
pwm_input_init();
USB_Init();
acc_scale_factor = calc_acc_scale(200);
compute_gyro_runtime_bias(sensors.gyro_rt_bias, 1000);
// wait usb ready
//while ((bDeviceState != CONFIGURED)&&(USBConnectTimeOut != 0))
//{}
current_mode = DT_ATT;
// endless loop
while(1)
{
uint8_t buf[64];
if(frame_100Hz){
frame_100Hz = 0;
buf[0] = 0;
if(current_mode == DT_RCDATA){
prepare_rc_data(buf);
usb_send_data(buf,64);
}else if(current_mode == DT_SENSOR){
buf[0] = DT_SENSOR;
buf[1] = 9;
read_raw_gyro((int16_t*)(buf+2));
read_raw_acc((int16_t*)(buf+8));
read_raw_mag((int16_t*)(buf+14));
usb_send_data(buf,64);
}
if(buf[0]){
usb_send_data(buf,64);
}
}
if(sensor_data_ready){
sensor_data_ready = 0;
if(sensors.sumTime_us){
update_AHRS();
if(current_mode == DT_ATT){
buf[0] = DT_ATT;
buf[1] = 3;
sensors.height = 0.0;
memcpy(buf+2,sensors.attitude,sizeof(sensors.attitude) + 4);
usb_send_data(buf,64);
}
LED4_TOGGLE;
LED5_TOGGLE;
LED10_TOGGLE;
}
// process sensor data
}
if(frame_200Hz){
frame_200Hz = 0;
// if L3GD20 already contains gyro data, rising edge will not occur
if( (current_mode == DT_ATT) && (gyro_hungry>1) ){
if(L3GD20_INT2){
int16_t gyro[3];
read_raw_gyro(gyro);
}
}
if(gyro_hungry < 10)
gyro_hungry++;
}
if(frame_1Hz){
frame_1Hz = 0;
LED3_TOGGLE;
}
}
}
void usb_send_data_async(uint8_t ep_in, uint8_t* dat, uint8_t len)
{
if(is_usb_writing)
return;
usb_send_data(ep_in, dat, len);
}
void usb_send_data_sync(uint8_t ep_in, uint8_t* dat, uint8_t len)
{
while(is_usb_writing)
{}
usb_send_data(ep_in, dat, len);
}
void
slip_arch_writeb(unsigned char c)
{
while(usb_send_data(DEV_TO_HOST, &c, 1) != 1);
}
