void use_rnb_data(uint8_t power)
{
uint8_t brightness_matrix[6][6];
pack_measurements_into_matrix(brightness_matrix);;
//print_brightness_matrix(brightness_matrix);
uint8_t emitter_total[6];
uint8_t sensor_total[6];
fill_S_and_T(brightness_matrix, sensor_total, emitter_total);
float bearing = get_bearing(sensor_total);
float heading = get_heading(emitter_total, bearing);
float initial_range = get_initial_range_guess(bearing, heading, sensor_total, emitter_total, brightness_matrix, power);
float range = range_estimate(brightness_matrix, initial_range, bearing, heading, power);
//TODO: Actually incorporate the ID #.
//rnb temp = {.range = range, .bearing = bearing, .heading = heading, .id_number = 0};
//printf("Bearing: %f | Heading: %f | Initial Range Guess: %f | Range: %f\r\n", rad_to_deg(bearing), rad_to_deg(heading), initial_range, range);
last_good_rnb.range = range;
last_good_rnb.bearing = bearing;
last_good_rnb.heading = heading;
last_good_rnb.brightness_matrix_ptr = brightness_matrix;
last_good_rnb.id_number = last_command_source_id;
rnb_updated=1;
}
void test_bearing_west() {
KalmanFilter f = alloc_filter_velocity2d(1.0);
for (int i = 0; i < 100; ++i) {
update_velocity2d(f, 0.0, i * -0.0001, 1.0);
}
double bearing = get_bearing(f);
assert(abs(bearing - 270.0) < 0.01);
free_filter(f);
}
AIRCRAFT_STATE
AircraftStateFilter::get_predicted_state(const fixed &in_time) const
{
AIRCRAFT_STATE state_next = m_state_last;
GeoVector vec(get_speed()*in_time, get_bearing());
state_next.Location = vec.end_point(m_state_last.Location);
state_next.NavAltitude = m_state_last.NavAltitude+get_climb_rate()*in_time;
state_next.Speed = get_speed();
state_next.Vario = get_climb_rate();
return state_next;
}
static void calc_home(void *d)
{
home.uav_bearing = (int) get_bearing(&priv.home_coord, &priv.uav_coord);
home.direction = home.uav_bearing + 180;
home.direction -= priv.heading;
if (home.direction < 0)
home.direction += 360;
home.distance = earth_distance(&priv.home_coord, &priv.uav_coord);
home.altitude = priv.altitude - priv.home_altitude;
}
void test_bearing_north() {
KalmanFilter f = alloc_filter_velocity2d(1.0);
for (int i = 0; i < 100; ++i) {
update_velocity2d(f, i * 0.0001, 0.0, 1.0);
}
double bearing = get_bearing(f);
assert(abs(bearing - 0.0) < 0.01);
/* Velocity should be 0.0001 x units per timestep */
double dlat, dlon;
get_velocity(f, &dlat, &dlon);
assert(abs(dlat - 0.0001) < 0.00001);
assert(abs(dlon) < 0.00001);
free_filter(f);
}
/**
* Displays geolocation data in the main dialog.
*/
static void
display_geolocation_data(int count,
double latitude, double longitude, double accuracy,
double altitude, bool altitude_valid,
double altitude_accuracy, bool altitude_accuracy_valid,
double heading, bool heading_valid,
double speed, bool speed_valid,
int num_satellites_used, bool num_satellites_valid)
{
char altitude_buf[128];
char altitude_accuracy_buf[128];
char heading_buf[128];
char speed_buf[128];
char num_satellites_used_buf[128];
snprintf(msg, MSG_SIZE,
"Geolocation report #%d\n"
"\tlatitude: % 13.8f degrees\n"
"\tlongitude: % 13.8f degrees\n"
"\taccuracy: % 7.3f m\n"
"\t%s\n"
"\t%s\n"
"\t%s\n"
"\t%s\n"
"\t%s\n",
count,
latitude, longitude, accuracy,
format_double_data(altitude_buf, sizeof altitude_buf, "altitude", "m",
altitude, altitude_valid),
format_double_data(altitude_accuracy_buf, sizeof altitude_accuracy_buf, "altitude accuracy", "m",
altitude_accuracy, altitude_accuracy_valid),
format_double_data(heading_buf, sizeof heading_buf, "heading", get_bearing(heading),
heading, heading_valid),
/* Speed is reported in m/s. To convert to km/h, use the formula:
* 1 m/s = 60*60/1000 km/h = 3.6 km/h
*/
format_double_data(speed_buf, sizeof speed_buf, "speed", "km/h",
speed*3.6, speed_valid),
format_int_data(num_satellites_used_buf, sizeof num_satellites_used_buf, "number of satellites used", "",
num_satellites_used, num_satellites_valid)
);
show_dialog_message(msg);
}
void test_bearing_east() {
KalmanFilter f = alloc_filter_velocity2d(1.0);
for (int i = 0; i < 100; ++i) {
update_velocity2d(f, 0.0, i * 0.0001, 1.0);
}
double bearing = get_bearing(f);
assert(abs(bearing - 90.0) < 0.01);
/* At this rate, it takes 10,000 timesteps to travel one longitude
unit, and thus 3,600,000 timesteps to travel the circumference of
the earth. Let's say one timestep is a second, so it takes
3,600,000 seconds, which is 60,000 minutes, which is 1,000
hours. Since the earth is about 25000 miles around, this means we
are traveling at about 25 miles per hour. */
double mph = get_mph(f);
assert(abs(mph - 25.0) < 2.0);
free_filter(f);
}
static void calc_home(struct timer *t, void *d)
{
mavlink_heartbeat_t *hb = mavdata_get(MAVLINK_MSG_ID_HEARTBEAT);
mavlink_global_position_int_t *gpi;
mavlink_message_t this_msg;
switch (home.lock) {
case HOME_NONE:
default:
if (mavdata_age(MAVLINK_MSG_ID_HEARTBEAT) > 5000)
return;
/* check arming status */
if (hb->base_mode & MAV_MODE_FLAG_SAFETY_ARMED)
home.lock = HOME_WAIT;
break;
case HOME_WAIT:
if (mavdata_age(MAVLINK_MSG_ID_MISSION_ITEM) > 2000) {
/* when UAV is armed, home is WP0 */
mavlink_msg_mission_request_pack(config.mav.osd_sysid, MAV_COMP_ID_OSD, &this_msg,
config.mav.uav_sysid, MAV_COMP_ID_ALL, 0);
mavlink_send_msg(&this_msg);
} else {
mavlink_mission_item_t *mi = mavdata_get(MAVLINK_MSG_ID_MISSION_ITEM);
priv.home_coord.lat = DEG2RAD(mi->x);
priv.home_coord.lon = DEG2RAD(mi->y);
priv.home_altitude = (unsigned int) mi->z;
home.lock = HOME_GOT;
}
break;
case HOME_GOT:
home.lock = HOME_LOCKED;
set_timer_period(t, 200);
break;
case HOME_LOCKED:
{
gpi = mavdata_get(MAVLINK_MSG_ID_GLOBAL_POSITION_INT);
priv.uav_coord.lat = DEG2RAD(gpi->lat / 10000000.0);
priv.uav_coord.lon = DEG2RAD(gpi->lon / 10000000.0);
priv.altitude = (unsigned int) (gpi->alt / 1000);
priv.heading = (int) (gpi->hdg / 100);
home.uav_bearing = (int) get_bearing(&priv.home_coord, &priv.uav_coord);
home.direction = home.uav_bearing + 180;
home.direction -= priv.heading;
if (home.direction < 0)
home.direction += 360;
home.distance = earth_distance(&priv.home_coord, &priv.uav_coord);
home.altitude = priv.altitude - priv.home_altitude;
break;
case HOME_RESET:
home.lock = HOME_NONE;
set_timer_period(t, 1000);
break;
case HOME_FORCE:
gpi = mavdata_get(MAVLINK_MSG_ID_GLOBAL_POSITION_INT);
priv.home_coord.lat = DEG2RAD(gpi->lat / 10000000.0);
priv.home_coord.lon = DEG2RAD(gpi->lon / 10000000.0);
priv.home_altitude = (unsigned int) (gpi->alt / 1000);
home.lock = HOME_GOT;
break;
}
}
}
void
Camera::move()
{
Vector3<float> target_vector(0,0,1); // x, y, z to target before rotating to planes axis, values are in meters
//decide what happens to camera depending on camera mode
switch(mode)
{
case 0:
//do nothing, i.e lock camera in place
return;
break;
case 1:
//stabilize
target_vector.x=0; //east to west gives +tive value (i.e. longitude)
target_vector.y=0; //south to north gives +tive value (i.e. latitude)
target_vector.z=100; //downwards is +tive
break;
case 2:
//track target
if(g_gps->fix)
{
target_vector=get_location_vector(¤t_loc,&camera_target);
}
break;
case 3: // radio manual control
case 4: // test (level the camera and point north)
break; // see code 25 lines bellow
}
Matrix3f m = dcm.get_dcm_transposed();
Vector3<float> targ = m*target_vector; //to do: find out notion of x y convention
switch(gimbal_type)
{
case 0: // pitch & roll (tilt & roll)
cam_pitch = degrees(atan2(-targ.x, targ.z)); //pitch
cam_roll = degrees(atan2(targ.y, targ.z)); //roll
break;
case 1: // yaw & pitch (pan & tilt)
cam_pitch = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1;
cam_yaw = 9000 + atan2(-targ.y, targ.x) * 5729.57795;
break;
/* case 2: // pitch, roll & yaw - not started
cam_ritch = 0;
cam_yoll = 0;
cam_paw = 0;
break; */
}
//some camera modes overwrite the gimbal_type calculations
switch(mode)
{
case 3: // radio manual control
if (rc_function[CAM_PITCH])
cam_pitch = map(rc_function[CAM_PITCH]->radio_in,
rc_function[CAM_PITCH]->radio_min,
rc_function[CAM_PITCH]->radio_max,
rc_function[CAM_PITCH]->angle_min,
rc_function[CAM_PITCH]->radio_max);
if (rc_function[CAM_ROLL])
cam_roll = map(rc_function[CAM_ROLL]->radio_in,
rc_function[CAM_ROLL]->radio_min,
rc_function[CAM_ROLL]->radio_max,
rc_function[CAM_ROLL]->angle_min,
rc_function[CAM_ROLL]->radio_max);
if (rc_function[CAM_YAW])
cam_yaw = map(rc_function[CAM_YAW]->radio_in,
rc_function[CAM_YAW]->radio_min,
rc_function[CAM_YAW]->radio_max,
rc_function[CAM_YAW]->angle_min,
rc_function[CAM_YAW]->radio_max);
break;
case 4: // test (level the camera and point north)
cam_pitch = -dcm.pitch_sensor;
cam_yaw = dcm.yaw_sensor; // do not invert because the servo is mounted upside-down on my system
// TODO: the "trunk" code can invert using parameters, but this branch still can't
cam_roll = -dcm.roll_sensor;
break;
}
#if CAM_DEBUG == ENABLED
//for debugging purposes
Serial.println();
Serial.print("current_loc: lat: ");
Serial.print(current_loc.lat);
Serial.print(", lng: ");
Serial.print(current_loc.lng);
Serial.print(", alt: ");
Serial.print(current_loc.alt);
Serial.println();
Serial.print("target_loc: lat: ");
Serial.print(camera_target.lat);
Serial.print(", lng: ");
Serial.print(camera_target.lng);
Serial.print(", alt: ");
Serial.print(camera_target.alt);
Serial.print(", distance: ");
Serial.print(get_distance(¤t_loc,&camera_target));
Serial.print(", bearing: ");
Serial.print(get_bearing(¤t_loc,&camera_target));
Serial.println();
Serial.print("dcm_angles: roll: ");
Serial.print(degrees(dcm.roll));
Serial.print(", pitch: ");
Serial.print(degrees(dcm.pitch));
Serial.print(", yaw: ");
Serial.print(degrees(dcm.yaw));
Serial.println();
Serial.print("target_vector: x: ");
Serial.print(target_vector.x,2);
Serial.print(", y: ");
Serial.print(target_vector.y,2);
Serial.print(", z: ");
Serial.print(target_vector.z,2);
Serial.println();
Serial.print("rotated_target_vector: x: ");
Serial.print(targ.x,2);
Serial.print(", y: ");
Serial.print(targ.y,2);
Serial.print(", z: ");
Serial.print(targ.z,2);
Serial.println();
Serial.print("gimbal type 0: roll: ");
Serial.print(roll);
Serial.print(", pitch: ");
Serial.print(pitch);
Serial.println();
/* Serial.print("gimbal type 1: pitch: ");
Serial.print(pan);
Serial.print(", roll: ");
Serial.print(tilt);
Serial.println(); */
/* Serial.print("gimbal type 2: pitch: ");
Serial.print(ritch);
Serial.print(", roll: ");
Serial.print(yoll);
Serial.print(", yaw: ");
Serial.print(paw);
Serial.println(); */
#endif
}
