void onboard_parameters_preflight_storage(onboard_parameters_t* onboard_parameters, mavlink_command_long_t* msg)
{
// Onboard parameters storage
if (msg->param1 == 0)
{
// read parameters from flash
print_util_dbg_print("Reading from flashc...\r\n");
if(onboard_parameters_read_parameters_from_flashc(onboard_parameters))
{
// TODO: update simulation calibration values
//simulation_calib_set(&sim_model);
}
}
else if (msg->param1 == 1)
{
// write parameters to flash
//print_util_dbg_print("No Writing to flashc\n");
print_util_dbg_print("Writing to flashc\r\n");
onboard_parameters_write_parameters_to_flashc(onboard_parameters);
}
mavlink_message_t ack_msg;
mavlink_msg_command_ack_pack( onboard_parameters->mavlink_stream->sysid,
onboard_parameters->mavlink_stream->compid,
&ack_msg,
MAV_CMD_PREFLIGHT_STORAGE,
MAV_RESULT_ACCEPTED );
mavlink_stream_send(onboard_parameters->mavlink_stream, &ack_msg);
}
//---------------------------------------------------------------------------------
//-- Handle Commands
void
MavESP8266Component::_handleCmdLong(MavESP8266Bridge* sender, mavlink_command_long_t* cmd, uint8_t compID)
{
bool reboot = false;
uint8_t result = MAV_RESULT_UNSUPPORTED;
if(cmd->command == MAV_CMD_PREFLIGHT_STORAGE) {
//-- Read from EEPROM
if((uint8_t)cmd->param1 == 0) {
result = MAV_RESULT_ACCEPTED;
getWorld()->getParameters()->loadAllFromEeprom();
//-- Write to EEPROM
} else if((uint8_t)cmd->param1 == 1) {
result = MAV_RESULT_ACCEPTED;
getWorld()->getParameters()->saveAllToEeprom();
delay(0);
//-- Restore defaults
} else if((uint8_t)cmd->param1 == 2) {
result = MAV_RESULT_ACCEPTED;
getWorld()->getParameters()->resetToDefaults();
}
} else if(cmd->command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN) {
//-- Reset "Companion Computer"
if((uint8_t)cmd->param2 == 1) {
result = MAV_RESULT_ACCEPTED;
reboot = true;
}
}
//-- Response
if(compID == MAV_COMP_ID_UDP_BRIDGE) {
mavlink_message_t msg;
mavlink_msg_command_ack_pack(
getWorld()->getVehicle()->systemID(),
//_forwardTo->systemID(),
MAV_COMP_ID_UDP_BRIDGE,
&msg,
cmd->command,
result
);
sender->sendMessage(&msg);
}
if(reboot) {
_wifiReboot(sender);
}
}
static void imu_start_calibration(imu_t* imu, mavlink_command_long_t* packet)
{
MAV_RESULT result;
int16_t i;
print_util_dbg_print("Calibration cmd received");
/* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */
if ( (imu->state->mav_state == MAV_STATE_STANDBY)||(imu->state->mav_state == MAV_STATE_CALIBRATING) )
{
if (packet->param1 == 1)
{
//if (!imu->calib_gyro.calibration)
//{
//print_util_dbg_print("Starting gyro calibration\r\n");
//imu->calib_gyro.calibration = true;
//imu->state->mav_state = MAV_STATE_CALIBRATING;
//print_util_dbg_print("Old biais:");
//print_util_dbg_print_vector(imu->calib_gyro.bias,2);
//}
//else
//{
//print_util_dbg_print("Stopping gyro calibration\r\n");
//imu->calib_gyro.calibration = false;
//imu->state->mav_state = MAV_STATE_STANDBY;
//
//for (i = 0; i < 3; i++)
//{
//imu->.bias[i] = (imu->calib_gyro.max_oriented_values[i] + imu->calib_gyro.min_oriented_values[i])/2.0f;
//imu->calib_gyro.max_oriented_values[i] = -10000.0;
//imu->calib_gyro.min_oriented_values[i] = 10000.0;
//}
//print_util_dbg_print("New biais:");
//print_util_dbg_print_vector(imu->calib_gyro.bias,2);
//}
//result = MAV_RESULT_ACCEPTED;
result = MAV_RESULT_UNSUPPORTED;
}
if (packet->param2 == 1)
{
if (!imu->calib_compass.calibration)
{
print_util_dbg_print("Starting magnetometers calibration\r\n");
imu->calib_compass.calibration = true;
imu->state->mav_state = MAV_STATE_CALIBRATING;
print_util_dbg_print("Old biais:");
print_util_dbg_print_vector(imu->calib_compass.bias,2);
}
else
{
print_util_dbg_print("Stopping compass calibration\r\n");
imu->calib_compass.calibration = false;
imu->state->mav_state = MAV_STATE_STANDBY;
for (i = 0; i < 3; i++)
{
imu->calib_compass.bias[i] = (imu->calib_compass.max_oriented_values[i] + imu->calib_compass.min_oriented_values[i])/2.0f;
imu->calib_compass.max_oriented_values[i] = -10000.0;
imu->calib_compass.min_oriented_values[i] = 10000.0;
}
print_util_dbg_print("New biais:");
print_util_dbg_print_vector(imu->calib_compass.bias,2);
}
result = MAV_RESULT_ACCEPTED;
}
if (packet->param3 == 1)
{
print_util_dbg_print("Starting ground pressure calibration\r\n");
result = MAV_RESULT_UNSUPPORTED;
}
if (packet->param4 == 1)
{
print_util_dbg_print("Starting radio calibration\r\n");
result = MAV_RESULT_UNSUPPORTED;
}
if (packet->param5 == 1)
{
//if (!imu->calib_accelero.calibration)
//{
//print_util_dbg_print("Starting accelerometers calibration\r\n");
//imu->calib_accelero.calibration = true;
//imu->state->mav_state = MAV_STATE_CALIBRATING;
//print_util_dbg_print("Old biais:");
//print_util_dbg_print_vector(imu->calib_accelero.bias,2);
//}
//else
//{
//print_util_dbg_print("Stopping accelerometer calibration\r\n");
//imu->calib_accelero.calibration = false;
//imu->state->mav_state = MAV_STATE_STANDBY;
//
//for (i = 0; i < 3; i++)
//{
//imu->calib_accelero.bias[i] = (imu->calib_accelero.max_oriented_values[i] + imu->calib_accelero.min_oriented_values[i])/2.0f;
//imu->calib_accelero.max_oriented_values[i] = -10000.0;
//imu->calib_accelero.min_oriented_values[i] = 10000.0;
//}
//print_util_dbg_print("New biais:");
//print_util_dbg_print_vector(imu->calib_accelero.bias,2);
//}
//result = MAV_RESULT_ACCEPTED;
result = MAV_RESULT_UNSUPPORTED;
}
}
else
{
result = MAV_RESULT_TEMPORARILY_REJECTED;
}
mavlink_message_t msg;
mavlink_msg_command_ack_pack( imu->mavlink_stream->sysid,
imu->mavlink_stream->compid,
&msg,
MAV_CMD_PREFLIGHT_CALIBRATION,
result);
mavlink_stream_send(imu->mavlink_stream, &msg);
}
void mavlink_message_handler_receive(mavlink_message_handler_t* message_handler, mavlink_received_t* rec)
{
mavlink_message_t* msg = &rec->msg;
if ( message_handler->debug )
{
mavlink_message_handler_msg_default_dbg(msg);
}
if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG)
{
// The message is a command
mavlink_command_long_t cmd;
mavlink_msg_command_long_decode(msg, &cmd);
//print packet command and parameters for debug
print_util_dbg_print("target sysID:");
print_util_dbg_print_num(cmd.target_system,10);
print_util_dbg_print(", target compID:");
print_util_dbg_print_num(cmd.target_component,10);
print_util_dbg_print("\r\n");
print_util_dbg_print("parameters: ");
print_util_dbg_print_num(cmd.param1,10);
print_util_dbg_print_num(cmd.param2,10);
print_util_dbg_print_num(cmd.param3,10);
print_util_dbg_print_num(cmd.param4,10);
print_util_dbg_print_num(cmd.param5,10);
print_util_dbg_print_num(cmd.param6,10);
print_util_dbg_print_num(cmd.param7,10);
print_util_dbg_print("\r\n");
print_util_dbg_print("command id:");
print_util_dbg_print_num(cmd.command,10);
print_util_dbg_print(", confirmation:");
print_util_dbg_print_num(cmd.confirmation,10);
print_util_dbg_print("\r\n");
if (cmd.command >= 0 && cmd.command < MAV_CMD_ENUM_END)
{
// The command has valid command ID
if( (cmd.target_system == message_handler->mavlink_stream->sysid)||(cmd.target_system == MAV_SYS_ID_ALL) )
{
mav_result_t result = MAV_RESULT_UNSUPPORTED;
// The command is for this system
for (uint32_t i = 0; i < message_handler->cmd_callback_set->callback_count; ++i)
{
if ( match_cmd(message_handler, &message_handler->cmd_callback_set->callback_list[i], msg, &cmd) )
{
mavlink_cmd_callback_function_t function = message_handler->cmd_callback_set->callback_list[i].function;
handling_module_struct_t module_struct = message_handler->cmd_callback_set->callback_list[i].module_struct;
// Call appropriate function callback
result = function(module_struct, &cmd);
break;
}
}
// Send acknowledgment message
mavlink_message_t msg;
mavlink_msg_command_ack_pack( message_handler->mavlink_stream->sysid,
message_handler->mavlink_stream->compid,
&msg,
cmd.command,
result);
mavlink_stream_send(message_handler->mavlink_stream, &msg);
}
}
}
else if ( msg->msgid >= 0 && msg->msgid < MAV_MSG_ENUM_END )
{
// The message has a valid message ID, and is not a command
for (uint32_t i = 0; i < message_handler->msg_callback_set->callback_count; ++i)
{
if ( match_msg(message_handler, &message_handler->msg_callback_set->callback_list[i], msg) )
{
mavlink_msg_callback_function_t function = message_handler->msg_callback_set->callback_list[i].function;
handling_module_struct_t module_struct = message_handler->msg_callback_set->callback_list[i].module_struct;
uint32_t sys_id = *message_handler->msg_callback_set->callback_list[i].sys_id;
// Call appropriate function callback
function(module_struct, sys_id, msg);
}
}
}
}
