dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Added missing calibration announcement for gyro cal in commander

This commit is contained in:
Lorenz Meier 2012-08-28 08:50:23 +02:00
parent 0bc9cfd0f9
commit 2fca24f803
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
apps/commander/commander.c
View File

@ -446,6 +446,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
{ {
/* set to gyro calibration mode */
status->preflight_gyro_calibration = true;
state_machine_publish(status_pub, status, mavlink_fd);
const int calibration_count = 3000; const int calibration_count = 3000;
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
@ -488,6 +492,10 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
mavlink_log_critical(mavlink_fd, "[commander] Setting Z gyro offset failed!"); mavlink_log_critical(mavlink_fd, "[commander] Setting Z gyro offset failed!");
} }
/* exit to gyro calibration mode */
status->preflight_gyro_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
char offset_output[50]; char offset_output[50];
sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
mavlink_log_info(mavlink_fd, offset_output); mavlink_log_info(mavlink_fd, offset_output);