MAVLink: enable fetching of single parameters
this makes it possible for the GCS to fetch a single parameter by name, which is useful for parameters that change in flight
This commit is contained in:
parent
73dcbc24c3
commit
ed064b2506
@ -15,6 +15,9 @@ static bool mavlink_active;
|
|||||||
// check if a message will fit in the payload space available
|
// check if a message will fit in the payload space available
|
||||||
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
|
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
|
||||||
|
|
||||||
|
// prototype this for use inside the GCS class
|
||||||
|
static void gcs_send_text_fmt(const prog_char_t *fmt, ...);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
!!NOTE!!
|
!!NOTE!!
|
||||||
|
|
||||||
@ -1287,6 +1290,36 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
|
||||||
|
{
|
||||||
|
// decode
|
||||||
|
mavlink_param_request_read_t packet;
|
||||||
|
mavlink_msg_param_request_read_decode(msg, &packet);
|
||||||
|
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
|
||||||
|
if (packet.param_index != -1) {
|
||||||
|
gcs_send_text_P(SEVERITY_LOW, PSTR("Param by index not supported"));
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
enum ap_var_type p_type;
|
||||||
|
AP_Param *vp = AP_Param::find(packet.param_id, &p_type);
|
||||||
|
if (vp == NULL) {
|
||||||
|
gcs_send_text_fmt(PSTR("Unknown parameter %s"), packet.param_id);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
char param_name[ONBOARD_PARAM_NAME_LENGTH];
|
||||||
|
vp->copy_name(param_name, sizeof(param_name), true);
|
||||||
|
|
||||||
|
float value = vp->cast_to_float(p_type);
|
||||||
|
mavlink_msg_param_value_send(
|
||||||
|
chan,
|
||||||
|
param_name,
|
||||||
|
value,
|
||||||
|
mav_var_type(p_type),
|
||||||
|
-1, -1);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: // 45
|
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: // 45
|
||||||
{
|
{
|
||||||
//send_text_P(SEVERITY_LOW,PSTR("waypoint clear all"));
|
//send_text_P(SEVERITY_LOW,PSTR("waypoint clear all"));
|
||||||
|
@ -13,6 +13,9 @@ static bool mavlink_active;
|
|||||||
// check if a message will fit in the payload space available
|
// check if a message will fit in the payload space available
|
||||||
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
|
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
|
||||||
|
|
||||||
|
// prototype this for use inside the GCS class
|
||||||
|
void gcs_send_text_fmt(const prog_char_t *fmt, ...);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
!!NOTE!!
|
!!NOTE!!
|
||||||
|
|
||||||
@ -1255,6 +1258,36 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
|
||||||
|
{
|
||||||
|
// decode
|
||||||
|
mavlink_param_request_read_t packet;
|
||||||
|
mavlink_msg_param_request_read_decode(msg, &packet);
|
||||||
|
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
|
||||||
|
if (packet.param_index != -1) {
|
||||||
|
gcs_send_text_P(SEVERITY_LOW, PSTR("Param by index not supported"));
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
enum ap_var_type p_type;
|
||||||
|
AP_Param *vp = AP_Param::find(packet.param_id, &p_type);
|
||||||
|
if (vp == NULL) {
|
||||||
|
gcs_send_text_fmt(PSTR("Unknown parameter %s"), packet.param_id);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
char param_name[ONBOARD_PARAM_NAME_LENGTH];
|
||||||
|
vp->copy_name(param_name, sizeof(param_name), true);
|
||||||
|
|
||||||
|
float value = vp->cast_to_float(p_type);
|
||||||
|
mavlink_msg_param_value_send(
|
||||||
|
chan,
|
||||||
|
param_name,
|
||||||
|
value,
|
||||||
|
mav_var_type(p_type),
|
||||||
|
-1, -1);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
|
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
|
||||||
{
|
{
|
||||||
// decode
|
// decode
|
||||||
|
Loading…
Reference in New Issue
Block a user