int main()
{
init_motors();
init_peripherals();
// find out how many cells are connected
u16 batteryCutoff;
while((batteryCutoff = getBatteryVoltage()) == 0);
if(batteryCutoff > 720) // voltage is higher than 6V -> 2cells
batteryCutoff = 720; // cutoff 6V
else
batteryCutoff = 360; // cutoff 3V
calibrate_sensors();
// enable interrupt driven gyro acquisation
startGyro();
u08 c = 0, maxThrust = (batteryCutoff>360)?70:100;
u08 cutBattery = 0;
while(1) {
if(Thrust > maxThrust)
Thrust = maxThrust;
calculate_balance(Thrust, Yaw, Pitch, Roll);
checkForInput();
// check battery
if(((c%100)==0)) {
u16 bat = getBatteryVoltage();
if(bat > 0 && bat <= batteryCutoff) {
if(maxThrust > 0)
maxThrust -= 5;
cutBattery = 1;
}
}
if(!cutBattery || !(c % 50))
PORTC ^= (1<<2);
c++;
//_delay_ms(100);
}
}
void play_simon_infinitely() {
display_draw_number(SIMON_NUMBER);
#ifdef DEBUG_BUILD
printf("\n*** Playing Simon ***\n");
#endif
// Grab the Simon and calibrate the sensors
game_arm_pull_simon();
simon_hover_buttons();
DELAY_MS(PLATFORM_WAIT);
platform_rubiks();
twist_rubiks_clock();
DELAY_MS(PLATFORM_WAIT);
calibrate_sensors(); // I don't think we need calibration with the new transistors
// Stop playing if we exceed 5 rounds
while (1) {
#ifdef DEBUG_BUILD
printf("\nROUND %i\n", u8_roundNum);
#endif
// If it's the first round, hit the start button
if (u8_roundNum == 1) {
// Push the start button and record the colors
simon_push_button(START_SIMON_BUTTON);
record_colors(u8_roundNum);
u8_simonFinished = 0;
u16_milliSeconds = 0;
simon_hover_button(YELLOW_BUTTON);
} else {
// Record a certain number of colors
record_colors(u8_roundNum);
}
// Play back the buttons
DELAY_MS(500);
play_buttons(u8_roundNum);
u8_roundNum++;
}
// Let go of the simon and pull the arms back
twist_rubiks_counter();
DELAY_MS(PLATFORM_WAIT);
platform_up();
DELAY_MS(PLATFORM_WAIT);
simon_retract_buttons();
}
/**
* @brief Main program
* @param None
* @retval None
*/
int main(void)
{
/*!< At this stage the microcontroller clock setting is already configured,
this is done through SystemInit() function which is called from startup
files (startup_stm32f40xx.s/startup_stm32f427x.s) before to branch to
application main.
To reconfigure the default setting of SystemInit() function, refer to
system_stm32f4xx.c file
*/
/* USART configuration -----------------------------------------------------*/
USART_Config();
/* SysTick configuration ---------------------------------------------------*/
SysTickConfig();
/* LEDs configuration ------------------------------------------------------*/
STM_EVAL_LEDInit(LED3);
STM_EVAL_LEDInit(LED4);
STM_EVAL_LEDInit(LED5);
STM_EVAL_LEDInit(LED6);
STM_EVAL_LEDOn(LED3);//orange
STM_EVAL_LEDOn(LED4);//verte
STM_EVAL_LEDOn(LED5);//rouge
STM_EVAL_LEDOn(LED6);//bleue
//PWM config (motor control)
TIM1_Config();
PWM1_Config(10000);
/* Tamper Button Configuration ---------------------------------------------*/
STM_EVAL_PBInit(BUTTON_USER,BUTTON_MODE_GPIO);
//Set motor speed
PWM_SetDC(1, SPEED_100); //PE9 | PC6//ON 2ms
PWM_SetDC(2, SPEED_100); //PE11 | PC 7
PWM_SetDC(3, SPEED_100); //PE13
PWM_SetDC(4, SPEED_100); //PE14
// /* Wait until Tamper Button is released */
while (STM_EVAL_PBGetState(BUTTON_USER));
PWM_SetDC(1, SPEED_0); //PE9 | PC6//ON 2ms
PWM_SetDC(2, SPEED_0); //PE11 | PC 7
PWM_SetDC(3, SPEED_0); //PE13
PWM_SetDC(4, SPEED_0); //PE14
/* Initialization of the accelerometer -------------------------------------*/
MPU6050_I2C_Init();
MPU6050_Initialize();
if (MPU6050_TestConnection()) {
// connection success
STM_EVAL_LEDOff(LED3);
}else{
STM_EVAL_LEDOff(LED4);
}
//Calibration process
// Use the following global variables and access functions
// to calibrate the acceleration sensor
calibrate_sensors();
zeroError();
//Ready to receive message
/* Enable DMA USART RX Stream */
DMA_Cmd(USARTx_RX_DMA_STREAM,ENABLE);
/* Enable USART DMA RX Requsts */
USART_DMACmd(USARTx, USART_DMAReq_Rx, ENABLE);
while(1){
//--------------------------------------------------------
//------ Used to configure the speed controller ----------
//--------------------------------------------------------
// press blue button to force motor at SPEED_100
if (STM_EVAL_PBGetState(BUTTON_USER)){
PWM_SetDC(1, SPEED_100); //PE9 | PC6//ON 2ms
PWM_SetDC(2, SPEED_100); //PE11 | PC 7
PWM_SetDC(3, SPEED_100); //PE13
PWM_SetDC(4, SPEED_100); //PE14
// /* Wait until Tamper Button is released */
while (STM_EVAL_PBGetState(BUTTON_USER));
PWM_SetDC(1, SPEED_0); //PE9 | PC6//ON 2ms
PWM_SetDC(2, SPEED_0); //PE11 | PC 7
PWM_SetDC(3, SPEED_0); //PE13
PWM_SetDC(4, SPEED_0); //PE14
Delay(100);
}
//--------------------------------------------------------
//------ Get gyro information ----------
//--------------------------------------------------------
// Read the raw values.
MPU6050_GetRawAccelGyro(AccelGyro);
// Get the time of reading for rotation computations
unsigned long t_now = millis();
STM_EVAL_LEDToggle(LED5);
// The temperature sensor is -40 to +85 degrees Celsius.
// It is a signed integer.
// According to the datasheet:
// 340 per degrees Celsius, -512 at 35 degrees.
// At 0 degrees: -512 – (340 * 35) = -12412
//dT = ( (double) AccelGyro[TEMP] + 12412.0) / 340.0;
// Convert gyro values to degrees/sec
gyro_x = (AccelGyro[GYRO_X] - base_x_gyro) / FSSEL;
gyro_y = (AccelGyro[GYRO_Y] - base_y_gyro) / FSSEL;
gyro_z = (AccelGyro[GYRO_Z] - base_z_gyro) / FSSEL;
// Get raw acceleration values
accel_x = AccelGyro[ACC_X];
accel_y = AccelGyro[ACC_Y];
accel_z = AccelGyro[ACC_Z];
// Get angle values from accelerometer
//float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2));
float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS2DEGREES;
float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS2DEGREES;
//float accel_angle_z = 0;
//// Compute the (filtered) gyro angles
//Get the value in second, a tick is every 10ms
dt = (t_now - last_read_time)/100.0;
float gyro_angle_x = gyro_x*dt + lastAngle[X];//get_last_x_angle();
float gyro_angle_y = gyro_y*dt + lastAngle[Y];//(get_last_y_angle();
float gyro_angle_z = gyro_z*dt + lastAngle[Z];//get_last_z_angle();
// Compute the drifting gyro angles
float unfiltered_gyro_angle_x = gyro_x*dt + lastGyroAngle[X];//get_last_gyro_x_angle();
float unfiltered_gyro_angle_y = gyro_y*dt + lastGyroAngle[Y];//get_last_gyro_y_angle();
float unfiltered_gyro_angle_z = gyro_z*dt + lastGyroAngle[Z];//get_last_gyro_z_angle();
// Apply the complementary filter to figure out the change in angle – choice of alpha is
// estimated now. Alpha depends on the sampling rate…
float alpha = 0.96;
angle_x = alpha * gyro_angle_x + (1.0 - alpha) * accel_angle_x;
angle_y = alpha * gyro_angle_y + (1.0 - alpha) * accel_angle_y;
angle_z = gyro_angle_z; //Accelerometer doesn’t give z-angle
//printf("%4.2f %4.2f %4.2f\r\n",angle_x,angle_y,angle_z);
//// Update the saved data with the latest values
set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z);
//Stabilisation
// Stable Mode
angl = getAngleFromRC(rcBluetooth[ROLL]);
levelRoll = (getAngleFromRC(rcBluetooth[ROLL]) - angle_x) * PID[LEVELROLL].P;
levelPitch = (getAngleFromRC(rcBluetooth[PITCH]) - angle_y) * PID[LEVELPITCH].P;
// Check if pilot commands are not in hover, don't auto trim
// if ((abs(receiver.getTrimData(ROLL)) > levelOff) || (abs(receiver.getTrimData(PITCH)) > levelOff)) {
// zeroIntegralError();
// }
// else {
PID[LEVELROLL].integratedError = constrain(PID[LEVELROLL].integratedError + (((getAngleFromRC(rcBluetooth[ROLL]) - angle_x) * dt) * PID[LEVELROLL].I), -LEVEL_LIMIT, LEVEL_LIMIT);
PID[LEVELPITCH].integratedError = constrain(PID[LEVELPITCH].integratedError + (((getAngleFromRC(rcBluetooth[PITCH]) + angle_y) * dt) * PID[LEVELROLL].I), -LEVEL_LIMIT, LEVEL_LIMIT);
// }
//motors.setMotorAxisCommand(ROLL,
motor[ROLL] = updatePID(rcBluetooth[ROLL] + levelRoll, gyro_x + 1500, &PID[LEVELGYROROLL],dt) + PID[LEVELROLL].integratedError;//);
//motors.setMotorAxisCommand(PITCH,
motor[PITCH] = updatePID(rcBluetooth[PITCH] + levelPitch, gyro_y + 1500, &PID[LEVELGYROPITCH],dt) + PID[LEVELPITCH].integratedError;//);
getLastSpeedFromMsg();
PWM_SetDC(1, SPEED_0 + SPEED_RANGE*rcSpeed[1] + motor[ROLL] *0.10f); //PE9 | PC6//ON 2ms
//Send data on UART
*(float*)(aTxBuffer) = angle_x;
*(float*)(aTxBuffer+4) = angle_y;
*(float*)(aTxBuffer+8) = angle_z;
*(float*)(aTxBuffer+12) = motor[ROLL];
*(float*)(aTxBuffer+16) = motor[PITCH];
sendTxDMA((uint32_t)aTxBuffer,20);
//Wait a little bit
Delay(3); //30 ms
}
}
void play_simon() {
display_draw_number(SIMON_NUMBER);
#ifdef DEBUG_BUILD
printf("\n*** Playing Simon ***\n");
#endif
// Grab the Simon and calibrate the sensors
game_arm_pull_simon();
simon_hover_buttons();
DELAY_MS(PLATFORM_WAIT);
platform_down();
twist_rubiks_clock();
DELAY_MS(PLATFORM_WAIT);
calibrate_sensors(); // I don't think we need calibration with the new transistors
// Stop playing if we exceed 5 rounds
while (u8_roundNum < 5) {
#ifdef DEBUG_BUILD
printf("\nROUND %i\n", u8_roundNum);
#endif
// If it's the first round, hit the start button
if (u8_roundNum == 1) {
// Push the start button and record the colors
u8_started = 1;
simon_push_button(START_SIMON_BUTTON);
record_colors(u8_roundNum);
u8_started = 0;
if (u8_simonFinished == 1) {
#ifdef DEBUG_BUILD
printf("Giving up\n");
#endif
twist_rubiks_counter();
platform_up();
DELAY_MS(PLATFORM_WAIT);
simon_retract_buttons();
return;
}
#ifdef DEBUG_BUILD
printf("Starting Simon time\n");
#endif
u8_simonFinished = 0;
u16_milliSeconds = 0;
T5CONbits.TON = 1; // Turn on the timer
simon_hover_button(YELLOW_BUTTON);
} else {
// Record a certain number of colors
record_colors(u8_roundNum);
}
// If we've surpassed 15 seconds, leave Simon
if (u8_simonFinished) {
#ifdef DEBUG_BUILD
printf("Stopping Simon Time\n");
#endif
u8_simonFinished = 0;
u16_milliSeconds = 0;
u8_roundNum = 1;
break;
}
// Play back the buttons
DELAY_MS(500);
play_buttons(u8_roundNum);
u8_roundNum++;
}
// Let go of the simon and pull the arms back
T5CONbits.TON = 0;
twist_rubiks_counter();
platform_up();
DELAY_MS(PLATFORM_WAIT);
simon_retract_buttons();
}
int main()
{
float gyro[3], accel[3], mag[3];
float vel[3] = { 0, 0, 0 };
/* Normaly we get/set UAVObjects but this one only needs to be set.
We will never expect to get this from another module*/
AttitudeActualData attitude_actual;
AHRSSettingsData ahrs_settings;
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Delay system */
PIOS_DELAY_Init();
/* Communication system */
PIOS_COM_Init();
/* ADC system */
AHRS_ADC_Config( adc_oversampling );
/* Setup the Accelerometer FS (Full-Scale) GPIO */
PIOS_GPIO_Enable( 0 );
SET_ACCEL_2G;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
/* Magnetic sensor system */
PIOS_I2C_Init();
PIOS_HMC5843_Init();
// Get 3 ID bytes
strcpy(( char * )mag_data.id, "ZZZ" );
PIOS_HMC5843_ReadID( mag_data.id );
#endif
/* SPI link to master */
// PIOS_SPI_Init();
AhrsInitComms();
AHRSCalibrationConnectCallback( calibration_callback );
GPSPositionConnectCallback( gps_callback );
ahrs_state = AHRS_IDLE;
while( !AhrsLinkReady() ) {
AhrsPoll();
while( ahrs_state != AHRS_DATA_READY ) ;
ahrs_state = AHRS_PROCESSING;
downsample_data();
ahrs_state = AHRS_IDLE;
if(( total_conversion_blocks % 50 ) == 0 )
PIOS_LED_Toggle( LED1 );
}
AHRSSettingsGet(&ahrs_settings);
/* Use simple averaging filter for now */
for( int i = 0; i < adc_oversampling; i++ )
fir_coeffs[i] = 1;
fir_coeffs[adc_oversampling] = adc_oversampling;
if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS) {
// compute a data point and initialize INS
downsample_data();
converge_insgps();
}
#ifdef DUMP_RAW
int previous_conversion;
while( 1 ) {
AhrsPoll();
int result;
uint8_t framing[16] = {
0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15
};
while( ahrs_state != AHRS_DATA_READY ) ;
ahrs_state = AHRS_PROCESSING;
if( total_conversion_blocks != previous_conversion + 1 )
PIOS_LED_On( LED1 ); // not keeping up
else
PIOS_LED_Off( LED1 );
previous_conversion = total_conversion_blocks;
downsample_data();
ahrs_state = AHRS_IDLE;;
// Dump raw buffer
result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 ); // framing header
result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) ); // dump block number
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX,
( uint8_t * ) & valid_data_buffer[0],
ADC_OVERSAMPLE *
ADC_CONTINUOUS_CHANNELS *
sizeof( valid_data_buffer[0] ) );
if( result == 0 )
PIOS_LED_Off( LED1 );
else {
PIOS_LED_On( LED1 );
}
}
#endif
timer_start();
/******************* Main EKF loop ****************************/
while( 1 ) {
AhrsPoll();
AHRSCalibrationData calibration;
AHRSCalibrationGet( &calibration );
BaroAltitudeData baro_altitude;
BaroAltitudeGet( &baro_altitude );
GPSPositionData gps_position;
GPSPositionGet( &gps_position );
AHRSSettingsGet(&ahrs_settings);
// Alive signal
if(( total_conversion_blocks % 100 ) == 0 )
PIOS_LED_Toggle( LED1 );
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
// Get magnetic readings
if( PIOS_HMC5843_NewDataAvailable() ) {
PIOS_HMC5843_ReadMag( mag_data.raw.axis );
mag_data.updated = 1;
}
attitude_raw.magnetometers[0] = mag_data.raw.axis[0];
attitude_raw.magnetometers[2] = mag_data.raw.axis[1];
attitude_raw.magnetometers[2] = mag_data.raw.axis[2];
#endif
// Delay for valid data
counter_val = timer_count();
running_counts = counter_val - last_counter_idle_end;
last_counter_idle_start = counter_val;
while( ahrs_state != AHRS_DATA_READY ) ;
counter_val = timer_count();
idle_counts = counter_val - last_counter_idle_start;
last_counter_idle_end = counter_val;
ahrs_state = AHRS_PROCESSING;
downsample_data();
/***************** SEND BACK SOME RAW DATA ************************/
// Hacky - grab one sample from buffer to populate this. Need to send back
// all raw data if it's happening
accel_data.raw.x = valid_data_buffer[0];
accel_data.raw.y = valid_data_buffer[2];
accel_data.raw.z = valid_data_buffer[4];
gyro_data.raw.x = valid_data_buffer[1];
gyro_data.raw.y = valid_data_buffer[3];
gyro_data.raw.z = valid_data_buffer[5];
gyro_data.temp.xy = valid_data_buffer[6];
gyro_data.temp.z = valid_data_buffer[7];
if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS) {
/******************** INS ALGORITHM **************************/
// format data for INS algo
gyro[0] = gyro_data.filtered.x;
gyro[1] = gyro_data.filtered.y;
gyro[2] = gyro_data.filtered.z;
accel[0] = accel_data.filtered.x,
accel[1] = accel_data.filtered.y,
accel[2] = accel_data.filtered.z,
// Note: The magnetometer driver returns registers X,Y,Z from the chip which are
// (left, backward, up). Remapping to (forward, right, down).
mag[0] = -( mag_data.raw.axis[1] - calibration.mag_bias[1] );
mag[1] = -( mag_data.raw.axis[0] - calibration.mag_bias[0] );
mag[2] = -( mag_data.raw.axis[2] - calibration.mag_bias[2] );
INSStatePrediction( gyro, accel,
1 / ( float )EKF_RATE );
INSCovariancePrediction( 1 / ( float )EKF_RATE );
if( gps_updated && gps_position.Status == GPSPOSITION_STATUS_FIX3D ) {
// Compute velocity from Heading and groundspeed
vel[0] =
gps_position.Groundspeed *
cos( gps_position.Heading * M_PI / 180 );
vel[1] =
gps_position.Groundspeed *
sin( gps_position.Heading * M_PI / 180 );
// Completely unprincipled way to make the position variance
// increase as data quality decreases but keep it bounded
// Variance becomes 40 m^2 and 40 (m/s)^2 when no gps
INSSetPosVelVar( 0.004 );
HomeLocationData home;
HomeLocationGet( &home );
float ned[3];
double lla[3] = {( double ) gps_position.Latitude / 1e7, ( double ) gps_position.Longitude / 1e7, ( double )( gps_position.GeoidSeparation + gps_position.Altitude )};
// convert from cm back to meters
double ecef[3] = {( double )( home.ECEF[0] / 100 ), ( double )( home.ECEF[1] / 100 ), ( double )( home.ECEF[2] / 100 )};
LLA2Base( lla, ecef, ( float( * )[3] ) home.RNE, ned );
if( gps_updated ) { //FIXME: Is this correct?
//TOOD: add check for altitude updates
FullCorrection( mag, ned,
vel,
baro_altitude.Altitude );
gps_updated = false;
} else {
GpsBaroCorrection( ned,
vel,
baro_altitude.Altitude );
}
gps_updated = false;
mag_data.updated = 0;
} else if( gps_position.Status == GPSPOSITION_STATUS_FIX3D
&& mag_data.updated == 1 ) {
MagCorrection( mag ); // only trust mags if outdoors
mag_data.updated = 0;
} else {
// Indoors, update with zero position and velocity and high covariance
INSSetPosVelVar( 0.1 );
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;
VelBaroCorrection( vel,
baro_altitude.Altitude );
// MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors
}
attitude_actual.q1 = Nav.q[0];
attitude_actual.q2 = Nav.q[1];
attitude_actual.q3 = Nav.q[2];
attitude_actual.q4 = Nav.q[3];
} else if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_SIMPLE ) {
float q[4];
float rpy[3];
/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
/* Very simple computation of the heading and attitude from accel. */
rpy[2] =
atan2(( mag_data.raw.axis[0] ),
( -1 * mag_data.raw.axis[1] ) ) * 180 /
M_PI;
rpy[1] =
atan2( accel_data.filtered.x,
accel_data.filtered.z ) * 180 / M_PI;
rpy[0] =
atan2( accel_data.filtered.y,
accel_data.filtered.z ) * 180 / M_PI;
RPY2Quaternion( rpy, q );
attitude_actual.q1 = q[0];
attitude_actual.q2 = q[1];
attitude_actual.q3 = q[2];
attitude_actual.q4 = q[3];
}
ahrs_state = AHRS_IDLE;
#ifdef DUMP_FRIENDLY
PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "b: %d\r\n",
total_conversion_blocks );
PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "a: %d %d %d\r\n",
( int16_t )( accel_data.filtered.x * 1000 ),
( int16_t )( accel_data.filtered.y * 1000 ),
( int16_t )( accel_data.filtered.z * 1000 ) );
PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "g: %d %d %d\r\n",
( int16_t )( gyro_data.filtered.x * 1000 ),
( int16_t )( gyro_data.filtered.y * 1000 ),
( int16_t )( gyro_data.filtered.z * 1000 ) );
PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "m: %d %d %d\r\n",
mag_data.raw.axis[0],
mag_data.raw.axis[1],
mag_data.raw.axis[2] );
PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX,
"q: %d %d %d %d\r\n",
( int16_t )( Nav.q[0] * 1000 ),
( int16_t )( Nav.q[1] * 1000 ),
( int16_t )( Nav.q[2] * 1000 ),
( int16_t )( Nav.q[3] * 1000 ) );
#endif
#ifdef DUMP_EKF
uint8_t framing[16] = {
15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1,
0
};
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
// Dump raw buffer
int8_t result;
result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 ); // framing header
result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) ); // dump block number
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX,
( uint8_t * ) & mag_data,
sizeof( mag_data ) );
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX,
( uint8_t * ) & gps_data,
sizeof( gps_data ) );
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX,
( uint8_t * ) & accel_data,
sizeof( accel_data ) );
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX,
( uint8_t * ) & gyro_data,
sizeof( gyro_data ) );
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & Q,
sizeof( float ) * NUMX * NUMX );
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & K,
sizeof( float ) * NUMX * NUMV );
result +=
PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & X,
sizeof( float ) * NUMX * NUMX );
if( result == 0 )
PIOS_LED_Off( LED1 );
else {
PIOS_LED_On( LED1 );
}
#endif
AttitudeActualSet( &attitude_actual );
/*FIXME: This is dangerous. There is no locking for UAVObjects
so it could stomp all over the airspeed/climb rate etc.
This used to be done in the OP module which was bad.
Having ~4ms latency for the round trip makes it worse here.
*/
PositionActualData pos;
PositionActualGet( &pos );
for( int ct = 0; ct < 3; ct++ ) {
pos.NED[ct] = Nav.Pos[ct];
pos.Vel[ct] = Nav.Vel[ct];
}
PositionActualSet( &pos );
static bool was_calibration = false;
AhrsStatusData status;
AhrsStatusGet( &status );
if( was_calibration != status.CalibrationSet ) {
was_calibration = status.CalibrationSet;
if( status.CalibrationSet ) {
calibrate_sensors();
AhrsStatusGet( &status );
status.CalibrationSet = true;
}
}
status.CPULoad = (( float )running_counts /
( float )( idle_counts + running_counts ) ) * 100;
status.IdleTimePerCyle = idle_counts / ( TIMER_RATE / 10000 );
status.RunningTimePerCyle = running_counts / ( TIMER_RATE / 10000 );
status.DroppedUpdates = ekf_too_slow;
AhrsStatusSet( &status );
}
return 0;
}
