diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-02 13:02:57 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-02 13:02:57 +0200 |
commit | 178462edcdb65d5144b5185551cdc642226be434 (patch) | |
tree | f1c2d9b05d4777f39b05167cc00432e81b03f24c /apps | |
parent | 5895a2e96619a0c35a4c2383898582bde0ffdb6e (diff) | |
download | px4-firmware-178462edcdb65d5144b5185551cdc642226be434.tar.gz px4-firmware-178462edcdb65d5144b5185551cdc642226be434.tar.bz2 px4-firmware-178462edcdb65d5144b5185551cdc642226be434.zip |
Minor cleanups in debug output and offboard control arming
Diffstat (limited to 'apps')
-rw-r--r-- | apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c | 50 | ||||
-rw-r--r-- | apps/commander/commander.c | 4 |
2 files changed, 29 insertions, 25 deletions
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 8fa41e150..46c1a6623 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -360,31 +360,31 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); uint64_t timing_diff = hrt_absolute_time() - timing_start; - /* print rotation matrix every 200th time */ - if (printcounter % 200 == 0) { - // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n", - // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2], - // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5], - // x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]); - - - // } - - printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); - printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]); - // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), - // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), - // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); - } - - int i = printcounter % 9; - - // for (int i = 0; i < 9; i++) { - char name[10]; - sprintf(name, "xapo #%d", i); - memcpy(dbg.key, name, sizeof(dbg.key)); - dbg.value = x_aposteriori[i]; - orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + // /* print rotation matrix every 200th time */ + // if (printcounter % 200 == 0) { + // // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n", + // // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2], + // // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5], + // // x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]); + + + // // } + + // printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); + // printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]); + // // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), + // // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), + // // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); + // } + + // int i = printcounter % 9; + + // // for (int i = 0; i < 9; i++) { + // char name[10]; + // sprintf(name, "xapo #%d", i); + // memcpy(dbg.key, name, sizeof(dbg.key)); + // dbg.value = x_aposteriori[i]; + // orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); printcounter++; diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 3e188eb63..77e4da850 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1012,8 +1012,12 @@ int commander_thread_main(int argc, char *argv[]) memset(¤t_status, 0, sizeof(current_status)); current_status.state_machine = SYSTEM_STATE_PREFLIGHT; current_status.flag_system_armed = false; + /* neither manual nor offboard control commands have been received */ current_status.offboard_control_signal_found_once = false; current_status.rc_signal_found_once = false; + /* mark all signals lost as long as they haven't been found */ + current_status.rc_signal_lost = true; + current_status.offboard_control_signal_lost = true; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); |