Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

process MAVLInk packets during gps detection and IMU calibration 

this allows the GCS to connect immediately on startup

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2994 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
tridge60@gmail.com 2011-08-01 08:08:52 +00:00
parent 1e915aae2b
commit 022314ed4c
3 changed files with 55 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
ArduCopterMega/GCS_Mavlink.pde
View File

@ -323,7 +323,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
case MAV_ACTION_CALIBRATE_ACC: case MAV_ACTION_CALIBRATE_ACC:
imu.init_accel(); imu.init_accel(mavlink_delay);
result=1; result=1;
break; break;
@ -1127,5 +1127,55 @@ GCS_MAVLINK::_queued_send()
} }
} }
#endif #endif // GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
static void send_rate(uint8_t low, uint8_t high) {
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(low, high);
#endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.data_stream_send(low,high);
#endif
}
/*
a delay() callback that processes MAVLink packets. We set this as the
callback in long running library initialisation routines to allow
MAVLink to process packets while waiting for the initialisation to
complete
*/
static void mavlink_delay(unsigned long t)
{
unsigned long tstart;
static unsigned long last_1hz, last_3hz, last_10hz, last_50hz;
tstart = millis();
do {
unsigned long tnow = millis();
if (tnow - last_1hz > 1000) {
last_1hz = tnow;
gcs.send_message(MSG_HEARTBEAT);
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.send_message(MSG_HEARTBEAT);
#endif
hil.data_stream_send(1,3);
}
if (tnow - last_3hz > 333) {
last_3hz = tnow;
hil.data_stream_send(3,5);
}
if (tnow - last_10hz > 100) {
last_10hz = tnow;
hil.data_stream_send(5,45);
}
if (tnow - last_50hz > 20) {
last_50hz = tnow;
gcs.update();
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.update();
#endif
hil.data_stream_send(45,1000);
}
delay(1);
} while (millis() - tstart < t);
}

2
ArduCopterMega/motors.pde
View File

@ -62,7 +62,7 @@ static void arm_motors()
//Serial.print(arming_counter, DEC); //Serial.print(arming_counter, DEC);
if (arming_counter > LEVEL_DELAY){ if (arming_counter > LEVEL_DELAY){
//Serial.print("init"); //Serial.print("init");
imu.init_accel(); imu.init_accel(mavlink_delay);
arming_counter = 0; arming_counter = 0;
}else if (arming_counter == DISARM_DELAY){ }else if (arming_counter == DISARM_DELAY){

3
ArduCopterMega/system.pde
View File

@ -177,6 +177,7 @@ static void init_ardupilot()
// Do GPS init // Do GPS init
g_gps = &g_gps_driver; g_gps = &g_gps_driver;
g_gps->init(); // GPS Initialization g_gps->init(); // GPS Initialization
g_gps->callback = mavlink_delay;
// init the GCS // init the GCS
#if GCS_PORT == 3 #if GCS_PORT == 3
@ -317,7 +318,7 @@ static void startup_ground(void)
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
// Warm up and read Gyro offsets // Warm up and read Gyro offsets
// ----------------------------- // -----------------------------
imu.init_gyro(); imu.init_gyro(mavlink_delay);
report_imu(); report_imu();
#endif #endif