Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: simulate Solo gimbal parameter handling

Browse Source 
this allows us to get into running state
This commit is contained in:
Andrew Tridgell 2018-11-22 10:15:34 +11:00
parent 2a08dc73b7
commit 635540b4d6
2 changed files with 96 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
libraries/SITL/SIM_Gimbal.cpp
View File

@ -174,12 +174,78 @@ void Gimbal::update(void)
send_report();
}
static struct gimbal_param {
const char *name;
float value;
} gimbal_params[] = {
{"GMB_OFF_ACC_X", 0},
{"GMB_OFF_ACC_Y", 0},
{"GMB_OFF_ACC_Z", 0},
{"GMB_GN_ACC_X", 0},
{"GMB_GN_ACC_Y", 0},
{"GMB_GN_ACC_Z", 0},
{"GMB_OFF_GYRO_X", 0},
{"GMB_OFF_GYRO_Y", 0},
{"GMB_OFF_GYRO_Z", 0},
{"GMB_OFF_JNT_X", 0},
{"GMB_OFF_JNT_Y", 0},
{"GMB_OFF_JNT_Z", 0},
{"GMB_K_RATE", 0},
{"GMB_POS_HOLD", 0},
{"GMB_MAX_TORQUE", 0},
{"GMB_SND_TORQUE", 0},
{"GMB_SYSID", 0},
{"GMB_FLASH", 0},
};
/*
find a parameter structure
*/
struct gimbal_param *Gimbal::param_find(const char *name)
{
for (uint8_t i=0; i<ARRAY_SIZE(gimbal_params); i++) {
if (strncmp(name, gimbal_params[i].name, 16) == 0) {
return &gimbal_params[i];
}
}
return nullptr;
}
/*
send a parameter to flight board
*/
void Gimbal::param_send(const struct gimbal_param *p)
{
mavlink_message_t msg;
mavlink_param_value_t param_value;
strncpy(param_value.param_id, p->name, sizeof(param_value.param_id));
param_value.param_value = p->value;
param_value.param_count = 0;
param_value.param_index = 0;
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);
uint8_t saved_seq = chan0_status->current_tx_seq;
chan0_status->current_tx_seq = mavlink.seq;
uint16_t len = mavlink_msg_param_value_encode(vehicle_system_id,
vehicle_component_id,
&msg, &param_value);
chan0_status->current_tx_seq = saved_seq;
uint8_t msgbuf[len];
len = mavlink_msg_to_send_buffer(msgbuf, &msg);
if (len > 0) {
mav_socket.send(msgbuf, len);
}
}
/*
send a report to the vehicle control code over MAVLink
*/
void Gimbal::send_report(void)
{
if (AP_HAL::millis() < 10000) {
uint32_t now = AP_HAL::millis();
if (now < 10000) {
// don't send gimbal reports until 10s after startup. This
// avoids a windows threading issue with non-blocking sockets
// and the initial wait on uartA
@ -193,6 +259,14 @@ void Gimbal::send_report(void)
return;
}
if (param_send_last_ms && now - param_send_last_ms > 100) {
param_send(&gimbal_params[param_send_idx]);
if (++param_send_idx == ARRAY_SIZE(gimbal_params)) {
printf("Finished sending parameters\n");
param_send_last_ms = 0;
}
}
// check for incoming MAVLink messages
uint8_t buf[100];
ssize_t ret;
@ -237,24 +311,23 @@ void Gimbal::send_report(void)
mavlink_msg_param_set_decode(&msg, &pkt);
printf("Gimbal got PARAM_SET %.16s %f\n", pkt.param_id, pkt.param_value);
mavlink_param_value_t param_value;
memcpy(param_value.param_id, pkt.param_id, sizeof(pkt.param_id));
param_value.param_value = pkt.param_value;
param_value.param_count = 0;
param_value.param_index = 0;
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);
uint8_t saved_seq = chan0_status->current_tx_seq;
chan0_status->current_tx_seq = mavlink.seq;
uint16_t len = mavlink_msg_param_value_encode(vehicle_system_id,
vehicle_component_id,
&msg, &param_value);
chan0_status->current_tx_seq = saved_seq;
uint8_t msgbuf[len];
len = mavlink_msg_to_send_buffer(msgbuf, &msg);
if (len > 0) {
mav_socket.send(msgbuf, len);
struct gimbal_param *p = param_find(pkt.param_id);
if (p) {
p->value = pkt.param_value;
param_send(p);
}
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
mavlink_param_request_list_t pkt;
mavlink_msg_param_request_list_decode(&msg, &pkt);
if (pkt.target_system == 0 && pkt.target_component == MAV_COMP_ID_GIMBAL) {
// start param send
param_send_idx = 0;
param_send_last_ms = AP_HAL::millis();
}
printf("Gimbal sending %u parameters\n", (unsigned)ARRAY_SIZE(gimbal_params));
break;
}
default:
@ -268,7 +341,6 @@ void Gimbal::send_report(void)
if (!seen_heartbeat) {
return;
}
uint32_t now = AP_HAL::millis();
mavlink_message_t msg;
uint16_t len;

5
libraries/SITL/SIM_Gimbal.h
View File

@ -102,7 +102,12 @@ private:
uint8_t seq;
} mavlink;
uint32_t param_send_last_ms;
uint8_t param_send_idx;
void send_report(void);
void param_send(const struct gimbal_param *p);
struct gimbal_param *param_find(const char *name);
};
} // namespace SITL