diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index ec6a372502..b0577f068e 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -111,8 +111,8 @@ Land::on_active() // send reposition cmd to get out of land mode (will loiter at current position and altitude) vehicle_command_s vehicle_command{}; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; - vehicle_command.param1 = -1; - vehicle_command.param2 = 1; + vehicle_command.param1 = -1.f; // Default speed + vehicle_command.param2 = 1.f; // Modes should switch, not setting this is unsupported vehicle_command.param5 = _navigator->get_global_position()->lat; vehicle_command.param6 = _navigator->get_global_position()->lon; // as we don't know the landing point altitude assume the worst case (abort at 0m above ground), diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 706433329a..7ad7b54bea 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -917,8 +917,8 @@ MissionBase::do_abort_landing() // send reposition cmd to get out of mission vehicle_command_s vehicle_command{}; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; - vehicle_command.param1 = -1; - vehicle_command.param2 = 1; + vehicle_command.param1 = -1.f; // Default speed + vehicle_command.param2 = 1.f; // Modes should switch, not setting this is unsupported vehicle_command.param5 = _mission_item.lat; vehicle_command.param6 = _mission_item.lon; vehicle_command.param7 = alt_sp; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 87dc337fc5..719129dca7 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -784,7 +784,7 @@ MissionBlock::set_land_item(struct mission_item_s *item) vehicle_command_s vehicle_command{}; vehicle_command.command = NAV_CMD_DO_VTOL_TRANSITION; vehicle_command.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - vehicle_command.param2 = 0.0f; + vehicle_command.param2 = 0.f; // normal unforced transition _navigator->publish_vehicle_command(&vehicle_command); } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 307c3c03d9..eb4bc37f8e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1409,15 +1409,15 @@ void Navigator::publish_vehicle_command(vehicle_command_s *vehicle_command) case NAV_CMD_IMAGE_START_CAPTURE: if (static_cast(vehicle_command->param3) == 1) { - // When sending a single capture we need to include the sequence number, thus camera_trigger needs to handle this cmd - vehicle_command->param1 = 0.0f; - vehicle_command->param2 = 0.0f; - vehicle_command->param3 = 0.0f; - vehicle_command->param4 = 0.0f; - vehicle_command->param5 = 1.0; - vehicle_command->param6 = 0.0; - vehicle_command->param7 = 0.0f; + // When sending a single capture we need to include the sequence number, thus camera_trigger needs to handle this command vehicle_command->command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL; + vehicle_command->param1 = 0.f; // Session control hide lens + vehicle_command->param2 = 0.f; // Zoom absolute position + vehicle_command->param3 = 0.f; // Zoom step + vehicle_command->param4 = 0.f; // Focus lock + vehicle_command->param5 = 1.; // Shoot command + vehicle_command->param6 = 0.; // Command identity + vehicle_command->param7 = 0.f; // Shot identifier } else { // We are only capturing multiple if param3 is 0 or > 1. @@ -1531,10 +1531,10 @@ void Navigator::acquire_gimbal_control() { vehicle_command_s vehicle_command{}; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; - vehicle_command.param1 = _vstatus.system_id; + vehicle_command.param1 = _vstatus.system_id; // Take primary control vehicle_command.param2 = _vstatus.component_id; - vehicle_command.param3 = -1.0f; // Leave unchanged. - vehicle_command.param4 = -1.0f; // Leave unchanged. + vehicle_command.param3 = -1.f; // Leave secondary control unchanged + vehicle_command.param4 = -1.f; publish_vehicle_command(&vehicle_command); } @@ -1542,10 +1542,10 @@ void Navigator::release_gimbal_control() { vehicle_command_s vehicle_command{}; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; - vehicle_command.param1 = -3.0f; // Remove control if it had it. - vehicle_command.param2 = -3.0f; // Remove control if it had it. - vehicle_command.param3 = -1.0f; // Leave unchanged. - vehicle_command.param4 = -1.0f; // Leave unchanged. + vehicle_command.param1 = -3.f; // Remove primary control if it was taken + vehicle_command.param2 = -3.f; + vehicle_command.param3 = -1.f; // Leave secondary control unchanged + vehicle_command.param4 = -1.f; publish_vehicle_command(&vehicle_command); } @@ -1556,7 +1556,7 @@ Navigator::stop_capturing_images() if (_is_capturing_images) { vehicle_command_s vehicle_command{}; vehicle_command.command = NAV_CMD_IMAGE_STOP_CAPTURE; - vehicle_command.param1 = 0.0f; + vehicle_command.param1 = 0.f; publish_vehicle_command(&vehicle_command); // _is_capturing_images is reset inside publish_vehicle_command. @@ -1608,8 +1608,8 @@ void Navigator::disable_camera_trigger() vehicle_command_s vehicle_command{}; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; // Pause trigger - vehicle_command.param1 = -1.0f; - vehicle_command.param3 = 1.0f; + vehicle_command.param1 = -1.f; + vehicle_command.param3 = 1.f; publish_vehicle_command(&vehicle_command); } @@ -1617,9 +1617,9 @@ void Navigator::set_gimbal_neutral() { vehicle_command_s vehicle_command{}; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW; - vehicle_command.param1 = NAN; + vehicle_command.param1 = NAN; // Don't set any angles vehicle_command.param2 = NAN; - vehicle_command.param3 = NAN; + vehicle_command.param3 = NAN; // Don't set any angular velocities vehicle_command.param4 = NAN; vehicle_command.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL; publish_vehicle_command(&vehicle_command);