// ------------------------------------------------------------------------------
// Stop Hil Mode
// ------------------------------------------------------------------------------
void Autopilot_Interface::stop_hil()
{
uint8_t newBaseMode;
uint32_t newCustomMode = 0;
newBaseMode = base_mode | ~MAV_MODE_FLAG_HIL_ENABLED;
//newBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
//newBaseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
printf("BaseMode : %u\n", base_mode);
printf("newBaseMode : %u\n", newBaseMode);
mavlink_message_t msg;
mavlink_msg_set_mode_pack(system_id, 0, &msg, (uint8_t)1, newBaseMode, newCustomMode);
//mavlink_msg_set_mode_pack(255, 0, &msg, (uint8_t)1, newBaseMode, 65536);
printf("Unsetting HIL mode \n");
int attempts = 0;
bool condition = 1;
condition = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) == 1;
while( condition )
{
condition = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) == 1;
write_message(msg);
attempts++;
printf("Unsetting HIL check : %d \n",condition);
usleep(100000);
}
hil_mode = true;
printf("Message = \n");
printf("len : %u \n", msg.len);
printf("seq : %u \n", msg.seq);
printf("sysid : %u \n", msg.sysid);
printf("compid : %u \n", msg.compid);
printf("msgid : %u \n", msg.msgid);
char* pchar = (char*)(&msg.payload64[0]);
printf("payload : %u %u %u %u %u %u \n", *(pchar+5), *(pchar+4), *(pchar+3), *(pchar+2), *(pchar+1), *(pchar+0));
printf("HIL mode unset! [%d attempts]\n",attempts);
}
void Quadcopter::changeMode(FlightMode mode)
{
mavlink_msg_set_mode_pack(
SYSTEMID,
COMPONENTID,
&message,
TARGET_SYSTEMID,
TARGET_COMPONENTID,
static_cast<uint32_t>(mode));
communicator.enqueueMessage(message);
}
SOL_API int
sol_mavlink_set_mode(struct sol_mavlink *mavlink, enum sol_mavlink_mode mode)
{
mavlink_message_t msg = { 0 };
uint8_t buf[MAVLINK_MAX_PACKET_LEN], custom_mode;
uint16_t len, res;
SOL_NULL_CHECK(mavlink, -EINVAL);
custom_mode = sol_mode_to_mavlink_mode_lookup(mavlink->type, mode);
SOL_INT_CHECK(custom_mode, == MAV_MODE_ENUM_END, -EINVAL);
mavlink_msg_set_mode_pack
(0, 0, &msg, mavlink->sysid, mavlink->base_mode, custom_mode);
len = mavlink_msg_to_send_buffer(buf, &msg);
res = write(mavlink->fd, buf, len);
SOL_INT_CHECK(res, < len, -errno);
return 0;
}
/* Mode Sender */
bool MAVLink::sendMode(uint32_t mode) {
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mySystemId,myComponentId,&msg,targetSystemId,mode);
return sendMessage(msg);
}
