fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Minor cleanups in debug output and offboard control arming

Browse Source
This commit is contained in:
Lorenz Meier 2012-10-02 13:02:57 +02:00
parent 5895a2e966
commit 178462edcd
3 changed files with 63 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
ROMFS/scripts/rc.PX4IOAR
View File

@ -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
#
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 '<command> 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

40
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
View File

@ -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++;

4
apps/commander/commander.c
View File

@ -1012,8 +1012,12 @@ int commander_thread_main(int argc, char *argv[])
memset(&current_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), &current_status);