int main(void)
{
//make led pins output
writeDigital(&DDRA, LED_2, true);
writeDigital(&DDRA, LED_3, true);
//make 7SD pins output
DDRC = (1 << SEVEN_SEGMENT_CLOCK) | (1 << SEVEN_SEGMENT_DATA) | (1 << SEVEN_SEGMENT_DATA_ENABLE);
//set all to off state
PORTC = 0x00;
PORTA = 0x00;
//initialize serial, config is 57600:8N1
serialInit_0();
//enable serial interrupts for buffered processing
serialEnableInterrupts_0(true, true);
//testcode for serialDecimal write
imu_data testData;
testData.x = 0;
testData.y = 0;
testData.z = 0;
while(1){
testData.x++;
testData.y+=2;
testData.z+=3;
serialWriteUint16_0(testData.x);
//writeSerial_0('\t');
//writeSerialDecimal_0(testData.y);
//writeSerial_0('\t');
//writeSerialDecimal_0(testData.z);
serialWrite_0('\n');
_delay_ms(200);
}
//enable twi
twiInitialize();
//disable sleep bit
imuEnable();
while(1){
serialWrite_0('s');
imu_data c_val;
imuRead(&c_val);
serialWriteUint16_0(c_val.x);
serialWrite_0('\t');
serialWriteUint16_0(c_val.y);
serialWrite_0('\t');
serialWriteUint16_0(c_val.z);
serialWrite_0('\n');
_delay_ms(400);
}
//do not return from main
while(1){}
}
static void stabilizerTask(void* param)
{
uint32_t lastWakeTime;
//uint32_t tempTime;
uint16_t heartbCounter = 0;
uint16_t attitudeCounter = 0;
uint16_t altHoldCounter = 0;
//uint32_t data[6];
//Wait for the system to be fully started to start stabilization loop
systemWaitStart();
lastWakeTime = xTaskGetTickCount ();
for( ; ;)
{
//tempTime = lastWakeTime;
vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ)); // 500Hz
heartbCounter ++;
/*
if (lastWakeTime < tempTime) {
tempTime = (0 - tempTime) + lastWakeTime;
} else {
tempTime = lastWakeTime - tempTime;
}
*/
while (heartbCounter >= HEART_UPDATE_RATE_DIVIDER) { // 1Hz
MAVLINK(mavlink_msg_heartbeat_send(MAVLINK_COMM_0, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_PREFLIGHT, 0, MAV_STATE_STANDBY);)
heartbCounter = 0;
}
imuRead(&gyro, &acc, &mag);
if (imu6IsCalibrated())
{
// 250HZ
if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER)
{
MahonyAHRSupdateIMU(gyro.y, gyro.x, gyro.z, acc.y, acc.x, acc.z);
//filterUpdate_mars(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z,mag.x,mag.y,mag.z);
//MahonyAHRSupdate(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z,mag.x,mag.y,mag.z);
//MahonyAHRSupdate(gyro.y, gyro.x, gyro.z, acc.y, acc.x, acc.z,mag.y,mag.x,mag.z);
//filterUpdate_mars(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z,mag.x,mag.y,mag.z);
//MahonyAHRSupdate(gyro.y, gyro.x, gyro.z, acc.y, acc.x, acc.z,mag.y,mag.x,mag.z);
sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual);
radRollActual = eulerRollActual * M_PI / 180.0f;
radPitchActual = eulerPitchActual * M_PI / 180.0f;
radYawActual = eulerYawActual * M_PI / 180.0f;
//float yh, xh;
#define yh (mag.y * cos(radRollActual) - mag.z * sin(radRollActual))
#define xh (mag.x*cos(radPitchActual) + mag.y*sin(radRollActual)*sin(radPitchActual) + mag.z * cos(radRollActual)*sin(radPitchActual))
radYawActual = atan2(-yh,xh);
MAVLINK(mavlink_msg_attitude_send(MAVLINK_COMM_0, lastWakeTime, \
radRollActual, radPitchActual, radYawActual, \
gyro.x, gyro.y, gyro.z);)
attitudeCounter = 0;
}
