int
pidlock_set(const char *lockfile, pid_t pid, enum pidlock_wait lockwait)
{
int fd;
int in_use = 0;
int e, terrno;
fd = open(lockfile, (O_WRONLY | O_CREAT | O_APPEND | O_NONBLOCK), 0600);
if(fd == -1){
return -1;
}
e = padlock_exlock(fd, lockwait);
if(e == -1){
if((errno == EACCES) || (errno == EAGAIN)){
/* lock held by another process, don't unlink it: */
in_use = 1;
}
goto fail;
}
if(pid){
if(do_pid(fd, pid) == -1)
goto fail;
}
if(fchmod(fd, 0644) == -1){
goto fail;
}
/* success: */
return fd;
fail:
terrno = errno;
{
do{
e = close(fd);
}while((e == -1) && (errno == EINTR));
if(!in_use){
padlock_unlock(fd, PADLOCK_NOW);
unlink(lockfile);
}
}
errno = terrno;
return -1;
}
// Nullify measurement-values
void nullify_measurements() {
time_last_control_update = time_since_startup;
acc_zk = (fVector3){.t = time_last_control_update, .x=0.0, .y=0.0, .z=0.0};
acc_n = 0.0;
mag_zk = (fVector3){.t = time_last_control_update, .x=0.0, .y=0.0, .z=0.0};
mag_n = 0.0;
gyro_zk = (fVector3){.t = time_last_control_update, .x=0.0, .y=0.0, .z=0.0};
gyro_n = 0.0;
angle_target = (fVector3){.t = time_last_control_update, .x=0.0, .y=0.0, .z=0.0};
}
// Start controller
void Init_Controller() {
nullify_measurements();
angle_target = (fVector3){.t = 0, .x=0.0, .y=0.0, .z=0.0};
power_target = 0.0;
kalman_init(&kal_pitch);
kalman_init(&kal_roll);
#ifdef LOCAL_TEST_ENVIRONMENT
fp_out = fopen(OUT_FILE, "w");
if (!fp_out) {
printf("Could not open %s for writing\n", OUT_FILE);
}
#endif
}
void Close_Controller() {
#ifdef LOCAL_TEST_ENVIRONMENT
if (fp_out) {
fclose(fp_out);
}
#endif
}
inline void avg_measurement(fVector3* val, float n) {
val->x /= n;
val->y /= n;
val->z /= n;
}
inline int signum(int var) {
return (0 < var) - (var < 0);
}
// Currently this only optimizes for pitch_torque
//
// 7 kN thrust max at 338 us speed-setting with about 25cm arms
// --> 5 Nm/unit torque
// for yaw unknow, assuming 1 Nm/unit for now
//
// Order in which targets will be tried to be met:
// max_power -> pitch -> roll -> yaw
void calculate_motor_power(uint16_t *speeds, float32_t Lx, float32_t Ly, float32_t Lz) {
float32_t m1, m2, m3, m4;
float32_t F = (float32_t) power_target;
// Calc PITCH-axis
float32_t pitch_power = F / 2.0;
float32_t diff_pitch_power = Lx * PITCH_TORQUE_PER_UNIT;
if ( diff_pitch_power > pitch_power) {
m2 = 0.0;
m4 = pitch_power;
} else if ( ( - diff_pitch_power ) > pitch_power ) {
m2 = pitch_power;
m4 = 0.0;
} else {
m2 = pitch_power - diff_pitch_power;
m4 = pitch_power + diff_pitch_power;
}
// Calc ROLL-axis
float32_t roll_power = F / 2.0;
float32_t diff_roll_power = Ly * ROLL_TORQUE_PER_UNIT;
if ( diff_roll_power > roll_power) {
m1 = 0.0;
m3 = roll_power;
} else if ( ( - diff_roll_power ) > roll_power ) {
m1 = roll_power;
m3 = 0.0;
} else {
m1 = roll_power - diff_roll_power;
m3 = roll_power + diff_roll_power;
}
speeds[0] = round(m1);
speeds[1] = round(m2);
speeds[2] = round(m3);
speeds[3] = round(m4);
}
uint8_t update_motor_settings(uint16_t *speeds) {
float32_t dt, current_time, z1, z2;
float32_t pitch_err, pitch_torque, pitch_rate_target, pitch_rate_error;
float32_t roll_err, roll_torque, roll_rate_target, roll_rate_error;
// printf("%i / %i \n", time_since_startup, time_last_control_update);
if (!kalman_initialized && (acc_n > KALMAN_NUM_INIT_MEASUREMENTS)) {
avg_measurement(&acc_zk, acc_n);
avg_measurement(&mag_zk, mag_n);
avg_measurement(&gyro_zk, gyro_n);
kal_pitch.x1 = atan2(acc_zk.y, acc_zk.z);
kal_pitch.x2 = 0.0;
kal_pitch.x3 = (RADS_PER_DEGREE*gyro_zk.y);
kal_roll.x1 = atan2(acc_zk.x, acc_zk.z);
kal_roll.x2 = 0.0;
kal_roll.x3 = (RADS_PER_DEGREE*gyro_zk.x);
kalman_initialized = 1;
return 0;
} else if (!kalman_initialized) {
return 0;
} else if ((time_last_control_update + CONTROL_SAMPLE_RATE < time_since_startup)
&& (acc_n>0) && (gyro_n>0)) {
current_time = time_since_startup;
dt = ( (float32_t) (current_time - time_last_control_update)) / 1000.0;
time_last_control_update = current_time;
// Per meetserie een estimate maken van de huidige waarde door het gemiddelde te nemen
avg_measurement(&acc_zk, acc_n);
avg_measurement(&mag_zk, mag_n);
avg_measurement(&gyro_zk, gyro_n);
z1 = atan2(acc_zk.y, acc_zk.z);
z2 = gyro_zk.y*RADS_PER_DEGREE;
kalman_innovate(&kal_pitch, z1, z2, dt);
#ifndef LOCAL_TEST_ENVIRONMENT
LedAngle(kal_pitch.x1 * DEGREES_PER_RAD * 10);
#endif
pitch_err = kal_pitch.x1 - angle_target.x;
pitch_rate_target = do_pid(&PID_pitch, pitch_err, dt);
pitch_rate_error = kal_pitch.x2 - pitch_rate_target;
pitch_torque = do_pid(&PID_pitch_rate, pitch_rate_error, dt);
#ifdef LOCAL_TEST_ENVIRONMENT
if (fp_out)
{
fprintf(fp_out, "%d\t%.7f\t%.7f\t%.7f\t%.7f\t%.7f\n",
time_since_startup, kal_pitch.x1, kal_pitch.x2, kal_pitch.x3, z1, z2);
}
else {
printf("%d\t%.5f\t%.5f\t%.5f\t%.5f\t%.5f\n",
time_since_startup, kal_pitch.x1, kal_pitch.x2, kal_pitch.x3, z1, z2);
}
#endif
z1 = atan2(acc_zk.x, acc_zk.z);
z2 = gyro_zk.x*RADS_PER_DEGREE;
kalman_innovate(&kal_roll, z1, z2, dt);
roll_err = kal_roll.x1 - angle_target.x;
roll_rate_target = do_pid(&PID_roll, roll_err, dt);
roll_rate_error = kal_roll.x2 - roll_rate_target;
roll_torque = do_pid(&PID_roll_rate, roll_rate_error, dt);
calculate_motor_power(&(speeds[0]), pitch_torque, roll_torque, 0.0);
#ifdef LOCAL_TEST_ENVIRONMENT
printf("%04i %04i %04i %04i %f %f\n", speeds[0], speeds[1], speeds[2], speeds[3], pitch_torque, roll_torque);
#endif
nullify_measurements();
return 1;
}
return 0;
}
// Setup the kalman data struct
void kalman_init(kalman_data *data)
{
data->x1 = 0.0f;
data->x2 = 0.0f;
data->x3 = 0.0f;
// Init P to diagonal matrix with large values since
// the initial state is not known
data->p11 = 1000.0f;
data->p12 = 0.0f;
data->p13 = 0.0f;
data->p21 = 0.0f;
data->p22 = 1000.0f;
data->p23 = 0.0f;
data->p31 = 0.0f;
data->p32 = 0.0f;
data->p33 = 1000.0f;
data->q1 = Q1;
data->q2 = Q2;
data->q3 = Q3;
data->r1 = R1;
data->r2 = R2;
}
