fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 14:37:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed problem causing a failure to obtain the shared memory lock on the AppsProc.

Browse Source
This commit is contained in:
jwilson
2016-02-17 20:03:15 -08:00
committed by Julian Oes
parent 4adfea7fa9
commit 75fad09263
6 changed files with 25 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
-42
View File
@@ -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
src/modules/systemlib/param/param_shmem.c
+2 -8
View File
@@ -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;
src/platforms/posix/px4_layer/shmem_posix.c
+10 -8
View File
@@ -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;
}
src/platforms/qurt/px4_layer/main.cpp
-6
View File
@@ -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();
}
src/platforms/qurt/px4_layer/shmem_qurt.c
+13 -8
View File
@@ -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;
}
src/systemcmds/param/param.c
-4
View File
@@ -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);
}