//.........这里部分代码省略.........
}
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/
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 */
/* 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);
}
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);
}
}
//.........这里部分代码省略.........
开发者ID:Hassaan，项目名称:rtx，代码行数:101，代码来源:wallclock.c
示例12: help_mouse_callback
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);
}
/*
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);
}
}
//.........这里部分代码省略.........
客服电话
APP下载
官方微信
请发表评论