diff options
| -rw-r--r-- | ROMFS/scripts/rc.PX4IOAR | 66 | ||||
| -rw-r--r-- | apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c | 50 | ||||
| -rw-r--r-- | apps/commander/commander.c | 4 |
3 files changed, 65 insertions, 55 deletions
diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR index 2a771cac4..532dd6a25 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 '<command> start'. -# -commander & - +commander start + # # Start the attitude estimator # -# XXX this should be '<command> 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 # -# Fire up the AR.Drone controller. +multirotor_att_control start + # -# XXX this should be '<command> start'. +# Fire up the AR.Drone interface. # -ardrone_control -d /dev/ttyS1 -m attitude & +ardrone_interface start # -# Start looking for a GPS. -# -# XXX this should not need to be backgrounded +# Start logging # -#gps -d /dev/ttyS3 -m all & +#sdlog start # -# Start logging to microSD if we can +# Start GPS capture # -#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 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); |