Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 01:04:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

HIL fixed, fixedwing control fixes

Browse Source
This commit is contained in:
Ivan Ovinnikov
2012-08-07 14:18:09 +02:00
parent 2b09a7914f
commit 9536bfa3ca
6 changed files with 179 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
+64 -57
View File
@@ -156,87 +156,96 @@ int attitude_estimator_bm_main(int argc, char *argv[])
unsigned int loopcounter = 0;
uint64_t last_checkstate_stamp = 0;
/* Main loop*/
while (true) {
/* wait for sensor update */
int ret = poll(fds, 1, 5000);
int ret = poll(fds, 1, 1000);
if (ret < 0) {
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* XXX this means no sensor data - should be critical or emergency */
printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n");
continue;
}
} else {
orb_copy(ORB_ID(sensor_combined), sub_raw, &sensor_combined_s_local);
orb_copy(ORB_ID(sensor_combined), sub_raw, &sensor_combined_s_local);
uint64_t now = hrt_absolute_time();
unsigned int time_elapsed = now - last_run;
last_run = now;
uint64_t now = hrt_absolute_time();
unsigned int time_elapsed = now - last_run;
last_run = now;
//#if 0
//#if 0
if (time_elapsed > loop_interval_alarm) {
//TODO: add warning, cpu overload here
if (overloadcounter == 20) {
printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
overloadcounter = 0;
}
if (time_elapsed > loop_interval_alarm) {
//TODO: add warning, cpu overload here
if (overloadcounter == 20) {
printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
overloadcounter = 0;
overloadcounter++;
}
overloadcounter++;
//#endif
// now = hrt_absolute_time();
/* filter values */
attitude_estimator_bm_update(&sensor_combined_s_local, &euler, &rates, &x_n_b, &y_n_b, &z_n_b);
// time_elapsed = hrt_absolute_time() - now;
// if (blubb == 20)
// {
// printf("Estimator: %lu\n", time_elapsed);
// blubb = 0;
// }
// blubb++;
// if (last_data > 0 && sensor_combined_s_local.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", sensor_combined_s_local.timestamp - last_data);
// printf("%llu -> %llu = %llu\n", last_data, sensor_combined_s_local.timestamp, sensor_combined_s_local.timestamp - last_data);
// last_data = sensor_combined_s_local.timestamp;
/*correct yaw */
// euler.z = euler.z + M_PI;
/* send out */
att.timestamp = sensor_combined_s_local.timestamp;
att.roll = euler.x;
att.pitch = euler.y;
att.yaw = euler.z + M_PI;
if (att.yaw > 2.0f * ((float)M_PI)) {
att.yaw -= 2.0f * ((float)M_PI);
}
att.rollspeed = rates.x;
att.pitchspeed = rates.y;
att.yawspeed = rates.z;
att.R[0][0] = x_n_b.x;
att.R[0][1] = x_n_b.y;
att.R[0][2] = x_n_b.z;
// Broadcast
if (publishing) orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
}
//#endif
// now = hrt_absolute_time();
/* filter values */
attitude_estimator_bm_update(&sensor_combined_s_local, &euler, &rates, &x_n_b, &y_n_b, &z_n_b);
// time_elapsed = hrt_absolute_time() - now;
// if (blubb == 20)
// {
// printf("Estimator: %lu\n", time_elapsed);
// blubb = 0;
// }
// blubb++;
// if (last_data > 0 && sensor_combined_s_local.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", sensor_combined_s_local.timestamp - last_data);
// printf("%llu -> %llu = %llu\n", last_data, sensor_combined_s_local.timestamp, sensor_combined_s_local.timestamp - last_data);
// last_data = sensor_combined_s_local.timestamp;
/*correct yaw */
// euler.z = euler.z + M_PI;
/* send out */
att.timestamp = sensor_combined_s_local.timestamp;
att.roll = euler.x;
att.pitch = euler.y;
att.yaw = euler.z + M_PI;
if (att.yaw > 2.0f * ((float)M_PI)) {
att.yaw -= 2.0f * ((float)M_PI);
}
att.rollspeed = rates.x;
att.pitchspeed = rates.y;
att.yawspeed = rates.z;
att.R[0][0] = x_n_b.x;
att.R[0][1] = x_n_b.y;
att.R[0][2] = x_n_b.z;
// XXX add remaining entries
if (loopcounter % 250 == 0) {
if (hrt_absolute_time() - last_checkstate_stamp > 500000) {
/* Check HIL state */
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
/* switching from non-HIL to HIL mode */
//printf("[attitude_estimator_bm] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
hil_enabled = true;
publishing = false;
close(pub_att);
int ret = close(pub_att);
printf("Closing attitude: %i \n", ret);
/* switching from HIL to non-HIL mode */
@@ -246,11 +255,9 @@ int attitude_estimator_bm_main(int argc, char *argv[])
hil_enabled = false;
publishing = true;
}
last_checkstate_stamp = hrt_absolute_time();
}
// Broadcast
if (publishing) orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
loopcounter++;
}