/*
send raw GPS position information (GPS_RAW_INT, GPS2_RAW, GPS_RTK and GPS2_RTK).
returns true if messages fit into transmit buffer, false otherwise.
*/
bool GCS_MAVLINK::send_gps_raw(AP_GPS &gps)
{
if (comm_get_txspace(chan) >=
MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
gps.send_mavlink_gps_raw(chan);
} else {
return false;
}
if (gps.highest_supported_status(0) > AP_GPS::GPS_OK_FIX_3D) {
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_GPS_RTK_LEN) {
gps.send_mavlink_gps_rtk(chan);
}
}
if (gps.num_sensors() > 1 && gps.status(1) > AP_GPS::NO_GPS) {
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_GPS2_RAW_LEN) {
gps.send_mavlink_gps2_raw(chan);
}
if (gps.highest_supported_status(1) > AP_GPS::GPS_OK_FIX_3D) {
if (comm_get_txspace(chan) >=
MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_GPS2_RTK_LEN) {
gps.send_mavlink_gps2_rtk(chan);
}
}
}
//TODO: Should check what else managed to get through...
return true;
}
/*
@brief Generates pseudorandom ICAO from gps time, lat, and lon.
Reference: DO282B, 2.2.4.5.1.3.2
Note gps.time is the number of seconds since UTC midnight
*/
uint32_t AP_ADSB::genICAO(const Location_Class &loc)
{
// gps_time is not seconds since UTC midnight, but it is an equivalent random number
// TODO: use UTC time instead of GPS time
AP_GPS gps = _ahrs.get_gps();
const uint64_t gps_time = gps.time_epoch_usec();
uint32_t timeSum = 0;
uint32_t M3 = 4096 * (loc.lat & 0x00000FFF) + (loc.lng & 0x00000FFF);
for (uint8_t i=0; i<24; i++) {
timeSum += (((gps_time & 0x00FFFFFF)>> i) & 0x00000001);
}
return( (timeSum ^ M3) & 0x00FFFFFF);
}
/*
send the SYSTEM_TIME message
*/
void GCS_MAVLINK::send_system_time(AP_GPS &gps)
{
mavlink_msg_system_time_send(
chan,
gps.time_epoch_usec(),
hal.scheduler->millis());
}
/*
send the SYSTEM_TIME message
*/
void GCS_MAVLINK::send_system_time(AP_GPS &gps)
{
mavlink_msg_system_time_send(
chan,
gps.time_epoch_usec(),
AP_HAL::millis());
}
void setup(void)
{
serial_manager.init();
ins.init(100);
baro.init();
ahrs.init();
gps.init(NULL, serial_manager);
}
void
GCS_MAVLINK::handle_gps_inject(const mavlink_message_t *msg, AP_GPS &gps)
{
mavlink_gps_inject_data_t packet;
mavlink_msg_gps_inject_data_decode(msg, &packet);
//TODO: check target
gps.inject_data(packet.data, packet.len);
}
void setup()
{
hal.console->println("GPS AUTO library test");
hal.console->println("Test Starting");
// Initialise the leds
board_led.init();
// Initialize the UART for GPS system
serial_manager.init();
gps.init(NULL, serial_manager);
}
void loop()
{
static uint32_t last_msg_ms;
// Update GPS state based on possible bytes received from the module.
gps.update();
// If new GPS data is received, output it's contents to the console
// Here we rely on the time of the message in GPS class and the time of last message
// saved in static variable last_msg_ms. When new message is received, the time
// in GPS class will be updated.
if (last_msg_ms != gps.last_message_time_ms()) {
// Reset the time of message
last_msg_ms = gps.last_message_time_ms();
// Acquire location
const Location &loc = gps.location();
// Print the contents of message
hal.console->print("Lat: ");
print_latlon(hal.console, loc.lat);
hal.console->print(" Lon: ");
print_latlon(hal.console, loc.lng);
hal.console->printf(" Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %u/%lu STATUS: %u\n",
loc.alt * 0.01f,
gps.ground_speed(),
(int)gps.ground_course_cd() / 100,
gps.num_sats(),
gps.time_week(),
(unsigned long)gps.time_week_ms(),
gps.status());
}
else
{
hal.console->println("It seemed like NO GPS");
}
// Delay for 10 mS will give us 100 Hz invocation rate
hal.scheduler->delay(10);
}
//to be called only once on boot for initializing objects
void setup()
{
hal.console->printf("GPS AUTO library test\n");
board_config.init();
// Initialise the leds
board_led.init();
// Initialize the UART for GPS system
serial_manager.init();
gps.init(serial_manager);
}
void setup(void)
{
ins.init(100);
ahrs.init();
serial_manager.init();
if( compass.init() ) {
hal.console->printf("Enabling compass\n");
ahrs.set_compass(&compass);
} else {
hal.console->printf("No compass detected\n");
}
gps.init(NULL, serial_manager);
}
/*
send raw GPS position information (GPS_RAW_INT, GPS2_RAW, GPS_RTK and GPS2_RTK).
returns true if messages fit into transmit buffer, false otherwise.
*/
bool GCS_MAVLINK::send_gps_raw(AP_GPS &gps)
{
int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
if (payload_space >= MAVLINK_MSG_ID_GPS_RAW_INT_LEN) {
gps.send_mavlink_gps_raw(chan);
} else {
return false;
}
#if GPS_RTK_AVAILABLE
if (gps.highest_supported_status(0) > AP_GPS::GPS_OK_FIX_3D) {
payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
if (payload_space >= MAVLINK_MSG_ID_GPS_RTK_LEN) {
gps.send_mavlink_gps_rtk(chan);
}
}
#endif
#if GPS_MAX_INSTANCES > 1
if (gps.num_sensors() > 1 && gps.status(1) > AP_GPS::NO_GPS) {
payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
if (payload_space >= MAVLINK_MSG_ID_GPS2_RAW_LEN) {
gps.send_mavlink_gps2_raw(chan);
}
#if GPS_RTK_AVAILABLE
if (gps.highest_supported_status(1) > AP_GPS::GPS_OK_FIX_3D) {
payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
if (payload_space >= MAVLINK_MSG_ID_GPS2_RTK_LEN) {
gps.send_mavlink_gps2_rtk(chan);
}
}
#endif
}
#endif
//TODO: Should check what else managed to get through...
return true;
}
/*
Send camera feedback to the GCS
*/
void AP_Camera::send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS &ahrs, const Location ¤t_loc)
{
float altitude, altitude_rel;
if (current_loc.flags.relative_alt) {
altitude = current_loc.alt+ahrs.get_home().alt;
altitude_rel = current_loc.alt;
} else {
altitude = current_loc.alt;
altitude_rel = current_loc.alt - ahrs.get_home().alt;
}
mavlink_msg_camera_feedback_send(chan,
gps.time_epoch_usec(),
0, 0, _image_index,
current_loc.lat, current_loc.lng,
altitude/100.0f, altitude_rel/100.0f,
ahrs.roll_sensor/100.0f, ahrs.pitch_sensor/100.0f, ahrs.yaw_sensor/100.0f,
0.0f,CAMERA_FEEDBACK_PHOTO);
}
void setup(void)
{
#ifdef APM2_HARDWARE
// we need to stop the barometer from holding the SPI bus
hal.gpio->pinMode(40, HAL_HAL_GPIO_OUTPUT);
hal.gpio->write(40, HIGH);
#endif
ins.init(AP_InertialSensor::RATE_100HZ);
ahrs.init();
serial_manager.init();
if( compass.init() ) {
hal.console->printf("Enabling compass\n");
ahrs.set_compass(&compass);
} else {
hal.console->printf("No compass detected\n");
}
gps.init(NULL, serial_manager);
}
/*
send an initialisation blob to configure the GPS
*/
void AP_GPS_MTK::send_init_blob(uint8_t instance, AP_GPS &gps)
{
gps.send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
}
void AP_ADSB::send_dynamic_out(const mavlink_channel_t chan)
{
// --------------
// Knowns
AP_GPS gps = _ahrs.get_gps();
Vector3f gps_velocity = gps.velocity();
int32_t latitude = _my_loc.lat;
int32_t longitude = _my_loc.lng;
int32_t altGNSS = _my_loc.alt*0.1f; // convert cm to mm
int16_t velVert = gps_velocity.z * 1E2; // convert m/s to cm/s
int16_t nsVog = gps_velocity.x * 1E2; // convert m/s to cm/s
int16_t ewVog = gps_velocity.y * 1E2; // convert m/s to cm/s
uint8_t fixType = gps.status(); // this lines up perfectly with our enum
uint8_t emStatus = 0; // TODO: implement this ENUM. no emergency = 0
uint8_t numSats = gps.num_sats();
uint16_t squawk = 1200; // Mode A code (typically 1200 [0x04B0] for VFR)
uint32_t accHoriz = UINT_MAX;
float accHoriz_f;
if (gps.horizontal_accuracy(accHoriz_f)) {
accHoriz = accHoriz_f * 1E3; // convert m to mm
}
uint16_t accVert = USHRT_MAX;
float accVert_f;
if (gps.vertical_accuracy(accVert_f)) {
accVert = accVert_f * 1E2; // convert m to cm
}
uint16_t accVel = USHRT_MAX;
float accVel_f;
if (gps.speed_accuracy(accVel_f)) {
accVel = accVel_f * 1E3; // convert m/s to mm/s
}
uint16_t state = 0;
if (out_state._is_in_auto_mode) {
state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_AUTOPILOT_ENABLED;
}
if (!out_state.is_flying) {
state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_ON_GROUND;
}
// --------------
// Not Sure
uint32_t utcTime = UINT_MAX; // uint32_t utcTime,
// TODO: confirm this sets utcTime correctly
const uint64_t gps_time = gps.time_epoch_usec();
utcTime = gps_time / 1000000ULL;
// --------------
// Unknowns
// TODO: implement http://www.srh.noaa.gov/images/epz/wxcalc/pressureAltitude.pdf
int32_t altPres = INT_MAX; //_ahrs.get_baro().get_altitude() relative to home, not MSL
mavlink_msg_uavionix_adsb_out_dynamic_send(
chan,
utcTime,
latitude,
longitude,
altGNSS,
fixType,
numSats,
altPres,
accHoriz,
accVert,
accVel,
velVert,
nsVog,
ewVog,
emStatus,
state,
squawk);
}
/**
handle a SERIAL_CONTROL message
*/
void GCS_MAVLINK::handle_serial_control(mavlink_message_t *msg, AP_GPS &gps)
{
mavlink_serial_control_t packet;
mavlink_msg_serial_control_decode(msg, &packet);
AP_HAL::UARTDriver *port = NULL;
if (packet.flags & SERIAL_CONTROL_FLAG_REPLY) {
// how did this packet get to us?
return;
}
bool exclusive = (packet.flags & SERIAL_CONTROL_FLAG_EXCLUSIVE) != 0;
switch (packet.device) {
case SERIAL_CONTROL_DEV_TELEM1:
port = hal.uartC;
lock_channel(MAVLINK_COMM_1, exclusive);
break;
case SERIAL_CONTROL_DEV_TELEM2:
port = hal.uartD;
lock_channel(MAVLINK_COMM_2, exclusive);
break;
case SERIAL_CONTROL_DEV_GPS1:
port = hal.uartB;
gps.lock_port(0, exclusive);
break;
case SERIAL_CONTROL_DEV_GPS2:
port = hal.uartE;
gps.lock_port(1, exclusive);
break;
default:
// not supported yet
return;
}
if (exclusive) {
// force flow control off for exclusive access. This protocol
// is used to talk to bootloaders which may not have flow
// control support
port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
}
// optionally change the baudrate
if (packet.baudrate != 0) {
port->begin(packet.baudrate);
}
// write the data
if (packet.count != 0) {
if ((packet.flags & SERIAL_CONTROL_FLAG_BLOCKING) == 0) {
port->write(packet.data, packet.count);
} else {
const uint8_t *data = &packet.data[0];
uint8_t count = packet.count;
while (count > 0) {
while (port->txspace() <= 0) {
hal.scheduler->delay(5);
}
uint16_t n = port->txspace();
if (n > packet.count) {
n = packet.count;
}
port->write(data, n);
data += n;
count -= n;
}
}
}
if ((packet.flags & SERIAL_CONTROL_FLAG_RESPOND) == 0) {
// no response expected
return;
}
uint8_t flags = packet.flags;
more_data:
// sleep for the timeout
while (packet.timeout != 0 &&
port->available() < (int16_t)sizeof(packet.data)) {
hal.scheduler->delay(1);
packet.timeout--;
}
packet.flags = SERIAL_CONTROL_FLAG_REPLY;
// work out how many bytes are available
int16_t available = port->available();
if (available < 0) {
available = 0;
}
if (available > (int16_t)sizeof(packet.data)) {
available = sizeof(packet.data);
}
// read any reply data
packet.count = 0;
memset(packet.data, 0, sizeof(packet.data));
while (available > 0) {
packet.data[packet.count++] = (uint8_t)port->read();
available--;
}
// and send the reply
_mav_finalize_message_chan_send(chan,
MAVLINK_MSG_ID_SERIAL_CONTROL,
(const char *)&packet,
MAVLINK_MSG_ID_SERIAL_CONTROL_LEN,
MAVLINK_MSG_ID_SERIAL_CONTROL_CRC);
if ((flags & SERIAL_CONTROL_FLAG_MULTI) && packet.count != 0) {
hal.scheduler->delay(1);
goto more_data;
}
}
