int main(int argc, char **argv) {
vmi_instance_t vmi;
addr_t start_address;
struct timeval ktv_start;
struct timeval ktv_end;
char *vm = argv[1];
int buf_size = atoi(argv[2]);
int loops = atoi(argv[3]);
int mode = atoi(argv[4]);
unsigned char *buf = malloc(buf_size);
int i = 0;
long int diff;
long int *data = malloc(loops * sizeof(long int));
int j = 0;
uint32_t value = 0;
if (mode != 1 && mode != 2) {
printf("invalid mode\n");
return 1;
}
/* initialize the xen access library */
vmi_init(&vmi, VMI_AUTO | VMI_INIT_COMPLETE, vm);
/* find address to work from */
start_address = vmi_translate_ksym2v(vmi, "PsInitialSystemProcess");
start_address = vmi_translate_kv2p(vmi, start_address);
for (i = 0; i < loops; ++i) {
if (mode == 1) {
gettimeofday(&ktv_start, 0);
vmi_read_pa(vmi, start_address, buf, buf_size);
gettimeofday(&ktv_end, 0);
} else {
gettimeofday(&ktv_start, 0);
for (j = 0; j < buf_size / 4; ++j) {
vmi_read_32_pa(vmi, start_address + j * 4, &value);
}
gettimeofday(&ktv_end, 0);
}
print_measurement(ktv_start, ktv_end, &diff);
data[i] = diff;
memset(buf, 0, buf_size);
sleep(1);
}
avg_measurement(data, loops);
vmi_destroy(vmi);
free(buf);
return 0;
}
int main(int argc, char **argv) {
vmi_instance_t vmi;
addr_t vaddr;
struct timeval ktv_start;
struct timeval ktv_end;
char *vm = argv[1];
int loops = atoi(argv[2]);
int i = 0;
long int diff;
long int *data = malloc(loops * sizeof(long int));
/* initialize the xen access library */
vmi_init(&vmi, VMI_AUTO | VMI_INIT_COMPLETE, vm);
for (i = 0; i < loops; ++i) {
gettimeofday(&ktv_start, 0);
vaddr = vmi_translate_ksym2v(vmi, "PsGetCurrentThread");
gettimeofday(&ktv_end, 0);
if (0 == vaddr) {
perror("failed to lookup kernel symbol");
goto error_exit;
}
print_measurement(ktv_start, ktv_end, &diff);
data[i] = diff;
sleep(2);
}
avg_measurement(data, loops);
error_exit: vmi_destroy(vmi);
free(data);
return 0;
}
// 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;
}
