diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-16 15:49:56 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-16 15:49:56 +0200 |
commit | 46c4b987ccdd3c56588bf639a5500d974d5d2be6 (patch) | |
tree | 4cacbb7884be709583ade13784a4b168ff5a36db | |
parent | e95662f505eb45f80036be63940d435e4b4871e1 (diff) | |
download | px4-firmware-46c4b987ccdd3c56588bf639a5500d974d5d2be6.tar.gz px4-firmware-46c4b987ccdd3c56588bf639a5500d974d5d2be6.tar.bz2 px4-firmware-46c4b987ccdd3c56588bf639a5500d974d5d2be6.zip |
Various fixes for params interface
-rw-r--r-- | apps/commander/Makefile | 4 | ||||
-rw-r--r-- | apps/commander/commander.c | 47 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 6 | ||||
-rw-r--r-- | apps/systemlib/systemlib.c | 23 | ||||
-rw-r--r-- | apps/uORB/parameter_storage.h | 8 |
5 files changed, 53 insertions, 35 deletions
diff --git a/apps/commander/Makefile b/apps/commander/Makefile index a1149eb8d..d12697274 100644 --- a/apps/commander/Makefile +++ b/apps/commander/Makefile @@ -36,8 +36,8 @@ # APPNAME = commander -PRIORITY = SCHED_PRIORITY_MAX - 10 -STACKSIZE = 3072 +PRIORITY = SCHED_PRIORITY_MAX - 30 +STACKSIZE = 2048 INCLUDES = $(TOPDIR)/../mavlink/include/mavlink diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 21bf91c61..34dfc22b5 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -532,8 +532,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration"); + mavlink_log_info(mavlink_fd, "[commander] CMD starting gyro calibration"); do_gyro_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "[commander] CMD finished gyro calibration"); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { @@ -549,12 +550,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "[commander] starting mag calibration"); + mavlink_log_info(mavlink_fd, "[commander] CMD starting mag calibration"); do_mag_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "[commander] CMD finished mag calibration"); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING mag calibration"); + mavlink_log_critical(mavlink_fd, "[commander] CMD REJECTING mag calibration"); result = MAV_RESULT_DENIED; } handled = true; @@ -562,8 +564,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* none found */ if (!handled) { - fprintf(stderr, "[commander] refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported calibration request"); + //fprintf(stderr, "[commander] refusing unsupported calibration request\n"); + mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsupported calibration request"); result = MAV_RESULT_UNSUPPORTED; } } @@ -576,12 +578,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if (((int)cmd->param1) == 0) { if (OK == get_params_from_eeprom(global_data_parameter_storage)) { - printf("[commander] Loaded EEPROM params in RAM\n"); - mavlink_log_info(mavlink_fd, "[commander] Loaded EEPROM params in RAM"); + //printf("[commander] Loaded EEPROM params in RAM\n"); + mavlink_log_info(mavlink_fd, "[commander] CMD Loaded EEPROM params in RAM"); result = MAV_RESULT_ACCEPTED; } else { - fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n"); + //fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n"); mavlink_log_critical(mavlink_fd, "[commander] ERROR loading EEPROM params in RAM"); result = MAV_RESULT_FAILED; } @@ -591,18 +593,18 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } else if (((int)cmd->param1) == 1) { if (OK == store_params_in_eeprom(global_data_parameter_storage)) { - printf("[commander] RAM params written to EEPROM\n"); + //printf("[commander] RAM params written to EEPROM\n"); mavlink_log_info(mavlink_fd, "[commander] RAM params written to EEPROM"); result = MAV_RESULT_ACCEPTED; } else { - fprintf(stderr, "[commander] ERROR writing RAM params to EEPROM\n"); + //fprintf(stderr, "[commander] ERROR writing RAM params to EEPROM\n"); mavlink_log_critical(mavlink_fd, "[commander] ERROR writing RAM params to EEPROM"); result = MAV_RESULT_FAILED; } } else { - fprintf(stderr, "[commander] refusing unsupported storage request\n"); + //fprintf(stderr, "[commander] refusing unsupported storage request\n"); mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported storage request"); result = MAV_RESULT_UNSUPPORTED; } @@ -622,8 +624,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* send any requested ACKs */ if (cmd->confirmation > 0) { /* send acknowledge command */ - mavlink_message_t msg; - mavlink_msg_command_ack_pack(0, 0, &msg, cmd->command, result); } } @@ -788,6 +788,7 @@ int commander_main(int argc, char *argv[]) /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + /* publish current state machine */ state_machine_publish(stat_pub, ¤t_status, mavlink_fd); if (stat_pub < 0) { @@ -874,9 +875,9 @@ int commander_main(int argc, char *argv[]) battery_voltage_valid = sensors.battery_voltage_valid; bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage); - flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); +// flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); - /* Slow but important 5 Hz checks */ + /* Slow but important 8 Hz checks */ if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || @@ -994,14 +995,14 @@ int commander_main(int argc, char *argv[]) // // // if (flight_env == PX4_FLIGHT_ENVIRONMENT_TESTING) //simulate position fix for quick indoor tests - update_state_machine_got_position_fix(stat_pub, ¤t_status, mavlink_fd); + //update_state_machine_got_position_fix(stat_pub, ¤t_status, mavlink_fd); /* end: check gps */ /* Check battery voltage */ /* write to sys_status */ current_status.voltage_battery = battery_voltage; - /* if battery voltage is getting lower, warn using buzzer, etc. */ + /* if battery voltage is getting lower, warn using buzzer, etc. */ if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_LOW_VOLTS && false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { @@ -1086,8 +1087,8 @@ int commander_main(int argc, char *argv[]) } else { static uint64_t last_print_time = 0; - /* print error message for first RC glitch and then every 2 s / 2000 ms) */ - if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 3000000)) { + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) { mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!"); last_print_time = hrt_absolute_time(); } @@ -1105,7 +1106,7 @@ int commander_main(int argc, char *argv[]) current_status.counter++; current_status.timestamp = hrt_absolute_time(); - if (voltage_previous != current_status.voltage_battery) orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); + /* If full run came back clean, transition to standby */ if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && @@ -1115,8 +1116,12 @@ int commander_main(int argc, char *argv[]) do_state_update(stat_pub, ¤t_status, SYSTEM_STATE_STANDBY, mavlink_fd); } + /* publish at least with 1 Hz */ + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); + } + /* Store old modes to detect and act on state transitions */ - // vehicle_state_previous = current_status.state_machine; voltage_previous = current_status.voltage_battery; fflush(stdout); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index dc8b2a2c8..8e0fe629b 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1270,13 +1270,13 @@ int mavlink_main(int argc, char *argv[]) pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); - pthread_attr_setstacksize(&receiveloop_attr, 2048); + pthread_attr_setstacksize(&receiveloop_attr, 4096); pthread_create(&receive_thread, &receiveloop_attr, receiveloop, NULL); pthread_attr_t uorb_attr; pthread_attr_init(&uorb_attr); - /* Set stack size, needs more than 2048 bytes */ - pthread_attr_setstacksize(&uorb_attr, 5096); + /* Set stack size, needs more than 5000 bytes */ + pthread_attr_setstacksize(&uorb_attr, 7000); pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, NULL); /* initialize waypoint manager */ diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c index 609cf5d34..b1a8de7bd 100644 --- a/apps/systemlib/systemlib.c +++ b/apps/systemlib/systemlib.c @@ -123,6 +123,11 @@ void kill_task(FAR _TCB *tcb, FAR void *arg) kill(tcb->pid, SIGUSR1); } +union param_union { + float f; + char c[4]; +}; + int store_params_in_eeprom(struct global_data_parameter_storage_t *params) { int ret = ERROR; @@ -147,10 +152,13 @@ int store_params_in_eeprom(struct global_data_parameter_storage_t *params) ret = ERROR; } else { - for (int i = 0; i < params->pm.size; i++) { - write_res = write(fd, params->pm.param_values + i, sizeof(params->pm.param_values[i])); + for (int i = 0; i < PARAM_MAX_COUNT; i++) { - if (write_res != sizeof(params->pm.param_values[i])) return ERROR; + union param_union p; + p.f = params->pm.param_values[i]; + write_res = write(fd, p.c, sizeof(p.f)); + + if (write_res != sizeof(p.f)) return ERROR; } /*Write end magic byte */ @@ -221,10 +229,11 @@ int get_params_from_eeprom(struct global_data_parameter_storage_t *params) /* read data */ if (lseek_res == OK) { - for (int i = 0; i < params->pm.size; i++) { - read_res = read(fd, params->pm.param_values + i, sizeof(params->pm.param_values[i])); - - if (read_res != sizeof(params->pm.param_values[i])) return ERROR; + for (int i = 0; i < PARAM_MAX_COUNT; i++) { + union param_union p; + read_res = read(fd, p.c, sizeof(p.f)); + params->pm.param_values[i] = p.f; + if (read_res != sizeof(p.f)) return ERROR; } ret = OK; diff --git a/apps/uORB/parameter_storage.h b/apps/uORB/parameter_storage.h index 54ae63cb9..aa4c1bb4f 100644 --- a/apps/uORB/parameter_storage.h +++ b/apps/uORB/parameter_storage.h @@ -112,25 +112,28 @@ enum PARAM { PARAM_MAX_COUNT ///< LEAVE THIS IN PLACE AS LAST ELEMENT! }; +#pragma pack(push, 1) struct px4_parameter_storage_t { float param_values[PARAM_MAX_COUNT]; ///< Parameter values - const char *param_names[PARAM_MAX_COUNT]; ///< Parameter names bool param_needs_write[PARAM_MAX_COUNT]; + const char *param_names[PARAM_MAX_COUNT]; ///< Parameter names uint16_t next_param; uint16_t size; }; +#pragma pack(pop) #define PX4_FLIGHT_ENVIRONMENT_INDOOR 0 #define PX4_FLIGHT_ENVIRONMENT_OUTDOOR 1 #define PX4_FLIGHT_ENVIRONMENT_TESTING 2 //NO check for position fix in this environment +#pragma pack(push, 1) struct global_data_parameter_storage_t { /* use of a counter and timestamp recommended (but not necessary) */ // uint16_t counter; //incremented by the writing thread everytime new data is stored -// uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data + uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data /* Actual data, this is specific to the type of data which is stored in this struct */ @@ -142,6 +145,7 @@ struct global_data_parameter_storage_t { //*****END: Add your variables here ***** }; +#pragma pack(pop) __attribute__ ((visibility ("default"))) extern struct global_data_parameter_storage_t *global_data_parameter_storage; //adjust this line! |