int main(int argc, char** argv) {
#ifndef _USING_FCGI_
FILE* fin, *fout;
#else
FCGI_FILE *fin, *fout;
#endif
int i, retval;
char req_path[MAXPATHLEN], reply_path[MAXPATHLEN];
char log_path[MAXPATHLEN], path[MAXPATHLEN];
unsigned int counter=0;
char* code_sign_key;
int length = -1;
log_messages.pid = getpid();
bool debug_log = false;
for (i=1; i<argc; i++) {
if (!strcmp(argv[i], "--batch")) {
batch = true;
continue;
} else if (!strcmp(argv[i], "--mark_jobs_done")) {
mark_jobs_done = true;
} else if (!strcmp(argv[i], "--debug_log")) {
debug_log = true;
#ifdef GCL_SIMULATOR
} else if (!strcmp(argv[i], "--simulator")) {
if(!argv[++i]) {
log_messages.printf(MSG_CRITICAL, "%s requires an argument\n\n", argv[--i]);
usage(argv[0]);
exit(1);
}
simtime = atof(argv[i]);
#endif
} else if(!strcmp(argv[i], "-h") || !strcmp(argv[i], "--help")) {
usage(argv[0]);
exit(0);
} else if(!strcmp(argv[i], "-v") || !strcmp(argv[i], "--version")) {
printf("%s\n", SVN_VERSION);
exit(0);
} else if (strlen(argv[i])) {
log_messages.printf(MSG_CRITICAL, "unknown command line argument: %s\n\n", argv[i]);
usage(argv[0]);
exit(1);
}
}
// install a signal handler that catches SIGTERMS sent by Apache if the CGI
// times out.
//
signal(SIGTERM, sigterm_handler);
if (debug_log) {
if (!freopen("debug_log", "w", stderr)) {
fprintf(stderr, "Can't redirect stderr\n");
exit(1);
}
} else {
char *stderr_buffer;
get_log_path(path, "scheduler.log");
#ifndef _USING_FCGI_
char buf[256];
if (!freopen(path, "a", stderr)) {
fprintf(stderr, "Can't redirect stderr\n");
sprintf(buf, "Server can't open log file (%s)", path);
send_message(buf, 3600);
exit(1);
}
#else
FCGI_FILE* f = FCGI::fopen(path, "a");
if (f) {
log_messages.redirect(f);
} else {
char buf[256];
fprintf(stderr, "Can't redirect FCGI log messages\n");
sprintf(buf, "Server can't open log file for FCGI (%s)", path);
send_message(buf, 3600);
exit(1);
}
#endif
// install a larger buffer for stderr. This ensures that
// log information from different scheduler requests running
// in parallel aren't intermingled in the log file.
//
if (config.scheduler_log_buffer) {
stderr_buffer = (char*)malloc(config.scheduler_log_buffer);
if (!stderr_buffer) {
log_messages.printf(MSG_CRITICAL,
"Unable to allocate stderr buffer\n"
);
} else {
#ifdef _USING_FCGI_
retval = setvbuf(
f->stdio_stream, stderr_buffer, _IOFBF,
config.scheduler_log_buffer
);
#else
retval = setvbuf(
stderr, stderr_buffer, _IOFBF, config.scheduler_log_buffer
);
#endif
if (retval) {
log_messages.printf(MSG_CRITICAL,
"Unable to change stderr buffering\n"
);
}
}
}
}
srand(time(0)+getpid());
log_messages.set_debug_level(DEBUG_LEVEL);
#if DUMP_CORE_ON_SEGV
set_core_dump_size_limit();
#endif
retval = config.parse_file();
if (retval) {
log_messages.printf(MSG_CRITICAL,
"Can't parse config.xml: %s\n", boincerror(retval)
);
send_message("Server can't parse configuration file", 3600);
exit(0);
}
log_messages.set_debug_level(config.sched_debug_level);
if (config.sched_debug_level == 4) g_print_queries = true;
gui_urls.init();
project_files.init();
sprintf(path, "%s/code_sign_public", config.key_dir);
retval = read_file_malloc(path, code_sign_key);
if (retval) {
log_messages.printf(MSG_CRITICAL,
"Can't read code sign key file (%s)\n", path
);
send_message("Server can't find key file", 3600);
exit(0);
}
strip_whitespace(code_sign_key);
g_pid = getpid();
#ifdef _USING_FCGI_
//while(FCGI_Accept() >= 0 && counter < MAX_FCGI_COUNT) {
while(FCGI_Accept() >= 0) {
counter++;
log_messages.set_indent_level(0);
#endif
if (config.debug_request_headers) {
log_request_headers(length);
}
if (!debug_log && check_stop_sched()) {
send_message("Project is temporarily shut down for maintenance", 3600);
goto done;
}
if (!ssp) {
attach_to_feeder_shmem();
}
if (!ssp) {
send_message("Server error: can't attach shared memory", 3600);
goto done;
}
if (strlen(config.debug_req_reply_dir)) {
struct stat statbuf;
// the code below is convoluted because,
// instead of going from stdin to stdout directly,
// we go via a pair of disk files
// (this makes it easy to save the input,
// and to know the length of the output).
// NOTE: to use this, you must create group-writeable dirs
// boinc_req and boinc_reply in the project dir
//
sprintf(req_path, "%s/%d_%u_sched_request.xml", config.debug_req_reply_dir, g_pid, counter);
sprintf(reply_path, "%s/%d_%u_sched_reply.xml", config.debug_req_reply_dir, g_pid, counter);
// keep an own 'log' per PID in case general logging fails
// this allows to associate at leas the scheduler request with the client
// IP address (as shown in httpd error log) in case of a crash
sprintf(log_path, "%s/%d_%u_sched.log", config.debug_req_reply_dir, g_pid, counter);
#ifndef _USING_FCGI_
fout = fopen(log_path, "a");
#else
fout = FCGI::fopen(log_path,"a");
#endif
fprintf(fout, "PID: %d Client IP: %s\n", g_pid, get_remote_addr());
fclose(fout);
log_messages.printf(MSG_DEBUG,
"keeping sched_request in %s, sched_reply in %s, custom log in %s\n",
req_path, reply_path, log_path
);
#ifndef _USING_FCGI_
fout = fopen(req_path, "w");
#else
fout = FCGI::fopen(req_path,"w");
#endif
if (!fout) {
log_messages.printf(MSG_CRITICAL,
"can't write request file\n"
);
exit(1);
}
copy_stream(stdin, fout);
fclose(fout);
stat(req_path, &statbuf);
if (length>=0 && (statbuf.st_size != length)) {
log_messages.printf(MSG_CRITICAL,
"Request length %d != CONTENT_LENGTH %d\n",
(int)statbuf.st_size, length
);
}
#ifndef _USING_FCGI_
fin = fopen(req_path, "r");
#else
fin = FCGI::fopen(req_path,"r");
#endif
if (!fin) {
log_messages.printf(MSG_CRITICAL,
"can't read request file\n"
);
exit(1);
}
#ifndef _USING_FCGI_
fout = fopen(reply_path, "w");
#else
fout = FCGI::fopen(reply_path, "w");
#endif
if (!fout) {
log_messages.printf(MSG_CRITICAL,
"can't write reply file\n"
);
exit(1);
}
handle_request(fin, fout, code_sign_key);
fclose(fin);
fclose(fout);
#ifndef _USING_FCGI_
fin = fopen(reply_path, "r");
#else
fin = FCGI::fopen(reply_path, "r");
#endif
if (!fin) {
log_messages.printf(MSG_CRITICAL,
"can't read reply file\n"
);
exit(1);
}
copy_stream(fin, stdout);
fclose(fin);
// if not contacted from a client, don't keep the log files
/* not sure what lead to the assumption of a client setting
CONTENT_LENGTH, but it's wrong at least on our current
project / Apache / Client configuration. Commented out.
if (getenv("CONTENT_LENGTH")) {
unlink(req_path);
unlink(reply_path);
}
*/
#ifndef _USING_FCGI_
} else if (batch) {
while (!feof(stdin)) {
handle_request(stdin, stdout, code_sign_key);
fflush(stdout);
}
#endif
} else {
handle_request(stdin, stdout, code_sign_key);
fflush(stderr);
}
done:
#ifdef _USING_FCGI_
if (config.debug_fcgi) {
log_messages.printf(MSG_NORMAL,
"FCGI: counter: %d\n", counter
);
log_messages.flush();
}
} // do()
if (counter == MAX_FCGI_COUNT) {
fprintf(stderr, "FCGI: counter passed MAX_FCGI_COUNT - exiting..\n");
} else {
fprintf(stderr, "FCGI: FCGI_Accept failed - exiting..\n");
}
// when exiting, write headers back to apache so it won't complain
// about "incomplete headers"
fprintf(stdout,"Content-type: text/plain\n\n");
#endif
if (db_opened) {
boinc_db.close();
}
}
void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg)
{
switch (msg->msgid) {
// If we are currently operating as a proxy for a remote,
// alas we have to look inside each packet to see if it's for us or for the remote
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{
handle_request_data_stream(msg, false);
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
handle_param_request_list(msg);
break;
}
case MAVLINK_MSG_ID_PARAM_SET:
{
handle_param_set(msg, nullptr);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
break;
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// decode
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
uint8_t result = MAV_RESULT_UNSUPPORTED;
// do command
send_text(MAV_SEVERITY_INFO,"Command received: ");
switch(packet.command) {
case MAV_CMD_PREFLIGHT_CALIBRATION:
{
if (is_equal(packet.param1,1.0f)) {
tracker.ins.init_gyro();
if (tracker.ins.gyro_calibrated_ok_all()) {
tracker.ahrs.reset_gyro_drift();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
}
if (is_equal(packet.param3,1.0f)) {
tracker.init_barometer(false);
// zero the altitude difference on next baro update
tracker.nav_status.need_altitude_calibration = true;
result = MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param4,1.0f)) {
// Can't trim radio
result = MAV_RESULT_UNSUPPORTED;
} else if (is_equal(packet.param5,1.0f)) {
result = MAV_RESULT_ACCEPTED;
// start with gyro calibration
tracker.ins.init_gyro();
// reset ahrs gyro bias
if (tracker.ins.gyro_calibrated_ok_all()) {
tracker.ahrs.reset_gyro_drift();
} else {
result = MAV_RESULT_FAILED;
}
// start accel cal
tracker.ins.acal_init();
tracker.ins.get_acal()->start(this);
} else if (is_equal(packet.param5,2.0f)) {
// start with gyro calibration
tracker.ins.init_gyro();
// accel trim
float trim_roll, trim_pitch;
if (tracker.ins.calibrate_trim(trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
}
break;
}
case MAV_CMD_COMPONENT_ARM_DISARM:
if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) {
if (is_equal(packet.param1,1.0f)) {
tracker.arm_servos();
result = MAV_RESULT_ACCEPTED;
} else if (is_zero(packet.param1)) {
tracker.disarm_servos();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_UNSUPPORTED;
}
} else {
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_GET_HOME_POSITION:
send_home(tracker.ahrs.get_home());
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_DO_SET_MODE:
switch ((uint16_t)packet.param1) {
case MAV_MODE_MANUAL_ARMED:
case MAV_MODE_MANUAL_DISARMED:
tracker.set_mode(MANUAL);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_MODE_AUTO_ARMED:
case MAV_MODE_AUTO_DISARMED:
tracker.set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
default:
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_DO_SET_SERVO:
if (tracker.servo_test_set_servo(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;
}
break;
// mavproxy/mavutil sends this when auto command is entered
case MAV_CMD_MISSION_START:
tracker.set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
{
if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) {
// when packet.param1 == 3 we reboot to hold in bootloader
hal.scheduler->reboot(is_equal(packet.param1,3.0f));
result = MAV_RESULT_ACCEPTED;
}
break;
}
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
if (is_equal(packet.param1,1.0f)) {
send_autopilot_version(FIRMWARE_VERSION);
result = MAV_RESULT_ACCEPTED;
}
break;
}
case MAV_CMD_DO_START_MAG_CAL:
case MAV_CMD_DO_ACCEPT_MAG_CAL:
case MAV_CMD_DO_CANCEL_MAG_CAL:
result = tracker.compass.handle_mag_cal_command(packet);
break;
default:
break;
}
mavlink_msg_command_ack_send(
chan,
packet.command,
result);
break;
}
// When mavproxy 'wp sethome'
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
{
// decode
mavlink_mission_write_partial_list_t packet;
mavlink_msg_mission_write_partial_list_decode(msg, &packet);
if (packet.start_index == 0)
{
// New home at wp index 0. Ask for it
waypoint_receiving = true;
waypoint_request_i = 0;
waypoint_request_last = 0;
send_message(MSG_NEXT_WAYPOINT);
waypoint_receiving = true;
}
break;
}
// XXX receive a WP from GCS and store in EEPROM if it is HOME
case MAVLINK_MSG_ID_MISSION_ITEM:
{
// decode
mavlink_mission_item_t packet;
uint8_t result = MAV_MISSION_ACCEPTED;
mavlink_msg_mission_item_decode(msg, &packet);
struct Location tell_command = {};
switch (packet.frame)
{
case MAV_FRAME_MISSION:
case MAV_FRAME_GLOBAL:
{
tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2f; // in as m converted to cm
tell_command.options = 0; // absolute altitude
break;
}
#ifdef MAV_FRAME_LOCAL_NED
case MAV_FRAME_LOCAL_NED: // local (relative to home position)
{
tell_command.lat = 1.0e7f*ToDeg(packet.x/
(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
tell_command.alt = -packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break;
}
#endif
#ifdef MAV_FRAME_LOCAL
case MAV_FRAME_LOCAL: // local (relative to home position)
{
tell_command.lat = 1.0e7f*ToDeg(packet.x/
(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
tell_command.alt = packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break;
}
#endif
case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
{
tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!!
break;
}
default:
result = MAV_MISSION_UNSUPPORTED_FRAME;
break;
}
if (result != MAV_MISSION_ACCEPTED) goto mission_failed;
// Check if receiving waypoints (mission upload expected)
if (!waypoint_receiving) {
result = MAV_MISSION_ERROR;
goto mission_failed;
}
// check if this is the HOME wp
if (packet.seq == 0) {
tracker.set_home(tell_command); // New home in EEPROM
send_text(MAV_SEVERITY_INFO,"New HOME received");
waypoint_receiving = false;
}
mission_failed:
// we are rejecting the mission/waypoint
mavlink_msg_mission_ack_send(
chan,
msg->sysid,
msg->compid,
result);
break;
}
case MAVLINK_MSG_ID_MANUAL_CONTROL:
{
mavlink_manual_control_t packet;
mavlink_msg_manual_control_decode(msg, &packet);
tracker.tracking_manual_control(packet);
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
// decode
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(msg, &packet);
tracker.tracking_update_position(packet);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
// decode
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(msg, &packet);
tracker.tracking_update_pressure(packet);
break;
}
case MAVLINK_MSG_ID_SET_MODE:
{
handle_set_mode(msg, FUNCTOR_BIND(&tracker, &Tracker::mavlink_set_mode, bool, uint8_t));
break;
}
case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
case MAVLINK_MSG_ID_LOG_ERASE:
tracker.in_log_download = true;
/* no break */
case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
if (!tracker.in_mavlink_delay) {
handle_log_message(msg, tracker.DataFlash);
}
break;
case MAVLINK_MSG_ID_LOG_REQUEST_END:
tracker.in_log_download = false;
if (!tracker.in_mavlink_delay) {
handle_log_message(msg, tracker.DataFlash);
}
break;
case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS:
tracker.DataFlash.remote_log_block_status_msg(chan, msg);
break;
case MAVLINK_MSG_ID_SERIAL_CONTROL:
handle_serial_control(msg, tracker.gps);
break;
case MAVLINK_MSG_ID_GPS_INJECT_DATA:
handle_gps_inject(msg, tracker.gps);
break;
case MAVLINK_MSG_ID_GPS_RTCM_DATA:
case MAVLINK_MSG_ID_GPS_INPUT:
tracker.gps.handle_msg(msg);
break;
case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
send_autopilot_version(FIRMWARE_VERSION);
break;
default:
handle_common_message(msg);
break;
} // end switch
} // end handle mavlink
void down_click_handler(ClickRecognizerRef recognizer, void *context)
{
send_message(DOWN);
}
int process_worker_request(worker_data *wd, message *msg, session *s)
{
message *answer = NULL;
char *buf = NULL;
char *fn, *size, *wrt_buf, *rea_buf;
int status;
switch (msg->op) {
case LSD:
buf = malloc(MAX_ARGS_LEN * sizeof *buf);
if (buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, LSD, status);
break;
}
int n_files = list_files(wd->lfs, buf, MAX_ARGS_LEN);
answer = create_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, LSD, n_files, buf);
break;
case EXS:
//We return "N filename" if it doesn't exist in the worker's lfs, or "Y filename" if it does.
buf = malloc((strlen(msg->args) + strlen("Y ") + 1) * sizeof *buf);
if (buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, EXS, status);
break;
}
if (file_exists(wd->lfs, msg->args)) {
sprintf(buf, "Y %s", msg->args);
}
else {
sprintf(buf, "N %s", msg->args);
}
answer = create_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, msg->op, 2, buf);
break;
case EXSCRE:
//We return "N filename" if it doesn't exist in the worker's lfs and is not being created, or "Y filename" otherwise.
buf = malloc((strlen(msg->args) + strlen("Y ") + 1) * sizeof *buf);
if (buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, EXS, status);
break;
}
if (file_exists(wd->lfs, msg->args) || is_file_being_created(wd->lfs, msg->args)) {
sprintf(buf, "Y %s", msg->args);
}
else {
sprintf(buf, "N %s", msg->args);
}
answer = create_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, msg->op, 2, buf);
break;
//If we get DEL from a worker, it means that worker has checked we have the file
//If everything goes ok, we return "DEL OK", if not, we return "DEL errorNo errorDesc"
case DEL:
status = remove_file(wd->lfs, msg->args);
if (status != 0) {
answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, DEL, status);
}
else {
buf = malloc((strlen("OK ") + strlen(msg->args) + 1) * sizeof *buf);
if (buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, DEL, status);
break;
}
sprintf(buf, "OK %s", msg->args);
answer = create_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, DEL, 2, buf);
}
break;
//If we get OPN from a worker, it means that worker has checked we have the file
case OPN:
//If the file exists, we open it. open_file saves the real file descriptor on the localFS struct, so we only need to know if it was opened successfully. Our answer in this case is "OK filename"
status = open_file(wd->lfs, msg->args, msg->thread_id);
if (status != 0) {
answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, OPN, status);
}
else {
buf = malloc((strlen("OK ") + strlen(msg->args) + 1) * sizeof *buf);
if (buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, OPN, status);
break;
}
sprintf(buf, "OK %s", msg->args);
answer = create_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, OPN, 2, buf);
}
break;
case WRT:
fn = strtok(msg->args, " ");
size = strtok(NULL, " ");
wrt_buf = strtok(NULL, "\0");
status = write_file(wd->lfs, fn, atoi(size), wrt_buf, msg->thread_id);
if (status < 0) {
answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, WRT, status);
}
else {
answer = create_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, WRT, 1, "OK");
}
break;
case REA:
fn = strtok(msg->args, " ");
size = strtok(NULL, " ");
rea_buf = malloc(atoi(size) * sizeof *rea_buf);
if (rea_buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, REA, status);
break;
}
status = read_file(wd->lfs, fn, atoi(size), rea_buf, msg->thread_id);
if (status < 0) {
answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, REA, status);
free(rea_buf);
rea_buf = NULL;
break;
}
else {
//buf will be "OK read_size read_characters\0"
buf = malloc((strlen("OK") + strlen(size) + strlen(rea_buf) + 3) * sizeof *buf);
if (buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, REA, status);
free(rea_buf);
rea_buf = NULL;
break;
}
sprintf(buf, "OK %d %s", status, rea_buf);
answer = create_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, REA, 3, buf);
}
free(rea_buf);
rea_buf = NULL;
break;
case CLO:
status = close_file(wd->lfs, msg->args);
if (status != 0) {
answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, CLO, status);
} else {
//We return "OK filename", because the worker who requested the close needs the filename to erase the FD from his data
buf = malloc((strlen(msg->args) + strlen("OK ") + 1) * sizeof *buf);
if (buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, CLO, status);
break;
}
sprintf(buf, "OK %s", msg->args);
answer = create_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, CLO, 2, buf);
}
break;
case BYE:
status = close_thread_files(wd->lfs, msg->thread_id);
if (status != 0) {
answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, BYE, status);
} else {
answer = create_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, BYE, 1, "OK");
}
break;
default:
break;
}
send_message(answer);
delete_message(answer);
if (buf != NULL) {
free(buf);
buf = NULL;
}
return 0;
}
static int sync_meta(csiebox_server *server, int conn_fd, csiebox_protocol_meta *meta)/*{{{*/
{
char full_path[PATH_MAX];
recv_message(conn_fd, full_path, sizeof(full_path));
// make a short path
char *client_path = make_path(full_path);
char *cur_dir = get_user_homedir(server, server->client[conn_fd]);
if (chdir(cur_dir) == -1) {
fprintf(stderr, "change to user directory error\n");
return 0;
}
// start to traverse
int file_check = 0;
char *object = strtok(client_path, "/");
while(object != NULL){
strcat(cur_dir, "/");
strcat(cur_dir, object);
struct stat object_stat;
memset(&object_stat, 0, sizeof(object_stat));
int open_success = lstat(cur_dir, &object_stat);
// if file not exist
if (open_success == -1){
if (S_ISDIR(meta->message.body.stat.st_mode)) {
mkdir(cur_dir, meta->message.body.stat.st_mode);
fprintf(stderr, "sync meta : create dir\n");
// dir utime
struct utimbuf meta_time;
memset(&meta_time, 0, sizeof(meta_time));
meta_time.actime = meta->message.body.stat.st_atime;
meta_time.modtime = meta->message.body.stat.st_mtime;
utime(cur_dir, &meta_time);
fprintf(stderr, "create dir\n");
}
if (S_ISREG(meta->message.body.stat.st_mode)) {
int fd = creat(cur_dir, meta->message.body.stat.st_mode);
// file utime
struct utimbuf meta_time;
memset(&meta_time, 0, sizeof(meta_time));
meta_time.actime = meta->message.body.stat.st_atime;
meta_time.modtime = meta->message.body.stat.st_mtime;
utime(cur_dir, &meta_time);
fprintf(stderr, "sync meta : create file\n");
close(fd);
file_check = 1;
}
if (S_ISLNK(meta->message.body.stat.st_mode)) {
fprintf(stderr, "sync meta : is symbolic link\n");
file_check = 1;
}
object = strtok(NULL, "/");
}
// file exist
else {
// is directory
if (S_ISDIR(object_stat.st_mode)){
if (chdir(cur_dir) == -1) {
fprintf(stderr, "chdir error\n");
return 0;
}
}
else {
// is file
// file chmod
chmod(cur_dir, meta->message.body.stat.st_mode);
chown(cur_dir, meta->message.body.stat.st_uid, meta->message.body.stat.st_gid);
// check if is same
uint8_t content_hash[MD5_DIGEST_LENGTH];
memset(content_hash, 0, sizeof(content_hash));
md5_file(cur_dir, content_hash);
if (memcmp(content_hash, meta->message.body.hash, MD5_DIGEST_LENGTH) != 0) file_check = 1;
// file utime
struct utimbuf meta_time;
memset(&meta_time, 0, sizeof(meta_time));
meta_time.actime = meta->message.body.stat.st_atime;
meta_time.modtime = meta->message.body.stat.st_mtime;
utime(cur_dir, &meta_time);
}
object = strtok(NULL, "/");
// dir chmod
if (object == NULL) {
chmod(cur_dir, meta->message.body.stat.st_mode);
chown(cur_dir, meta->message.body.stat.st_uid, meta->message.body.stat.st_gid);
struct utimbuf meta_time;
// dir utime
memset(&meta_time, 0, sizeof(meta_time));
meta_time.actime = meta->message.body.stat.st_atime;
meta_time.modtime = meta->message.body.stat.st_mtime;
utime(cur_dir, &meta_time);
}
}
}
// send back message
csiebox_protocol_header header;
memset(&header, 0, sizeof(header));
header.res.magic = CSIEBOX_PROTOCOL_MAGIC_RES;
header.res.op = CSIEBOX_PROTOCOL_OP_SYNC_META;
header.res.status = (file_check == 1)? CSIEBOX_PROTOCOL_STATUS_MORE : CSIEBOX_PROTOCOL_STATUS_OK;
header.res.datalen = 0;
send_message(conn_fd, &header, sizeof(header));
fprintf(stderr, "sync meta : success\n");
return 1;
}/*}}}*/
int sap_transfer_apdu_rsp(void *sap_device, uint8_t result, uint8_t *apdu,
uint16_t length)
{
struct sap_connection *conn = sap_device;
char buf[SAP_BUF_SIZE];
struct sap_message *msg = (struct sap_message *) buf;
struct sap_parameter *param = (struct sap_parameter *) msg->param;
size_t size = sizeof(struct sap_message);
DBG("1 size(%d)", size);
if (!conn)
return -EINVAL;
DBG("state %d pr 0x%02x", conn->state, conn->processing_req);
if (conn->processing_req != SAP_TRANSFER_APDU_REQ)
{
DBG("returned !SAP_TRANSFER_APDU_REQ");
return 0;
}
if (result == SAP_RESULT_OK && (!apdu || (apdu && length == 0x00)))
{
DBG("returned EINVAL");
return -EINVAL;
}
DBG("original APDU length(%d)", length);
memset(buf, 0, sizeof(buf));
msg->id = SAP_TRANSFER_APDU_RESP;
msg->nparam = 0x01;
size += add_result_parameter(result, param);
DBG("2 size(%d)", size);
/* Add APDU response. */
if (result == SAP_RESULT_OK) {
DBG("result OK");
msg->nparam++;
param = (struct sap_parameter *) &buf[size];
param->id = SAP_PARAM_ID_RESPONSE_APDU;
param->len = htons(length);
DBG("param->len(%d)", param->len);
size += PARAMETER_SIZE(length);
DBG("3 size(%d)", size);
DBG("PARAMETER size(%d)", size);
if (size > SAP_BUF_SIZE)
{
DBG("returned EOVERFLOW");
return -EOVERFLOW;
}
memcpy(param->val, apdu, length);
} else {
DBG("result not OK");
}
conn->processing_req = SAP_NO_REQ;
DBG("buf : %s", buf);
DBG("buf_len(%d), size(%d)", strlen(buf), size);
return send_message(sap_device, buf, size);
}
void rms_send_free_text_msg () {
UNSIGNED_BYTE free_text_buf[MAX_BUF_SIZE];
UNSIGNED_BYTE *free_text_buf_ptr;
char *Msg;
int ret, i, msg_size;
LB_info info;
ushort message_size_halfwords, num_halfwords;
/* Search for the latest free text message. */
if((ret = ORPGDA_seek(ORPGDAT_RMS_TEXT_MSG, 0, LB_LATEST, &info)) < 0) {
LE_send_msg(RMS_LE_ERROR, "Failed seek RMMS free text message (%d).\n", ret );
}else {
/* Allocate memory for the message. */
Msg = (char*)malloc(info.size);
/* Read the latest message. */
ret = ORPGDA_read (ORPGDAT_RMS_TEXT_MSG, (char *) Msg, info.size, info.id);
if(ret <0 ){
LE_send_msg(RMS_LE_ERROR, "Failed read RMMS free text message (%d).\n", ret );
}else {
/* Set the pointer to beginning of buffer */
free_text_buf_ptr = free_text_buf;
/* Set the pointer past the header */
free_text_buf_ptr += MESSAGE_START;
/* Compute the size of the free text string in halfwords */
message_size_halfwords = (info.size/2);
/* Adjust for rounding */
message_size_halfwords += (info.size%2);
/* Put free text string size in output buffer */
conv_ushort_unsigned(free_text_buf_ptr,&message_size_halfwords);
free_text_buf_ptr += PLUS_SHORT;
/* Put free text string in output buffer */
for (i=0; i< info.size; i++){
conv_char_unsigned(free_text_buf_ptr, &Msg[i], PLUS_BYTE);
free_text_buf_ptr += PLUS_BYTE;
} /* End loop */
/* Compute the size of the message */
msg_size = (free_text_buf_ptr - free_text_buf);
/* Pad the message */
pad_message (free_text_buf_ptr, msg_size, TEXT_PAD_SIZE);
free_text_buf_ptr += (TEXT_PAD_SIZE - msg_size);
/* Add the terminator to the message */
add_terminator(free_text_buf_ptr);
free_text_buf_ptr += PLUS_INT;
/* Compute the size of the message in halfwords */
num_halfwords = ((free_text_buf_ptr - free_text_buf) / 2);
/* Add header to message */
ret = build_header(&num_halfwords, FREE_TYPE, free_text_buf, 0);
if (ret != 1){
LE_send_msg (RMS_LE_ERROR,
"RMS build header failed for rms send free text message");
} /* End if */
/* Send message to the FAA/RMMs */
ret = send_message(free_text_buf,FREE_TYPE,RMS_STANDARD);
if (ret != 1){
LE_send_msg (RMS_LE_ERROR,
"Send message failed (ret %d) for rms send free text message", ret);
} /* end if */
} /* End else */
free(Msg);
} /* End else */
} /*End rms send free text msg */
void
GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *str)
{
if (severity < MAV_SEVERITY_WARNING &&
comm_get_txspace(chan) >=
MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) {
// send immediately
char msg[50] {};
strncpy(msg, str, sizeof(msg));
mavlink_msg_statustext_send(chan, severity, msg);
} else {
// send via the deferred queuing system
mavlink_statustext_t *s = &pending_status;
s->severity = (uint8_t)severity;
strncpy((char *)s->text, str, sizeof(s->text));
send_message(MSG_STATUSTEXT);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN && APM_BUILD_TYPE(APM_BUILD_ArduCopter)
if (!strcmp(str, "compass disabled")) // Messages defined in compassmot.pde (10x)
textId = 1;
else if (!strcmp(str, "check compass"))
textId = 2;
else if (!strcmp(str, "RC not calibrated"))
textId = 3;
else if (!strcmp(str, "thr not zero"))
textId = 4;
else if (!strcmp(str, "Not landed"))
textId = 5;
else if (!strcmp(str, "STARTING CALIBRATION"))
textId = 6;
else if (!strcmp(str, "CURRENT"))
textId = 7;
else if (!strcmp(str, "THROTTLE"))
textId = 8;
else if (!strcmp(str, "Calibration Successful"))
textId = 9;
else if (!strcmp(str, "Failed"))
textId = 10;
else if (!strcmp(str, "AutoTune: Started")) // Messages defined in control_autotune.pde (4x)
textId = 21;
else if (!strcmp(str, "AutoTune: Stopped"))
textId = 22;
else if (!strcmp(str, "AutoTune: Success"))
textId = 23;
else if (!strcmp(str, "AutoTune: Failed"))
textId = 24;
else if (!strcmp(str, "Crash: Disarming")) // Messages defined in crash_check.pde (3x)
textId = 35;
else if (!strcmp(str, "Parachute: Released"))
textId = 36;
else if (!strcmp(str, "Parachute: Too Low"))
textId = 37;
else if (!strcmp(str, "EKF variance")) // Messages defined in ekf_check.pde (2x)
textId = 48;
else if (!strcmp(str, "DCM bad heading"))
textId = 49;
else if (!strcmp(str, "Low Battery")) // Messages defined in events.pde (2x)
textId = 60;
else if (!strcmp(str, "Lost GPS"))
textId = 61;
else if (!strcmp(str, "bad rally point message ID")) // Messages defined in GCS_Mavlink.pde (6x)
textId = 72;
else if (!strcmp(str, "bad rally point message count"))
textId = 73;
else if (!strcmp(str, "error setting rally point"))
textId = 74;
else if (!strcmp(str, "bad rally point index"))
textId = 75;
else if (!strcmp(str, "failed to set rally point"))
textId = 76;
else if (!strcmp(str, "Initialising APM..."))
textId = 77;
else if (!strcmp(str, "Erasing logs")) // Messages defined in Log.pde (2x)
textId = 88;
else if (!strcmp(str, "Log erase complete"))
textId = 89;
else if (!strcmp(str, "ARMING MOTORS")) // Messages defined in motors.pde (30x)
textId = 100;
else if (!strcmp(str, "Arm: Gyro cal failed"))
textId = 101;
else if (!strcmp(str, "PreArm: RC not calibrated"))
textId = 102;
else if (!strcmp(str, "PreArm: Baro not healthy"))
textId = 103;
else if (!strcmp(str, "PreArm: Alt disparity"))
textId = 104;
else if (!strcmp(str, "PreArm: Compass not healthy"))
textId = 105;
else if (!strcmp(str, "PreArm: Compass not calibrated"))
textId = 106;
else if (!strcmp(str, "PreArm: Compass offsets too high"))
textId = 107;
else if (!strcmp(str, "PreArm: Check mag field"))
textId = 108;
else if (!strcmp(str, "PreArm: compasses inconsistent"))
textId = 109;
else if (!strcmp(str, "PreArm: INS not calibrated"))
textId = 110;
else if (!strcmp(str, "PreArm: Accels not healthy"))
textId = 111;
else if (!strcmp(str, "PreArm: Accels inconsistent"))
textId = 112;
else if (!strcmp(str, "PreArm: Gyros not healthy"))
textId = 113;
else if (!strcmp(str, "PreArm: Gyro cal failed"))
textId = 114;
else if (!strcmp(str, "PreArm: Gyros inconsistent"))
textId = 115;
else if (!strcmp(str, "PreArm: Check Board Voltage"))
textId = 116;
else if (!strcmp(str, "PreArm: Ch7&Ch8 Opt cannot be same"))
textId = 117;
else if (!strcmp(str, "PreArm: Check FS_THR_VALUE"))
textId = 118;
else if (!strcmp(str, "PreArm: Check ANGLE_MAX"))
textId = 119;
else if (!strcmp(str, "PreArm: ACRO_BAL_ROLL/PITCH"))
textId = 120;
else if (!strcmp(str, "PreArm: GPS Glitch"))
textId = 121;
else if (!strcmp(str, "PreArm: Need 3D Fix"))
textId = 122;
else if (!strcmp(str, "PreArm: Bad Velocity"))
textId = 123;
else if (!strcmp(str, "PreArm: High GPS HDOP"))
textId = 124;
else if (!strcmp(str, "Arm: Alt disparity"))
textId = 125;
else if (!strcmp(str, "Arm: Thr below FS"))
textId = 126;
else if (!strcmp(str, "Arm: Leaning"))
textId = 127;
else if (!strcmp(str, "Arm: Safety Switch"))
textId = 128;
else if (!strcmp(str, "DISARMING MOTORS"))
textId = 129;
else if (!strcmp(str, "Motor Test: RC not calibrated")) // Messages defined in motor_test.pde (3x)
textId = 140;
else if (!strcmp(str, "Motor Test: vehicle not landed"))
textId = 141;
else if (!strcmp(str, "Motor Test: Safety Switch"))
textId = 142;
else if (!strcmp(str, "Calibrating barometer")) // Messages defined in sensors.pde (2x)
textId = 153;
else if (!strcmp(str, "barometer calibration complete"))
textId = 154;
else if (!strcmp(str, "Trim saved")) // Messages defined in switches.pde (1x)
textId = 165;
else if (!strcmp(str, "No dataflash inserted")) // Messages defined in system.pde (4x)
textId = 176;
else if (!strcmp(str, "ERASING LOGS"))
textId = 177;
else if (!strcmp(str, "Waiting for first HIL_STATE message"))
textId = 178;
else if (!strcmp(str, "GROUND START"))
textId = 179;
else if (!strcmp(str, "PreArm: Baro not healthy")) // Messages defined in AP_Arming.cpp (9x)
textId = 190;
else if (!strcmp(str, "PreArm: Compass not healthy"))
textId = 191;
else if (!strcmp(str, "PreArm: Compass not calibrated"))
textId = 192;
else if (!strcmp(str, "PreArm: Bad GPS Pos"))
textId = 193;
else if (!strcmp(str, "PreArm: Battery failsafe on"))
textId = 194;
else if (!strcmp(str, "PreArm: Hardware Safety Switch"))
textId = 195;
else if (!strcmp(str, "PreArm: Radio failsafe on"))
textId = 196;
else if (!strcmp(str, "Throttle armed"))
textId = 197;
else if (!strcmp(str, "Throttle disarmed"))
textId = 198;
else if (!strcmp(str, "flight plan update rejected")) // Messages defined in GCS_Common.cpp (2x)
textId = 209;
else if (!strcmp(str, "flight plan received"))
textId = 210;
else
textId = 0;
g_severity = severity;
#endif
}
/* we have gotten to the end of the local file we are sending;
* close the file descriptor on that file, and send a message to the
* other side indicating that the whole file has been sent.
*/
static void end_sending_file()
{
send_message(byte_buf, 1, transfer_end_msg);
state = state_nominal;
close(fd);
}
static LRESULT WizardDlgProc(HWND hDlg,UINT msg, WPARAM wParam,LPARAM lParam)
{
int i;
switch (msg)
{
case WM_INITDIALOG:
hWizardDlg = hDlg;
hWizardText = GetDlgItem(hDlg,IDC_OUTPUT);
SendMessage(hWizardText,WM_SETFONT,(WPARAM)hfCourier,1);
display_game_status();
SetTimer(hDlg,10,20,NULL);
return 0;
case WM_TIMER: do_events();return 1;
case WM_COMMAND:
switch (LOWORD(wParam))
{
case IDCANCEL: EndDialog(hDlg,0);return 0;
case IDC_CLEARMAP:
{
HWND listwnd = PrepareListWindow(hDlg);
HWND list = GetDlgItem(listwnd,IDC_LIST);
int res;
ListBox_AddString(list,"Clear Monsters");
ListBox_AddString(list,"Clear Items");
res = PumpDialogMessages(listwnd);
if (res == IDOK)
{
if (ListBox_GetSel(list,0))
{
for(i = 0;i<MAX_MOBS;i++)
if (mobs[i].vlajky & MOB_LIVE)
{
vybrana_zbran = -1;
select_player = -1;
mob_hit(mobs+i,mobs[i].lives);
}
}
if (ListBox_GetSel(list,1))
{
for(i = 0;i<mapsize*4;i++)
{
destroy_items(map_items[i]);
free(map_items[i]);
map_items[i] = NULL;
}
for(i = 0;i<vyk_max;i++)
{
destroy_items(map_vyk[i].items);
map_vyk[i].items[0] = 0;
}
}
}
CloseListWindow(listwnd);
}
break;
case IDC_ADVENCE:
{
int i,j,c;
if (!wzscanf("Advence to level <postava -1= vsichni><uroven>:","%d %d",&i,&j)) return 0;
c = MessageBox(GetActiveWindow(),"Automaticky?","?",MB_YESNO|MB_ICONQUESTION);
if (i>0) advence_player(i-1,j,c == IDYES);else
for(i = 0;i<POCET_POSTAV;i++) if (postavy[i].used) advence_player(i,j,c == IDYES);
return 0;
}
case IDC_GOTO:
{
char prompt[50];
sprintf(prompt,"Goto sector <1-%d>:",mapsize-1);
if (!wzscanf(prompt,"%d",&viewsector)) return 0;
chod_s_postavama(1);
SEND_LOG("(WIZARD) Goto %d",viewsector,0);
return 0;
}
case IDC_LOADMAP:
if (!wzscanf("Load Map <filename><sector>","%s %hd",loadlevel.name,&loadlevel.start_pos)) return 0;
for(i = 0;i<POCET_POSTAV;i++)postavy[i].sektor = loadlevel.start_pos;
SEND_LOG("(WIZARD) Load map '%s' %d",loadlevel.name,loadlevel.start_pos);
EndDialog(hDlg,0);
send_message(E_CLOSE_MAP);
return 0;
case IDC_OPENDOOR:if (map_sectors[viewsector].step_next[viewdir])
delay_action(3,viewsector,viewdir,0x2000000,0,0);
else
delay_action(3,viewsector,viewdir,0,0,0);
return 0;
case IDC_TAKEMONEY:if (take_money()) return 0;break;
case IDC_PURGE:if (purge_map()) return 0;break;
case IDC_HEAL:if (heal_meditate()) return 0;break;
case IDC_RAISEDEATH:if (raise_death()) return 0;break;
case IDC_RAISEMONSTER:if (raise_killed_monster(hDlg)) return 0;break;
case IDC_IMMORTAL:set_immortality();break;
case IDC_NETECNOST:set_nohassle();break;
case IDC_UNAFFECT :unaffect();break;
case IDC_WEAPONSKILL:if (advance_weapon()) return 0;break;
case IDC_REFRESH:display_game_status();break;
case IDC_RELOADMOBILES:
i = MessageBox(hDlg,"Tato funkce precte znova parametry vsech existujicich nestvur. "
"Pouzivej jen v pripade, ze se tyto parametry zmenili a nesouhlasi tak "
"obsah ulozene pozice. Pokracovat? ","??",MB_YESNO|MB_ICONQUESTION);
if (i == IDYES) reload_mobs();
return 0;
case IDC_LOADITEM:
{
HWND listdlg = PrepareListWindow(hDlg);
HWND list = GetDlgItem(listdlg,IDC_LIST);
char buff[256];
int i;
int res;
for (i = 0;i<item_count;i++)
{
_snprintf(buff,sizeof(buff),"%d. %s",i,glob_items[i].jmeno);
kamenik2windows(buff,strlen(buff),buff);
ListBox_AddString(list,buff);
}
res = PumpDialogMessages(listdlg);
while (res == IDOK)
{
int cnt;
for (i = 0,cnt = ListBox_GetCount(list);i<cnt;i++) if (ListBox_GetSel(list,i))
{
SEND_LOG("(WIZARD) Load Item %d (%s)",i,glob_items[i].jmeno);
macro_drop_item(viewsector,viewdir,i);
wzprintf("Dropped item: %d\r\n",i);
}
res = PumpDialogMessages(listdlg);
}
CloseListWindow(listdlg);
}
return 0;
}
default: return 0;
}
return 1;
}
void start_wallclock()
{
uint32_t status;
//initialise
offset = 0;
ref = 0;
clock_display_en = 0; //clock not displayed by default
timeout_env = request_msg_env();
send_env = request_msg_env();
msg_q = msg_env_queue_create();
status = request_delay ( ONE_SECOND_DELAY, WAKEUP_CODE, timeout_env);
if (status != CODE_SUCCESS)
{
params[0] = &status;
params[1] = NULL;
rtx_sprintf(str, "request_delay failed with status %d\r\n", params);
print_ack(str, send_env, msg_q);
}
MsgEnv* env;
while (1)
{
if (msg_env_queue_is_empty(msg_q))
{
env = receive_message();
}
else
{
env = msg_env_queue_dequeue(msg_q);
}
//envelope from timing services
if (env->msg_type == WAKEUP_CODE)
{
status = request_delay( ONE_SECOND_DELAY, WAKEUP_CODE, timeout_env);
if (status != CODE_SUCCESS)
{
params[0] = &status;
params[1] = NULL;
rtx_sprintf(str, "request_delay failed with status %d\r\n", params);
print_ack(str, send_env, msg_q);
}
//86400 = 24hrs in secs
int32_t clock_time = (int32_t)((get_system_time()-ref)/100
+offset)%SEC_IN_HR;
if (clock_display_en)
{
int32_t hr, min, sec;
hr = clock_time/3600;
min = (clock_time%3600)/60;
sec = clock_time%60;
params[0] = &hr;
params[1] = &min;
params[2] = &sec;
params[3] = NULL;
rtx_sprintf(str, SAVE_CURSOR MOVE_CURSOR CLOCK_FORMAT
RESTORE_CURSOR, params);
print_ack(str, send_env, msg_q);
}
}
else if (env->msg_type == CLOCK_ON)
{
_displayWallClock (1);
env->msg_type = CLOCK_RET;
send_message(env->send_pid,env);
}
else if (env->msg_type == CLOCK_OFF)
{
_displayWallClock (0);
env->msg_type = CLOCK_RET;
send_message(env->send_pid,env);
}
else if (env->msg_type == CLOCK_SET)
{
int status = _setWallClock (env->msg);
if (status == ERROR_ILLEGAL_ARG)
{
params[0] = NULL;
rtx_sprintf( str, "c\r\n"
"Sets the console clock (24h).\r\n"
"Usage: c <hh:mm:ss>\r\n"
"hh must be 00-23\r\n"
"mm must be 00-59\r\n"
"ss must be 00-59\r\n", params );
print_ack(str, send_env, msg_q);
}
else if (status != CODE_SUCCESS)
{
params[0] = &status;
params[1] = NULL;
rtx_sprintf( str, "CCI_setClock failed with status %d\r\n", params);
print_ack(str, send_env, msg_q);
}
env->msg_type = CLOCK_RET;
send_message(env->send_pid,env);
}
}
}
static void
help_mouse_callback (Widget * w, mouse_msg_t msg, mouse_event_t * event)
{
int x, y;
GSList *current_area;
if (msg != MSG_MOUSE_CLICK)
return;
if ((event->buttons & GPM_B_RIGHT) != 0)
{
/* Right button click */
help_back (whelp);
return;
}
/* Left bytton click */
/* The event is relative to the dialog window, adjust it: */
x = event->x - 1;
y = event->y - 1;
/* Test whether the mouse click is inside one of the link areas */
for (current_area = link_area; current_area != NULL; current_area = g_slist_next (current_area))
{
Link_Area *la = (Link_Area *) current_area->data;
/* Test one line link area */
if (y == la->y1 && x >= la->x1 && y == la->y2 && x <= la->x2)
break;
/* Test two line link area */
if (la->y1 + 1 == la->y2)
{
/* The first line || The second line */
if ((y == la->y1 && x >= la->x1) || (y == la->y2 && x <= la->x2))
break;
}
/* Mouse will not work with link areas of more than two lines */
}
/* Test whether a link area was found */
if (current_area != NULL)
{
Link_Area *la = (Link_Area *) current_area->data;
/* The click was inside a link area -> follow the link */
history_ptr = (history_ptr + 1) % HISTORY_SIZE;
history[history_ptr].page = currentpoint;
history[history_ptr].link = la->link_name;
currentpoint = help_follow_link (currentpoint, la->link_name);
selected_item = NULL;
}
else if (y < 0)
move_backward (help_lines - 1);
else if (y >= help_lines)
move_forward (help_lines - 1);
else if (y < help_lines / 2)
move_backward (1);
else
move_forward (1);
/* Show the new node */
send_message (w->owner, NULL, MSG_DRAW, 0, NULL);
}
static void message_loop() {
static char buf1[4096], buf2[4096];
struct nlmsghdr *nlh, *nlh2 = NULL;
int skipped = 0;
while (1) {
struct userui_msg_params *msg;
if (nlh2) {
nlh = nlh2;
nlh2 = NULL;
memcpy(&buf1, &buf2, 4096);
memset(&buf2, 0, 4096);
} else
if (!(nlh = fetch_message(buf1, sizeof(buf1), 0)))
return; /* EOF */
//nlh2 = fetch_message(buf2, sizeof(buf2), 1);
msg = NLMSG_DATA(nlh);
/* If there are two or more messages waiting and the current
message type is the same as the type of the next message,
skip this one. */
if (nlh2 && nlh->nlmsg_type == nlh2->nlmsg_type && (++skipped < 5))
continue;
skipped = 0;
might_switch_ops();
switch (nlh->nlmsg_type) {
case USERUI_MSG_MESSAGE:
active_ops->message(msg->a, msg->b, msg->c, msg->text);
break;
case USERUI_MSG_PROGRESS:
active_ops->update_progress(msg->a, msg->b, msg->text);
break;
case USERUI_MSG_GET_STATE:
suspend_action = *(uint32_t*)NLMSG_DATA(nlh);
break;
case USERUI_MSG_GET_DEBUG_STATE:
suspend_debug = *(uint32_t*)NLMSG_DATA(nlh);
break;
case USERUI_MSG_GET_LOGLEVEL:
console_loglevel = *(uint32_t*)NLMSG_DATA(nlh);
set_console_loglevel(0);
break;
case USERUI_MSG_IS_DEBUGGING:
debugging_enabled = *(uint32_t *)NLMSG_DATA(nlh);
break;
case USERUI_MSG_GET_POWERDOWN_METHOD:
powerdown_method = *(uint32_t *)NLMSG_DATA(nlh);
break;
case USERUI_MSG_CLEANUP:
active_ops->cleanup();
send_message(USERUI_MSG_CLEANUP, NULL, 0);
close(nlsock);
exit(0);
case USERUI_MSG_POST_ATOMIC_RESTORE:
send_message(USERUI_MSG_GET_LOGLEVEL, NULL, 0);
send_message(USERUI_MSG_GET_STATE, NULL, 0);
send_message(USERUI_MSG_GET_DEBUG_STATE, NULL, 0);
send_message(USERUI_MSG_GET_POWERDOWN_METHOD, NULL, 0);
resuming = 1;
unblank_screen();
active_ops->redraw();
break;
case NLMSG_ERROR:
report_nl_error(nlh);
break;
case NLMSG_DONE:
break;
default:
printf("userui: Received unknown message %d\n", nlh->nlmsg_type);
break;
}
if (need_loglevel_change) {
need_loglevel_change = 0;
active_ops->log_level_change();
}
}
}
static void notify_space_pressed() {
send_message(USERUI_MSG_SPACE, NULL, 0);
}
void svc( SYSTEM_CALL_DATA *SystemCallData ) {
short call_type;
static short do_print = 10;
short i;
int Time;
int ID;
int k=0,m=0,n=0;
int PR;
int sid,tid,mlen;
int actual_length;
long tmp;
INT32 diskstatus;
long tmpid;
void *next_context;
char *tmpmsg;
PCB *head;
PCB *head1;
PCB *pcb = (PCB *)malloc(sizeof(PCB));
call_type = (short)SystemCallData->SystemCallNumber;
if ( do_print > 0 ) {
printf( "SVC handler: %s\n", call_names[call_type]);
for (i = 0; i < SystemCallData->NumberOfArguments - 1; i++ ) {
//Value = (long)*SystemCallData->Argument[i];
printf( "Arg %d: Contents = (Decimal) %8ld, (Hex) %8lX\n", i,
(unsigned long )SystemCallData->Argument[i],
(unsigned long )SystemCallData->Argument[i]);
}
}
switch(call_type) {
case SYSNUM_GET_TIME_OF_DAY:
MEM_READ(Z502ClockStatus, &Time);
//Z502_REG1=Time;
*SystemCallData->Argument[0]=Time;
break;
case SYSNUM_TERMINATE_PROCESS:
tmpid = (long)SystemCallData->Argument[0];
if(tmpid>=0) {
os_delete_process_ready(tmpid);
Z502_REG9 = ERR_SUCCESS;
}
else if(tmpid == -1)
{
head =Running;
Running = NULL;
while(readyfront==NULL&&timerfront==NULL) {
Z502Halt();
}
//free(head);
dispatcher();
Z502SwitchContext( SWITCH_CONTEXT_SAVE_MODE, &(Running->context) );
}
else
Z502Halt();
break;
//the execution of sleep();
case SYSNUM_SLEEP:
start_timer( (int)SystemCallData->Argument[0] );
break;
case SYSNUM_GET_PROCESS_ID:
*SystemCallData->Argument[1] = os_get_process_id((char *)SystemCallData->Argument[0]);
break;
case SYSNUM_CREATE_PROCESS:
strcpy(pcb->Processname , (char *)SystemCallData->Argument[0]);
pcb->Priority = (long)SystemCallData->Argument[2];
head = readyfront;
head1 = readyfront;
if(Pid < 20) {
if(pcb->Priority >0) {
if(readyfront == NULL&&readyrear == NULL) {
readyfront = pcb;
readyrear = pcb;
//*SystemCallData->Argument[4] = ERR_SUCCESS;
Z502_REG9 = ERR_SUCCESS;
pcb->pid = Pid;
pcb->next=NULL;
Toppriority = pcb->Priority;
*SystemCallData->Argument[3] = Pid;
//pcb->context = (void *)SystemCallData->Argument[1];
Z502MakeContext( &next_context, (void *)SystemCallData->Argument[1], USER_MODE );
pcb->context = next_context;
Pid++;
}
else if(readyfront!=NULL&&readyrear!=NULL) {
if(checkPCBname(pcb) == 1) {
if(pcb->Priority < Toppriority) {
Z502_REG9 = ERR_SUCCESS;
pcb->next = readyfront;
readyfront = pcb;
pcb->pid = Pid;
pcb->next=NULL;
Z502MakeContext( &next_context, (void *)SystemCallData->Argument[1], USER_MODE );
pcb->context = next_context;
*SystemCallData->Argument[3] = Pid;
Pid++;
}
else {
while(head->next!=NULL) {
if(pcb->Priority < head->Priority)
break;
else
head = head->next;
}
if(head->next!=NULL) {
while(head1->next!=head)
{
head1=head1->next;
}
Z502_REG9 = ERR_SUCCESS;
head1->next = pcb;
pcb->next=head;
pcb->pid = Pid;
Z502MakeContext( &next_context, (void *)SystemCallData->Argument[1], USER_MODE );
pcb->context = next_context;
*SystemCallData->Argument[3] = Pid;
Pid++;
}
else {
if(pcb->Priority < head->Priority) {
while(head1->next!=head)
{
head1=head1->next;
}
Z502_REG9 = ERR_SUCCESS;
head1->next=pcb;
pcb->next=head;
pcb->pid = Pid;
Z502MakeContext( &next_context, (void *)SystemCallData->Argument[1], USER_MODE );
pcb->context = next_context;
*SystemCallData->Argument[3] = Pid;
Pid++;
}
else {
Z502_REG9 = ERR_SUCCESS;
head->next = pcb;
readyrear = pcb;
pcb->next=NULL;
pcb->pid = Pid;
Z502MakeContext( &next_context, (void *)SystemCallData->Argument[1], USER_MODE );
pcb->context = next_context;
*SystemCallData->Argument[3] = Pid;
Pid++;
}
}
}
}
else free(pcb);
}
} else free(pcb);
}
else {
Z502_REG9++;
free(pcb);
}
break;
case SYSNUM_SUSPEND_PROCESS:
ID = (int)SystemCallData->Argument[0];
head = suspendfront;
while(head!=NULL) {
if(head->pid == ID) {
n = 1;
break;
}
else
head = head->next;
}
if(n!=1) {
head = readyfront;
while(head!=NULL) {
if(head->pid == ID) {
Z502_REG9 = ERR_SUCCESS;
suspend_process_ready(head);
k = 1;
break;
}
else
head = head->next;
}
if(k == 0) {
head = timerfront;
while(head!=NULL) {
if(head->pid == ID) {
Z502_REG9 = ERR_SUCCESS;
suspend_process_timer(head);
m = 1;
break;
}
else
head=head->next;
}
if(m == 0&&k == 0) {
printf("illegal PID\n");
}
}
}
if(n == 1) {
printf("can not suspend suspended process\n");
}
break;
case SYSNUM_RESUME_PROCESS:
ID = (int)SystemCallData->Argument[0];
head = suspendfront;
while(head!=NULL) {
if(head->pid == ID) {
k=1;
break;
}
else
head=head->next;
}
if(k==1)
resume_process(head);
else
printf("error\n");
break;
case SYSNUM_CHANGE_PRIORITY:
ID = (int)SystemCallData->Argument[0];
PR = (int)SystemCallData->Argument[1];
if(ID == -1) {
Running->Priority = PR;
Z502_REG9 = ERR_SUCCESS;
}
else {
if(PR < 100) {
if(checkReady(ID) == 1) {
head = readyfront;
while(head->pid != ID)
head=head->next;
change_priority_ready(head,PR);
Z502_REG9 = ERR_SUCCESS;
}
else if(checkTimer(ID) == 1) {
head = timerfront;
while(head->pid != ID)
head=head->next;
change_priority_timer(head,PR);
Z502_REG9 = ERR_SUCCESS;
}
else if(checkSuspend(ID) == 1) {
head = suspendfront;
while(head->pid != ID)
head = head->next;
change_priority_suspend(head,PR);
Z502_REG9 = ERR_SUCCESS;
}
else {
printf("ID ERROR!\n");
}
}
else {
printf("illegal Priority");
}
}
break;
case SYSNUM_SEND_MESSAGE:
sid = Running->pid;
tid = (int)SystemCallData->Argument[0];
tmpmsg =(char *)SystemCallData->Argument[1];
mlen = (int)SystemCallData->Argument[2];
if(maxbuffer < 8) {
if(tid < 100) {
if(mlen < 100)
{
if(tid>0)
{ send_message(sid,tid,mlen,tmpmsg);
maxbuffer++;
}
else if(tid == -1) {
send_message_to_all(sid,mlen,tmpmsg);
maxbuffer++;
}
}
else {
printf("illegal length!\n");
}
} else
printf("illegal id!\n");
}
else
{ printf("no space!\n");
Z502_REG9++;
}
break;
case SYSNUM_RECEIVE_MESSAGE:
sid = (int)SystemCallData->Argument[0];
mlen = (int)SystemCallData->Argument[2];
if(sid < 100) {
if(mlen < 100) {
if(sid == -1) {
receive_message_fromall();
if(msgnum>0) {
actual_length = strlen(checkmsg->msg_buffer);
if(mlen >actual_length) {
msg_out_queue(checkmsg);
*SystemCallData->Argument[3] = actual_length;
*SystemCallData->Argument[4] = checkmsg->source_pid;
strcpy((char *)SystemCallData->Argument[1] ,checkmsg->msg_buffer);
Z502_REG9 = ERR_SUCCESS;
}
else {
printf("small buffer!\n");
}
}
}
else {
receive_message_fromone(sid);
if(msgnum>0) {
actual_length = strlen(checkmsg->msg_buffer);
if(mlen >actual_length) {
msg_out_queue(checkmsg);
*SystemCallData->Argument[3] = actual_length;
*SystemCallData->Argument[4] = checkmsg->source_pid;
strcpy((char *)SystemCallData->Argument[1], checkmsg->msg_buffer);
Z502_REG9 = ERR_SUCCESS;
}
else {
printf("small buffer!\n");
}
}
}
}
else
printf("illegal length!\n");
}
else
printf("illegal id!\n");
break;
case SYSNUM_DISK_READ:
disk_read(SystemCallData->Argument[0],SystemCallData->Argument[1],SystemCallData->Argument[2]);
break;
case SYSNUM_DISK_WRITE:
disk_write(SystemCallData->Argument[0],SystemCallData->Argument[1],SystemCallData->Argument[2]);
break;
default:
printf("call_type %d cannot be recognized\n",call_type);
break;
}
do_print--;
}
/*
handle an incoming mission item
return true if this is the last mission item, otherwise false
*/
bool GCS_MAVLINK::handle_mission_item(mavlink_message_t *msg, AP_Mission &mission)
{
mavlink_mission_item_t packet;
MAV_MISSION_RESULT result = MAV_MISSION_ACCEPTED;
struct AP_Mission::Mission_Command cmd = {};
bool mission_is_complete = false;
mavlink_msg_mission_item_decode(msg, &packet);
// convert mavlink packet to mission command
if (!AP_Mission::mavlink_to_mission_cmd(packet, cmd)) {
result = MAV_MISSION_INVALID;
goto mission_ack;
}
if (packet.current == 2) {
// current = 2 is a flag to tell us this is a "guided mode"
// waypoint and not for the mission
handle_guided_request(cmd);
// verify we received the command
result = MAV_MISSION_ACCEPTED;
goto mission_ack;
}
if (packet.current == 3) {
//current = 3 is a flag to tell us this is a alt change only
// add home alt if needed
handle_change_alt_request(cmd);
// verify we recevied the command
result = MAV_MISSION_ACCEPTED;
goto mission_ack;
}
// Check if receiving waypoints (mission upload expected)
if (!waypoint_receiving) {
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// check if this is the requested waypoint
if (packet.seq != waypoint_request_i) {
result = MAV_MISSION_INVALID_SEQUENCE;
goto mission_ack;
}
// if command index is within the existing list, replace the command
if (packet.seq < mission.num_commands()) {
if (mission.replace_cmd(packet.seq,cmd)) {
result = MAV_MISSION_ACCEPTED;
}else{
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// if command is at the end of command list, add the command
} else if (packet.seq == mission.num_commands()) {
if (mission.add_cmd(cmd)) {
result = MAV_MISSION_ACCEPTED;
}else{
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// if beyond the end of the command list, return an error
} else {
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// update waypoint receiving state machine
waypoint_timelast_receive = hal.scheduler->millis();
waypoint_request_i++;
if (waypoint_request_i >= waypoint_request_last) {
mavlink_msg_mission_ack_send_buf(
msg,
chan,
msg->sysid,
msg->compid,
MAV_MISSION_ACCEPTED);
send_text_P(SEVERITY_LOW,PSTR("flight plan received"));
waypoint_receiving = false;
mission_is_complete = true;
// XXX ignores waypoint radius for individual waypoints, can
// only set WP_RADIUS parameter
} else {
waypoint_timelast_request = hal.scheduler->millis();
// if we have enough space, then send the next WP immediately
if (comm_get_txspace(chan) >=
MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_MISSION_ITEM_LEN) {
queued_waypoint_send();
} else {
send_message(MSG_NEXT_WAYPOINT);
}
}
return mission_is_complete;
mission_ack:
// we are rejecting the mission/waypoint
mavlink_msg_mission_ack_send_buf(
msg,
chan,
msg->sysid,
msg->compid,
result);
return mission_is_complete;
}
int sap_connect_rsp(void *sap_device, uint8_t status, uint16_t maxmsgsize)
{
struct sap_connection *conn = sap_device;
char buf[SAP_BUF_SIZE];
struct sap_message *msg = (struct sap_message *) buf;
struct sap_parameter *param = (struct sap_parameter *) msg->param;
size_t size = sizeof(struct sap_message);
if (!conn)
return -EINVAL;
DBG("state %d pr 0x%02x status 0x%02x", conn->state,
conn->processing_req, status);
if (conn->state != SAP_STATE_CONNECT_IN_PROGRESS)
return -EPERM;
memset(buf, 0, sizeof(buf));
msg->id = SAP_CONNECT_RESP;
msg->nparam = 0x01;
/* Add connection status */
param->id = SAP_PARAM_ID_CONN_STATUS;
param->len = htons(SAP_PARAM_ID_CONN_STATUS_LEN);
*param->val = status;
size += PARAMETER_SIZE(SAP_PARAM_ID_CONN_STATUS_LEN);
/* Add MaxMsgSize */
if (maxmsgsize && (status == SAP_STATUS_MAX_MSG_SIZE_NOT_SUPPORTED ||
status == SAP_STATUS_MAX_MSG_SIZE_TOO_SMALL)) {
uint16_t *len;
msg->nparam++;
param = (struct sap_parameter *) &buf[size];
param->id = SAP_PARAM_ID_MAX_MSG_SIZE;
param->len = htons(SAP_PARAM_ID_MAX_MSG_SIZE_LEN);
len = (uint16_t *) ¶m->val;
*len = htons(maxmsgsize);
size += PARAMETER_SIZE(SAP_PARAM_ID_MAX_MSG_SIZE_LEN);
}
if (status == SAP_STATUS_OK) {
gboolean connected = TRUE;
DBG("sap - emitting property changed signal connected=true");
emit_property_changed(connection, server->path,
SAP_SERVER_INTERFACE,
"Connected", DBUS_TYPE_BOOLEAN, &connected);
conn->state = SAP_STATE_CONNECTED;
} else {
DBG("sap connection status=%d",status);
conn->state = SAP_STATE_DISCONNECTED;
/* Timer will shutdown channel if client doesn't send
* CONNECT_REQ or doesn't shutdown channel itself.*/
start_guard_timer(conn, SAP_TIMER_NO_ACTIVITY);
}
conn->processing_req = SAP_NO_REQ;
return send_message(sap_device, buf, size);
}
void
GCS_MAVLINK::update(run_cli_fn run_cli)
{
// receive new packets
mavlink_message_t msg;
mavlink_status_t status;
status.packet_rx_drop_count = 0;
// process received bytes
uint16_t nbytes = comm_get_available(chan);
for (uint16_t i=0; i<nbytes; i++)
{
uint8_t c = comm_receive_ch(chan);
if (run_cli) {
/* allow CLI to be started by hitting enter 3 times, if no
* heartbeat packets have been received */
if ((mavlink_active==0) && (hal.scheduler->millis() - _cli_timeout) < 20000 &&
comm_is_idle(chan)) {
if (c == '\n' || c == '\r') {
crlf_count++;
} else {
crlf_count = 0;
}
if (crlf_count == 3) {
run_cli(_port);
}
}
}
// Try to get a new message
if (mavlink_parse_char(chan, c, &msg, &status)) {
// we exclude radio packets to make it possible to use the
// CLI over the radio
if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
mavlink_active |= (1U<<(chan-MAVLINK_COMM_0));
}
// if a snoop handler has been setup then use it
if (msg_snoop != NULL) {
msg_snoop(&msg);
}
if (routing.check_and_forward(chan, &msg)) {
handleMessage(&msg);
}
}
}
if (!waypoint_receiving) {
return;
}
uint32_t tnow = hal.scheduler->millis();
uint32_t wp_recv_time = 1000U + (stream_slowdown*20);
if (waypoint_receiving &&
waypoint_request_i <= waypoint_request_last &&
tnow - waypoint_timelast_request > wp_recv_time) {
waypoint_timelast_request = tnow;
send_message(MSG_NEXT_WAYPOINT);
}
// stop waypoint receiving if timeout
if (waypoint_receiving && (tnow - waypoint_timelast_receive) > wp_recv_time+waypoint_receive_timeout) {
waypoint_receiving = false;
}
}
static void up_click_handler(ClickRecognizerRef recognizer, void *context) {
send_message(3);
}
void test_failed ( char * message )
{
send_message ( message );
exit(1);
}
int process_thread_request(worker_data *wd, message *msg, session *s)
{
message *answer = NULL;
message *local_answer = NULL;
char *buf = NULL;
int status = 0;
char *snd_arg, *fourth_arg, *fd;
open_fd *current;
switch (msg->op) {
case LSD:
buf = malloc(MAX_ARGS_LEN * sizeof *buf);
if (buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, WORKER, wd->id, msg->thread_id, ANSWER, LSD, status);
break;
}
//We save in pending our own list of files
int n_files = list_files(wd->lfs, buf, MAX_ARGS_LEN);
local_answer = create_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, LSD, n_files, buf);
add_msg_to_pending(s, local_answer);
//We broadcast to the rest of the workers a LSD request
answer = create_message(WORKER, wd->id, WORKER, -1, msg->thread_id, REQUEST, LSD, 0, NULL);
s->waiting = NWORKERS - 1;
break;
//First we check in our local filesystem. If it already exists here, we don't need to send a broadcast, we just send a message for ourselves
case DEL:
case OPN:
if (file_exists(wd->lfs, msg->args)) {
answer = create_message(WORKER, wd->id, WORKER, wd->id, msg->thread_id, REQUEST, msg->op, 1, msg->args);
s->waiting = 1;
break;
}
//Else, if we don't have it, we save that into pending and send a broadcast to the rest of the workers
buf = malloc((strlen(msg->args) + strlen("N ") + 1) * sizeof *buf);
if (buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, WORKER, wd->id, msg->thread_id, ANSWER, EXS, status);
break;
}
sprintf(buf, "N %s", msg->args);
local_answer = create_message(WORKER, wd->id, WORKER, wd->id, msg->thread_id, ANSWER, EXS, 2, buf);
add_msg_to_pending(s, local_answer);
answer = create_message(WORKER, wd->id, WORKER, -1, msg->thread_id, REQUEST, EXS, 1, msg->args);
s->waiting = NWORKERS - 1;
break;
//First we check in our local filesystem. If it already exists here, we don't need to send a broadcast, we just return error
case CRE:
if (file_exists(wd->lfs, msg->args) || is_file_being_created(wd->lfs, msg->args)) {
answer = create_error_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, EEXIST);
break;
}
//Else, if it doesn't exists here, we save that into pending and send a broadcast to the rest of the workers
buf = malloc((strlen(msg->args) + strlen("N ") + 1) * sizeof *buf);
if (buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, EXSCRE, status);
break;
}
sprintf(buf, "N %s", msg->args);
local_answer = create_message(WORKER, wd->id, WORKER, wd->id, msg->thread_id, ANSWER, EXSCRE, 2, buf);
add_msg_to_pending(s, local_answer);
//We send EXSCRE instead of simply EXS, because we also need to check that the rest of the workers haven't been asked to create the same file, and are in the process of doing so
answer = create_message(WORKER, wd->id, WORKER, -1, msg->thread_id, REQUEST, EXSCRE, 1, msg->args);
s->waiting = NWORKERS - 1;
//We add the file to our list of files being created
add_to_files_being_created(wd->lfs, msg->args);
break;
case WRT:
snd_arg = msg->args + strlen("FD ");
fourth_arg = snd_arg;
while (*fourth_arg != ' ') {
fourth_arg++;
}
fourth_arg += strlen(" SIZE ");
fd = snd_arg;
current = s->open_fds;
while(current != NULL && current->file_descriptor != atoi(fd)) {
current = current->next;
}
//If we didn't find the file descriptor in the list of open files, we return EBADF
if (current == NULL) {
answer = create_error_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, EBADF);
break;
}
char *buf = malloc(MAX_ARGS_LEN * sizeof *buf);
if (buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, THREAD, msg->origin_id, msg->thread_id, ANSWER, WRT, status);
break;
}
// We send a request to the worker who owns the file, with a message "WRT filename length_buffer buffer"
sprintf(buf, "%s %s", current->file_name, fourth_arg);
answer = create_message(WORKER, wd->id, WORKER, current->worker_owner, msg->thread_id, REQUEST, WRT, 3, buf);
s->waiting = 1;
free(buf);
break;
case REA:
snd_arg = msg->args + strlen("FD ");
fourth_arg = snd_arg;
while (*fourth_arg != ' ') {
fourth_arg++;
}
fourth_arg += strlen(" SIZE ");
fd = snd_arg;
current = s->open_fds;
while(current != NULL && current->file_descriptor != atoi(fd)) {
current = current->next;
}
//If we didn't find the file descriptor in the list of open files, we return EBADF
if (current == NULL) {
answer = create_error_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, EBADF);
break;
}
//buf will contain "filename read_size\0"
buf = malloc((strlen(current->file_name) + strlen(fourth_arg) + 2) * sizeof *buf);
if (buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, THREAD, msg->origin_id, msg->thread_id, ANSWER, REA, status);
break;
}
// We send a request to the worker who owns the file, with a message "REA filename length"
sprintf(buf, "%s %s", current->file_name, fourth_arg);
answer = create_message(WORKER, wd->id, WORKER, current->worker_owner, msg->thread_id, REQUEST, REA, 2, buf);
s->waiting = 1;
free(buf);
break;
case CLO:
fd = msg->args + strlen("FD ");
current = s->open_fds;
while(current != NULL && current->file_descriptor != atoi(fd)) {
current = current->next;
}
if (current == NULL) {
answer = create_error_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, EBADF);
} else {
//We send the request to the worker who owns the file. The open_fd structure, owned by this worker, will be deleted if the other worker closes the file successfully
answer = create_message(WORKER, wd->id, WORKER, current->worker_owner, msg->thread_id, REQUEST, CLO, 1, current->file_name);
s->waiting = 1;
}
break;
//We close the files of the client that are open in this worker, and send a broadcast to the rest asking to do the same
case BYE:
status = close_thread_files(wd->lfs, msg->thread_id);
if (status != 0) {
local_answer = create_error_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, BYE, status);
} else {
local_answer = create_message(WORKER, wd->id, WORKER, msg->origin_id, msg->thread_id, ANSWER, BYE, 1, "OK");
}
add_msg_to_pending(s, local_answer);
answer = create_message(WORKER, wd->id, WORKER, -1, msg->thread_id, REQUEST, BYE, 0, NULL);
s->waiting = NWORKERS - 1;
break;
default:
break;
}
s->last_op = msg->op;
send_message(answer);
delete_message(answer);
if (buf != NULL) {
free(buf);
buf = NULL;
}
return 0;
}
void wall_clock_proc(void) {
/*sends message to kcd to register the command types*/
ENVELOPE *msg = (ENVELOPE *)request_memory_block();
msg->message_type = MSG_COMMAND_REGISTRATION;
msg->sender_pid = WALL_CLOCK_PID;
msg->destination_pid = KCD_PID;
set_message(msg, "%WR" + '\0', 4*sizeof(char));
send_message(KCD_PID, msg);
msg = (ENVELOPE *)request_memory_block();
msg->sender_pid = WALL_CLOCK_PID;
msg->destination_pid = KCD_PID;
msg->message_type = MSG_COMMAND_REGISTRATION;
set_message(msg, "%WS" + '\0', 4*sizeof(char));
send_message(KCD_PID, msg);
msg = (ENVELOPE *)request_memory_block();
msg->sender_pid = WALL_CLOCK_PID;
msg->destination_pid = KCD_PID;
msg->message_type = MSG_COMMAND_REGISTRATION;
set_message(msg, "%WT" + '\0', 4*sizeof(char));
send_message(KCD_PID, msg);
while(1){
ENVELOPE * rec_msg= (ENVELOPE*) receive_message(NULL);
char * char_message = (char *) rec_msg->message;
if(rec_msg->message_type == MSG_WALL_CLOCK &&
rec_msg->sender_pid == WALL_CLOCK_PID && show_wclock ==1) {
int curr_time = 0;
int s1, s2, m1, m2, h1, h2;
ENVELOPE * delay_msg = (ENVELOPE*) request_memory_block();
ENVELOPE * w_clock = (ENVELOPE*) request_memory_block();
w_clock->sender_pid = WALL_CLOCK_PID;
w_clock->destination_pid = CRT_PID;
w_clock->message_type = MSG_CRT_DISPLAY;
curr_time = g_timer_count - elapsed + base;
w_secs= (curr_time /1000)%60;
s2=w_secs%10;
s1=w_secs/10;
w_mins = (curr_time/(60000))%60;
m2=w_mins%10;
m1=w_mins/10;
w_hours= (curr_time/(3600000))%24;
h2=w_hours%10;
h1=w_hours/10;
w_clock->message = w_clock + HEADER_OFFSET;
sprintf(w_clock->message, "%d%d:%d%d:%d%d\n\r", h1,h2,m1,m2,s1,s2);
send_message(CRT_PID, w_clock);
delay_msg->message_type=MSG_WALL_CLOCK;
delay_msg->sender_pid=WALL_CLOCK_PID;
delay_msg->destination_pid=WALL_CLOCK_PID;
delay_msg->message=NULL;
delayed_send(WALL_CLOCK_PID, delay_msg, 1000);
}
else {
if (char_message[2]== 'R') {
ENVELOPE *msg =(ENVELOPE *) request_memory_block();
msg->message_type=MSG_WALL_CLOCK;
msg->sender_pid=WALL_CLOCK_PID;
msg->destination_pid=WALL_CLOCK_PID;
msg->message=NULL;
w_secs=0;
w_mins=0;
w_hours=0;
base = 0;
elapsed = g_timer_count;
if (show_wclock == 0){
show_wclock = 1;
send_message(WALL_CLOCK_PID, msg);
}
}
if (char_message[2]== 'S') {
if ((char_message[4] >= '0')&&(char_message[4] <= '9')&&(char_message[5] >= '0')&&(char_message[5] <= '9')&&(char_message[6] == ':')
&&(char_message[7] >= '0')&&(char_message[7] <= '9')&&(char_message[8] >= '0')&&(char_message[8] <= '9')&&(char_message[9] == ':')
&&(char_message[10] >= '0')&&(char_message[10] <= '9')&&(char_message[11] >= '0')&&(char_message[11] <= '9')
&&((char_message[12] == ' ')||(char_message[12] == '\0')))
{
int h1,h2,m1,m2,s1,s2;
ENVELOPE *msg =(ENVELOPE *) request_memory_block();
msg->message_type=MSG_WALL_CLOCK;
msg->sender_pid=WALL_CLOCK_PID;
msg->destination_pid=WALL_CLOCK_PID;
msg->message=NULL;
h1=char_message[4] - '0';
h2=char_message[5] - '0';
w_hours=h1*10 + h2;
m1=char_message[7] - '0';
m2=char_message[8] - '0';
w_mins=m1*10 + m2;
s1=char_message[10] - '0';
s2=char_message[11] - '0';
w_secs= s1*10 + s2;
elapsed = g_timer_count;
base = ((w_hours * 3600) + (w_mins * 60) + w_secs) * 1000;
if (show_wclock == 0){
show_wclock = 1;
send_message(WALL_CLOCK_PID, msg);
}
}
}
else if (char_message[2]== 'T') {
show_wclock=0;
}
}
release_memory_block(rec_msg);
}//end while
}
int process_worker_answer(worker_data *wd, message *msg, session *s)
{
//If we are waiting a message for this session, we check how many messages we have so far.
if (s->thread_id == msg->thread_id && s->waiting > 1){
//We save the message we just received, and if we still haven't got them all, we return
add_msg_to_pending(s, msg);
s->waiting--;
return 0;
}
add_msg_to_pending(s, msg);
s->waiting--;
message *answer = NULL;
char *buf = NULL;
int i;
int status;
int n_args;
switch (msg->op) {
case LSD:
buf = malloc(MAX_ARGS_LEN * sizeof *buf);
if (buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, status);
break;
}
n_args = concat_messages_args(s->pending, NWORKERS, buf, MAX_ARGS_LEN);
answer = create_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, OK, n_args, buf);
break;
//If we got all the answers we were waiting for to create a file
case EXSCRE:
if (s->last_op == CRE) {
//First we remove the file from the list of files being created
remove_file_being_created(wd->lfs, msg->args + strlen("N "));
//Now we check if someone has it
for (i = 0; i < NWORKERS; i++) {
if (strncmp(s->pending[i]->args, "Y", 1) == 0) {
break;
}
}
//If the file already exists
if (i < NWORKERS) {
answer = create_error_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, EEXIST);
}
else {
status = create_file(wd->lfs, (msg->args + strlen("N ")));
if (status != 0) {
answer = create_error_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, status);
} else {
answer = create_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, OK, 0, NULL);
}
}
}
break;
case EXS:
if(s->last_op == OPN) {
for (i = 0; i < NWORKERS; i++) {
if (strncmp(s->pending[i]->args, "Y", 1) == 0) {
break;
}
}
//If no worker returned "Y", the file doesn't exist in any of them
if (i == NWORKERS) {
answer = create_error_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, ENOENT);
}
//If a worker has it, we ask it to open the file
else {
answer = create_message(WORKER, wd->id, WORKER, s->pending[i]->origin_id, msg->thread_id, REQUEST, OPN, 1, (msg->args + strlen("Y ")));
//We are waiting for just one answer
s->waiting = 1;
}
}
else if(s->last_op == DEL) {
for (i = 0; i < NWORKERS; i++) {
if (strncmp(s->pending[i]->args, "Y", 1) == 0) {
break;
}
}
//If no worker returned "Y", the file doesn't exist in any of them
if (i == NWORKERS) {
answer = create_error_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, ENOENT);
}
//If a worker has it, we ask it to delete the file
else {
answer = create_message(WORKER, wd->id, WORKER, s->pending[i]->origin_id, msg->thread_id, REQUEST, DEL, 1, (msg->args + strlen("Y ")));
//We are waiting for just one answer
s->waiting = 1;
}
}
break;
case DEL:
if (strncmp(msg->args, "OK", strlen("OK")) != 0) {
answer = create_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, 2, msg->args);
}
else {
answer = create_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, OK, 0, NULL);
}
break;
case OPN:
if (strncmp(msg->args, "OK", strlen("OK")) != 0) {
answer = create_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, 2, msg->args);
}
else {
add_fd_to_session(s, (msg->args + strlen("OK ")), msg->origin_id, wd->next_fd);
buf = malloc((MAX_FD_LENGTH + strlen("FD ") + 1) * sizeof *buf);
if (buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, status);
break;
}
sprintf(buf,"FD %d", wd->next_fd);
answer = create_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, OK, 2, buf);
wd->next_fd++;
}
break;
case WRT:
if (strncmp(msg->args, "OK", strlen("OK")) != 0) {
answer = create_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, 2, msg->args);
}
else {
answer = create_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, OK, 0, NULL);
}
break;
case REA:
if (strncmp(msg->args, "OK", strlen("OK")) != 0) {
answer = create_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, 2, msg->args);
break;
}
else {
char *args = msg->args + strlen("OK ");
//buf will have "SIZE read_size read_characters\0"
buf = malloc((strlen("SIZE ") + strlen(args) + 1) * sizeof *buf);
if (buf == NULL) {
status = errno;
answer = create_error_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, status);
break;
}
sprintf(buf, "SIZE %s", args);
answer = create_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, OK, 3, buf);
}
break;
case CLO:
if (strncmp(msg->args, "OK ", strlen("OK ")) != 0) {
answer = create_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, 2, msg->args);
}
else {
char *filename = msg->args + strlen("OK ");
delete_fd_from_session(s, filename);
answer = create_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, OK, 0, NULL);
}
break;
case BYE:
for (i = 0; i < NWORKERS; i++) {
if (strncmp(s->pending[i]->args, "OK", strlen("OK")) != 0) {
break;
}
}
//If one of the workers returned an error
if (i < NWORKERS) {
answer = create_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, ERROR, 2, s->pending[i]->args);
}
else {
answer = create_message(WORKER, wd->id, THREAD, msg->thread_id, msg->thread_id, ANSWER, OK, 0, NULL);
delete_session(wd, msg->thread_id);
send_message(answer);
delete_message(answer);
if (buf != NULL) {
free(buf);
}
return 0;
}
break;
default:
break;
}
clear_session_last_operation(s);
send_message(answer);
delete_message(answer);
if (buf != NULL) {
free(buf);
}
return 0;
}
/*
* Forms the packet according to mode of operation.
*/
void
send_update(struct interface *ifp, struct msghdr *mh, int state, unsigned int agronly)
{
int max_entries, num_entries;
int metric_out, poisonf;
int need_nhp = 0;
struct in6_addr *addrp, *current_nhp, *new_nhp;
struct rip6 *pkt;
struct route_entry *rp;
struct rt_plen *rtp, *drt;
struct tree_node *node;
struct rte_stat *stat;
struct gateway *gp;
struct nexthop_rte *nhrp; /* nexthop list */
struct aggregate *agp;
struct route_entry r_tmp;
const int nohopout = 0;
if (snd_data == NULL)
return;
if (!(ifp->if_flag & IFF_RUNNING))
return; /* doesn't have any linklocal */
if (mh == NULL) { /* unsolicited */
mh = &smsgh;
bzero(mh, sizeof(smsgh));
}
mh->msg_iov = &siov; /* overwrite does no harm */
mh->msg_iovlen = 1;
mh->msg_flags = 0; /* clear */
mh->msg_iov->iov_base = snd_data;
pkt = (struct rip6 *)snd_data;
max_entries = get_max_rte(ifp);
need_nhp = 0;
ONCE_FOR_EACH_PACKET;
/* gendefault: if generate then no other RTE is needed */
if ((stat = ifp->if_config->int_dstat) != NULL) {
if (!state) { /* regular update */
/* default route cannot be aggregated further */
bzero((void *)&r_tmp, sizeof(r_tmp));
r_tmp.rip6_metric = stat->rts_metric +
ifp->if_config->int_metric_out;
if (r_tmp.rip6_metric > HOPCOUNT_INFINITY)
r_tmp.rip6_metric = HOPCOUNT_INFINITY;
r_tmp.rip6_rtag = htons(stat->rts_tagval);
new_nhp = current_nhp;
CHECK_NHOP(r_tmp);
WRITE_PAIR(r_tmp);
}
goto lastone; /* and return */
}
/* init aggregate list */
for (agp = ifp->if_config->int_aggr; agp; agp = agp->agr_next)
agp->agr_sent = AGR_NOTSENT;
/* propagate default */
if (ifp->if_config->int_propagate &&
(drt = locate_local_route(&default_rte, &node)) != NULL &&
(drt->rp_state & RTS6_STATIC) == 0 && /* learn from neighbour */
(!state || drt->rp_state & state) && !agronly) {
/* sanity check omitted */
if (drt->rp_gway->gw_ifp == ifp) { /* same interface */
switch (ifp->if_config->int_scheme) {
case RT6_NOHORZN: /* no intelligence */
metric_out = drt->rp_metric +
ifp->if_config->int_metric_out;
break;
case RT6_HORZN:
goto loop; /* no need to advertise */
default: /* case RT6_POISON: */
metric_out = HOPCOUNT_INFINITY;
break;
}
} else { /* other interface */
metric_out = drt->rp_metric +
ifp->if_config->int_metric_out;
}
CHECK_AGR(&drt->rp_leaf->key);
/* This is 1st. RTE, any aggregation has not been sent yet */
if (agp) {
r_tmp.rip6_prflen = agp->agr_pref.prf_len;
r_tmp.rip6_addr = agp->agr_pref.prf_addr;
r_tmp.rip6_metric = agp->agr_stat.rts_metric
+ ifp->if_config->int_metric_out;
r_tmp.rip6_rtag = htons(agp->agr_stat.rts_tagval);
agp->agr_sent = AGR_SENT;
} else {
r_tmp.rip6_prflen = 0; /* DEFAULT ROUTE drt->rp_len */
r_tmp.rip6_addr = drt->rp_leaf->key;
r_tmp.rip6_metric = metric_out;
r_tmp.rip6_rtag = htons(drt->rp_tag);
}
if (r_tmp.rip6_metric > HOPCOUNT_INFINITY)
r_tmp.rip6_metric = HOPCOUNT_INFINITY;
new_nhp = &ci_info(ifp->if_cinfo).ipi6_addr;
CHECK_NHOP(r_tmp);
WRITE_PAIR(r_tmp);
} /* end propagate */
loop:
for (gp = gway; gp; gp = gp->gw_next) {
if (!(gp->gw_ifp) || (gp->gw_ifp->if_flag & IFF_UP) == 0)
continue;
if (gp->gw_ifp == ifp) {
switch (ifp->if_config->int_scheme) {
case RT6_NOHORZN: /* no intelligence */
poisonf = 0;
break;
case RT6_HORZN:
continue;
/* never reached */
default: /* case RT6_POISON: */
poisonf = 1;
break;
}
} else
poisonf = 0;
for (rtp = gp->gw_dest; rtp; rtp = rtp->rp_ndst) {
if (state && (rtp->rp_state & state) == 0)
continue;
addrp = &(rtp->rp_leaf->key);
if (IN6_IS_ADDR_SITELOCAL(addrp) &&
!(ifp->if_config->int_site))
continue;
if (IN6_IS_ADDR_UNSPECIFIED(addrp) &&
rtp->rp_len == MAX_PREFLEN)
continue; /* already propagated */
CHECK_AGR(addrp);
if (agp) {
if (agp->agr_sent == AGR_SENT)
continue; /* already sent */
r_tmp.rip6_prflen = agp->agr_pref.prf_len;
r_tmp.rip6_addr = agp->agr_pref.prf_addr;
r_tmp.rip6_metric = agp->agr_stat.rts_metric +
ifp->if_config->int_metric_out;
r_tmp.rip6_rtag = htons(agp->agr_stat.rts_tagval);
agp->agr_sent = AGR_SENT;
} else {
if (agronly)
continue; /* send only aggregation */
r_tmp.rip6_prflen = rtp->rp_len;
r_tmp.rip6_addr = *addrp;
r_tmp.rip6_metric = poisonf
? HOPCOUNT_INFINITY
: rtp->rp_metric + ifp->if_config->int_metric_out;
r_tmp.rip6_rtag = htons(rtp->rp_tag);
}
if (r_tmp.rip6_metric > HOPCOUNT_INFINITY)
r_tmp.rip6_metric = HOPCOUNT_INFINITY;
new_nhp = &ci_info(ifp->if_cinfo).ipi6_addr;
CHECK_NHOP(r_tmp);
WRITE_PAIR(r_tmp);
} /* for each rtp */
} /* for each gateway */
lastone:
if (num_entries) {
mh->msg_iov->iov_len = ((char *)rp - (char *)mh->msg_iov->iov_base);
send_message(mh, ifp, agronly);
}
return;
}
void
GCS_MAVLINK_Tracker::data_stream_send(void)
{
if (_queued_parameter != nullptr) {
if (streamRates[STREAM_PARAMS].get() <= 0) {
streamRates[STREAM_PARAMS].set(10);
}
if (stream_trigger(STREAM_PARAMS)) {
send_message(MSG_NEXT_PARAM);
}
}
if (tracker.in_mavlink_delay) {
// don't send any other stream types while in the delay callback
return;
}
if (stream_trigger(STREAM_RAW_SENSORS)) {
send_message(MSG_RAW_IMU1);
send_message(MSG_RAW_IMU2);
send_message(MSG_RAW_IMU3);
}
if (stream_trigger(STREAM_EXTENDED_STATUS)) {
send_message(MSG_EXTENDED_STATUS1);
send_message(MSG_EXTENDED_STATUS2);
send_message(MSG_NAV_CONTROLLER_OUTPUT);
send_message(MSG_GPS_RAW);
}
if (stream_trigger(STREAM_POSITION)) {
send_message(MSG_LOCATION);
send_message(MSG_LOCAL_POSITION);
}
if (stream_trigger(STREAM_RAW_CONTROLLER)) {
send_message(MSG_SERVO_OUT);
}
if (stream_trigger(STREAM_RC_CHANNELS)) {
send_message(MSG_RADIO_IN);
send_message(MSG_RADIO_OUT);
}
if (stream_trigger(STREAM_EXTRA1)) {
send_message(MSG_ATTITUDE);
}
if (stream_trigger(STREAM_EXTRA3)) {
send_message(MSG_AHRS);
send_message(MSG_HWSTATUS);
send_message(MSG_SIMSTATE);
send_message(MSG_MAG_CAL_REPORT);
send_message(MSG_MAG_CAL_PROGRESS);
}
}
void
GCS_MAVLINK_Rover::data_stream_send(void)
{
gcs().set_out_of_time(false);
send_queued_parameters();
if (gcs().out_of_time()) {
return;
}
if (rover.in_mavlink_delay) {
#if HIL_MODE != HIL_MODE_DISABLED
// in HIL we need to keep sending servo values to ensure
// the simulator doesn't pause, otherwise our sensor
// calibration could stall
if (stream_trigger(STREAM_RAW_CONTROLLER)) {
send_message(MSG_SERVO_OUT);
}
if (stream_trigger(STREAM_RC_CHANNELS)) {
send_message(MSG_SERVO_OUTPUT_RAW);
}
#endif
// don't send any other stream types while in the delay callback
return;
}
if (gcs().out_of_time()) {
return;
}
if (stream_trigger(STREAM_RAW_SENSORS)) {
send_message(MSG_RAW_IMU1);
send_message(MSG_RAW_IMU2);
send_message(MSG_RAW_IMU3);
}
if (gcs().out_of_time()) {
return;
}
if (stream_trigger(STREAM_EXTENDED_STATUS)) {
send_message(MSG_EXTENDED_STATUS1);
send_message(MSG_EXTENDED_STATUS2);
send_message(MSG_CURRENT_WAYPOINT);
send_message(MSG_GPS_RAW);
send_message(MSG_GPS_RTK);
send_message(MSG_GPS2_RAW);
send_message(MSG_GPS2_RTK);
send_message(MSG_NAV_CONTROLLER_OUTPUT);
send_message(MSG_FENCE_STATUS);
}
if (gcs().out_of_time()) {
return;
}
if (stream_trigger(STREAM_POSITION)) {
// sent with GPS read
send_message(MSG_LOCATION);
send_message(MSG_LOCAL_POSITION);
}
if (gcs().out_of_time()) {
return;
}
if (stream_trigger(STREAM_RAW_CONTROLLER)) {
send_message(MSG_SERVO_OUT);
}
if (gcs().out_of_time()) {
return;
}
if (stream_trigger(STREAM_RC_CHANNELS)) {
send_message(MSG_SERVO_OUTPUT_RAW);
send_message(MSG_RADIO_IN);
}
if (gcs().out_of_time()) {
return;
}
if (stream_trigger(STREAM_EXTRA1)) {
send_message(MSG_ATTITUDE);
send_message(MSG_SIMSTATE);
send_message(MSG_PID_TUNING);
}
if (gcs().out_of_time()) {
return;
}
if (stream_trigger(STREAM_EXTRA2)) {
send_message(MSG_VFR_HUD);
}
if (gcs().out_of_time()) {
return;
}
if (stream_trigger(STREAM_EXTRA3)) {
send_message(MSG_AHRS);
send_message(MSG_HWSTATUS);
send_message(MSG_RANGEFINDER);
send_message(MSG_SYSTEM_TIME);
send_message(MSG_BATTERY2);
send_message(MSG_BATTERY_STATUS);
send_message(MSG_MAG_CAL_REPORT);
send_message(MSG_MAG_CAL_PROGRESS);
send_message(MSG_MOUNT_STATUS);
send_message(MSG_EKF_STATUS_REPORT);
send_message(MSG_VIBRATION);
send_message(MSG_RPM);
}
}
static void query_resources(wrt_client_t *c, mrp_json_t *req)
{
const char *type = RESWRT_QUERY_RESOURCES;
mrp_json_t *reply, *rarr, *r, *ao;
const char **resources;
int seq, cnt;
mrp_attr_t *attrs, *a;
mrp_attr_t buf[ATTRIBUTE_MAX];
uint32_t id;
if (!mrp_json_get_integer(req, "seq", &seq)) {
ignore_invalid_request(c, req, "missing 'seq' field");
return;
}
rarr = r = ao = NULL;
resources = mrp_resource_definition_get_all_names(0, NULL);
if (resources == NULL) {
error_reply(c, type, seq, ENOMEM, "failed to query class names");
return;
}
reply = alloc_reply(type, seq);
if (reply == NULL)
return;
rarr = mrp_json_create(MRP_JSON_ARRAY);
if (rarr == NULL)
goto fail;
for (id = 0; resources[id]; id++) {
r = mrp_json_create(MRP_JSON_OBJECT);
if (r == NULL)
goto fail;
if (!mrp_json_add_string (r, "name", resources[id]))
goto fail;
attrs = mrp_resource_definition_read_all_attributes(id,
ATTRIBUTE_MAX, buf);
ao = mrp_json_create(MRP_JSON_OBJECT);
if (ao == NULL)
goto fail;
for (a = attrs, cnt = 0; a->name; a++, cnt++) {
switch (a->type) {
case mqi_string:
if (!mrp_json_add_string(ao, a->name, a->value.string))
goto fail;
break;
case mqi_integer:
case mqi_unsignd:
if (!mrp_json_add_integer(ao, a->name, a->value.integer))
goto fail;
break;
case mqi_floating:
if (!mrp_json_add_double(ao, a->name, a->value.floating))
goto fail;
break;
default:
mrp_log_error("attribute '%s' of resource '%s' "
"has unknown type %d", a->name, resources[id],
a->type);
break;
}
}
if (cnt > 0)
mrp_json_add(r, "attributes", ao);
else
mrp_json_unref(ao);
ao = NULL;
if (!mrp_json_array_append(rarr, r))
goto fail;
else
r = NULL;
}
if (mrp_json_add_integer(reply, "status" , 0)) {
mrp_json_add (reply, "resources", rarr);
send_message(c, reply);
}
mrp_json_unref(reply);
mrp_free(resources);
return;
fail:
mrp_json_unref(reply);
mrp_json_unref(rarr);
mrp_json_unref(r);
mrp_json_unref(ao);
mrp_free(resources);
}
//Basic test for create/attach and exit.
int pc_thread_cont_mutatee()
{
int result;
int error = 0;
int i;
struct local_data *localData = NULL;
handshake can_stop;
allow_exit can_exit;
num_threads = 0;
for (i = 0; i < gargc; i++) {
if (strcmp(gargv[i], "-mt") == 0) {
num_threads = atoi(gargv[i+1]);
break;
}
}
if( num_threads > 0 ) {
// Each thread needs its own mutex and access to the barrier
localData = (struct local_data *)malloc(sizeof(struct local_data));
initBarrier(&localData->barrier, num_threads+1);
localData->myLocks = (testlock_t *)malloc(sizeof(testlock_t)*num_threads);
for(i = 0; i < num_threads; ++i) {
initLock(&localData->myLocks[i]);
testLock(&localData->myLocks[i]);
}
}
result = initProcControlTest(threadFunc, (void *)localData);
if (result != 0) {
output->log(STDERR, "Initialization failed\n");
if( num_threads > 0 ) freeLocalData(localData);
return -1;
}
if( num_threads > 0 ) {
logstatus("initial thread: waiting on barrier\n");
waitTestBarrier(&localData->barrier);
// Alert the mutator that all the threads have gotten through the lock
// and can safely be stopped now
can_stop.code = HANDSHAKE_CODE;
send_message((unsigned char *)&can_stop, sizeof(handshake));
// Wait for mutator to indicate that the stop finished
memset(&can_stop, 0, sizeof(handshake));
recv_message((unsigned char *)&can_stop, sizeof(handshake));
if (can_stop.code != HANDSHAKE_CODE) {
output->log(STDERR, "Received event that wasn't handshake\n");
error = 1;
}
logstatus("initial thread: received stop handshake, releasing threads\n");
// Release all the locks
for(i = 0; i < num_threads; ++i) {
testUnlock(&localData->myLocks[i]);
}
// Alert mutator that all threads can be continued when ready
memset(&can_stop, 0, sizeof(handshake));
can_stop.code = HANDSHAKE_CODE;
send_message((unsigned char *)&can_stop, sizeof(handshake));
logstatus("initial thread: all threads can be continued\n");
}
recv_message((unsigned char *) &can_exit, sizeof(allow_exit));
if (can_exit.code != ALLOWEXIT_CODE) {
output->log(STDERR, "Recieved event that wasn't allow_exit\n");
error = 1;
}
result = finiProcControlTest(0);
if (result != 0) {
output->log(STDERR, "Finalization failed\n");
if( num_threads > 0 ) freeLocalData(localData);
return -1;
}
if (error) {
if( num_threads > 0 ) freeLocalData(localData);
return -1;
}
if( num_threads > 0 ) freeLocalData(localData);
test_passes(testname);
return 0;
}
void select_click_handler(ClickRecognizerRef recognizer, void *context)
{
send_message(CEN);
}
/**************************************************************************
* mmioSeek [WINMM.@]
*/
LONG WINAPI mmioSeek(HMMIO hmmio, LONG lOffset, INT iOrigin)
{
LPWINE_MMIO wm;
LONG offset;
TRACE("(%p, %08X, %d);\n", hmmio, lOffset, iOrigin);
if ((wm = MMIO_Get(hmmio)) == NULL)
return MMSYSERR_INVALHANDLE;
/* not buffered, direct seek on file */
if (!wm->info.pchBuffer)
return send_message(wm->ioProc, &wm->info, MMIOM_SEEK, lOffset, iOrigin, FALSE);
switch (iOrigin) {
case SEEK_SET:
offset = lOffset;
break;
case SEEK_CUR:
offset = wm->info.lBufOffset + (wm->info.pchNext - wm->info.pchBuffer) + lOffset;
break;
case SEEK_END:
offset = ((wm->info.fccIOProc == FOURCC_MEM)? wm->info.cchBuffer : wm->dwFileSize) - lOffset;
break;
default:
return -1;
}
if (offset && offset >= wm->dwFileSize && wm->info.fccIOProc != FOURCC_MEM) {
/* should check that write mode exists */
if (MMIO_Flush(wm, 0) != MMSYSERR_NOERROR)
return -1;
wm->info.lBufOffset = offset;
wm->info.pchEndRead = wm->info.pchBuffer;
wm->info.pchEndWrite = wm->info.pchBuffer + wm->info.cchBuffer;
if ((wm->info.dwFlags & MMIO_RWMODE) == MMIO_READ) {
wm->info.lDiskOffset = wm->dwFileSize;
}
} else if ((wm->info.cchBuffer > 0) &&
((offset < wm->info.lBufOffset) ||
(offset >= wm->info.lBufOffset + wm->info.cchBuffer) ||
!wm->bBufferLoaded)) {
/* stay in same buffer ? */
/* some memory mapped buffers are defined with -1 as a size */
/* condition to change buffer */
if ((wm->info.fccIOProc == FOURCC_MEM) ||
MMIO_Flush(wm, 0) != MMSYSERR_NOERROR ||
/* this also sets the wm->info.lDiskOffset field */
send_message(wm->ioProc, &wm->info, MMIOM_SEEK,
(offset / wm->info.cchBuffer) * wm->info.cchBuffer,
SEEK_SET, FALSE) == -1)
return -1;
MMIO_GrabNextBuffer(wm, TRUE);
}
wm->info.pchNext = wm->info.pchBuffer + (offset - wm->info.lBufOffset);
TRACE("=> %d\n", offset);
return offset;
}
