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