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

AP_InertialSensor: allow MAVLink packets to flow during accelcal 

this uses the snoop functionality of GCS_MAVLink to allow the delay
callback to be used during accel calibration
This commit is contained in:
Andrew Tridgell 2015-03-07 21:30:52 +11:00
parent af7765c57c
commit 434d094993
2 changed files with 29 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.cpp
View File

@ -4,27 +4,38 @@
#include <AP_HAL.h>
#include <GCS_MAVLink.h>
#include "AP_InertialSensor_UserInteract_MAVLink.h"
#include <GCS.h>
extern const AP_HAL::HAL& hal;
// set by _snoop on COMMAND_ACK
static bool _got_ack;
/*
watch for COMMAND_ACK messages
*/
static void _snoop(const mavlink_message_t* msg)
{
if (msg->msgid == MAVLINK_MSG_ID_COMMAND_ACK) {
_got_ack = true;
}
}
bool AP_InertialSensor_UserInteract_MAVLink::blocking_read(void)
{
uint32_t start_ms = hal.scheduler->millis();
/* Wait for a COMMAND_ACK message to be received from the ground station */
// setup snooping of packets so we can see the COMMAND_ACK
_gcs->set_snoop(_snoop);
_got_ack = false;
while (hal.scheduler->millis() - start_ms < 30000U) {
while (!comm_get_available(_chan)) {
hal.scheduler->delay(1);
}
uint8_t c = comm_receive_ch(_chan);
mavlink_message_t msg;
mavlink_status_t status;
if (mavlink_parse_char(_chan, c, &msg, &status)) {
if (msg.msgid == MAVLINK_MSG_ID_COMMAND_ACK) {
return true;
}
hal.scheduler->delay(10);
if (_got_ack) {
_gcs->set_snoop(NULL);
return true;
}
}
hal.console->println_P(PSTR("Timed out waiting for user response"));
_gcs->set_snoop(NULL);
return false;
}
@ -39,9 +50,7 @@ void AP_InertialSensor_UserInteract_MAVLink::_printf_P(const prog_char* fmt, ...
// STATUSTEXT messages should not add linefeed
msg[strlen(msg)-1] = 0;
}
while (comm_get_txspace(_chan) < MAVLINK_NUM_NON_PAYLOAD_BYTES + (int)sizeof(mavlink_statustext_t)) {
hal.scheduler->delay(1);
}
mavlink_msg_statustext_send(_chan, SEVERITY_USER_RESPONSE, msg);
_gcs->send_text(SEVERITY_HIGH, msg);
hal.scheduler->delay(10);
}

8
libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.h
View File

@ -6,18 +6,20 @@
#include <GCS_MAVLink.h>
#include "AP_InertialSensor_UserInteract.h"
class GCS_MAVLINK;
/**
* AP_InertialSensor_UserInteract, implemented in terms of a MAVLink connection
*/
class AP_InertialSensor_UserInteract_MAVLink : public AP_InertialSensor_UserInteract {
public:
AP_InertialSensor_UserInteract_MAVLink(mavlink_channel_t chan) :
_chan(chan) {}
AP_InertialSensor_UserInteract_MAVLink(GCS_MAVLINK *gcs) :
_gcs(gcs) {}
bool blocking_read();
void _printf_P(const prog_char *, ...);
private:
mavlink_channel_t _chan;
GCS_MAVLINK *_gcs;
};
#endif // __AP_INERTIAL_SENSOR_USER_INTERACT_MAVLINK_H__