From 82c4dbaaa88c2cfc591e402817e6268de708de3b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 2 Nov 2012 15:21:37 +0100 Subject: param load / store cleanup, storage location selection now exclusively through dedicated "param select " command --- apps/commander/commander.c | 78 +++++++++++-- apps/mavlink/mavlink_parameters.c | 124 +++++++++++---------- .../multirotor_att_control_main.c | 4 +- apps/systemcmds/param/param.c | 9 +- apps/systemlib/param/param.c | 2 +- 5 files changed, 136 insertions(+), 81 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index fafbd7e94..9dbdf49e2 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -143,7 +143,7 @@ int commander_thread_main(int argc, char *argv[]); static int buzzer_init(void); static void buzzer_deinit(void); -static void tune_confirm(); +static void tune_confirm(void); static int led_init(void); static void led_deinit(void); static int led_toggle(int led); @@ -264,7 +264,7 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u return 0; } -void tune_confirm() { +void tune_confirm(void) { ioctl(buzzer, TONE_SET_ALARM, 3); } @@ -914,17 +914,69 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } break; - /* - * do not report an error for commands that are - * handled directly by MAVLink. - */ - case MAV_CMD_PREFLIGHT_STORAGE: + case MAV_CMD_PREFLIGHT_STORAGE: { + if (current_status.flag_system_armed) { + mavlink_log_info(mavlink_fd, "[cmd] REJECTING param command while armed"); + } else { + + // XXX move this to LOW PRIO THREAD of commander app + /* Read all parameters from EEPROM to RAM */ + + if (((int)(cmd->param1)) == 0) { + + /* read all parameters from EEPROM to RAM */ + int read_ret = param_load_default(); + if (read_ret == OK) { + //printf("[mavlink pm] Loaded EEPROM params in RAM\n"); + mavlink_log_info(mavlink_fd, "[cmd] OK loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = MAV_RESULT_ACCEPTED; + } else if (read_ret == 1) { + mavlink_log_info(mavlink_fd, "[cmd] OK no changes in"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = MAV_RESULT_ACCEPTED; + } else { + if (read_ret < -1) { + mavlink_log_info(mavlink_fd, "[cmd] ERR loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } else { + mavlink_log_info(mavlink_fd, "[cmd] ERR no param file named"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + result = MAV_RESULT_FAILED; + } + + } else if (((int)(cmd->param1)) == 1) { + + /* write all parameters from RAM to EEPROM */ + int write_ret = param_save_default(); + if (write_ret == OK) { + mavlink_log_info(mavlink_fd, "[cmd] OK saved param file"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = MAV_RESULT_ACCEPTED; + + } else { + if (write_ret < -1) { + mavlink_log_info(mavlink_fd, "[cmd] ERR params file does not exit:"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } else { + mavlink_log_info(mavlink_fd, "[cmd] ERR writing params to"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + result = MAV_RESULT_FAILED; + } + + } else { + mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); + result = MAV_RESULT_UNSUPPORTED; + } + } + } break; default: { - mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command"); + mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); result = MAV_RESULT_UNSUPPORTED; - usleep(200000); /* announce command rejection */ ioctl(buzzer, TONE_SET_ALARM, 4); } @@ -932,7 +984,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } /* supported command handling stop */ - + if (result == MAV_RESULT_FAILED || + result == MAV_RESULT_DENIED || + result == MAV_RESULT_UNSUPPORTED) { + ioctl(buzzer, TONE_SET_ALARM, 5); + } else if (result == MAV_RESULT_ACCEPTED) { + tune_confirm(); + } /* send any requested ACKs */ if (cmd->confirmation > 0) { diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c index 6d434ed3d..7cb1582b4 100644 --- a/apps/mavlink/mavlink_parameters.c +++ b/apps/mavlink/mavlink_parameters.c @@ -233,66 +233,68 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess } break; - case MAVLINK_MSG_ID_COMMAND_LONG: { - mavlink_command_long_t cmd_mavlink; - mavlink_msg_command_long_decode(msg, &cmd_mavlink); - - uint8_t result = MAV_RESULT_UNSUPPORTED; - - if (cmd_mavlink.target_system == mavlink_system.sysid && - ((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { - - /* preflight parameter load / store */ - if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) { - /* Read all parameters from EEPROM to RAM */ - - if (((int)(cmd_mavlink.param1)) == 0) { - - /* read all parameters from EEPROM to RAM */ - int read_ret = param_load_default(); - if (read_ret == OK) { - //printf("[mavlink pm] Loaded EEPROM params in RAM\n"); - mavlink_missionlib_send_gcs_string("[mavlink pm] OK loaded EEPROM params"); - result = MAV_RESULT_ACCEPTED; - } else if (read_ret == 1) { - mavlink_missionlib_send_gcs_string("[mavlink pm] No stored parameters to load"); - result = MAV_RESULT_ACCEPTED; - } else { - if (read_ret < -1) { - mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params from EEPROM"); - } else { - mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params, no EEPROM found"); - } - result = MAV_RESULT_FAILED; - } - - } else if (((int)(cmd_mavlink.param1)) == 1) { - - /* write all parameters from RAM to EEPROM */ - int write_ret = param_save_default(); - if (write_ret == OK) { - mavlink_missionlib_send_gcs_string("[mavlink pm] OK params written to EEPROM"); - result = MAV_RESULT_ACCEPTED; - - } else { - if (write_ret < -1) { - mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params to EEPROM"); - } else { - mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params, no EEPROM found"); - } - result = MAV_RESULT_FAILED; - } - - } else { - //fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n"); - mavlink_missionlib_send_gcs_string("[mavlink pm] refusing unsupported STOR request"); - result = MAV_RESULT_UNSUPPORTED; - } - } - } - - /* send back command result */ - //mavlink_msg_command_ack_send(chan, cmd.command, result); - } break; + // case MAVLINK_MSG_ID_COMMAND_LONG: { + // mavlink_command_long_t cmd_mavlink; + // mavlink_msg_command_long_decode(msg, &cmd_mavlink); + + // uint8_t result = MAV_RESULT_UNSUPPORTED; + + // if (cmd_mavlink.target_system == mavlink_system.sysid && + // ((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + + // // XXX move this to LOW PRIO THREAD of commander app + + // /* preflight parameter load / store */ + // if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) { + // /* Read all parameters from EEPROM to RAM */ + + // if (((int)(cmd_mavlink.param1)) == 0) { + + // /* read all parameters from EEPROM to RAM */ + // int read_ret = param_load_default(); + // if (read_ret == OK) { + // //printf("[mavlink pm] Loaded EEPROM params in RAM\n"); + // mavlink_missionlib_send_gcs_string("[pm] OK loading %s", param_get_default_file()); + // result = MAV_RESULT_ACCEPTED; + // } else if (read_ret == 1) { + // mavlink_missionlib_send_gcs_string("[pm] OK no changes %s", param_get_default_file()); + // result = MAV_RESULT_ACCEPTED; + // } else { + // if (read_ret < -1) { + // mavlink_missionlib_send_gcs_string("[pm] ERR loading %s", param_get_default_file()); + // } else { + // mavlink_missionlib_send_gcs_string("[pm] ERR no file %s", param_get_default_file()); + // } + // result = MAV_RESULT_FAILED; + // } + + // } else if (((int)(cmd_mavlink.param1)) == 1) { + + // /* write all parameters from RAM to EEPROM */ + // int write_ret = param_save_default(); + // if (write_ret == OK) { + // mavlink_missionlib_send_gcs_string("[pm] OK saved %s", param_get_default_file()); + // result = MAV_RESULT_ACCEPTED; + + // } else { + // if (write_ret < -1) { + // mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file()); + // } else { + // mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file()); + // } + // result = MAV_RESULT_FAILED; + // } + + // } else { + // //fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n"); + // mavlink_missionlib_send_gcs_string("[pm] refusing unsupp. STOR request"); + // result = MAV_RESULT_UNSUPPORTED; + // } + // } + // } + + // /* send back command result */ + // //mavlink_msg_command_ack_send(chan, cmd.command, result); + // } break; } } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 63c296f96..29463d744 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -220,9 +220,9 @@ mc_thread_main(int argc, char *argv[]) /* only move setpoint if manual input is != 0 */ // XXX turn into param - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.25f) { + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f; - } else if (manual.throttle <= 0.25f) { + } else if (manual.throttle <= 0.3f) { att_sp.yaw_body = att.yaw; } att_sp.thrust = manual.throttle; diff --git a/apps/systemcmds/param/param.c b/apps/systemcmds/param/param.c index ce701b5ee..210841604 100644 --- a/apps/systemcmds/param/param.c +++ b/apps/systemcmds/param/param.c @@ -96,7 +96,8 @@ param_main(int argc, char *argv[]) } else { param_set_default_file(NULL); } - warnx("selected parameter file %s", param_get_default_file()); + warnx("selected parameter default file %s", param_get_default_file()); + exit(0); } if (!strcmp(argv[1], "show")) @@ -141,17 +142,11 @@ do_load(const char* param_file_name) if (fd < 0) err(1, "open '%s'", param_file_name); - /* set new default file name */ - param_set_default_file(param_file_name); - int result = param_load(fd); close(fd); if (result < 0) { errx(1, "error importing from '%s'", param_file_name); - } else { - /* set default file name for next storage operation */ - param_set_default_file(param_file_name); } exit(0); diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index 9a00c91a5..ddf9b0975 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -482,7 +482,7 @@ param_reset_all(void) } static const char *param_default_file = "/eeprom/parameters"; -static char *param_user_file; +static char *param_user_file = NULL; int param_set_default_file(const char* filename) -- cgit v1.2.3