diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 821f6d8d3d..9122403898 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2196,10 +2196,6 @@ int commander_thread_main(int argc, char *argv[])
 		if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
 		    (hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) {
 #else
-		// TODO-JYW: TESTING-TESTING
-		warnx("status.rc_input_blocked: %d, sp_man.timestamp: %d", status.rc_input_blocked, sp_man.timestamp);
-		// TODO-JYW: TESTING-TESTING
-
 		// HACK: remove old data check due to timestamp issue in QURT
 		if (!status.rc_input_blocked && sp_man.timestamp != 0) {
 #endif
@@ -2218,14 +2214,6 @@ int commander_thread_main(int argc, char *argv[])
 
 			status.rc_signal_lost = false;
 
-			// TODO-JYW: TESTING-TESTING
-			warnx("lower left stick: status.is_rotary_wing: %d, status.rc_input_mode: %d, status.arming_state: %d",
-					status.is_rotary_wing, status.rc_input_mode, status.arming_state, status.main_state, status.condition_landed, (double)sp_man.r, (double)sp_man.z);
-			warnx("lower left stick: status.is_rotary_wing: %d, status.rc_input_mode: %d, status.arming_state: %d, status.main_state: %d, status.condition_landed: %d, sp_man.r: %f, sp_man.z: %f",
-					status.is_rotary_wing, status.rc_input_mode, status.arming_state, status.main_state, status.condition_landed, (double)sp_man.r, (double)sp_man.z);
-			// TODO-JYW: TESTING-TESTING
-
-
 			/* check if left stick is in lower left position and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) -> disarm
 			 * do it only for rotary wings */
 			if (status.is_rotary_wing && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF &&
@@ -2258,13 +2246,6 @@ int commander_thread_main(int argc, char *argv[])
 				stick_off_counter = 0;
 			}
 
-			// TODO-JYW: TESTING-TESTING
-			warnx("lower right position: ON_OFF_LIMIT: %f, stick_off_counter: %d, stick_on_counter: %d",
-					(double)STICK_ON_OFF_LIMIT, stick_off_counter, stick_on_counter);
-			warnx("lower right position: sp_man.r: %.6f, sp_man.z: %.6f, status.rc_input_mode: %d",
-					(double)sp_man.r, (double)sp_man.z, status.rc_input_mode);
-			// TODO-JYW: TESTING-TESTING
-
 			/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
 			if (sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF ) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
@@ -2276,32 +2257,22 @@ int commander_thread_main(int argc, char *argv[])
 
 					if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) &&
 						(status.main_state != vehicle_status_s::MAIN_STATE_STAB)) {
-						// TOOD-JYW: TESTING-TESTING:
-						warnx("NOT ARMING: Switch to MANUAL mode first.");
 						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 
 					} else if (!status.condition_home_position_valid &&
 								geofence_action == geofence_result_s::GF_ACTION_RTL) {
-						// TOOD-JYW: TESTING-TESTING:
-						warnx("NOT ARMING: Geofence RTL requires valid home");
 						print_reject_arm("NOT ARMING: Geofence RTL requires valid home");
 
 					} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
-						// TODO-JYW: TESTING-TESTING
-						warnx("attempting arming_state_transition");
 						arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
 										     mavlink_fd);
 
 						if (arming_ret == TRANSITION_CHANGED) {
 							arming_state_changed = true;
 						} else {
-							// TOOD-JYW: TESTING-TESTING:
-							warnx("NOT ARMING: Configuration error");
 							print_reject_arm("NOT ARMING: Configuration error");
 						}
 					}
-					// TOOD-JYW: TESTING-TESTING:
-					warnx("on counter set to zero");
 					stick_on_counter = 0;
 
 				} else {
@@ -2309,14 +2280,9 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 			} else {
-				// TOOD-JYW: TESTING-TESTING:
-				warnx("on counter set to zero");
 				stick_on_counter = 0;
 			}
 
-			// TOOD-JYW: TESTING-TESTING:
-			warnx("arming_ret: %d, arming_state_changed: %d, status.main_state: %d, status.arming_state: %d", arming_ret, arming_state_changed, status.main_state, status.arming_state);
-
 			if (arming_ret == TRANSITION_CHANGED) {
 				if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 					mavlink_log_info(mavlink_fd, "ARMED by RC");
@@ -2612,8 +2578,6 @@ int commander_thread_main(int argc, char *argv[])
 				 */
 				armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5 * 1000 * 1000);
 			}
-			// TODO-JYW: TESTING-TESTING:
-			warnx("publishing arming status, armed.armed: %d", armed.armed);
 			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		}
 
@@ -3200,12 +3164,6 @@ set_control_mode()
 	default:
 		break;
 	}
-
-	// TODO-JYW: TESTING-TESTING:
-	warnx("status.nav_state: %d", status.nav_state);
-	warnx("status.is_rotary_wing: %d", status.is_rotary_wing);
-	warnx("control_mode.flag_control_rates_enabled: %d", control_mode.flag_control_rates_enabled);
-	warnx("control_mode.flag_control_attitude_enabled: %d", control_mode.flag_control_attitude_enabled);
 }
 
 bool
diff --git a/src/modules/systemlib/param/param_shmem.c b/src/modules/systemlib/param/param_shmem.c
index 7af6644968..c41d59114b 100644
--- a/src/modules/systemlib/param/param_shmem.c
+++ b/src/modules/systemlib/param/param_shmem.c
@@ -33,9 +33,6 @@
  *
  ****************************************************************************/
 
-// TODO-JYW: TESTING-TESTING
-#define DEBUG_BUILD 1
-
 /**
  * @file param.c
  *
@@ -113,7 +110,7 @@ const int bits_per_allocation_unit  = (sizeof(*param_changed_storage) * 8);
 
 //#define ENABLE_SHMEM_DEBUG
 
-extern int get_shmem_lock(void);
+extern int get_shmem_lock(const char *caller_file_name, int caller_line_number);
 extern void release_shmem_lock(void);
 
 struct param_wbuf_s *param_find_changed(param_t param);
@@ -257,9 +254,6 @@ param_notify_changes(bool is_saved)
 {
 	struct parameter_update_s pup = { .timestamp = hrt_absolute_time(), .saved = is_saved };
 
-	// TODO-JYW: TESTING-TESTING:
-	warnx("param_notify_changes: notifying of changes, timestamp: %lld, is_saved: %d", pup.timestamp, pup.saved);
-
 	/*
 	 * If we don't have a handle to our topic, create one now; otherwise
 	 * just publish.
@@ -822,7 +816,7 @@ param_save_default(void)
 
 	const char *filename = param_get_default_file();
 
-	if (get_shmem_lock() != 0) {
+	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
 		PX4_ERR("Could not get shmem lock\n");
 		res = ERROR;
 		goto exit;
diff --git a/src/platforms/posix/px4_layer/shmem_posix.c b/src/platforms/posix/px4_layer/shmem_posix.c
index 319d4287b6..b18a8f528b 100644
--- a/src/platforms/posix/px4_layer/shmem_posix.c
+++ b/src/platforms/posix/px4_layer/shmem_posix.c
@@ -59,7 +59,7 @@ unsigned char *map_base, *virt_addr;
 struct shmem_info *shmem_info_p;
 static void *map_memory(off_t target);
 
-int get_shmem_lock(void);
+int get_shmem_lock(const char *caller_file_name, int caller_line_number);
 void release_shmem_lock(void);
 void init_shared_memory(void);
 void copy_params_to_shmem(struct param_info_s *);
@@ -94,17 +94,19 @@ static void *map_memory(off_t target)
 		exit(1);
 	}
 
+	PX4_DEBUG("Initializing map memory: mem_fd: %d, 0x%X", mem_fd, map_base + (target & MAP_MASK) + LOCK_SIZE);
+
 	return (map_base + (target & MAP_MASK) + LOCK_SIZE);
 
 }
 
-int get_shmem_lock(void)
+int get_shmem_lock(const char *caller_file_name, int caller_line_number)
 {
 	int i = 0;
 
 	/*ioctl calls cmpxchg*/
 	while (ioctl(mem_fd, LOCK_MEM) != 0) {
-		PX4_INFO("Could not get lock, spinning\n");
+		PX4_INFO("Could not get lock, file name: %s, line number: %d\n", caller_file_name, caller_line_number);
 		usleep(100000); //sleep for 100 msec
 		i++;
 
@@ -122,7 +124,7 @@ int get_shmem_lock(void)
 
 void release_shmem_lock(void)
 {
-	*(virt_addr - LOCK_SIZE) = 1;
+	ioctl(mem_fd, UNLOCK_MEM);
 }
 
 void init_shared_memory(void)
@@ -139,7 +141,7 @@ void copy_params_to_shmem(struct param_info_s *param_info_base)
 	param_t param;
 	unsigned int i;
 
-	if (get_shmem_lock() != 0) {
+	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
 		PX4_ERR("Could not get shmem lock\n");
 		return;
 	}
@@ -188,7 +190,7 @@ void update_to_shmem(param_t param, union param_value_u value)
 {
 	unsigned int byte_changed, bit_changed;
 
-	if (get_shmem_lock() != 0) {
+	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
 		fprintf(stderr, "Could not get shmem lock\n");
 		return;
 	}
@@ -222,7 +224,7 @@ static void update_index_from_shmem(void)
 {
 	unsigned int i;
 
-	if (get_shmem_lock() != 0) {
+	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
 		fprintf(stderr, "Could not get shmem lock\n");
 		return;
 	}
@@ -240,7 +242,7 @@ static void update_value_from_shmem(param_t param, union param_value_u *value)
 {
 	unsigned int byte_changed, bit_changed;
 
-	if (get_shmem_lock() != 0) {
+	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
 		fprintf(stderr, "Could not get shmem lock\n");
 		return;
 	}
diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp
index 807519beca..6b424d8239 100644
--- a/src/platforms/qurt/px4_layer/main.cpp
+++ b/src/platforms/qurt/px4_layer/main.cpp
@@ -220,12 +220,6 @@ int dspal_entry(int argc, char *argv[])
 	process_commands(apps, get_commands());
 	sleep(1); // give time for all commands to execute before starting external function
 
-	// TODO-JYW: TESTING-TESTING:
-	int default_mav_type = vehicle_status_s::VEHICLE_TYPE_QUADROTOR;
-	param_set(param_find("MAV_TYPE"), &default_mav_type);
-	PX4_INFO("setting default mav_type: %d", default_mav_type);
-	// TODO-JYW: TESTING-TESTING:
-
 	if (qurt_external_hook) {
 		qurt_external_hook();
 	}
diff --git a/src/platforms/qurt/px4_layer/shmem_qurt.c b/src/platforms/qurt/px4_layer/shmem_qurt.c
index 7d583e04ac..bbbb01c08d 100644
--- a/src/platforms/qurt/px4_layer/shmem_qurt.c
+++ b/src/platforms/qurt/px4_layer/shmem_qurt.c
@@ -51,7 +51,7 @@ int mem_fd;
 unsigned char *map_base, *virt_addr;
 struct shmem_info *shmem_info_p;
 static void *map_memory(off_t target);
-int get_shmem_lock(void);
+int get_shmem_lock(const char *caller_file_name, int caller_line_number);
 void release_shmem_lock(void);
 void init_shared_memory(void);
 void copy_params_to_shmem(struct param_info_s *);
@@ -74,20 +74,25 @@ static void *map_memory(off_t target)
 
 }
 
-int get_shmem_lock(void)
+int get_shmem_lock(const char *caller_file_name, int caller_line_number)
 {
 	unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET);
 	unsigned int i = 0;
 
 	while (!atomic_compare_and_set(lock, 1, 0)) {
-		PX4_INFO("Could not get lock. spinning\n");
+		PX4_INFO("Could not get lock, file name: %s, line number: %d.\n",
+				caller_file_name, caller_line_number);
 		i++;
 		usleep(1000);
 
 		if (i > 100) { break; }
 	}
 
-	if (i > 100) { return -1; }
+	if (i > 100) {
+		return -1;
+	} else {
+		PX4_DEBUG("Lock acquired, file name: %s, line number: %d\n", caller_file_name, caller_line_number);
+	}
 
 	return 0; //got the lock
 
@@ -116,7 +121,7 @@ void copy_params_to_shmem(struct param_info_s *param_info_base)
 	param_t	param;
 	unsigned int i;
 
-	if (get_shmem_lock() != 0) {
+	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
 		PX4_INFO("Could not get shmem lock\n");
 		return;
 	}
@@ -163,7 +168,7 @@ void update_to_shmem(param_t param, union param_value_u value)
 		return;
 	}
 
-	if (get_shmem_lock() != 0) {
+	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
 		PX4_ERR("Could not get shmem lock\n");
 		return;
 	}
@@ -198,7 +203,7 @@ static void update_index_from_shmem(void)
 {
 	unsigned int i;
 
-	if (get_shmem_lock() != 0) {
+	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
 		PX4_ERR("Could not get shmem lock\n");
 		return;
 	}
@@ -217,7 +222,7 @@ static void update_value_from_shmem(param_t param, union param_value_u *value)
 {
 	unsigned int byte_changed, bit_changed;
 
-	if (get_shmem_lock() != 0) {
+	if (get_shmem_lock(__FILE__, __LINE__) != 0) {
 		PX4_ERR("Could not get shmem lock\n");
 		return;
 	}
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 49f6554994..19be8fa0d6 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -449,9 +449,6 @@ do_set(const char *name, const char *val, bool fail_on_not_found)
 				       param_name(param));
 				printf("curr: %ld", (long)i);
 				param_set_no_autosave(param, &newval);
-				// TODO-JYW: TESTING-TESTING: Reenabling param_set functionality.
-				param_set_no_autosave(param, &newval);
->>>>>>> bce2306... Resolved shared memory parameter problems and removed additional debug code.
 				printf(" -> new: %ld\n", (long)newval);
 			}
 		}
@@ -473,7 +470,6 @@ do_set(const char *name, const char *val, bool fail_on_not_found)
 				       param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
 				       param_name(param));
 				printf("curr: %4.4f", (double)f);
-				// TODO-JYW: TESTING-TESTING: Reenabling param_set functionality.
 				param_set_no_autosave(param, &newval);
 				printf(" -> new: %4.4f\n", (double)newval);
 			}