mirror of https://github.com/ArduPilot/ardupilot
Copter: removed common functions
This commit is contained in:
Andrew Tridgell
parent
ca67c24eef
commit
7f100ebe31
|
|
@ -868,30 +868,6 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = {
|
||||||
AP_GROUPEND
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
void
|
|
||||||
GCS_MAVLINK::init(AP_HAL::UARTDriver* port)
|
|
||||||
{
|
|
||||||
GCS_Class::init(port);
|
|
||||||
if (port == (AP_HAL::BetterStream*)hal.uartA) {
|
|
||||||
mavlink_comm_0_port = port;
|
|
||||||
chan = MAVLINK_COMM_0;
|
|
||||||
initialised = true;
|
|
||||||
} else if (port == (AP_HAL::BetterStream*)hal.uartC) {
|
|
||||||
mavlink_comm_1_port = port;
|
|
||||||
chan = MAVLINK_COMM_1;
|
|
||||||
initialised = true;
|
|
||||||
#if MAVLINK_COMM_NUM_BUFFERS > 2
|
|
||||||
} else if (port == (AP_HAL::BetterStream*)hal.uartD) {
|
|
||||||
mavlink_comm_2_port = port;
|
|
||||||
chan = MAVLINK_COMM_2;
|
|
||||||
initialised = true;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
_queued_parameter = NULL;
|
|
||||||
reset_cli_timeout();
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
void
|
||||||
GCS_MAVLINK::update(void)
|
GCS_MAVLINK::update(void)
|
||||||
{
|
{
|
||||||
|
|
@ -2051,92 +2027,6 @@ mission_failed:
|
||||||
} // end switch
|
} // end switch
|
||||||
} // end handle mavlink
|
} // end handle mavlink
|
||||||
|
|
||||||
uint16_t
|
|
||||||
GCS_MAVLINK::_count_parameters()
|
|
||||||
{
|
|
||||||
// if we haven't cached the parameter count yet...
|
|
||||||
if (0 == _parameter_count) {
|
|
||||||
AP_Param *vp;
|
|
||||||
AP_Param::ParamToken token;
|
|
||||||
|
|
||||||
vp = AP_Param::first(&token, NULL);
|
|
||||||
do {
|
|
||||||
_parameter_count++;
|
|
||||||
} while (NULL != (vp = AP_Param::next_scalar(&token, NULL)));
|
|
||||||
}
|
|
||||||
return _parameter_count;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* queued_param_send - Send the next pending parameter, called from deferred message
|
|
||||||
* handling code
|
|
||||||
*/
|
|
||||||
void
|
|
||||||
GCS_MAVLINK::queued_param_send()
|
|
||||||
{
|
|
||||||
if (!initialised || _queued_parameter == NULL) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t bytes_allowed;
|
|
||||||
uint8_t count;
|
|
||||||
uint32_t tnow = millis();
|
|
||||||
|
|
||||||
// use at most 30% of bandwidth on parameters. The constant 26 is
|
|
||||||
// 1/(1000 * 1/8 * 0.001 * 0.3)
|
|
||||||
bytes_allowed = g.serial1_baud * (tnow - _queued_parameter_send_time_ms) * 26;
|
|
||||||
if (bytes_allowed > comm_get_txspace(chan)) {
|
|
||||||
bytes_allowed = comm_get_txspace(chan);
|
|
||||||
}
|
|
||||||
count = bytes_allowed / (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
|
|
||||||
|
|
||||||
while (_queued_parameter != NULL && count--) {
|
|
||||||
AP_Param *vp;
|
|
||||||
float value;
|
|
||||||
|
|
||||||
// copy the current parameter and prepare to move to the next
|
|
||||||
vp = _queued_parameter;
|
|
||||||
|
|
||||||
// if the parameter can be cast to float, report it here and break out of the loop
|
|
||||||
value = vp->cast_to_float(_queued_parameter_type);
|
|
||||||
|
|
||||||
char param_name[AP_MAX_NAME_SIZE];
|
|
||||||
vp->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true);
|
|
||||||
|
|
||||||
mavlink_msg_param_value_send(
|
|
||||||
chan,
|
|
||||||
param_name,
|
|
||||||
value,
|
|
||||||
mav_var_type(_queued_parameter_type),
|
|
||||||
_queued_parameter_count,
|
|
||||||
_queued_parameter_index);
|
|
||||||
|
|
||||||
_queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type);
|
|
||||||
_queued_parameter_index++;
|
|
||||||
}
|
|
||||||
_queued_parameter_send_time_ms = tnow;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* queued_waypoint_send - Send the next pending waypoint, called from deferred message
|
|
||||||
* handling code
|
|
||||||
*/
|
|
||||||
void
|
|
||||||
GCS_MAVLINK::queued_waypoint_send()
|
|
||||||
{
|
|
||||||
if (waypoint_receiving &&
|
|
||||||
waypoint_request_i <= waypoint_request_last) {
|
|
||||||
mavlink_msg_mission_request_send(
|
|
||||||
chan,
|
|
||||||
waypoint_dest_sysid,
|
|
||||||
waypoint_dest_compid,
|
|
||||||
waypoint_request_i);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void GCS_MAVLINK::reset_cli_timeout() {
|
|
||||||
_cli_timeout = millis();
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* a delay() callback that processes MAVLink packets. We set this as the
|
* a delay() callback that processes MAVLink packets. We set this as the
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue