void FlightControl::takeOff() {
FlightControl1.calibrateGyro();
FlightControl1.calibrateAccZ();
takeOffStart = millis();
gyroSetPoint = gyroBias;
flightMode = TakeOff;
}
int main( int argc, char **argv)
{
bool programExit = false;
bool controlMode = false; // false: interactive CMD line, true: auto
ros::init( argc, argv, "flight_control" );
ros::NodeHandle n("~");
ros::Rate loop_rate( 10 );
n.getParam( "controlMode", controlMode );
ROS_INFO( "control mode is %d", controlMode );
//FlightControl flightControl(TTL);
FlightControl *flightControl = new FlightControl(WIFI);
while (!ros::ok());
//system( "rosrun ardrone_autonomy ardrone_driver" );
ROS_INFO( "waiting for drone..." );
// wait till drone is ready
while (!flightControl->check_drone_ready());
flightControl->print_connection_mode();
ROS_INFO( "flight_control start" );
// TODO: start with push button
// TODO: in automode
// count down 10 sec and launch
while (ros::ok() && !programExit)
{
if (controlMode)
{
programExit = flight_auto( flightControl );
}
else
{
programExit = flight_interactive( flightControl );
}
}
ros::spinOnce();
loop_rate.sleep();
return 0;
}
int main(int argc, char *argv[]) {
QCoreApplication a(argc, argv);
a.setApplicationName("Kittinger");
QSettings settings;
Logging log;
GPSManager gpsManager;
SMSManager smsManager;
smsManager.setSmsNumber(settings.value(SMS_NUMBER_KEY).toString());
smsManager.setSimulate(settings.value(SIMULATE_KEY).toBool());
FlightControl fc;
fc.init(&smsManager, &gpsManager);
fc.connect(&fc, SIGNAL(log(QString)), &log, SLOT(log(QString)));
gpsManager.init();
smsManager.init();
fc.reset();
return a.exec();
}
// this code is normaly called in a loop in the original I4Copter code. thats not possible on the simulator.
void flightcontrol_update(const SimQuadCopter::Control &control, SimQuadCopter::QuadCopter &copter)
{
//while(1){
//receive Data
//i4cos_msg_recv( &port, &data, (size_t)16, O_NONBLOCK );
float x,z;
copter.calcAnglesFromAcceleration(x,z);
//flightcontrol.setSetpoint(control.throttle * 12.0f,control.roll,control.pitch);
//flightcontrol.control( -x, -copter.gyroX.getValue() ,z, copter.gyroZ.getValue());
flightcontrol.setSetpoint(control.throttle * 12.0f,control.roll,control.pitch);
flightcontrol.control( z, copter.gyroZ.getValue() ,x, copter.gyroX.getValue());
//waiting for next execution-cycle
//i4cos_msleep(CONTROLTASK_PERIOD_SEC*1000);// *1000 because of millisecond
//}//while
}
// To call the main loop function in the timer interrupt
void wrapper() {
FlightControl * mySelf = (FlightControl *) pointerToObj;
mySelf->mainLoop();
}
void flightcontrol_init()
{
flightcontrol.init();
for(int i=0;i<sizeof(data);++i)data[i]=0;
}
