void TimerClass::sig_handler_(int signum)
{
pthread_mutex_lock(&TimerMutex_);
PICInterface.getRX();
Timer.calcdt_();
AHRS.update();
Control.update();
LogMan.update();
// pthread_t logThread;
// pthread_create(&logThread, NULL, thunk<LoggerClass, &LoggerClass::update>, &LogMan);
// std::cout << std::dec << AHRS.rawData_.mag_x << ", " << AHRS.rawData_.mag_y << ", " << AHRS.rawData_.mag_z << std::endl;
// std::cout << PICInterface.pwmwidths.frontright << ", " << PICInterface.pwmwidths.rearright << ", " << PICInterface.pwmwidths.rearleft << ", " << PICInterface.pwmwidths.frontleft << std::endl;
// std::cout << PICInterface.rx.pitch << ", " << PICInterface.rx.roll << ", " << PICInterface.rx.throttle << ", " << PICInterface.rx.yaw << ", " << PICInterface.rx.sw1 << ", " << PICInterface.rx.sw2 << std::endl;
// std::cout << PICInterface.rxWidths.pitch << ", " << PICInterface.rxWidths.roll << ", " << PICInterface.rxWidths.throttle << ", " << PICInterface.rxWidths.yaw << ", " << PICInterface.rxWidths.sw1 << ", " << PICInterface.rxWidths.sw2 << ", " << std::endl;
// std::cout << AHRS.orientation.phi << ", " << AHRS.orientation.psi << ", " << AHRS.orientation.theta << std::endl;
// std::cout << AHRS.rawData_.x << ", " << AHRS.rawData_.y << ", " << AHRS.rawData_.z << ", " << AHRS.rawData_.p << ", " << AHRS.rawData_.q << ", " << AHRS.rawData_.r << std::endl;
// std::cout << AHRS.calibratedData.x << ", " << AHRS.calibratedData.y << ", " << AHRS.calibratedData.z << ", " << AHRS.calibratedData.p << ", " << AHRS.calibratedData.q << ", " << AHRS.calibratedData.r << std::endl;
Timer.compensate_();
pthread_mutex_unlock(&TimerMutex_);
}
void TimerClass::sig_handler_(int signum)
{
pthread_mutex_lock(&TimerMutex_);
//1-Get and Execute Command from remote
remote.exec_remoteCMD();
//2- get attitude of the drone
imu.getAttitude();
//3- Timer dt
Timer.calcdt_();
//4-1 Calculate PID on attitude
if (abs(Timer.ypr_setpoint[YAW])<5) {
Timer.ypr_setpoint[YAW] = imu.ypr[YAW];
}
#ifdef PID_STAB
for (int i=0;i<DIM;i++){
Timer.PIDout[i] =
yprSTAB[i].update_pid_std(Timer.ypr_setpoint[i],
imu.ypr[i],
Timer.dt);
}
// printf("PITCH: %7.2f %7.2f %7.2f\n",Timer.ypr_setpoint[PITCH],
// imu.ypr[PITCH],
// Timer.PIDout[PITCH]);
// printf("ROLL: %7.2f %7.2f %7.2f\n",Timer.ypr_setpoint[ROLL],
// imu.ypr[ROLL],
// Timer.PIDout[ROLL]);
for (int i=0;i<DIM;i++){
Timer.PIDout[i] =
yprRATE[i].update_pid_std(Timer.PIDout[i],
imu.gyro[i],
Timer.dt);
}
// printf("PITCH: %7.2f %7.2f %7.2f\n",Timer.ypr_setpoint[PITCH],
// imu.gyro[PITCH],
// Timer.PIDout[PITCH]);
// printf("ROLL: %7.2f %7.2f %7.2f\n",Timer.ypr_setpoint[ROLL],
// imu.gyro[ROLL],
// Timer.PIDout[ROLL]);
#endif
//4-2 Calculate PID on rotational rate
#ifdef PID_RATE
for (int i=0;i<DIM;i++){
Timer.PIDout[i] =
yprRATE[i].update_pid_std(Timer.ypr_setpoint[i],
imu.gyro[i],
Timer.dt);
}
//printf("%7.2f %7.2f\n",imu.gyro[PITCH],Timer.PIDout[PITCH]);
#endif
if (abs(Timer.ypr_setpoint[YAW])<5) {
//if yaw is used feed directly the ESCs
Timer.PIDout[YAW] = Timer.ypr_setpoint[YAW]*10;
}
//5- ESC update and compensate Timer
// if timer has not been stopped
if (Timer.started){
ESC.update(Timer.thr,Timer.PIDout);
//printf("%7.2f %7.2f\n",Timer.thr,Timer.PIDout[PITCH]);
Timer.compensate_();
}
pthread_mutex_unlock(&TimerMutex_);
}