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 <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#define AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS 1000
const extern AP_HAL::HAL& hal; const extern AP_HAL::HAL& hal;
static bool _start_collect_sample; static bool _start_collect_sample;
static void _snoop(const mavlink_message_t* msg); static void _snoop(const mavlink_message_t* msg);
@ -69,33 +71,41 @@ void AP_AccelCal::update()
if (step != _step) { if (step != _step) {
_step = step; _step = step;
const char *msg; if(_use_gcs_snoop) {
switch (step) { const char *msg;
case ACCELCAL_VEHICLE_POS_LEVEL: switch (step) {
msg = "level"; case ACCELCAL_VEHICLE_POS_LEVEL:
break; msg = "level";
case ACCELCAL_VEHICLE_POS_LEFT: break;
msg = "on its LEFT side"; case ACCELCAL_VEHICLE_POS_LEFT:
break; msg = "on its LEFT side";
case ACCELCAL_VEHICLE_POS_RIGHT: break;
msg = "on its RIGHT side"; case ACCELCAL_VEHICLE_POS_RIGHT:
break; msg = "on its RIGHT side";
case ACCELCAL_VEHICLE_POS_NOSEDOWN: break;
msg = "nose DOWN"; case ACCELCAL_VEHICLE_POS_NOSEDOWN:
break; msg = "nose DOWN";
case ACCELCAL_VEHICLE_POS_NOSEUP: break;
msg = "nose UP"; case ACCELCAL_VEHICLE_POS_NOSEUP:
break; msg = "nose UP";
case ACCELCAL_VEHICLE_POS_BACK: break;
msg = "on its BACK"; case ACCELCAL_VEHICLE_POS_BACK:
break; msg = "on its BACK";
default: break;
fail(); default:
return; 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; break;
} }
@ -167,6 +177,8 @@ void AP_AccelCal::start(GCS_MAVLINK *gcs)
_started = true; _started = true;
_saving = false; _saving = false;
_gcs = gcs; _gcs = gcs;
_use_gcs_snoop = true;
_last_position_request_ms = 0;
_step = 0; _step = 0;
update_status(); 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, ...) void AP_AccelCal::_printf(const char* fmt, ...)
{ {
if (!_gcs) { if (!_gcs) {

6
libraries/AP_AccelCal/AP_AccelCal.h
View File

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