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

AP_AccelCal: send and receive MAV_CMD_ACCELCAL_VEHICLE_POS

Browse Source 
Message is sent at a maximum of 1Hz
Status text is sent, but if command is received it stops and only uses commands
This commit is contained in:
Francisco Ferreira 2016-11-12 09:49:08 +00:00 committed by Tom Pittenger
parent 7463d4c8f7
commit e763896b6a
2 changed files with 56 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
libraries/AP_AccelCal/AP_AccelCal.cpp
View File

@ -17,6 +17,8 @@
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_HAL/AP_HAL.h>
#define AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS 1000
const extern AP_HAL::HAL& hal;
static bool _start_collect_sample;
static void _snoop(const mavlink_message_t* msg);
@ -69,33 +71,41 @@ void AP_AccelCal::update()
if (step != _step) {
_step = step;
const char *msg;
switch (step) {
case ACCELCAL_VEHICLE_POS_LEVEL:
msg = "level";
break;
case ACCELCAL_VEHICLE_POS_LEFT:
msg = "on its LEFT side";
break;
case ACCELCAL_VEHICLE_POS_RIGHT:
msg = "on its RIGHT side";
break;
case ACCELCAL_VEHICLE_POS_NOSEDOWN:
msg = "nose DOWN";
break;
case ACCELCAL_VEHICLE_POS_NOSEUP:
msg = "nose UP";
break;
case ACCELCAL_VEHICLE_POS_BACK:
msg = "on its BACK";
break;
default:
fail();
return;
if(_use_gcs_snoop) {
const char *msg;
switch (step) {
case ACCELCAL_VEHICLE_POS_LEVEL:
msg = "level";
break;
case ACCELCAL_VEHICLE_POS_LEFT:
msg = "on its LEFT side";
break;
case ACCELCAL_VEHICLE_POS_RIGHT:
msg = "on its RIGHT side";
break;
case ACCELCAL_VEHICLE_POS_NOSEDOWN:
msg = "nose DOWN";
break;
case ACCELCAL_VEHICLE_POS_NOSEUP:
msg = "nose UP";
break;
case ACCELCAL_VEHICLE_POS_BACK:
msg = "on its BACK";
break;
default:
fail();
return;
}
_printf("Place vehicle %s and press any key.", msg);
// setup snooping of packets so we can see the COMMAND_ACK
_gcs->set_snoop(_snoop);
}
_printf("Place vehicle %s and press any key.", msg);
// setup snooping of packets so we can see the COMMAND_ACK
_gcs->set_snoop(_snoop);
}
uint32_t now = AP_HAL::millis();
if (now - _last_position_request_ms > AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS) {
_last_position_request_ms = now;
_gcs->send_accelcal_vehicle_position(step);
}
break;
}
@ -167,6 +177,8 @@ void AP_AccelCal::start(GCS_MAVLINK *gcs)
_started = true;
_saving = false;
_gcs = gcs;
_use_gcs_snoop = true;
_last_position_request_ms = 0;
_step = 0;
update_status();
@ -329,6 +341,18 @@ static void _snoop(const mavlink_message_t* msg)
}
}
bool AP_AccelCal::gcs_vehicle_position(float position)
{
_use_gcs_snoop = false;
if (_status == ACCEL_CAL_WAITING_FOR_ORIENTATION && is_equal((float) _step, position)) {
_start_collect_sample = true;
return true;
}
return false;
}
void AP_AccelCal::_printf(const char* fmt, ...)
{
if (!_gcs) {

6
libraries/AP_AccelCal/AP_AccelCal.h
View File

@ -10,6 +10,7 @@ class AP_AccelCal_Client;
class AP_AccelCal {
public:
AP_AccelCal():
_use_gcs_snoop(true),
_started(false),
_saving(false)
{ update_status(); }
@ -25,12 +26,17 @@ public:
// get the status of the calibrator server as a whole
accel_cal_status_t get_status() { return _status; }
// Set vehicle position sent by the GCS
bool gcs_vehicle_position(float position);
// interface to the clients for registration
static void register_client(AP_AccelCal_Client* client);
private:
GCS_MAVLINK *_gcs;
bool _use_gcs_snoop;
uint32_t _last_position_request_ms;
uint8_t _step;
accel_cal_status_t _status;