static qboolean cmd_valid(cmd_t *cmd, int c, qboolean partial) {
int len = strlen(cmd_argv(0));
return (partial ? !strncmp(cmd->name, cmd_argv(0), len)
: !strcmp(cmd->name, cmd_argv(0)))
&& (c < 0 || cmd->clients[c])
&& (cmd->type != CT_NORMAL || cmd->type != CT_PUBLIC || client_active(c));
}
// check active client. if
void tag_auto_switch()
{
client *c = client_active(0);
if (c && c->cache->tags && !(c->cache->tags & current_tag))
{
int i, n = 0; Window w; client *o; tag_descend(i, w, o, current_tag) n++;
if (!n) tag_raise(desktop_to_tag(tag_to_desktop(c->cache->tags)));
}
}
void AP_AccelCal::collect_sample()
{
if (_status != ACCEL_CAL_WAITING_FOR_ORIENTATION) {
return;
}
for(uint8_t i=0; i<_num_clients; i++) {
if (client_active(i) && !_clients[i]->_acal_ready_to_sample()) {
_printf("Not ready to sample");
return;
}
}
AccelCalibrator *cal;
for(uint8_t i=0 ; (cal = get_calibrator(i)) ; i++) {
cal->collect_sample();
}
update_status();
}
void AP_AccelCal::collect_sample()
{
if (_status != ACCEL_CAL_WAITING_FOR_ORIENTATION) {
return;
}
for(uint8_t i=0; i<_num_clients; i++) {
if (client_active(i) && !_clients[i]->_acal_get_ready_to_sample()) {
_printf("Not ready to sample");
return;
}
}
AccelCalibrator *cal;
for(uint8_t i=0 ; (cal = get_calibrator(i)) ; i++) {
cal->collect_sample();
}
// setup snooping of packets so we can see the COMMAND_ACK
_gcs->set_snoop(NULL);
update_status();
}
void AP_AccelCal::update()
{
if (!get_calibrator(0)) {
// no calibrators
return;
}
if (_started) {
update_status();
AccelCalibrator *cal;
uint8_t num_active_calibrators = 0;
for(uint8_t i=0; (cal = get_calibrator(i)); i++) {
num_active_calibrators++;
}
if (num_active_calibrators != _num_active_calibrators) {
fail();
return;
}
if(_start_collect_sample) {
collect_sample();
}
switch(_status) {
case ACCEL_CAL_NOT_STARTED:
fail();
return;
case ACCEL_CAL_WAITING_FOR_ORIENTATION: {
// if we're waiting for orientation, first ensure that all calibrators are on the same step
uint8_t step;
if ((cal = get_calibrator(0)) == nullptr) {
fail();
return;
}
step = cal->get_num_samples_collected()+1;
for(uint8_t i=1 ; (cal = get_calibrator(i)) ; i++) {
if (step != cal->get_num_samples_collected()+1) {
fail();
return;
}
}
// if we're on a new step, print a message describing the step
if (step != _step) {
_step = step;
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);
_waiting_for_mavlink_ack = true;
}
}
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;
}
case ACCEL_CAL_COLLECTING_SAMPLE:
// check for timeout
for(uint8_t i=0; (cal = get_calibrator(i)); i++) {
cal->check_for_timeout();
}
update_status();
if (_status == ACCEL_CAL_FAILED) {
fail();
}
return;
case ACCEL_CAL_SUCCESS:
// save
if (_saving) {
bool done = true;
for(uint8_t i=0; i<_num_clients; i++) {
if (client_active(i) && _clients[i]->_acal_get_saving()) {
done = false;
break;
}
}
if (done) {
success();
}
return;
} else {
for(uint8_t i=0; i<_num_clients; i++) {
if(client_active(i) && _clients[i]->_acal_get_fail()) {
fail();
return;
}
}
for(uint8_t i=0; i<_num_clients; i++) {
if(client_active(i)) {
_clients[i]->_acal_save_calibrations();
}
}
_saving = true;
}
return;
default:
case ACCEL_CAL_FAILED:
fail();
return;
}
} else if (_last_result != ACCEL_CAL_NOT_STARTED) {
// only continuously report if we have ever completed a calibration
uint32_t now = AP_HAL::millis();
if (now - _last_position_request_ms > AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS) {
_last_position_request_ms = now;
switch (_last_result) {
case ACCEL_CAL_SUCCESS:
_gcs->send_accelcal_vehicle_position(ACCELCAL_VEHICLE_POS_SUCCESS);
break;
case ACCEL_CAL_FAILED:
_gcs->send_accelcal_vehicle_position(ACCELCAL_VEHICLE_POS_FAILED);
break;
default:
// should never hit this state
break;
}
}
}
}
void AP_AccelCal::update()
{
if (!get_calibrator(0)) {
// no calibrators
return;
}
if (_started) {
update_status();
AccelCalibrator *cal;
uint8_t num_active_calibrators = 0;
for(uint8_t i=0; (cal = get_calibrator(i)); i++) {
num_active_calibrators++;
}
if (num_active_calibrators != _num_active_calibrators) {
fail();
return;
}
if(_start_collect_sample) {
collect_sample();
_gcs->set_snoop(NULL);
_start_collect_sample = false;
}
switch(_status) {
case ACCEL_CAL_NOT_STARTED:
fail();
return;
case ACCEL_CAL_WAITING_FOR_ORIENTATION: {
// if we're waiting for orientation, first ensure that all calibrators are on the same step
uint8_t step;
if ((cal = get_calibrator(0)) == NULL) {
fail();
return;
}
step = cal->get_num_samples_collected()+1;
for(uint8_t i=1 ; (cal = get_calibrator(i)) ; i++) {
if (step != cal->get_num_samples_collected()+1) {
fail();
return;
}
}
// if we're on a new step, print a message describing the step
if (step != _step) {
_step = step;
const char *msg;
switch (step) {
case 1:
msg = "level";
break;
case 2:
msg = "on its LEFT side";
break;
case 3:
msg = "on its RIGHT side";
break;
case 4:
msg = "nose DOWN";
break;
case 5:
msg = "nose UP";
break;
case 6:
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);
break;
}
case ACCEL_CAL_COLLECTING_SAMPLE:
// check for timeout
for(uint8_t i=0; (cal = get_calibrator(i)); i++) {
cal->check_for_timeout();
}
update_status();
if (_status == ACCEL_CAL_FAILED) {
fail();
}
return;
case ACCEL_CAL_SUCCESS:
// save
if (_saving) {
bool done = true;
for(uint8_t i=0; i<_num_clients; i++) {
if (client_active(i) && _clients[i]->_acal_get_saving()) {
done = false;
break;
}
}
if (done) {
success();
}
return;
} else {
for(uint8_t i=0; i<_num_clients; i++) {
if(client_active(i) && _clients[i]->_acal_get_fail()) {
fail();
return;
}
}
for(uint8_t i=0; i<_num_clients; i++) {
if(client_active(i)) {
_clients[i]->_acal_save_calibrations();
}
}
_saving = true;
}
return;
default:
case ACCEL_CAL_FAILED:
fail();
return;
}
}
}
static void cmd_execute_real(int c, int caller, char *name, int type) {
cmd_stack_push();
s->caller = caller;
parse_cmd(name, -1);
cmd_t *cmd = NULL;
int cmds = 0;
while ((cmd = cmd_find(cmd, c, type, qfalse)) != NULL) {
cmds++;
int start = c;
int end = c;
qboolean switch_screen = qfalse;
int old_type = type;
if (cmd_type_compatible(cmd->type, type))
type = cmd->type;
if ((type == CT_BROADCAST || type == CT_CVAR) && c < 0) {
start = 0;
end = CLIENTS - 1;
} else if (type == CT_BROADCAST_ALL) {
start = -1;
end = CLIENTS - 1;
} else if (type == CT_FIND_FREE && c < 0) {
int i;
qboolean found = qfalse;
for (i = 0; i < CLIENTS && !found; i++) {
if (!client_active(i)) {
start = i;
end = i;
found = qtrue;
switch_screen = qtrue;
break;
}
}
if (!found) {
ui_output(c, "No free client found.\n");
cmd_stack_pop();
return;
}
}
for (s->client = start; s->client <= end; s->client++) {
if (type == CT_SERVER)
client_command(s->client, "%s", cmd_args(0));
else
cmd->f();
if (switch_screen)
set_screen(s->client + 1);
}
type = old_type;
}
if (cmds == 0) {
if (cmd_type_compatible(type, CT_PUBLIC)) {
if (cmd_argv(0)[0])
client_say(c, "Unknown command: \"%s\"", cmd_argv(0));
} else if (!cmd_type_compatible(type, CT_EVENT)) {
ui_output(c, "Unknown command: \"%s\"\n", cmd_argv(0));
}
cmd_stack_pop();
return;
}
cmd_stack_pop();
}
