Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 17:18:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: allow MAVLink packets to flow during accelcal

Browse Source 
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

37
libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.cpp
View File

@ -4,27 +4,38 @@
#include <AP_HAL.h> #include <AP_HAL.h>
#include <GCS_MAVLink.h> #include <GCS_MAVLink.h>
#include "AP_InertialSensor_UserInteract_MAVLink.h" #include "AP_InertialSensor_UserInteract_MAVLink.h"
#include <GCS.h>
extern const AP_HAL::HAL& hal; 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) bool AP_InertialSensor_UserInteract_MAVLink::blocking_read(void)
{ {
uint32_t start_ms = hal.scheduler->millis(); 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 (hal.scheduler->millis() - start_ms < 30000U) {
while (!comm_get_available(_chan)) { hal.scheduler->delay(10);
hal.scheduler->delay(1); if (_got_ack) {
} _gcs->set_snoop(NULL);
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; return true;
} }
} }
}
hal.console->println_P(PSTR("Timed out waiting for user response")); hal.console->println_P(PSTR("Timed out waiting for user response"));
_gcs->set_snoop(NULL);
return false; return false;
} }
@ -39,9 +50,7 @@ void AP_InertialSensor_UserInteract_MAVLink::_printf_P(const prog_char* fmt, ...
// STATUSTEXT messages should not add linefeed // STATUSTEXT messages should not add linefeed
msg[strlen(msg)-1] = 0; msg[strlen(msg)-1] = 0;
} }
while (comm_get_txspace(_chan) < MAVLINK_NUM_NON_PAYLOAD_BYTES + (int)sizeof(mavlink_statustext_t)) { _gcs->send_text(SEVERITY_HIGH, msg);
hal.scheduler->delay(1); hal.scheduler->delay(10);
}
mavlink_msg_statustext_send(_chan, SEVERITY_USER_RESPONSE, msg);
} }

8
libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.h
View File

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