void mav_param_set(mavlink_message_t *msg, mavlink_status_t *status, void *d)
{
unsigned char sys, comp;
mavlink_message_t msg2;
unsigned int len;
char param_name[17];
float param_value;
int idx;
sys = mavlink_msg_param_set_get_target_system(msg);
comp = mavlink_msg_param_set_get_target_component(msg);
//console_printf("set_param_start %d,%d\n", sys, comp);
if ((comp != MAV_COMP_ID_ALCEOSD) || (sys != osd_sysid))
return;
len = mavlink_msg_param_set_get_param_id(msg, param_name);
param_name[16] = '\0';
param_value = mavlink_msg_param_set_get_param_value(msg);
console_printf("set_param: %s\n", param_name);
idx = params_set_value(param_name, param_value, 1);
/* broadcast new parameter value */
mavlink_msg_param_value_pack(osd_sysid, MAV_COMP_ID_ALCEOSD, &msg2,
param_name, param_value, MAVLINK_TYPE_FLOAT,
total_params, idx);
mavlink_send_msg(&msg2);
}
void mav_param_request_read(mavlink_message_t *msg, mavlink_status_t *status, void *d)
{
unsigned char sys, comp;
mavlink_message_t msg2;
char param_name[17];
float param_value;
int idx;
sys = mavlink_msg_param_request_read_get_target_system(msg);
comp = mavlink_msg_param_request_read_get_target_component(msg);
//console_printf("get_param_start %d,%d\n", sys, comp);
if ((comp != MAV_COMP_ID_ALCEOSD) || (sys != osd_sysid))
return;
idx = mavlink_msg_param_request_read_get_param_index(msg);
if (idx == -1) {
mavlink_msg_param_request_read_get_param_id(msg, param_name);
param_name[16]= '\0';
}
param_value = params_get_value(idx, param_name);
mavlink_msg_param_value_pack(osd_sysid, MAV_COMP_ID_ALCEOSD, &msg2,
param_name, param_value, MAVLINK_TYPE_FLOAT,
total_params, idx);
mavlink_send_msg(&msg2);
console_printf("param_req_read %d %s=%f\n", idx, param_name, param_value);
}
static void mav_heartbeat(struct timer *t, void *d)
{
mavlink_message_t msg;
mavlink_msg_heartbeat_pack(osd_sysid,
MAV_COMP_ID_ALCEOSD, &msg, MAV_TYPE_ALCEOSD,
MAV_AUTOPILOT_INVALID,
MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, // base_mode
0, //custom_mode
MAV_STATE_ACTIVE);
mavlink_send_msg(&msg);
}
static void send_param_list_cbk(struct timer *t, void *d)
{
mavlink_message_t msg;
float param_value;
char param_name[17];
if (pidx == total_params) {
console_printf("send param end\n", pidx);
remove_timer(t);
return;
}
param_value = params_get_value(pidx, param_name);
mavlink_msg_param_value_pack(osd_sysid, MAV_COMP_ID_ALCEOSD, &msg,
param_name, param_value, MAVLINK_TYPE_FLOAT,
total_params, pidx++);
mavlink_send_msg(&msg);
}
static void mavlink_request_data_stream(unsigned char id, unsigned char rate)
{
mavlink_message_t msg;
unsigned char i, start_stop = (rate == 0) ? 0 : 1;
if (id >= sizeof(mavlink_stream_map))
return;
if (id == 0) {
for (i = 0; i < sizeof(mavlink_stream_map)-1; i++)
config.mav.streams[i] = rate;
} else {
config.mav.streams[id - 1] = rate;
}
mavlink_msg_request_data_stream_pack(config.mav.osd_sysid, MAV_COMP_ID_PERIPHERAL, &msg,
config.mav.uav_sysid, MAV_COMP_ID_ALL,
mavlink_stream_map[id], rate, start_stop);
mavlink_send_msg(&msg);
}
static void mav_heartbeat(struct timer *t, void *d)
{
mavlink_message_t msg;
LED = ~LED;
if (get_millis() - uav_last_seen > UAV_LAST_SEEN_TIMEOUT) {
set_timer_period(t, 5);
return;
} else {
set_timer_period(t, 10);
}
mavlink_msg_heartbeat_pack(config.mav.osd_sysid,
MAV_COMP_ID_OSD, &msg, MAV_TYPE_ALCEOSD,
MAV_AUTOPILOT_INVALID,
MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, // base_mode
0, //custom_mode
MAV_STATE_ACTIVE);
mavlink_send_msg(&msg);
}
/*-----------------------------------------------------------------------------
* Service MAVlink protocol message routine
*/
void Timer1_IRQHandler() {
mavlink_message_t msg;
MadgwickAHRSupdateIMU(
DEG_TO_RAD(params[PARAM_GX].val),
DEG_TO_RAD(params[PARAM_GY].val),
DEG_TO_RAD(params[PARAM_GZ].val),
params[PARAM_AX].val,
params[PARAM_AY].val,
params[PARAM_AZ].val);
mavlink_msg_heartbeat_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
MAV_TYPE_GROUND_ROVER,
MAV_AUTOPILOT_GENERIC,
mavlink_sys_mode,
0,
mavlink_sys_state);
mavlink_send_msg(&msg);
mavlink_msg_sys_status_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
MAV_SYS_STATUS_SENSOR_3D_GYRO
| MAV_SYS_STATUS_SENSOR_3D_ACCEL
| MAV_SYS_STATUS_SENSOR_3D_MAG
| MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE
| MAV_SYS_STATUS_SENSOR_GPS,
MAV_SYS_STATUS_SENSOR_3D_GYRO
| MAV_SYS_STATUS_SENSOR_3D_ACCEL
| MAV_SYS_STATUS_SENSOR_3D_MAG,
MAV_SYS_STATUS_SENSOR_3D_GYRO
| MAV_SYS_STATUS_SENSOR_3D_ACCEL
| MAV_SYS_STATUS_SENSOR_3D_MAG
| MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE
| MAV_SYS_STATUS_SENSOR_GPS,
50, // TODO remove hardoded values
0,
-1,
-1,
0,
0,
0,
0,
0,
0);
mavlink_send_msg(&msg);
mavlink_msg_autopilot_version_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
0, // No capabilities (MAV_PROTOCOL_CAPABILITY). TBD
42,
42,
42,
42,
"DEADBEEF",
"DEADBEEF",
"DEADBEEF",
42,
42,
42);
mavlink_send_msg(&msg);
mavlink_msg_highres_imu_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
usec(),
params[PARAM_AX].val,
params[PARAM_AY].val,
params[PARAM_AZ].val,
params[PARAM_GX].val,
params[PARAM_GY].val,
params[PARAM_GZ].val,
params[PARAM_MX].val,
params[PARAM_MY].val,
params[PARAM_MZ].val,
0,
0,
0,
params[PARAM_T].val,
(1 << 12) | ((1 << 9) - 1));
mavlink_send_msg(&msg);
mavlink_msg_attitude_quaternion_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
usec(),
q0,
q1,
q2,
q3,
DEG_TO_RAD(params[PARAM_GX].val),
DEG_TO_RAD(params[PARAM_GY].val),
DEG_TO_RAD(params[PARAM_GZ].val));
mavlink_send_msg(&msg);
usec_service_routine();
MSS_TIM1_clear_irq();
}
static void calc_home(struct timer *t, void *d)
{
mavlink_heartbeat_t *hb = mavdata_get(MAVLINK_MSG_ID_HEARTBEAT);
mavlink_global_position_int_t *gpi;
mavlink_message_t this_msg;
switch (home.lock) {
case HOME_NONE:
default:
if (mavdata_age(MAVLINK_MSG_ID_HEARTBEAT) > 5000)
return;
/* check arming status */
if (hb->base_mode & MAV_MODE_FLAG_SAFETY_ARMED)
home.lock = HOME_WAIT;
break;
case HOME_WAIT:
if (mavdata_age(MAVLINK_MSG_ID_MISSION_ITEM) > 2000) {
/* when UAV is armed, home is WP0 */
mavlink_msg_mission_request_pack(config.mav.osd_sysid, MAV_COMP_ID_OSD, &this_msg,
config.mav.uav_sysid, MAV_COMP_ID_ALL, 0);
mavlink_send_msg(&this_msg);
} else {
mavlink_mission_item_t *mi = mavdata_get(MAVLINK_MSG_ID_MISSION_ITEM);
priv.home_coord.lat = DEG2RAD(mi->x);
priv.home_coord.lon = DEG2RAD(mi->y);
priv.home_altitude = (unsigned int) mi->z;
home.lock = HOME_GOT;
}
break;
case HOME_GOT:
home.lock = HOME_LOCKED;
set_timer_period(t, 200);
break;
case HOME_LOCKED:
{
gpi = mavdata_get(MAVLINK_MSG_ID_GLOBAL_POSITION_INT);
priv.uav_coord.lat = DEG2RAD(gpi->lat / 10000000.0);
priv.uav_coord.lon = DEG2RAD(gpi->lon / 10000000.0);
priv.altitude = (unsigned int) (gpi->alt / 1000);
priv.heading = (int) (gpi->hdg / 100);
home.uav_bearing = (int) get_bearing(&priv.home_coord, &priv.uav_coord);
home.direction = home.uav_bearing + 180;
home.direction -= priv.heading;
if (home.direction < 0)
home.direction += 360;
home.distance = earth_distance(&priv.home_coord, &priv.uav_coord);
home.altitude = priv.altitude - priv.home_altitude;
break;
case HOME_RESET:
home.lock = HOME_NONE;
set_timer_period(t, 1000);
break;
case HOME_FORCE:
gpi = mavdata_get(MAVLINK_MSG_ID_GLOBAL_POSITION_INT);
priv.home_coord.lat = DEG2RAD(gpi->lat / 10000000.0);
priv.home_coord.lon = DEG2RAD(gpi->lon / 10000000.0);
priv.home_altitude = (unsigned int) (gpi->alt / 1000);
home.lock = HOME_GOT;
break;
}
}
}
