Hello,
i added two new mavlink messages but i have problems sending an response back.
coraldrone get status gets printed to the console, but i get no output back on my c test program
so the message does not get send back to my program.
any ideas on this?
thanks
greetings
andre
case MAVLINK_MSG_ID_GET_CORALDRONE_STATUS:
{
hal.console->printf(“coraldrone get status\n”);
AP_AHRS& ahrs = AP::ahrs();
struct Location drone_position_current_loc;
ahrs.get_position(drone_position_current_loc); // return value ignored; we send stale data
Vector3f vel;
if (!ahrs.get_velocity_NED(vel)) {
vel.zero();
}
char active[1];
if (!(copter.flightmode->name() == "CORALDRONE")) {
active[0] = 0;
}
else {
active[0] = 1;
}
float posD;
AP::ahrs().get_relative_position_D_home(posD);
posD *= -1000.0f;
mavlink_msg_coraldrone_status_send(
chan,
AP_HAL::millis(),
drone_position_current_loc.lat, // in 1E7 degrees
drone_position_current_loc.lng, // in 1E7 degrees
drone_position_current_loc.alt * 10UL, // millimeters above ground/sea level
posD, // millimeters above home
vel.x * 100, // X speed cm/s (+ve North)
vel.y * 100, // Y speed cm/s (+ve East)
vel.z * 100, // Z speed cm/s (+ve Down)
ahrs.yaw_sensor//, // compass heading in 1/100 degree
active);
break;
}