forked from Archive/PX4-Autopilot
navigator fix vehicle_command_ack (don't copy external)
This commit is contained in:
parent
aa2566970e
commit
80dd87536e
|
@ -839,7 +839,8 @@ bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, int cancel_sub)
|
|||
|
||||
orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd);
|
||||
|
||||
if (cmd.from_external) { // ignore internal commands, such as VEHICLE_CMD_DO_MOUNT_CONTROL from vmount
|
||||
// ignore internal commands, such as VEHICLE_CMD_DO_MOUNT_CONTROL from vmount
|
||||
if (cmd.from_external) {
|
||||
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION &&
|
||||
(int)cmd.param1 == 0 &&
|
||||
(int)cmd.param2 == 0 &&
|
||||
|
|
|
@ -1206,7 +1206,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
|||
case vehicle_command_s::VEHICLE_CMD_LOGGING_START:
|
||||
case vehicle_command_s::VEHICLE_CMD_LOGGING_STOP:
|
||||
case vehicle_command_s::VEHICLE_CMD_NAV_DELAY:
|
||||
/* ignore commands that handled in low prio loop */
|
||||
/* ignore commands that are handled by other parts of the system */
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
|
@ -373,8 +373,8 @@ Navigator::task_main()
|
|||
|
||||
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
|
||||
|
||||
struct position_setpoint_triplet_s *rep = get_reposition_triplet();
|
||||
struct position_setpoint_triplet_s *curr = get_position_setpoint_triplet();
|
||||
position_setpoint_triplet_s *rep = get_reposition_triplet();
|
||||
position_setpoint_triplet_s *curr = get_position_setpoint_triplet();
|
||||
|
||||
// store current position as previous position and goal as next
|
||||
rep->previous.yaw = get_global_position()->yaw;
|
||||
|
@ -394,8 +394,9 @@ Navigator::task_main()
|
|||
rep->current.yaw = NAN;
|
||||
}
|
||||
|
||||
// Position change with optional altitude change
|
||||
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
|
||||
|
||||
// Position change with optional altitude change
|
||||
rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
|
||||
rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
|
||||
|
||||
|
@ -406,18 +407,17 @@ Navigator::task_main()
|
|||
rep->current.alt = get_global_position()->alt;
|
||||
}
|
||||
|
||||
// Altitude without position change
|
||||
|
||||
} else if (PX4_ISFINITE(cmd.param7) && curr->current.valid
|
||||
&& PX4_ISFINITE(curr->current.lat)
|
||||
&& PX4_ISFINITE(curr->current.lon)) {
|
||||
|
||||
// Altitude without position change
|
||||
rep->current.lat = curr->current.lat;
|
||||
rep->current.lon = curr->current.lon;
|
||||
rep->current.alt = cmd.param7;
|
||||
|
||||
// All three set to NaN - hold in current position
|
||||
|
||||
} else {
|
||||
// All three set to NaN - hold in current position
|
||||
rep->current.lat = get_global_position()->lat;
|
||||
rep->current.lon = get_global_position()->lon;
|
||||
rep->current.alt = get_global_position()->alt;
|
||||
|
@ -430,7 +430,7 @@ Navigator::task_main()
|
|||
// CMD_DO_REPOSITION is acknowledged by commander
|
||||
|
||||
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
|
||||
struct position_setpoint_triplet_s *rep = get_takeoff_triplet();
|
||||
position_setpoint_triplet_s *rep = get_takeoff_triplet();
|
||||
|
||||
// store current position as previous position and goal as next
|
||||
rep->previous.yaw = get_global_position()->yaw;
|
||||
|
@ -477,6 +477,7 @@ Navigator::task_main()
|
|||
|
||||
if (land_start != -1) {
|
||||
vehicle_command_s vcmd = {};
|
||||
vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
|
||||
vcmd.param1 = land_start;
|
||||
publish_vehicle_cmd(&vcmd);
|
||||
|
||||
|
@ -493,6 +494,8 @@ Navigator::task_main()
|
|||
_mission.set_current_offboard_mission_index(cmd.param1);
|
||||
}
|
||||
|
||||
// CMD_MISSION_START is acknowledged by commander
|
||||
|
||||
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
|
||||
if (cmd.param2 > FLT_EPSILON) {
|
||||
// XXX not differentiating ground and airspeed yet
|
||||
|
@ -744,18 +747,6 @@ Navigator::start()
|
|||
void
|
||||
Navigator::status()
|
||||
{
|
||||
/* TODO: add this again */
|
||||
// PX4_INFO("Global position is %svalid", _global_pos_valid ? "" : "in");
|
||||
|
||||
// if (_global_pos.global_valid) {
|
||||
// PX4_INFO("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
|
||||
// PX4_INFO("Altitude %5.5f meters, altitude above home %5.5f meters",
|
||||
// (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
|
||||
// PX4_INFO("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f",
|
||||
// (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d);
|
||||
// PX4_INFO("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
|
||||
// }
|
||||
|
||||
_geofence.printStatus();
|
||||
|
||||
}
|
||||
|
@ -849,8 +840,6 @@ Navigator::reset_triplets()
|
|||
_pos_sp_triplet_updated = true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
float
|
||||
Navigator::get_cruising_throttle()
|
||||
{
|
||||
|
@ -1059,7 +1048,7 @@ Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t res
|
|||
command_ack.command = cmd.command;
|
||||
command_ack.target_system = cmd.source_system;
|
||||
command_ack.target_component = cmd.source_component;
|
||||
command_ack.from_external = cmd.from_external;
|
||||
command_ack.from_external = false;
|
||||
|
||||
command_ack.result = result;
|
||||
command_ack.result_param1 = 0;
|
||||
|
|
Loading…
Reference in New Issue