aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: integrate move of p1 from Location to mission cmd

Browse Source
This commit is contained in:
Randy Mackay 2014-03-03 14:38:32 +09:00
parent 87126c9b71
commit c3162f67e8
4 changed files with 17 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduCopter/GCS_Mavlink.pde
View File

@ -1339,7 +1339,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
// convert mavlink packet to mission command
if (!AP_Mission::mavlink_to_mission_cmd(packet, cmd)) {
if (!AP_Mission::mavlink_to_mission_cmd(packet, cmd, true)) {
result = MAV_MISSION_ERROR;
goto mission_item_receive_failed;
}
@ -1444,7 +1444,7 @@ mission_item_receive_failed:
// convert mission command to mavlink mission item packet
mavlink_mission_item_t ret_packet;
memset(&ret_packet, 0, sizeof(ret_packet));
if (!AP_Mission::mission_cmd_to_mavlink(cmd, ret_packet)) {
if (!AP_Mission::mission_cmd_to_mavlink(cmd, ret_packet, true)) {
result = MAV_MISSION_ERROR;
goto mission_item_send_failed;
}
@ -1493,9 +1493,9 @@ mission_item_send_failed:
}
// set current command
mission.set_current_cmd(packet.seq);
mavlink_msg_mission_current_send(chan, mission.get_current_do_cmd().index);
if (mission.set_current_cmd(packet.seq)) {
mavlink_msg_mission_current_send(chan, mission.get_current_nav_cmd().index);
}
break;
}
@ -1545,6 +1545,7 @@ mission_item_send_failed:
if (!mission.clear()) {
// return error if we were unable to clear the mission (possibly because we're currently flying the mission)
mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ERROR);
break;
}
// set variables to help handle the expected receiving of commands from the GCS

2
ArduCopter/Log.pde
View File

@ -443,7 +443,7 @@ static void Log_Write_Cmd(const AP_Mission::Mission_Command& cmd)
command_number : cmd.index,
waypoint_id : cmd.id,
waypoint_options : cmd.content.location.options,
waypoint_param1 : cmd.content.location.p1,
waypoint_param1 : cmd.p1,
waypoint_altitude : cmd.content.location.alt,
waypoint_latitude : cmd.content.location.lat,
waypoint_longitude : cmd.content.location.lng

20
ArduCopter/commands_logic.pde
View File

@ -88,20 +88,20 @@ static bool start_command(const AP_Mission::Mission_Command& cmd)
break;
case MAV_CMD_DO_SET_SERVO:
ServoRelayEvents.do_set_servo(cmd.content.location.p1, cmd.content.location.alt);
ServoRelayEvents.do_set_servo(cmd.p1, cmd.content.location.alt);
break;
case MAV_CMD_DO_SET_RELAY:
ServoRelayEvents.do_set_relay(cmd.content.location.p1, cmd.content.location.alt);
ServoRelayEvents.do_set_relay(cmd.p1, cmd.content.location.alt);
break;
case MAV_CMD_DO_REPEAT_SERVO:
ServoRelayEvents.do_repeat_servo(cmd.content.location.p1, cmd.content.location.alt,
ServoRelayEvents.do_repeat_servo(cmd.p1, cmd.content.location.alt,
cmd.content.location.lat, cmd.content.location.lng);
break;
case MAV_CMD_DO_REPEAT_RELAY:
ServoRelayEvents.do_repeat_relay(cmd.content.location.p1, cmd.content.location.alt,
ServoRelayEvents.do_repeat_relay(cmd.p1, cmd.content.location.alt,
cmd.content.location.lat);
break;
@ -274,8 +274,7 @@ static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
// this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0;
// this is the delay, stored in seconds and expanded to millis
// To-Do: move waypoint delay out of location structure and into cmd
loiter_time_max = cmd.content.location.p1;
loiter_time_max = cmd.p1;
// if no delay set the waypoint as "fast"
if (loiter_time_max == 0 ) {
wp_nav.set_fast_waypoint(true);
@ -358,8 +357,7 @@ static void do_circle(const AP_Mission::Mission_Command& cmd)
auto_circle_start(circle_center);
// record number of desired rotations from mission command
// To-Do: move p1 from location structure to cmd structure
circle_desired_rotations = cmd.content.location.p1;
circle_desired_rotations = cmd.p1;
}
// do_loiter_time - initiate loitering at a point for a given time period
@ -389,7 +387,7 @@ static void do_loiter_time(const AP_Mission::Mission_Command& cmd)
// setup loiter timer
loiter_time = 0;
loiter_time_max = cmd.content.location.p1; // units are (seconds)
loiter_time_max = cmd.p1; // units are (seconds)
}
/********************************************************************************/
@ -637,12 +635,12 @@ static bool do_guided(const AP_Mission::Mission_Command& cmd)
static void do_change_speed(const AP_Mission::Mission_Command& cmd)
{
wp_nav.set_horizontal_velocity(cmd.content.location.p1 * 100);
wp_nav.set_horizontal_velocity(cmd.p1 * 100);
}
static void do_set_home(const AP_Mission::Mission_Command& cmd)
{
if(cmd.content.location.p1 == 1) {
if(cmd.p1 == 1) {
init_home();
} else {
ahrs.set_home(cmd.content.location.lat, cmd.content.location.lng, 0);

2
ArduCopter/control_modes.pde
View File

@ -205,7 +205,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
// set our location ID to 16, MAV_CMD_NAV_WAYPOINT
cmd.id = MAV_CMD_NAV_TAKEOFF;
cmd.content.location.options = 0;
cmd.content.location.p1 = 0;
cmd.p1 = 0;
cmd.content.location.lat = 0;
cmd.content.location.lng = 0;
cmd.content.location.alt = max(current_loc.alt,100);