From 178462edcdb65d5144b5185551cdc642226be434 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 2 Oct 2012 13:02:57 +0200 Subject: [PATCH] Minor cleanups in debug output and offboard control arming --- ROMFS/scripts/rc.PX4IOAR | 72 ++++++++++--------- .../attitude_estimator_ekf_main.c | 40 +++++------ apps/commander/commander.c | 4 ++ 3 files changed, 63 insertions(+), 53 deletions(-) diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR index 2a771cac48..532dd6a256 100644 --- a/ROMFS/scripts/rc.PX4IOAR +++ b/ROMFS/scripts/rc.PX4IOAR @@ -2,69 +2,75 @@ # # Flight startup script for PX4FMU on PX4IOAR carrier board. # - + +set USB no + echo "[init] doing PX4IOAR startup..." - + # # Start the ORB # uorb start - + +# +# Init the EEPROM +# +echo "[init] eeprom" +eeprom start +if [ -f /eeprom/parameters ] +then + eeprom load_param /eeprom/parameters +fi + # # Start the sensors. # sh /etc/init.d/rc.sensors - + # # Start MAVLink # mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 - + # # Start the commander. # -# XXX this should be ' start'. -# -commander & - +commander start + # # Start the attitude estimator # -# XXX this should be ' start'. -# -attitude_estimator_bm & -#position_estimator & - +attitude_estimator_ekf start + # # Configure PX4FMU for operation with PX4IOAR # -# XXX arguments? +fmu mode_gpio_serial + # -#fmu mode_ +# Fire up the multi rotor attitude controller +# +multirotor_att_control start + +# +# Fire up the AR.Drone interface. +# +ardrone_interface start # -# Fire up the AR.Drone controller. +# Start logging # -# XXX this should be ' start'. -# -ardrone_control -d /dev/ttyS1 -m attitude & +#sdlog start # -# Start looking for a GPS. +# Start GPS capture # -# XXX this should not need to be backgrounded -# -#gps -d /dev/ttyS3 -m all & - -# -# Start logging to microSD if we can -# -#sh /etc/init.d/rc.logging - +gps start + # # startup is done; we don't want the shell because we -# use the same UART for telemetry (dumb). +# use the same UART for telemetry # -echo "[init] startup done, exiting." -exit +echo "[init] startup done" +exit \ No newline at end of file diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 8fa41e1502..46c1a66236 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]); + // /* 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)); - } + // 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; + // 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); + // // 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 3e188eb634..77e4da8501 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);