static void test_one_offset(const struct Location &loc, float ofs_north, float ofs_east, float dist, float bearing) { struct Location loc2; float dist2, bearing2; loc2 = loc; uint32_t t1 = AP_HAL::micros(); location_offset(loc2, ofs_north, ofs_east); hal.console->printf("location_offset took %u usec\n", (unsigned)(AP_HAL::micros() - t1)); dist2 = get_distance(loc, loc2); bearing2 = get_bearing_cd(loc, loc2) * 0.01f; float brg_error = bearing2-bearing; if (brg_error > 180) { brg_error -= 360; } else if (brg_error < -180) { brg_error += 360; } if (fabsf(dist - dist2) > 1.0f || brg_error > 1.0f) { hal.console->printf("Failed offset test brg_error=%f dist_error=%f\n", brg_error, dist-dist2); } }
/* seek to the right offset for disk_block */ void AP_Terrain::seek_offset(void) { struct grid_block &block = disk_block.block; // work out how many longitude blocks there are at this latitude Location loc1, loc2; loc1.lat = block.lat_degrees*10*1000*1000L; loc1.lng = block.lon_degrees*10*1000*1000L; loc2.lat = block.lat_degrees*10*1000*1000L; loc2.lng = (block.lon_degrees+1)*10*1000*1000L; // shift another two blocks east to ensure room is available location_offset(loc2, 0, 2*grid_spacing*TERRAIN_GRID_BLOCK_SIZE_Y); Vector2f offset = location_diff(loc1, loc2); uint16_t east_blocks = offset.y / (grid_spacing*TERRAIN_GRID_BLOCK_SIZE_Y); uint32_t file_offset = (east_blocks * block.grid_idx_x + block.grid_idx_y) * sizeof(union grid_io_block); if (::lseek(fd, file_offset, SEEK_SET) != file_offset) { #if TERRAIN_DEBUG hal.console->printf("Seek %lu failed - %s\n", (unsigned long)file_offset, strerror(errno)); #endif ::close(fd); fd = -1; io_failure = true; } }
// get target's estimated location bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const { // exit immediately if not enabled if (!_enabled) { return false; } // check for timeout if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) { return false; } // calculate time since last actual position update const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f; // get velocity estimate if (!get_velocity_ned(vel_ned, dt)) { return false; } // project the vehicle position Location last_loc = _target_location; location_offset(last_loc, vel_ned.x * dt, vel_ned.y * dt); last_loc.alt -= vel_ned.z * 10.0f * dt; // convert m/s to cm/s, multiply by dt. minus because NED // return latest position estimate loc = last_loc; return true; }
/* update location from position */ void Aircraft::update_position(void) { location = home; location_offset(location, position.x, position.y); location.alt = home.alt - position.z*100.0f; // we only advance time if it hasn't been advanced already by the // backend if (last_time_us == time_now_us) { time_now_us += frame_time_us; } last_time_us = time_now_us; if (use_time_sync) { sync_frame_time(); } #if 0 // logging of raw sitl data Vector3f accel_ef = dcm * accel_body; DataFlash_Class::instance()->Log_Write("SITL", "TimeUS,VN,VE,VD,AN,AE,AD,PN,PE,PD", "Qfffffffff", AP_HAL::micros64(), velocity_ef.x, velocity_ef.y, velocity_ef.z, accel_ef.x, accel_ef.y, accel_ef.z, position.x, position.y, position.z); #endif }
// Return the last calculated latitude, longitude and height in WGS-84 // If a calculated location isn't available, return a raw GPS measurement // The status will return true if a calculation or raw measurement is available // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control bool NavEKF3_core::getLLH(struct Location &loc) const { const AP_GPS &gps = AP::gps(); if(validOrigin) { // Altitude returned is an absolute altitude relative to the WGS-84 spherioid loc.alt = 100 * (int32_t)(ekfGpsRefHgt - (double)outputDataNew.position.z); loc.flags.relative_alt = 0; loc.flags.terrain_alt = 0; // there are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no aiding) if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) { loc.lat = EKF_origin.lat; loc.lng = EKF_origin.lng; location_offset(loc, outputDataNew.position.x, outputDataNew.position.y); return true; } else { // we could be in constant position mode because the vehicle has taken off without GPS, or has lost GPS // in this mode we cannot use the EKF states to estimate position so will return the best available data if ((gps.status() >= AP_GPS::GPS_OK_FIX_2D)) { // we have a GPS position fix to return const struct Location &gpsloc = gps.location(); loc.lat = gpsloc.lat; loc.lng = gpsloc.lng; return true; } else { // if no GPS fix, provide last known position before entering the mode location_offset(loc, lastKnownPositionNE.x, lastKnownPositionNE.y); return false; } } } else { // If no origin has been defined for the EKF, then we cannot use its position states so return a raw // GPS reading if available and return false if ((gps.status() >= AP_GPS::GPS_OK_FIX_3D)) { const struct Location &gpsloc = gps.location(); loc = gpsloc; loc.flags.relative_alt = 0; loc.flags.terrain_alt = 0; } return false; } }
// return our current position estimate using // dead-reckoning or GPS bool AP_AHRS_DCM::get_position(struct Location *loc) { if (!_have_position) { return false; } loc->lat = _last_lat; loc->lng = _last_lng; location_offset(loc, _position_offset_north, _position_offset_east); return true; }
// return our current position estimate using // dead-reckoning or GPS bool AP_AHRS_DCM::get_position(struct Location &loc) { loc.lat = _last_lat; loc.lng = _last_lng; loc.alt = _baro.get_altitude() * 100 + _home.alt; location_offset(loc, _position_offset_north, _position_offset_east); if (_flags.fly_forward && _have_position) { location_update(loc, degrees(yaw), _gps->ground_speed_cm * 0.01 * _gps->get_lag()); } return _have_position; }
// set desired location as an offset from the EKF origin in NED frame bool Mode::set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd) { Location destination_ned; // initialise destination to ekf origin if (!ahrs.get_origin(destination_ned)) { return false; } // apply offset location_offset(destination_ned, destination.x, destination.y); set_desired_location(destination_ned, next_leg_bearing_cd); return true; }
// return our current position estimate using // dead-reckoning or GPS bool AP_AHRS_DCM::get_position(struct Location &loc) const { loc.lat = _last_lat; loc.lng = _last_lng; loc.alt = _baro.get_altitude() * 100 + _home.alt; loc.flags.relative_alt = 0; loc.flags.terrain_alt = 0; location_offset(loc, _position_offset_north, _position_offset_east); if (_flags.fly_forward && _have_position) { location_update(loc, _gps.ground_course_cd() * 0.01f, _gps.ground_speed() * _gps.get_lag()); } return _have_position; }
// return our current position estimate using // dead-reckoning or GPS bool AP_AHRS_DCM::get_position(struct Location &loc) const { loc.lat = _last_lat; loc.lng = _last_lng; loc.alt = AP::baro().get_altitude() * 100 + _home.alt; loc.flags.relative_alt = 0; loc.flags.terrain_alt = 0; location_offset(loc, _position_offset_north, _position_offset_east); const AP_GPS &_gps = AP::gps(); if (_flags.fly_forward && _have_position) { float gps_delay_sec = 0; _gps.get_lag(gps_delay_sec); location_update(loc, _gps.ground_course_cd() * 0.01f, _gps.ground_speed() * gps_delay_sec); } return _have_position; }
/* given a location, calculate the 32x28 grid SW corner, plus the grid indices */ void AP_Terrain::calculate_grid_info(const Location &loc, struct grid_info &info) const { // grids start on integer degrees. This makes storing terrain data // on the SD card a bit easier info.lat_degrees = (loc.lat<0?(loc.lat-9999999L):loc.lat) / (10*1000*1000L); info.lon_degrees = (loc.lng<0?(loc.lng-9999999L):loc.lng) / (10*1000*1000L); // create reference position for this rounded degree position Location ref; ref.lat = info.lat_degrees*10*1000*1000L; ref.lng = info.lon_degrees*10*1000*1000L; // find offset from reference Vector2f offset = location_diff(ref, loc); // get indices in terms of grid_spacing elements uint32_t idx_x = offset.x / grid_spacing; uint32_t idx_y = offset.y / grid_spacing; // find indexes into 32*28 grids for this degree reference. Note // the use of TERRAIN_GRID_BLOCK_SPACING_{X,Y} which gives a one square // overlap between grids info.grid_idx_x = idx_x / TERRAIN_GRID_BLOCK_SPACING_X; info.grid_idx_y = idx_y / TERRAIN_GRID_BLOCK_SPACING_Y; // find the indices within the 32*28 grid info.idx_x = idx_x % TERRAIN_GRID_BLOCK_SPACING_X; info.idx_y = idx_y % TERRAIN_GRID_BLOCK_SPACING_Y; // find the fraction (0..1) within the square info.frac_x = (offset.x - idx_x * grid_spacing) / grid_spacing; info.frac_y = (offset.y - idx_y * grid_spacing) / grid_spacing; // calculate lat/lon of SW corner of 32*28 grid_block location_offset(ref, info.grid_idx_x * TERRAIN_GRID_BLOCK_SPACING_X * (float)grid_spacing, info.grid_idx_y * TERRAIN_GRID_BLOCK_SPACING_Y * (float)grid_spacing); info.grid_lat = ref.lat; info.grid_lon = ref.lng; ASSERT_RANGE(info.idx_x,0,TERRAIN_GRID_BLOCK_SPACING_X-1); ASSERT_RANGE(info.idx_y,0,TERRAIN_GRID_BLOCK_SPACING_Y-1); ASSERT_RANGE(info.frac_x,0,1); ASSERT_RANGE(info.frac_y,0,1); }
/** update_vehicle_position_estimate - updates estimate of vehicle positions should be called at 50hz */ void Tracker::update_vehicle_pos_estimate() { // calculate time since last actual position update float dt = (AP_HAL::micros() - vehicle.last_update_us) * 1.0e-6f; // if less than 5 seconds since last position update estimate the position if (dt < TRACKING_TIMEOUT_SEC) { // project the vehicle position to take account of lost radio packets vehicle.location_estimate = vehicle.location; float north_offset = vehicle.vel.x * dt; float east_offset = vehicle.vel.y * dt; location_offset(vehicle.location_estimate, north_offset, east_offset); vehicle.location_estimate.alt += vehicle.vel.z * 100.0f * dt; // set valid_location flag vehicle.location_valid = true; } else { // vehicle has been lost, set lost flag vehicle.location_valid = false; } }
// calculate vehicle stopping point using current location, velocity and maximum acceleration void Mode::calc_stopping_location(Location& stopping_loc) { // default stopping location stopping_loc = rover.current_loc; // get current velocity vector and speed const Vector2f velocity = ahrs.groundspeed_vector(); const float speed = velocity.length(); // avoid divide by zero if (!is_positive(speed)) { stopping_loc = rover.current_loc; return; } // get stopping distance in meters const float stopping_dist = attitude_control.get_stopping_distance(speed); // calculate stopping position from current location in meters const Vector2f stopping_offset = velocity.normalized() * stopping_dist; location_offset(stopping_loc, stopping_offset.x, stopping_offset.y); }
// check_position - returns true if gps position is acceptable, false if not void GPS_Glitch::check_position() { uint32_t now = hal.scheduler->millis(); // current system time float sane_dt; // time since last sane gps reading float accel_based_distance; // movement based on max acceleration Location curr_pos; // our current position estimate Location gps_pos; // gps reported position float distance_cm; // distance from gps to current position estimate in cm bool all_ok; // true if the new gps position passes sanity checks // exit immediately if we don't have gps lock if (_gps == NULL || _gps->status() != GPS::GPS_OK_FIX_3D) { _flags.glitching = true; return; } // if not initialised or disabled update last good position and exit if (!_flags.initialised || !_enabled) { _last_good_update = now; _last_good_lat = _gps->latitude; _last_good_lon = _gps->longitude; _last_good_vel.x = _gps->velocity_north(); _last_good_vel.y = _gps->velocity_east(); _flags.initialised = true; _flags.glitching = false; return; } // calculate time since last sane gps reading in ms sane_dt = (now - _last_good_update) / 1000.0f; // project forward our position from last known velocity curr_pos.lat = _last_good_lat; curr_pos.lng = _last_good_lon; location_offset(curr_pos, _last_good_vel.x * sane_dt, _last_good_vel.y * sane_dt); // calculate distance from recent gps position to current estimate gps_pos.lat = _gps->latitude; gps_pos.lng = _gps->longitude; distance_cm = get_distance_cm(curr_pos, gps_pos); // all ok if within a given hardcoded radius if (distance_cm <= _radius_cm) { all_ok = true; }else{ // or if within the maximum distance we could have moved based on our acceleration accel_based_distance = 0.5f * _accel_max_cmss * sane_dt * sane_dt; all_ok = (distance_cm <= accel_based_distance); } // store updates to gps position if (all_ok) { // position is acceptable _last_good_update = now; _last_good_lat = _gps->latitude; _last_good_lon = _gps->longitude; _last_good_vel.x = _gps->velocity_north(); _last_good_vel.y = _gps->velocity_east(); } // update glitching flag _flags.glitching = !all_ok; }
/* send a report to the vehicle control code over MAVLink */ void ADSB::send_report(void) { if (AP_HAL::millis() < 10000) { // simulated aircraft don't appear until 10s after startup. This avoids a windows // threading issue with non-blocking sockets and the initial wait on uartA return; } if (!mavlink.connected && mav_socket.connect(target_address, target_port)) { ::printf("ADSB connected to %s:%u\n", target_address, (unsigned)target_port); mavlink.connected = true; } if (!mavlink.connected) { return; } // check for incoming MAVLink messages uint8_t buf[100]; ssize_t ret; while ((ret=mav_socket.recv(buf, sizeof(buf), 0)) > 0) { for (uint8_t i=0; i<ret; i++) { mavlink_message_t msg; mavlink_status_t status; if (mavlink_frame_char_buffer(&mavlink.rxmsg, &mavlink.status, buf[i], &msg, &status) == MAVLINK_FRAMING_OK) { switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { if (!seen_heartbeat) { seen_heartbeat = true; vehicle_component_id = msg.compid; vehicle_system_id = msg.sysid; ::printf("ADSB using srcSystem %u\n", (unsigned)vehicle_system_id); } break; } } } } } if (!seen_heartbeat) { return; } uint32_t now = AP_HAL::millis(); mavlink_message_t msg; uint16_t len; if (now - last_heartbeat_ms >= 1000) { mavlink_heartbeat_t heartbeat; heartbeat.type = MAV_TYPE_ADSB; heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA; heartbeat.base_mode = 0; heartbeat.system_status = 0; heartbeat.mavlink_version = 0; heartbeat.custom_mode = 0; /* save and restore sequence number for chan0, as it is used by generated encode functions */ mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint8_t saved_seq = chan0_status->current_tx_seq; chan0_status->current_tx_seq = mavlink.seq; len = mavlink_msg_heartbeat_encode(vehicle_system_id, vehicle_component_id, &msg, &heartbeat); chan0_status->current_tx_seq = saved_seq; mav_socket.send(&msg.magic, len); last_heartbeat_ms = now; } /* send a ADSB_VEHICLE messages */ uint32_t now_us = AP_HAL::micros(); if (now_us - last_report_us >= reporting_period_ms*1000UL) { for (uint8_t i=0; i<num_vehicles; i++) { ADSB_Vehicle &vehicle = vehicles[i]; Location loc = home; location_offset(loc, vehicle.position.x, vehicle.position.y); // re-init when exceeding radius range if (get_distance(home, loc) > _sitl->adsb_radius_m) { vehicle.initialised = false; } mavlink_adsb_vehicle_t adsb_vehicle {}; last_report_us = now_us; adsb_vehicle.ICAO_address = vehicle.ICAO_address; adsb_vehicle.lat = loc.lat; adsb_vehicle.lon = loc.lng; adsb_vehicle.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH; adsb_vehicle.altitude = -vehicle.position.z * 1000; adsb_vehicle.heading = wrap_360_cd(100*degrees(atan2f(vehicle.velocity_ef.y, vehicle.velocity_ef.x))); adsb_vehicle.hor_velocity = norm(vehicle.velocity_ef.x, vehicle.velocity_ef.y) * 100; adsb_vehicle.ver_velocity = -vehicle.velocity_ef.z * 100; memcpy(adsb_vehicle.callsign, vehicle.callsign, sizeof(adsb_vehicle.callsign)); adsb_vehicle.emitter_type = ADSB_EMITTER_TYPE_LARGE; adsb_vehicle.tslc = 1; adsb_vehicle.flags = ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_HEADING | ADSB_FLAGS_VALID_VELOCITY | ADSB_FLAGS_VALID_CALLSIGN | ADSB_FLAGS_SIMULATED; adsb_vehicle.squawk = 0; // NOTE: ADSB_FLAGS_VALID_SQUAWK bit is not set mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint8_t saved_seq = chan0_status->current_tx_seq; chan0_status->current_tx_seq = mavlink.seq; len = mavlink_msg_adsb_vehicle_encode(vehicle_system_id, MAV_COMP_ID_ADSB, &msg, &adsb_vehicle); chan0_status->current_tx_seq = saved_seq; uint8_t msgbuf[len]; len = mavlink_msg_to_send_buffer(msgbuf, &msg); if (len > 0) { mav_socket.send(msgbuf, len); } } } // ADSB_transceiever is enabled, send the status report. if (_sitl->adsb_tx && now - last_tx_report_ms > 1000) { last_tx_report_ms = now; mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint8_t saved_seq = chan0_status->current_tx_seq; uint8_t saved_flags = chan0_status->flags; chan0_status->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; chan0_status->current_tx_seq = mavlink.seq; const mavlink_uavionix_adsb_transceiver_health_report_t health_report = {UAVIONIX_ADSB_RF_HEALTH_OK}; len = mavlink_msg_uavionix_adsb_transceiver_health_report_encode(vehicle_system_id, MAV_COMP_ID_ADSB, &msg, &health_report); chan0_status->current_tx_seq = saved_seq; chan0_status->flags = saved_flags; uint8_t msgbuf[len]; len = mavlink_msg_to_send_buffer(msgbuf, &msg); if (len > 0) { mav_socket.send(msgbuf, len); ::printf("ADSBsim send tx health packet\n"); } } }
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { handle_request_data_stream(msg, true); break; } case MAVLINK_MSG_ID_STATUSTEXT: { // ignore any statustext messages not from our GCS: if (msg->sysid != rover.g.sysid_my_gcs) { break; } mavlink_statustext_t packet; mavlink_msg_statustext_decode(msg, &packet); char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+4] = { 'G', 'C', 'S', ':'}; memcpy(&text[4], packet.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); rover.DataFlash.Log_Write_Message(text); break; } case MAVLINK_MSG_ID_COMMAND_INT: { // decode packet mavlink_command_int_t packet; mavlink_msg_command_int_decode(msg, &packet); uint8_t result = MAV_RESULT_UNSUPPORTED; switch (packet.command) { #if MOUNT == ENABLED case MAV_CMD_DO_SET_ROI: { // param1 : /* Region of interest mode (not used)*/ // param2 : /* MISSION index/ target ID (not used)*/ // param3 : /* ROI index (not used)*/ // param4 : /* empty */ // x : lat // y : lon // z : alt // sanity check location if (!check_latlng(packet.x, packet.y)) { break; } Location roi_loc; roi_loc.lat = packet.x; roi_loc.lng = packet.y; roi_loc.alt = (int32_t)(packet.z * 100.0f); if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { // switch off the camera tracking if enabled if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { rover.camera_mount.set_mode_to_default(); } } else { // send the command to the camera mount rover.camera_mount.set_roi_target(roi_loc); } result = MAV_RESULT_ACCEPTED; break; } #endif default: result = MAV_RESULT_UNSUPPORTED; break; } // send ACK or NAK mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result); break; } case MAVLINK_MSG_ID_COMMAND_LONG: { // decode mavlink_command_long_t packet; mavlink_msg_command_long_decode(msg, &packet); uint8_t result = MAV_RESULT_UNSUPPORTED; // do command switch (packet.command) { case MAV_CMD_START_RX_PAIR: result = handle_rc_bind(packet); break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: rover.set_mode(RTL); result = MAV_RESULT_ACCEPTED; break; #if MOUNT == ENABLED // Sets the region of interest (ROI) for the camera case MAV_CMD_DO_SET_ROI: // sanity check location if (!check_latlng(packet.param5, packet.param6)) { break; } Location roi_loc; roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); roi_loc.alt = (int32_t)(packet.param7 * 100.0f); if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { // switch off the camera tracking if enabled if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { rover.camera_mount.set_mode_to_default(); } } else { // send the command to the camera mount rover.camera_mount.set_roi_target(roi_loc); } result = MAV_RESULT_ACCEPTED; break; #endif #if CAMERA == ENABLED case MAV_CMD_DO_DIGICAM_CONFIGURE: rover.camera.configure(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6, packet.param7); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_DO_DIGICAM_CONTROL: if (rover.camera.control(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6)) { rover.log_picture(); } result = MAV_RESULT_ACCEPTED; break; #endif // CAMERA == ENABLED case MAV_CMD_DO_MOUNT_CONTROL: #if MOUNT == ENABLED rover.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); result = MAV_RESULT_ACCEPTED; #endif break; case MAV_CMD_MISSION_START: rover.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_PREFLIGHT_CALIBRATION: if (hal.util->get_soft_armed()) { result = MAV_RESULT_FAILED; break; } if (is_equal(packet.param1, 1.0f)) { rover.ins.init_gyro(); if (rover.ins.gyro_calibrated_ok_all()) { rover.ahrs.reset_gyro_drift(); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (is_equal(packet.param3, 1.0f)) { rover.init_barometer(false); result = MAV_RESULT_ACCEPTED; } else if (is_equal(packet.param4, 1.0f)) { rover.trim_radio(); result = MAV_RESULT_ACCEPTED; } else if (is_equal(packet.param5, 1.0f)) { result = MAV_RESULT_ACCEPTED; // start with gyro calibration rover.ins.init_gyro(); // reset ahrs gyro bias if (rover.ins.gyro_calibrated_ok_all()) { rover.ahrs.reset_gyro_drift(); } else { result = MAV_RESULT_FAILED; } rover.ins.acal_init(); rover.ins.get_acal()->start(this); } else if (is_equal(packet.param5, 2.0f)) { // start with gyro calibration rover.ins.init_gyro(); // accel trim float trim_roll, trim_pitch; if (rover.ins.calibrate_trim(trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine rover.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else { send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration"); } break; case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: { uint8_t compassNumber = -1; if (is_equal(packet.param1, 2.0f)) { compassNumber = 0; } else if (is_equal(packet.param1, 5.0f)) { compassNumber = 1; } else if (is_equal(packet.param1, 6.0f)) { compassNumber = 2; } if (compassNumber != (uint8_t) -1) { rover.compass.set_and_save_offsets(compassNumber, packet.param2, packet.param3, packet.param4); result = MAV_RESULT_ACCEPTED; } break; } case MAV_CMD_DO_SET_MODE: switch ((uint16_t)packet.param1) { case MAV_MODE_MANUAL_ARMED: case MAV_MODE_MANUAL_DISARMED: rover.set_mode(MANUAL); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_AUTO_ARMED: case MAV_MODE_AUTO_DISARMED: rover.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_STABILIZE_DISARMED: case MAV_MODE_STABILIZE_ARMED: rover.set_mode(LEARNING); result = MAV_RESULT_ACCEPTED; break; default: result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_DO_SET_SERVO: if (rover.ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_REPEAT_SERVO: if (rover.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_SET_RELAY: if (rover.ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_REPEAT_RELAY: if (rover.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_equal(packet.param1, 1.0f) || is_equal(packet.param1, 3.0f)) { // when packet.param1 == 3 we reboot to hold in bootloader hal.scheduler->reboot(is_equal(packet.param1, 3.0f)); result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_COMPONENT_ARM_DISARM: if (is_equal(packet.param1, 1.0f)) { // run pre_arm_checks and arm_checks and display failures if (rover.arm_motors(AP_Arming::MAVLINK)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (is_zero(packet.param1)) { if (rover.disarm_motors()) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else { result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_GET_HOME_POSITION: if (rover.home_is_set != HOME_UNSET) { send_home(rover.ahrs.get_home()); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } break; case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { if (is_equal(packet.param1, 1.0f)) { send_autopilot_version(FIRMWARE_VERSION); result = MAV_RESULT_ACCEPTED; } break; } case MAV_CMD_DO_SET_HOME: { // param1 : use current (1=use current location, 0=use specified location) // param5 : latitude // param6 : longitude // param7 : altitude result = MAV_RESULT_FAILED; // assume failure if (is_equal(packet.param1, 1.0f)) { rover.init_home(); } else { if (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7)) { // don't allow the 0,0 position break; } // sanity check location if (!check_latlng(packet.param5, packet.param6)) { break; } Location new_home_loc {}; new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f); new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f); new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); rover.ahrs.set_home(new_home_loc); rover.home_is_set = HOME_SET_NOT_LOCKED; rover.Log_Write_Home_And_Origin(); GCS_MAVLINK::send_home_all(new_home_loc); result = MAV_RESULT_ACCEPTED; rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", (double)(new_home_loc.lat * 1.0e-7f), (double)(new_home_loc.lng * 1.0e-7f), (uint32_t)(new_home_loc.alt * 0.01f)); } break; } case MAV_CMD_DO_START_MAG_CAL: case MAV_CMD_DO_ACCEPT_MAG_CAL: case MAV_CMD_DO_CANCEL_MAG_CAL: result = rover.compass.handle_mag_cal_command(packet); break; case MAV_CMD_NAV_SET_YAW_SPEED: { // param1 : yaw angle to adjust direction by in centidegress // param2 : Speed - normalized to 0 .. 1 // exit if vehicle is not in Guided mode if (rover.control_mode != GUIDED) { break; } rover.guided_mode = Guided_Angle; rover.guided_yaw_speed.msg_time_ms = AP_HAL::millis(); rover.guided_yaw_speed.turn_angle = packet.param1; rover.guided_yaw_speed.target_speed = constrain_float(packet.param2, 0.0f, 1.0f); rover.nav_set_yaw_speed(); result = MAV_RESULT_ACCEPTED; break; } case MAV_CMD_ACCELCAL_VEHICLE_POS: result = MAV_RESULT_FAILED; if (rover.ins.get_acal()->gcs_vehicle_position(packet.param1)) { result = MAV_RESULT_ACCEPTED; } break; default: break; } mavlink_msg_command_ack_send_buf( msg, chan, packet.command, result); break; } case MAVLINK_MSG_ID_SET_MODE: { handle_set_mode(msg, FUNCTOR_BIND(&rover, &Rover::mavlink_set_mode, bool, uint8_t)); break; } case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { handle_mission_request_list(rover.mission, msg); break; } // XXX read a WP from EEPROM and send it to the GCS case MAVLINK_MSG_ID_MISSION_REQUEST_INT: case MAVLINK_MSG_ID_MISSION_REQUEST: { handle_mission_request(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_ACK: { // not used break; } case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { // mark the firmware version in the tlog send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING); #if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION); #endif handle_param_request_list(msg); break; } case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { handle_mission_clear_all(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { handle_mission_set_current(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_COUNT: { handle_mission_count(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: { handle_mission_write_partial_list(rover.mission, msg); break; } // GCS has sent us a mission item, store to EEPROM case MAVLINK_MSG_ID_MISSION_ITEM: { if (handle_mission_item(msg, rover.mission)) { rover.DataFlash.Log_Write_EntireMission(rover.mission); } break; } case MAVLINK_MSG_ID_MISSION_ITEM_INT: { if (handle_mission_item(msg, rover.mission)) { rover.DataFlash.Log_Write_EntireMission(rover.mission); } break; } case MAVLINK_MSG_ID_PARAM_SET: { handle_param_set(msg, &rover.DataFlash); break; } case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { // allow override of RC channel values for HIL // or for complete GCS control of switch position // and RC PWM values. if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs break; } mavlink_rc_channels_override_t packet; int16_t v[8]; mavlink_msg_rc_channels_override_decode(msg, &packet); v[0] = packet.chan1_raw; v[1] = packet.chan2_raw; v[2] = packet.chan3_raw; v[3] = packet.chan4_raw; v[4] = packet.chan5_raw; v[5] = packet.chan6_raw; v[6] = packet.chan7_raw; v[7] = packet.chan8_raw; hal.rcin->set_overrides(v, 8); rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_RC, false); break; } case MAVLINK_MSG_ID_HEARTBEAT: { // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes if (msg->sysid != rover.g.sysid_my_gcs) { break; } rover.last_heartbeat_ms = rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false); break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84 { // decode packet mavlink_set_position_target_local_ned_t packet; mavlink_msg_set_position_target_local_ned_decode(msg, &packet); // exit if vehicle is not in Guided mode if (rover.control_mode != GUIDED) { break; } // check for supported coordinate frames if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED && packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED && packet.coordinate_frame != MAV_FRAME_BODY_NED && packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) { break; } bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; // prepare and send target position if (!pos_ignore) { Location loc = rover.current_loc; switch (packet.coordinate_frame) { case MAV_FRAME_BODY_NED: case MAV_FRAME_BODY_OFFSET_NED: { // rotate from body-frame to NE frame float ne_x = packet.x*rover.ahrs.cos_yaw() - packet.y*rover.ahrs.sin_yaw(); float ne_y = packet.x*rover.ahrs.sin_yaw() + packet.y*rover.ahrs.cos_yaw(); // add offset to current location location_offset(loc, ne_x, ne_y); } break; case MAV_FRAME_LOCAL_OFFSET_NED: // add offset to current location location_offset(loc, packet.x, packet.y); break; default: // MAV_FRAME_LOCAL_NED interpret as an offset from home loc = rover.ahrs.get_home(); location_offset(loc, packet.x, packet.y); break; } rover.guided_WP = loc; rover.rtl_complete = false; rover.set_guided_WP(); } break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86 { // decode packet mavlink_set_position_target_global_int_t packet; mavlink_msg_set_position_target_global_int_decode(msg, &packet); // exit if vehicle is not in Guided mode if (rover.control_mode != GUIDED) { break; } // check for supported coordinate frames if (packet.coordinate_frame != MAV_FRAME_GLOBAL_INT && packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT && packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) { break; } bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; // prepare and send target position if (!pos_ignore) { Location loc = rover.current_loc; loc.lat = packet.lat_int; loc.lng = packet.lon_int; rover.guided_WP = loc; rover.rtl_complete = false; rover.set_guided_WP(); } break; } case MAVLINK_MSG_ID_GPS_RTCM_DATA: case MAVLINK_MSG_ID_GPS_INPUT: case MAVLINK_MSG_ID_HIL_GPS: { rover.gps.handle_msg(msg); break; } #if HIL_MODE != HIL_MODE_DISABLED case MAVLINK_MSG_ID_HIL_STATE: { mavlink_hil_state_t packet; mavlink_msg_hil_state_decode(msg, &packet); // sanity check location if (!check_latlng(packet.lat, packet.lon)) { break; } // set gps hil sensor Location loc; loc.lat = packet.lat; loc.lng = packet.lon; loc.alt = packet.alt/10; Vector3f vel(packet.vx, packet.vy, packet.vz); vel *= 0.01f; gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D, packet.time_usec/1000, loc, vel, 10, 0); // rad/sec Vector3f gyros; gyros.x = packet.rollspeed; gyros.y = packet.pitchspeed; gyros.z = packet.yawspeed; // m/s/s Vector3f accels; accels.x = packet.xacc * (GRAVITY_MSS/1000.0f); accels.y = packet.yacc * (GRAVITY_MSS/1000.0f); accels.z = packet.zacc * (GRAVITY_MSS/1000.0f); ins.set_gyro(0, gyros); ins.set_accel(0, accels); compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); break; } #endif // HIL_MODE #if CAMERA == ENABLED // deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: { break; } // deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE case MAVLINK_MSG_ID_DIGICAM_CONTROL: { rover.camera.control_msg(msg); rover.log_picture(); break; } #endif // CAMERA == ENABLED #if MOUNT == ENABLED // deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE case MAVLINK_MSG_ID_MOUNT_CONFIGURE: { rover.camera_mount.configure_msg(msg); break; } // deprecated. Use MAV_CMD_DO_MOUNT_CONTROL case MAVLINK_MSG_ID_MOUNT_CONTROL: { rover.camera_mount.control_msg(msg); break; } #endif // MOUNT == ENABLED case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO_STATUS: { handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM)); break; } case MAVLINK_MSG_ID_LOG_REQUEST_DATA: rover.in_log_download = true; /* no break */ case MAVLINK_MSG_ID_LOG_ERASE: /* no break */ case MAVLINK_MSG_ID_LOG_REQUEST_LIST: if (!rover.in_mavlink_delay) { handle_log_message(msg, rover.DataFlash); } break; case MAVLINK_MSG_ID_LOG_REQUEST_END: rover.in_log_download = false; if (!rover.in_mavlink_delay) { handle_log_message(msg, rover.DataFlash); } break; case MAVLINK_MSG_ID_SERIAL_CONTROL: handle_serial_control(msg, rover.gps); break; case MAVLINK_MSG_ID_GPS_INJECT_DATA: handle_gps_inject(msg, rover.gps); break; case MAVLINK_MSG_ID_DISTANCE_SENSOR: rover.sonar.handle_msg(msg); break; case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS: rover.DataFlash.remote_log_block_status_msg(chan, msg); break; case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: send_autopilot_version(FIRMWARE_VERSION); break; case MAVLINK_MSG_ID_LED_CONTROL: // send message to Notify AP_Notify::handle_led_control(msg); break; case MAVLINK_MSG_ID_PLAY_TUNE: // send message to Notify AP_Notify::handle_play_tune(msg); break; default: handle_common_message(msg); break; } // end switch } // end handle mavlink
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { handle_request_data_stream(msg, true); break; } case MAVLINK_MSG_ID_COMMAND_INT: { // decode packet mavlink_command_int_t packet; mavlink_msg_command_int_decode(msg, &packet); MAV_RESULT result = MAV_RESULT_UNSUPPORTED; switch (packet.command) { case MAV_CMD_DO_SET_HOME: { // assume failure result = MAV_RESULT_FAILED; if (is_equal(packet.param1, 1.0f)) { // if param1 is 1, use current location if (rover.set_home_to_current_location(true)) { result = MAV_RESULT_ACCEPTED; } break; } // ensure param1 is zero if (!is_zero(packet.param1)) { break; } // check frame type is supported if (packet.frame != MAV_FRAME_GLOBAL && packet.frame != MAV_FRAME_GLOBAL_INT && packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { break; } // sanity check location if (!check_latlng(packet.x, packet.y)) { break; } Location new_home_loc {}; new_home_loc.lat = packet.x; new_home_loc.lng = packet.y; new_home_loc.alt = packet.z * 100; // handle relative altitude if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { if (!rover.ahrs.home_is_set()) { // cannot use relative altitude if home is not set break; } new_home_loc.alt += rover.ahrs.get_home().alt; } if (rover.set_home(new_home_loc, true)) { result = MAV_RESULT_ACCEPTED; } break; } #if MOUNT == ENABLED case MAV_CMD_DO_SET_ROI: { // param1 : /* Region of interest mode (not used)*/ // param2 : /* MISSION index/ target ID (not used)*/ // param3 : /* ROI index (not used)*/ // param4 : /* empty */ // x : lat // y : lon // z : alt // sanity check location if (!check_latlng(packet.x, packet.y)) { break; } Location roi_loc; roi_loc.lat = packet.x; roi_loc.lng = packet.y; roi_loc.alt = (int32_t)(packet.z * 100.0f); if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { // switch off the camera tracking if enabled if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { rover.camera_mount.set_mode_to_default(); } } else { // send the command to the camera mount rover.camera_mount.set_roi_target(roi_loc); } result = MAV_RESULT_ACCEPTED; break; } #endif default: result = MAV_RESULT_UNSUPPORTED; break; } // send ACK or NAK mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result); break; } case MAVLINK_MSG_ID_COMMAND_LONG: { // decode mavlink_command_long_t packet; mavlink_msg_command_long_decode(msg, &packet); MAV_RESULT result = MAV_RESULT_UNSUPPORTED; // do command switch (packet.command) { case MAV_CMD_NAV_RETURN_TO_LAUNCH: rover.set_mode(rover.mode_rtl, MODE_REASON_GCS_COMMAND); result = MAV_RESULT_ACCEPTED; break; #if MOUNT == ENABLED // Sets the region of interest (ROI) for the camera case MAV_CMD_DO_SET_ROI: // sanity check location if (!check_latlng(packet.param5, packet.param6)) { break; } Location roi_loc; roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); roi_loc.alt = (int32_t)(packet.param7 * 100.0f); if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { // switch off the camera tracking if enabled if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { rover.camera_mount.set_mode_to_default(); } } else { // send the command to the camera mount rover.camera_mount.set_roi_target(roi_loc); } result = MAV_RESULT_ACCEPTED; break; #endif case MAV_CMD_DO_MOUNT_CONTROL: #if MOUNT == ENABLED rover.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); result = MAV_RESULT_ACCEPTED; #endif break; case MAV_CMD_MISSION_START: rover.set_mode(rover.mode_auto, MODE_REASON_GCS_COMMAND); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_equal(packet.param1, 1.0f) || is_equal(packet.param1, 3.0f)) { // when packet.param1 == 3 we reboot to hold in bootloader hal.scheduler->reboot(is_equal(packet.param1, 3.0f)); result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_COMPONENT_ARM_DISARM: if (is_equal(packet.param1, 1.0f)) { // run pre_arm_checks and arm_checks and display failures if (rover.arm_motors(AP_Arming::MAVLINK)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (is_zero(packet.param1)) { if (rover.disarm_motors()) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else { result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_DO_FENCE_ENABLE: result = MAV_RESULT_ACCEPTED; switch ((uint16_t)packet.param1) { case 0: rover.g2.fence.enable(false); break; case 1: rover.g2.fence.enable(true); break; default: result = MAV_RESULT_FAILED; break; } break; case MAV_CMD_DO_SET_HOME: { // param1 : use current (1=use current location, 0=use specified location) // param5 : latitude // param6 : longitude // param7 : altitude result = MAV_RESULT_FAILED; // assume failure if (is_equal(packet.param1, 1.0f)) { if (rover.set_home_to_current_location(true)) { result = MAV_RESULT_ACCEPTED; } } else { // ensure param1 is zero if (!is_zero(packet.param1)) { break; } Location new_home_loc {}; new_home_loc.lat = static_cast<int32_t>(packet.param5 * 1.0e7f); new_home_loc.lng = static_cast<int32_t>(packet.param6 * 1.0e7f); new_home_loc.alt = static_cast<int32_t>(packet.param7 * 100.0f); if (rover.set_home(new_home_loc, true)) { result = MAV_RESULT_ACCEPTED; } } break; } case MAV_CMD_NAV_SET_YAW_SPEED: { // param1 : yaw angle to adjust direction by in centidegress // param2 : Speed - normalized to 0 .. 1 // exit if vehicle is not in Guided mode if (rover.control_mode != &rover.mode_guided) { break; } // send yaw change and target speed to guided mode controller const float speed_max = rover.control_mode->get_speed_default(); const float target_speed = constrain_float(packet.param2 * speed_max, -speed_max, speed_max); rover.mode_guided.set_desired_heading_delta_and_speed(packet.param1, target_speed); result = MAV_RESULT_ACCEPTED; break; } case MAV_CMD_ACCELCAL_VEHICLE_POS: result = MAV_RESULT_FAILED; if (rover.ins.get_acal()->gcs_vehicle_position(packet.param1)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_MOTOR_TEST: // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) // param3 : throttle (range depends upon param2) // param4 : timeout (in seconds) result = rover.mavlink_motor_test_start(chan, static_cast<uint8_t>(packet.param1), static_cast<uint8_t>(packet.param2), static_cast<int16_t>(packet.param3), packet.param4); break; default: result = handle_command_long_message(packet); break; } mavlink_msg_command_ack_send_buf( msg, chan, packet.command, result); break; } case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { // allow override of RC channel values for HIL // or for complete GCS control of switch position // and RC PWM values. if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs break; } mavlink_rc_channels_override_t packet; mavlink_msg_rc_channels_override_decode(msg, &packet); RC_Channels::set_override(0, packet.chan1_raw); RC_Channels::set_override(1, packet.chan2_raw); RC_Channels::set_override(2, packet.chan3_raw); RC_Channels::set_override(3, packet.chan4_raw); RC_Channels::set_override(4, packet.chan5_raw); RC_Channels::set_override(5, packet.chan6_raw); RC_Channels::set_override(6, packet.chan7_raw); RC_Channels::set_override(7, packet.chan8_raw); rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_RC, false); break; } case MAVLINK_MSG_ID_MANUAL_CONTROL: { if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs break; } mavlink_manual_control_t packet; mavlink_msg_manual_control_decode(msg, &packet); if (packet.target != rover.g.sysid_this_mav) { break; // only accept control aimed at us } const int16_t roll = (packet.y == INT16_MAX) ? 0 : rover.channel_steer->get_radio_min() + (rover.channel_steer->get_radio_max() - rover.channel_steer->get_radio_min()) * (packet.y + 1000) / 2000.0f; const int16_t throttle = (packet.z == INT16_MAX) ? 0 : rover.channel_throttle->get_radio_min() + (rover.channel_throttle->get_radio_max() - rover.channel_throttle->get_radio_min()) * (packet.z + 1000) / 2000.0f; RC_Channels::set_override(uint8_t(rover.rcmap.roll() - 1), roll); RC_Channels::set_override(uint8_t(rover.rcmap.throttle() - 1), throttle); rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_RC, false); break; } case MAVLINK_MSG_ID_HEARTBEAT: { // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes if (msg->sysid != rover.g.sysid_my_gcs) { break; } rover.last_heartbeat_ms = rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false); break; } case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82 { // decode packet mavlink_set_attitude_target_t packet; mavlink_msg_set_attitude_target_decode(msg, &packet); // exit if vehicle is not in Guided mode if (rover.control_mode != &rover.mode_guided) { break; } // ensure type_mask specifies to use thrust if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) { break; } // convert thrust to ground speed packet.thrust = constrain_float(packet.thrust, -1.0f, 1.0f); const float target_speed = rover.control_mode->get_speed_default() * packet.thrust; // if the body_yaw_rate field is ignored, convert quaternion to heading if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE) != 0) { // convert quaternion to heading float target_heading_cd = degrees(Quaternion(packet.q[0], packet.q[1], packet.q[2], packet.q[3]).get_euler_yaw()) * 100.0f; rover.mode_guided.set_desired_heading_and_speed(target_heading_cd, target_speed); } else { // use body_yaw_rate field rover.mode_guided.set_desired_turn_rate_and_speed((RAD_TO_DEG * packet.body_yaw_rate) * 100.0f, target_speed); } break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84 { // decode packet mavlink_set_position_target_local_ned_t packet; mavlink_msg_set_position_target_local_ned_decode(msg, &packet); // exit if vehicle is not in Guided mode if (rover.control_mode != &rover.mode_guided) { break; } // check for supported coordinate frames if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED && packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED && packet.coordinate_frame != MAV_FRAME_BODY_NED && packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) { break; } bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; // prepare target position Location target_loc = rover.current_loc; if (!pos_ignore) { switch (packet.coordinate_frame) { case MAV_FRAME_BODY_NED: case MAV_FRAME_BODY_OFFSET_NED: { // rotate from body-frame to NE frame const float ne_x = packet.x * rover.ahrs.cos_yaw() - packet.y * rover.ahrs.sin_yaw(); const float ne_y = packet.x * rover.ahrs.sin_yaw() + packet.y * rover.ahrs.cos_yaw(); // add offset to current location location_offset(target_loc, ne_x, ne_y); } break; case MAV_FRAME_LOCAL_OFFSET_NED: // add offset to current location location_offset(target_loc, packet.x, packet.y); break; default: // MAV_FRAME_LOCAL_NED interpret as an offset from home target_loc = rover.ahrs.get_home(); location_offset(target_loc, packet.x, packet.y); break; } } float target_speed = 0.0f; float target_yaw_cd = 0.0f; // consume velocity and convert to target speed and heading if (!vel_ignore) { const float speed_max = rover.control_mode->get_speed_default(); // convert vector length into a speed target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max); // convert vector direction to target yaw target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f; // rotate target yaw if provided in body-frame if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor); } } // consume yaw heading if (!yaw_ignore) { target_yaw_cd = ToDeg(packet.yaw) * 100.0f; // rotate target yaw if provided in body-frame if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor); } } // consume yaw rate float target_turn_rate_cds = 0.0f; if (!yaw_rate_ignore) { target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f; } // handling case when both velocity and either yaw or yaw-rate are provided // by default, we consider that the rover will drive forward float speed_dir = 1.0f; if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) { // Note: we are using the x-axis velocity to determine direction even though // the frame may have been provided in MAV_FRAME_LOCAL_OFFSET_NED or MAV_FRAME_LOCAL_NED if (is_negative(packet.vx)) { speed_dir = -1.0f; } } // set guided mode targets if (!pos_ignore && vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) { // consume position target rover.mode_guided.set_desired_location(target_loc); } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) { // consume velocity rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) { // consume velocity and turn rate rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed); } else if (pos_ignore && !vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) { // consume velocity rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); } else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) { // consume just target heading (probably only skid steering vehicles can do this) rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f); } else if (pos_ignore && vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) { // consume just turn rate(probably only skid steering vehicles can do this) rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f); } break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86 { // decode packet mavlink_set_position_target_global_int_t packet; mavlink_msg_set_position_target_global_int_decode(msg, &packet); // exit if vehicle is not in Guided mode if (rover.control_mode != &rover.mode_guided) { break; } // check for supported coordinate frames if (packet.coordinate_frame != MAV_FRAME_GLOBAL && packet.coordinate_frame != MAV_FRAME_GLOBAL_INT && packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT && packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT && packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) { break; } bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; // prepare target position Location target_loc = rover.current_loc; if (!pos_ignore) { // sanity check location if (!check_latlng(packet.lat_int, packet.lon_int)) { // result = MAV_RESULT_FAILED; break; } target_loc.lat = packet.lat_int; target_loc.lng = packet.lon_int; } float target_speed = 0.0f; float target_yaw_cd = 0.0f; // consume velocity and convert to target speed and heading if (!vel_ignore) { const float speed_max = rover.control_mode->get_speed_default(); // convert vector length into a speed target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max); // convert vector direction to target yaw target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f; // rotate target yaw if provided in body-frame if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor); } } // consume yaw heading if (!yaw_ignore) { target_yaw_cd = ToDeg(packet.yaw) * 100.0f; // rotate target yaw if provided in body-frame if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor); } } // consume yaw rate float target_turn_rate_cds = 0.0f; if (!yaw_rate_ignore) { target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f; } // handling case when both velocity and either yaw or yaw-rate are provided // by default, we consider that the rover will drive forward float speed_dir = 1.0f; if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) { // Note: we are using the x-axis velocity to determine direction even though // the frame is provided in MAV_FRAME_GLOBAL_xxx if (is_negative(packet.vx)) { speed_dir = -1.0f; } } // set guided mode targets if (!pos_ignore && vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) { // consume position target rover.mode_guided.set_desired_location(target_loc); } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) { // consume velocity rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) { // consume velocity and turn rate rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed); } else if (pos_ignore && !vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) { // consume velocity rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); } else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) { // consume just target heading (probably only skid steering vehicles can do this) rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f); } else if (pos_ignore && vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) { // consume just turn rate(probably only skid steering vehicles can do this) rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f); } break; } #if HIL_MODE != HIL_MODE_DISABLED case MAVLINK_MSG_ID_HIL_STATE: { mavlink_hil_state_t packet; mavlink_msg_hil_state_decode(msg, &packet); // sanity check location if (!check_latlng(packet.lat, packet.lon)) { break; } // set gps hil sensor Location loc; loc.lat = packet.lat; loc.lng = packet.lon; loc.alt = packet.alt/10; Vector3f vel(packet.vx, packet.vy, packet.vz); vel *= 0.01f; gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D, packet.time_usec/1000, loc, vel, 10, 0); // rad/sec Vector3f gyros; gyros.x = packet.rollspeed; gyros.y = packet.pitchspeed; gyros.z = packet.yawspeed; // m/s/s Vector3f accels; accels.x = packet.xacc * (GRAVITY_MSS/1000.0f); accels.y = packet.yacc * (GRAVITY_MSS/1000.0f); accels.z = packet.zacc * (GRAVITY_MSS/1000.0f); ins.set_gyro(0, gyros); ins.set_accel(0, accels); compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); break; } #endif // HIL_MODE #if MOUNT == ENABLED // deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE case MAVLINK_MSG_ID_MOUNT_CONFIGURE: { rover.camera_mount.configure_msg(msg); break; } // deprecated. Use MAV_CMD_DO_MOUNT_CONTROL case MAVLINK_MSG_ID_MOUNT_CONTROL: { rover.camera_mount.control_msg(msg); break; } #endif // MOUNT == ENABLED case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO_STATUS: { handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM)); break; } // send or receive fence points with GCS case MAVLINK_MSG_ID_FENCE_POINT: // MAV ID: 160 case MAVLINK_MSG_ID_FENCE_FETCH_POINT: rover.g2.fence.handle_msg(*this, msg); break; case MAVLINK_MSG_ID_DISTANCE_SENSOR: rover.rangefinder.handle_msg(msg); rover.g2.proximity.handle_msg(msg); break; default: handle_common_message(msg); break; } // end switch } // end handle mavlink
void pf_field::update(local_member* p_ac, AP_AHRS_DCM* ahrs, AP_Airspeed* airspeed) { #endif #if HIL_MODE == HIL_MODE_ATTITUDE void pf_field::update(local_member* p_ac, AP_AHRS_HIL* ahrs, AP_Airspeed* airspeed) { #endif const Relative* tmp_p_rel = p_ac->get_rel(); //Gets the current relative information from the ac object Vector3i tmp_gV_L = *p_ac->get_gV_L(); uint16_t tmp_psi_ac = *p_ac->get_hdg(); //Get our heading (deg) //Calculate total repulsive function from i flock members Vector3f tmp_r_phi; //initialize vector (NED components) //Sum repulsive functions from i flock members for(int i=0;i<tmp_p_rel->Num_members;i++) { //indexing convention to organize member pointers int j = tmp_p_rel->Member_ids[i]-1; //Relative distance to flock members (NED components) Vector3f tmp_dX; //initialize vector each iteration tmp_dX.x=tmp_p_rel->dX[j]/100.0; //[m] tmp_dX.y=tmp_p_rel->dY[j]/100.0; //[m] tmp_dX.z=tmp_p_rel->dZ[j]/100.0; //[m] //Calculate repulsive parameters based on relative states //Gaussian repulsive function accumulation tmp_r_phi.x += (-2*(tmp_dX.x)*(_tau/100.0)/(_zeta/100.0))*exp(-(square(tmp_dX.x)+square(tmp_dX.y)+square(tmp_dX.z))/(_zeta/100.0)); tmp_r_phi.y += (-2*(tmp_dX.y)*(_tau/100.0)/(_zeta/100.0))*exp(-(square(tmp_dX.x)+square(tmp_dX.y)+square(tmp_dX.z))/(_zeta/100.0)); tmp_r_phi.z += (-2*(tmp_dX.z)*(_tau/100.0)/(_zeta/100.0))*exp(-(square(tmp_dX.x)+square(tmp_dX.y)+square(tmp_dX.z))/(_zeta/100.0)); } _phi_r = tmp_r_phi; //Repulsive potential gradient vector (NED component) //Relative distance vector to leader (NED components) Vector3i tmp_dL; //initialize vector (NED components) tmp_dL.x=tmp_p_rel->dXL; //[m*100] tmp_dL.y=tmp_p_rel->dYL; //[m*100] tmp_dL.z=tmp_p_rel->dZL; //[m*100] //Relative distance magnitude to leader int32_t tmp_d2L=tmp_p_rel->d2L; //[m*100] //Relative velocity to leader (NED components) //Vector3i tmp_dV; //initialize vector (NED components) Vector3i tmp_gV_LNAV; //initialize dVaspd (Local Navigation frame) //tmp_dV.x=tmp_p_rel->dVXL; //[m/s*100] //tmp_dV.y=tmp_p_rel->dVYL; //[m/s*100] //tmp_dV.z=tmp_p_rel->dVZL; //[m/s*100] float Cpsi = cos(ToRad(tmp_psi_ac/100.0)); float Spsi = sin(ToRad(tmp_psi_ac/100.0)); tmp_gV_LNAV.x=int16_t((tmp_gV_L.x)*Cpsi+(tmp_gV_L.y)*Spsi); //[m/s*100] tmp_gV_LNAV.y=int16_t(-(tmp_gV_L.x)*Spsi+(tmp_gV_L.y)*Cpsi); //[m/s*100] tmp_gV_LNAV.z=tmp_p_rel->dVZL; //[m/s*100] //Calculate the attractive potential gradient (NED components) /*Note: Attractive potential is comprised of two types of potential functions -In the far-field regime we use a linear potential field -In the near-field regime we use a quadratic potential field These fields must be matched at _chi, the near-field, far-field regime cut-off such that -Gradient of quadratic at _chi = gradient of linear at _chi In far-field, we are guided towards the leader's position (no offset) - this should improve convergence In near-field, we are guided towards the offset position, the side of which is dictated by swarm algorithm */ Vector3i tmp_pf_offset_NED; //Initialize vector uint16_t tmp_psi_L = tmp_p_rel->hdgL; //Get the heading of the Leader float CpsiL = cos(ToRad(tmp_p_rel->hdgL/100.0)); float SpsiL = sin(ToRad(tmp_p_rel->hdgL/100.0)); //set offset (Leader's Local frame) Vector3i tmp_pf_offset = _pf_offset_l; //update offset with side correction tmp_pf_offset.y = tmp_pf_offset.y*_side; //Rotate the offset distances (Leader's Local Navigation Frame) to NED frame tmp_pf_offset_NED.x = tmp_pf_offset.x*CpsiL - tmp_pf_offset.y*SpsiL; tmp_pf_offset_NED.y = tmp_pf_offset.x*SpsiL + tmp_pf_offset.y*CpsiL; tmp_pf_offset_NED.z = tmp_pf_offset.z; //Extrapolate Goal for NF case Location tmp_goal_loc = *p_ac->get_loc(); Location tmp_current_loc = *p_ac->get_loc(); //A bunch of new stuff for this extrapolation technique location_offset(&tmp_goal_loc,tmp_dL.x/100.0, tmp_dL.y/100.0); location_offset(&tmp_goal_loc,-tmp_pf_offset_NED.x/100.0, -tmp_pf_offset_NED.y/100.0); float tmp_N_extrap = 10*CpsiL; float tmp_E_extrap = 10*SpsiL; location_offset(&tmp_goal_loc,tmp_N_extrap, tmp_E_extrap); int32_t tmp_B_extrap = get_bearing_cd(&tmp_current_loc,&tmp_goal_loc); Vector3f extrap_uvec_NED; extrap_uvec_NED.x = cos(ToRad(tmp_B_extrap/100.0)); extrap_uvec_NED.y = sin(ToRad(tmp_B_extrap/100.0)); extrap_uvec_NED.z = 0; tmp_dL -=tmp_pf_offset_NED; tmp_d2L = int32_t(100*safe_sqrt(square(tmp_dL.x/100.0)+square(tmp_dL.y/100.0)+square(tmp_dL.z/100.0))); //Subtract offset distance vector from leader distance vector to get modified relative distance vector to leader if(tmp_d2L<(_chi*100)) //Near-field regime condition { _phi_a.x = 2*(_lambda.x/100.0)*(tmp_dL.x/100.0); //Calculate Quad PF X gradient with weighting parameter (NED frame) _phi_a.y = 2*(_lambda.y/100.0)*(tmp_dL.y/100.0); //Calculate Quad PF Y gradient with weighting parameter (NED frame) _phi_a.z = 2*(_lambda.z/100.0)*(tmp_dL.z/100.0); //Calculate Quad PF Z gradient with weighting parameter (NED frame) float tmp_phi_a_mag = _phi_a.length(); _phi_a_c.x = tmp_phi_a_mag*extrap_uvec_NED.x; _phi_a_c.y = tmp_phi_a_mag*extrap_uvec_NED.y; _phi_a_c.z = 0; } else { //float tmp_M =2*safe_sqrt(square(_lambda.x/100.0)+square(_lambda.y/100.0)+square(_lambda.z/100.0))*(_chi); //Slope of linear potential field (Magnitude of gradient) = nominal magnitude of quadratic gradient at chi float tmp_M = 2*_chi; float tmp_R = safe_sqrt(square((_lambda.x/100.0)*(tmp_dL.x/100.0))+square((_lambda.y/100.0)*(tmp_dL.y/100.0))+square((_lambda.z/100.0)*(tmp_dL.z/100))); _phi_a.x = tmp_M*square(_lambda.x/100.0)*(tmp_dL.x/100.0)/tmp_R; //Calculate Lin PF X gradient with weighting parameter (NED frame) _phi_a.y = tmp_M*square(_lambda.y/100.0)*(tmp_dL.y/100.0)/tmp_R; //Calculate Lin PF Y gradient with weighting parameter (NED frame) _phi_a.z = tmp_M*square(_lambda.z/100.0)*(tmp_dL.z/100.0)/tmp_R; //Calculate Lin PF Z gradient with weighting parameter (NED frame) } //Calculate the total potential gradient components _phi_NED = _phi_a + _phi_r; _phi_c_NED = _phi_a_c + _phi_r; //Calculate the magnitude of the total potential function gradient if(_phi_NED.length() > 0) //divide-by-zero check (safety first) { //Calculate the normalized potential gradient components _Nphi_NED = _phi_NED.normalized(); _Nphi_c_NED = _phi_c_NED.normalized(); //Find X component in Local Navigation frame by rotating NED vector about heading _Nphi_l.x = _Nphi_NED.x*Cpsi + _Nphi_NED.y*Spsi; if(tmp_d2L<_chi*100) //near-field consideration- We don't want the VWP behind the body { _regime_mask = 0x01; //Make first bit in regime mask 1 to reflect that we're in near-field regime /* //Correct normalized vector for VWP in near-field if(_Nphi_l.x<0) //Vector is pointing behind ac in Local Navigation frame { //Rotate NED pf gradiend components into Local Navigation components _phi_l.x = _phi_NED.x*cos(ToRad(tmp_psi_ac/100.0)) + _phi_NED.y*sin(ToRad(tmp_psi_ac/100.0)); _phi_l.y = - _phi_NED.x*sin(ToRad(tmp_psi_ac/100.0)) + _phi_NED.y*cos(ToRad(tmp_psi_ac/100.0)); //Correct potential function to point forward /* Note: Corrected by using airspeed instead of relative distance... almost a predictor of where the goal "will" be in a second / float airspeed_est; if(ahrs->airspeed_estimate(&airspeed_est)) { _phi_l.x = 2*(_lambda.x/100.0)*(airspeed_est); } else { _phi_l.x = 2*(_lambda.x/100.0)*10; //Not the most elegant way to do this... 10 is an average airspeed for the skysurfer } //Corrected potential rotated back into NED frame from Local Navigation frame _phi_c_NED.x= _phi_l.x*cos(ToRad(tmp_psi_ac/100.0)) - _phi_l.y*sin(ToRad(tmp_psi_ac/100.0)); _phi_c_NED.y= _phi_l.x*sin(ToRad(tmp_psi_ac/100.0)) + _phi_l.y*cos(ToRad(tmp_psi_ac/100.0)); //Normalize the corrected potential gradient vector _Nphi_c_NED = _phi_c_NED.normalized(); //Modify regime mask to reflect that both the first bit and the second bit are 1 (near-field regime, and using a corrected _regime_mask = 0x03; } //Make sure regime mask reflects that only else _regime_mask = 0x01; */ } else { //Regime mask should reflect that we are not in the near-field, and we therefore aren't using a corrected potential field _regime_mask = 0x00; } } else { //If the magnitude of the gradient is not greater than 0, make the norm 0 to avoid divide by 0 problems _Nphi_NED.zero(); _Nphi_c_NED.zero(); } float tmp_dNorth_com; float tmp_dEast_com; float tmp_dalt_com; float tmp_dspd_com; if(tmp_d2L<_chi*100) { //Calculate the North and East Offset [m] tmp_dNorth_com = _VWP_offset*_Nphi_c_NED.x; tmp_dEast_com = _VWP_offset*_Nphi_c_NED.y; //Calculate the change in altitude [m] //k_alt_V is the equivalent of a derivative gain- set to 0 for simplicity. //tmp_dalt_com = _VWP_Z_offset*_Nphi_NED.z; tmp_dalt_com = tmp_dL.z/100.0; _phi_norm = _Nphi_c_NED; } else { //Far-Field Case tmp_dNorth_com = _VWP_offset*_Nphi_NED.x; tmp_dEast_com = _VWP_offset*_Nphi_NED.y; //tmp_dalt_com = _VWP_Z_offset*_Nphi_NED.z; tmp_dalt_com = tmp_dL.z/100.0; _phi_norm = _Nphi_NED; } const Location* p_current_location = p_ac->get_loc(); _next_VWP = *p_current_location; location_offset(&_next_VWP, tmp_dNorth_com, tmp_dEast_com); _next_VWP.alt+=(int32_t)(tmp_dalt_com*100); //double-check sign here. constrain(_next_VWP.alt,PFG_MIN_ALT,PFG_MAX_ALT); if(tmp_d2L<_chi*100) { //Near-Field Case //New speed matching/ Speed from PFG command algorithm _next_airspeed_com = int32_t(100*((tmp_gV_LNAV.x/100.0*(PFG_K_P_dV/100.0)) +(_Nphi_l.x*(PFG_K_I_dV/100.0)))); constrain(_next_airspeed_com,PFG_MIN_AIRSPEED_CM,PFG_MAX_AIRSPEED_CM); } else { //Far-Field Case _next_airspeed_com = PFG_MAX_AIRSPEED_CM; //In far-field, set airspeed to maximum allowable (to catch up) } } const Location* pf_field::get_VWP(){ return (const Location*)&_next_VWP; } const int32_t* pf_field::get_new_speed(){ return (const int32_t*)&_next_airspeed_com; } const Vector3f* pf_field::get_pfg_att(){ return (const Vector3f*)&_phi_a; } const Vector3f* pf_field::get_pfg_rep(){ return (const Vector3f*)&_phi_r; } const Vector3f* pf_field::get_pfg_norm(){ return (const Vector3f*)&_phi_norm; } uint8_t pf_field::get_regime_mask(){ return _regime_mask; } pf_field PF_FIELD;
/* smooth sensors for kinematic consistancy when we interact with the ground */ void Aircraft::smooth_sensors(void) { uint64_t now = time_now_us; Vector3f delta_pos = position - smoothing.position; if (smoothing.last_update_us == 0 || delta_pos.length() > 10) { smoothing.position = position; smoothing.rotation_b2e = dcm; smoothing.accel_body = accel_body; smoothing.velocity_ef = velocity_ef; smoothing.gyro = gyro; smoothing.last_update_us = now; smoothing.location = location; printf("Smoothing reset at %.3f\n", now * 1.0e-6f); return; } float delta_time = (now - smoothing.last_update_us) * 1.0e-6f; // calculate required accel to get us to desired position and velocity in the time_constant const float time_constant = 0.1; Vector3f dvel = (velocity_ef - smoothing.velocity_ef) + (delta_pos / time_constant); Vector3f accel_e = dvel / time_constant + (dcm * accel_body + Vector3f(0,0,GRAVITY_MSS)); const float accel_limit = 14*GRAVITY_MSS; accel_e.x = constrain_float(accel_e.x, -accel_limit, accel_limit); accel_e.y = constrain_float(accel_e.y, -accel_limit, accel_limit); accel_e.z = constrain_float(accel_e.z, -accel_limit, accel_limit); smoothing.accel_body = smoothing.rotation_b2e.transposed() * (accel_e + Vector3f(0,0,-GRAVITY_MSS)); // calculate rotational rate to get us to desired attitude in time constant Quaternion desired_q, current_q, error_q; desired_q.from_rotation_matrix(dcm); desired_q.normalize(); current_q.from_rotation_matrix(smoothing.rotation_b2e); current_q.normalize(); error_q = desired_q / current_q; error_q.normalize(); Vector3f angle_differential; error_q.to_axis_angle(angle_differential); smoothing.gyro = gyro + angle_differential / time_constant; float R,P,Y; smoothing.rotation_b2e.to_euler(&R,&P,&Y); float R2,P2,Y2; dcm.to_euler(&R2,&P2,&Y2); #if 0 DataFlash_Class::instance()->Log_Write("SMOO", "TimeUS,AEx,AEy,AEz,DPx,DPy,DPz,R,P,Y,R2,P2,Y2", "Qffffffffffff", AP_HAL::micros64(), degrees(angle_differential.x), degrees(angle_differential.y), degrees(angle_differential.z), delta_pos.x, delta_pos.y, delta_pos.z, degrees(R), degrees(P), degrees(Y), degrees(R2), degrees(P2), degrees(Y2)); #endif // integrate to get new attitude smoothing.rotation_b2e.rotate(smoothing.gyro * delta_time); smoothing.rotation_b2e.normalize(); // integrate to get new position smoothing.velocity_ef += accel_e * delta_time; smoothing.position += smoothing.velocity_ef * delta_time; smoothing.location = home; location_offset(smoothing.location, smoothing.position.x, smoothing.position.y); smoothing.location.alt = home.alt - smoothing.position.z*100.0f; smoothing.last_update_us = now; smoothing.enabled = true; }
/* * extrapolate latitude/longitude given bearing and distance * Note that this function is accurate to about 1mm at a distance of * 100m. This function has the advantage that it works in relative * positions, so it keeps the accuracy even when dealing with small * distances and floating point numbers */ void location_update(struct Location &loc, float bearing, float distance) { float ofs_north = cosf(radians(bearing))*distance; float ofs_east = sinf(radians(bearing))*distance; location_offset(loc, ofs_north, ofs_east); }
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { // allow override of RC channel values for HIL // or for complete GCS control of switch position // and RC PWM values. if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs break; } uint32_t tnow = AP_HAL::millis(); mavlink_rc_channels_override_t packet; mavlink_msg_rc_channels_override_decode(msg, &packet); RC_Channels::set_override(0, packet.chan1_raw, tnow); RC_Channels::set_override(1, packet.chan2_raw, tnow); RC_Channels::set_override(2, packet.chan3_raw, tnow); RC_Channels::set_override(3, packet.chan4_raw, tnow); RC_Channels::set_override(4, packet.chan5_raw, tnow); RC_Channels::set_override(5, packet.chan6_raw, tnow); RC_Channels::set_override(6, packet.chan7_raw, tnow); RC_Channels::set_override(7, packet.chan8_raw, tnow); break; } case MAVLINK_MSG_ID_MANUAL_CONTROL: { if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs break; } mavlink_manual_control_t packet; mavlink_msg_manual_control_decode(msg, &packet); if (packet.target != rover.g.sysid_this_mav) { break; // only accept control aimed at us } uint32_t tnow = AP_HAL::millis(); const int16_t roll = (packet.y == INT16_MAX) ? 0 : rover.channel_steer->get_radio_min() + (rover.channel_steer->get_radio_max() - rover.channel_steer->get_radio_min()) * (packet.y + 1000) / 2000.0f; const int16_t throttle = (packet.z == INT16_MAX) ? 0 : rover.channel_throttle->get_radio_min() + (rover.channel_throttle->get_radio_max() - rover.channel_throttle->get_radio_min()) * (packet.z + 1000) / 2000.0f; RC_Channels::set_override(uint8_t(rover.rcmap.roll() - 1), roll, tnow); RC_Channels::set_override(uint8_t(rover.rcmap.throttle() - 1), throttle, tnow); break; } case MAVLINK_MSG_ID_HEARTBEAT: { // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes if (msg->sysid != rover.g.sysid_my_gcs) { break; } rover.last_heartbeat_ms = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false); break; } case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82 { // decode packet mavlink_set_attitude_target_t packet; mavlink_msg_set_attitude_target_decode(msg, &packet); // exit if vehicle is not in Guided mode if (rover.control_mode != &rover.mode_guided) { break; } // ensure type_mask specifies to use thrust if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) { break; } // convert thrust to ground speed packet.thrust = constrain_float(packet.thrust, -1.0f, 1.0f); const float target_speed = rover.control_mode->get_speed_default() * packet.thrust; // if the body_yaw_rate field is ignored, convert quaternion to heading if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE) != 0) { // convert quaternion to heading float target_heading_cd = degrees(Quaternion(packet.q[0], packet.q[1], packet.q[2], packet.q[3]).get_euler_yaw()) * 100.0f; rover.mode_guided.set_desired_heading_and_speed(target_heading_cd, target_speed); } else { // use body_yaw_rate field rover.mode_guided.set_desired_turn_rate_and_speed((RAD_TO_DEG * packet.body_yaw_rate) * 100.0f, target_speed); } break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84 { // decode packet mavlink_set_position_target_local_ned_t packet; mavlink_msg_set_position_target_local_ned_decode(msg, &packet); // exit if vehicle is not in Guided mode if (rover.control_mode != &rover.mode_guided) { break; } // check for supported coordinate frames if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED && packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED && packet.coordinate_frame != MAV_FRAME_BODY_NED && packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) { break; } bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; // prepare target position Location target_loc = rover.current_loc; if (!pos_ignore) { switch (packet.coordinate_frame) { case MAV_FRAME_BODY_NED: case MAV_FRAME_BODY_OFFSET_NED: { // rotate from body-frame to NE frame const float ne_x = packet.x * rover.ahrs.cos_yaw() - packet.y * rover.ahrs.sin_yaw(); const float ne_y = packet.x * rover.ahrs.sin_yaw() + packet.y * rover.ahrs.cos_yaw(); // add offset to current location location_offset(target_loc, ne_x, ne_y); } break; case MAV_FRAME_LOCAL_OFFSET_NED: // add offset to current location location_offset(target_loc, packet.x, packet.y); break; default: // MAV_FRAME_LOCAL_NED interpret as an offset from home target_loc = rover.ahrs.get_home(); location_offset(target_loc, packet.x, packet.y); break; } } float target_speed = 0.0f; float target_yaw_cd = 0.0f; // consume velocity and convert to target speed and heading if (!vel_ignore) { const float speed_max = rover.control_mode->get_speed_default(); // convert vector length into a speed target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max); // convert vector direction to target yaw target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f; // rotate target yaw if provided in body-frame if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor); } } // consume yaw heading if (!yaw_ignore) { target_yaw_cd = ToDeg(packet.yaw) * 100.0f; // rotate target yaw if provided in body-frame if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor); } } // consume yaw rate float target_turn_rate_cds = 0.0f; if (!yaw_rate_ignore) { target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f; } // handling case when both velocity and either yaw or yaw-rate are provided // by default, we consider that the rover will drive forward float speed_dir = 1.0f; if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) { // Note: we are using the x-axis velocity to determine direction even though // the frame may have been provided in MAV_FRAME_LOCAL_OFFSET_NED or MAV_FRAME_LOCAL_NED if (is_negative(packet.vx)) { speed_dir = -1.0f; } } // set guided mode targets if (!pos_ignore) { // consume position target rover.mode_guided.set_desired_location(target_loc); } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) { // consume velocity rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) { // consume velocity and turn rate rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed); } else if (pos_ignore && !vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) { // consume velocity rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); } else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) { // consume just target heading (probably only skid steering vehicles can do this) rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f); } else if (pos_ignore && vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) { // consume just turn rate(probably only skid steering vehicles can do this) rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f); } break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86 { // decode packet mavlink_set_position_target_global_int_t packet; mavlink_msg_set_position_target_global_int_decode(msg, &packet); // exit if vehicle is not in Guided mode if (rover.control_mode != &rover.mode_guided) { break; } // check for supported coordinate frames if (packet.coordinate_frame != MAV_FRAME_GLOBAL && packet.coordinate_frame != MAV_FRAME_GLOBAL_INT && packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT && packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT && packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) { break; } bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; // prepare target position Location target_loc = rover.current_loc; if (!pos_ignore) { // sanity check location if (!check_latlng(packet.lat_int, packet.lon_int)) { // result = MAV_RESULT_FAILED; break; } target_loc.lat = packet.lat_int; target_loc.lng = packet.lon_int; } float target_speed = 0.0f; float target_yaw_cd = 0.0f; // consume velocity and convert to target speed and heading if (!vel_ignore) { const float speed_max = rover.control_mode->get_speed_default(); // convert vector length into a speed target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max); // convert vector direction to target yaw target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f; // rotate target yaw if provided in body-frame if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor); } } // consume yaw heading if (!yaw_ignore) { target_yaw_cd = ToDeg(packet.yaw) * 100.0f; // rotate target yaw if provided in body-frame if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) { target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor); } } // consume yaw rate float target_turn_rate_cds = 0.0f; if (!yaw_rate_ignore) { target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f; } // handling case when both velocity and either yaw or yaw-rate are provided // by default, we consider that the rover will drive forward float speed_dir = 1.0f; if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) { // Note: we are using the x-axis velocity to determine direction even though // the frame is provided in MAV_FRAME_GLOBAL_xxx if (is_negative(packet.vx)) { speed_dir = -1.0f; } } // set guided mode targets if (!pos_ignore) { // consume position target rover.mode_guided.set_desired_location(target_loc); } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) { // consume velocity rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) { // consume velocity and turn rate rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed); } else if (pos_ignore && !vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) { // consume velocity rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed); } else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) { // consume just target heading (probably only skid steering vehicles can do this) rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f); } else if (pos_ignore && vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) { // consume just turn rate(probably only skid steering vehicles can do this) rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f); } break; } #if HIL_MODE != HIL_MODE_DISABLED case MAVLINK_MSG_ID_HIL_STATE: { mavlink_hil_state_t packet; mavlink_msg_hil_state_decode(msg, &packet); // sanity check location if (!check_latlng(packet.lat, packet.lon)) { break; } // set gps hil sensor Location loc; loc.lat = packet.lat; loc.lng = packet.lon; loc.alt = packet.alt/10; Vector3f vel(packet.vx, packet.vy, packet.vz); vel *= 0.01f; gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D, packet.time_usec/1000, loc, vel, 10, 0); // rad/sec Vector3f gyros; gyros.x = packet.rollspeed; gyros.y = packet.pitchspeed; gyros.z = packet.yawspeed; // m/s/s Vector3f accels; accels.x = packet.xacc * (GRAVITY_MSS/1000.0f); accels.y = packet.yacc * (GRAVITY_MSS/1000.0f); accels.z = packet.zacc * (GRAVITY_MSS/1000.0f); ins.set_gyro(0, gyros); ins.set_accel(0, accels); compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); break; } #endif // HIL_MODE #if MOUNT == ENABLED // deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE case MAVLINK_MSG_ID_MOUNT_CONFIGURE: { rover.camera_mount.configure_msg(msg); break; } // deprecated. Use MAV_CMD_DO_MOUNT_CONTROL case MAVLINK_MSG_ID_MOUNT_CONTROL: { rover.camera_mount.control_msg(msg); break; } #endif // MOUNT == ENABLED case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO_STATUS: { handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM)); break; } // send or receive fence points with GCS case MAVLINK_MSG_ID_FENCE_POINT: // MAV ID: 160 case MAVLINK_MSG_ID_FENCE_FETCH_POINT: rover.g2.fence.handle_msg(*this, msg); break; case MAVLINK_MSG_ID_DISTANCE_SENSOR: rover.rangefinder.handle_msg(msg); rover.g2.proximity.handle_msg(msg); break; default: handle_common_message(msg); break; } // end switch } // end handle mavlink
void SoaringController::get_target(Location &wp) { wp = _prev_update_location; location_offset(wp, _ekf.X[2], _ekf.X[3]); }