task_return_t scheduler_send_rt_stats(scheduler_t* scheduler)
{
const mavlink_stream_t* mavlink_stream = scheduler->mavlink_stream;
task_entry_t* stab_task = scheduler_get_task_by_id(scheduler,0);
mavlink_message_t msg;
mavlink_msg_named_value_float_pack( mavlink_stream->sysid,
mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
"stabAvgDelay",
stab_task->delay_avg);
mavlink_stream_send(mavlink_stream, &msg);
mavlink_msg_named_value_float_pack( mavlink_stream->sysid,
mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
"stabDelayVar",
sqrt(stab_task->delay_var_squared));
mavlink_stream_send(mavlink_stream, &msg);
mavlink_msg_named_value_float_pack( mavlink_stream->sysid,
mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
"stabMaxDelay",
stab_task->delay_max);
mavlink_stream_send(mavlink_stream, &msg);
mavlink_msg_named_value_float_pack( mavlink_stream->sysid,
mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
"stabRTvio",
stab_task->rt_violations);
mavlink_stream_send(mavlink_stream, &msg);
mavlink_msg_named_value_float_pack( mavlink_stream->sysid,
mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
"stabExTime",
stab_task->execution_time);
mavlink_stream_send(mavlink_stream, &msg);
stab_task->rt_violations = 0;
stab_task->delay_max = 0;
return TASK_RUN_SUCCESS;
}
static task_return_t onboard_parameters_send_scheduled_parameters(onboard_parameters_t* onboard_parameters)
{
onboard_parameters_set_t* param_set = onboard_parameters->param_set;
for (uint8_t i = 0; i < param_set->param_count; i++)
{
if (param_set->parameters[i].schedule_for_transmission)
{
mavlink_message_t msg;
mavlink_msg_param_value_pack( onboard_parameters->mavlink_stream->sysid,
onboard_parameters->mavlink_stream->compid,
&msg,
(char*)param_set->parameters[i].param_name,
*(param_set->parameters[i].param),
// onboard_parameters_read_parameter(onboard_parameters, i),
param_set->parameters[i].data_type,
param_set->param_count,
i );
mavlink_stream_send(onboard_parameters->mavlink_stream, &msg);
param_set->parameters[i].schedule_for_transmission=false;
}
}//end of for loop
return TASK_RUN_SUCCESS;
}
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);
}
void stabilisation_copter_send_outputs(stabilisation_copter_t* stabilisation_copter, const mavlink_stream_t* mavlink_stream, mavlink_message_t* msg)
{
aero_attitude_t attitude_yaw_inverse;
quat_t q_rot,qtmp;
attitude_yaw_inverse = coord_conventions_quat_to_aero(stabilisation_copter->ahrs->qe);
attitude_yaw_inverse.rpy[0] = 0.0f;
attitude_yaw_inverse.rpy[1] = 0.0f;
attitude_yaw_inverse.rpy[2] = attitude_yaw_inverse.rpy[2];
q_rot = coord_conventions_quaternion_from_aero(attitude_yaw_inverse);
qtmp=quaternions_create_from_vector(stabilisation_copter->stabiliser_stack.velocity_stabiliser.output.rpy);
quat_t rpy_local;
quaternions_rotate_vector(quaternions_inverse(q_rot), qtmp.v, rpy_local.v);
mavlink_msg_debug_vect_pack( mavlink_stream->sysid,
mavlink_stream->compid,
msg,
"OutVel",
time_keeper_get_micros(),
-rpy_local.v[X] * 1000,
rpy_local.v[Y] * 1000,
stabilisation_copter->stabiliser_stack.velocity_stabiliser.output.rpy[YAW] * 1000);
mavlink_stream_send(mavlink_stream,msg);
mavlink_msg_debug_vect_pack( mavlink_stream->sysid,
mavlink_stream->compid,
msg,
"OutAtt",
time_keeper_get_micros(),
stabilisation_copter->stabiliser_stack.attitude_stabiliser.output.rpy[ROLL] * 1000,
stabilisation_copter->stabiliser_stack.attitude_stabiliser.output.rpy[PITCH] * 1000,
stabilisation_copter->stabiliser_stack.attitude_stabiliser.output.rpy[YAW] * 1000);
mavlink_stream_send(mavlink_stream,msg);
mavlink_msg_debug_vect_pack( mavlink_stream->sysid,
mavlink_stream->compid,
msg,
"OutRate",
time_keeper_get_micros(),
stabilisation_copter->stabiliser_stack.rate_stabiliser.output.rpy[ROLL] * 1000,
stabilisation_copter->stabiliser_stack.rate_stabiliser.output.rpy[PITCH] * 1000,
stabilisation_copter->stabiliser_stack.rate_stabiliser.output.rpy[YAW] * 1000);
mavlink_stream_send(mavlink_stream,msg);
}
task_return_t imu_send_raw(imu_t* imu)
{
mavlink_message_t msg;
mavlink_msg_raw_imu_pack( imu->mavlink_stream->sysid,
imu->mavlink_stream->compid,
&msg,
time_keeper_get_micros(),
imu->oriented_accelero.data[IMU_X],
imu->oriented_accelero.data[IMU_Y],
imu->oriented_accelero.data[IMU_Z],
imu->oriented_gyro.data[IMU_X],
imu->oriented_gyro.data[IMU_Y],
imu->oriented_gyro.data[IMU_Z],
imu->oriented_compass.data[IMU_X],
imu->oriented_compass.data[IMU_Y],
imu->oriented_compass.data[IMU_Z]);
mavlink_stream_send(imu->mavlink_stream,&msg);
return TASK_RUN_SUCCESS;
}
task_return_t imu_send_scaled(imu_t* imu)
{
mavlink_message_t msg;
mavlink_msg_scaled_imu_pack(imu->mavlink_stream->sysid,
imu->mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
1000 * imu->scaled_accelero.data[IMU_X],
1000 * imu->scaled_accelero.data[IMU_Y],
1000 * imu->scaled_accelero.data[IMU_Z],
1000 * imu->scaled_gyro.data[IMU_X],
1000 * imu->scaled_gyro.data[IMU_Y],
1000 * imu->scaled_gyro.data[IMU_Z],
1000 * imu->scaled_compass.data[IMU_X],
1000 * imu->scaled_compass.data[IMU_Y],
1000 * imu->scaled_compass.data[IMU_Z]);
mavlink_stream_send(imu->mavlink_stream,&msg);
return TASK_RUN_SUCCESS;
}
task_return_t remote_send_scaled(const remote_t* remote)
{
mavlink_message_t msg;
mavlink_msg_rc_channels_scaled_pack( remote->mavlink_stream->sysid,
remote->mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
0,
remote->channels[0] * 10000.0f,
remote->channels[1] * 10000.0f,
remote->channels[2] * 10000.0f,
remote->channels[3] * 10000.0f,
remote->channels[4] * 10000.0f,
remote->channels[5] * 10000.0f,
remote->channels[6] * 10000.0f,
remote->channels[7] * 10000.0f,
remote->mode.current_desired_mode.byte );
// remote->signal_quality );
mavlink_stream_send(remote->mavlink_stream, &msg);
return TASK_RUN_SUCCESS;
}
task_return_t remote_send_raw(const remote_t* remote)
{
mavlink_message_t msg;
mavlink_msg_rc_channels_raw_pack( remote->mavlink_stream->sysid,
remote->mavlink_stream->compid,
&msg,
time_keeper_get_millis(),
0,
remote->sat->channels[0] + 1024,
remote->sat->channels[1] + 1024,
remote->sat->channels[2] + 1024,
remote->sat->channels[3] + 1024,
remote->sat->channels[4] + 1024,
remote->sat->channels[5] + 1024,
remote->sat->channels[6] + 1024,
remote->sat->channels[7] + 1024,
// remote->mode.current_desired_mode.byte);
remote->signal_quality );
mavlink_stream_send(remote->mavlink_stream, &msg);
return TASK_RUN_SUCCESS;
}
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);
}
}
}
}
