From c192398a6517648d3a46729c2ddf7268beed89a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:36:19 +0200 Subject: [PATCH] mavlink app: Be more verbose on param load fails --- src/modules/mavlink/mavlink_parameters.cpp | 23 +++++++++++++++++----- src/modules/mavlink/mavlink_parameters.h | 5 +++-- 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 524effb205..73d7580d21 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +36,7 @@ * Mavlink parameters manager implementation. * * @author Anton Babushkin + * @author Lorenz Meier */ #include @@ -130,7 +131,17 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) } else { /* when index is >= 0, send this parameter again */ - send_param(param_for_used_index(req_read.param_index)); + int ret = send_param(param_for_used_index(req_read.param_index)); + + if (ret == 1) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index); + _mavlink->send_statustext_info(buf); + } else if (ret == 2) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] failed loading param from storage ID: %u", req_read.param_index); + _mavlink->send_statustext_info(buf); + } } } break; @@ -207,11 +218,11 @@ MavlinkParametersManager::send(const hrt_abstime t) } } -void +int MavlinkParametersManager::send_param(param_t param) { if (param == PARAM_INVALID) { - return; + return 1; } mavlink_param_value_t msg; @@ -221,7 +232,7 @@ MavlinkParametersManager::send_param(param_t param) * space during transmission, copy param onto float val_buf */ if (param_get(param, &msg.param_value) != OK) { - return; + return 2; } msg.param_count = param_count_used(); @@ -248,4 +259,6 @@ MavlinkParametersManager::send_param(param_t param) } _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); + + return 0; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index b6736f2128..3dfed084b3 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +36,7 @@ * Mavlink parameters manager definition. * * @author Anton Babushkin + * @author Lorenz Meier */ #pragma once @@ -113,7 +114,7 @@ protected: void send(const hrt_abstime t); - void send_param(param_t param); + int send_param(param_t param); orb_advert_t _rc_param_map_pub; struct rc_parameter_map_s _rc_param_map;