static void test_doubles(void) {
printf("\n--- enu_of_ecef double ---\n");
// struct LlaCoor_f ref_coor;
// ref_coor.lat = RAD_OF_DEG(43.605278);
// ref_coor.lon = RAD_OF_DEG(1.442778);
// ref_coor.alt = 180.0;
struct EcefCoor_d ref_coor = { 4624497.0 , 116475.0, 4376563.0};
printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z);
struct LtpDef_d ltp_def;
ltp_def_from_ecef_d(<p_def, &ref_coor);
printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), ltp_def.lla.alt);
struct EcefCoor_d my_ecef_point = ref_coor;
struct EnuCoor_d my_enu_point;
enu_of_ecef_point_d(&my_enu_point, <p_def, &my_ecef_point);
printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n",
my_ecef_point.x, my_ecef_point.y, my_ecef_point.z,
my_enu_point.x, my_enu_point.y, my_enu_point.z );
printf("\n");
}
void dc_send_shot_position(void)
{
int16_t phi = DegOfRad(estimator_phi*10.0f);
int16_t theta = DegOfRad(estimator_theta*10.0f);
float gps_z = ((float)gps.hmsl) / 1000.0f;
int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));
int16_t photo_nr = -1;
if (dc_buffer < DC_IMAGE_BUFFER) {
dc_buffer++;
dc_photo_nr++;
photo_nr = dc_photo_nr;
}
DOWNLINK_SEND_DC_SHOT(DefaultChannel,
&photo_nr,
&gps.utm_pos.east,
&gps.utm_pos.north,
&gps_z,
&gps.utm_pos.zone,
&phi,
&theta,
&course,
&gps.gspeed,
&gps.tow);
}
void dc_send_shot_position(void)
{
// angles in decideg
int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi*10.0f);
int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta*10.0f);
int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi*10.0f);
// course in decideg
int16_t course = DegOfRad(*stateGetHorizontalSpeedDir_f()) * 10;
// ground speed in cm/s
uint16_t speed = (*stateGetHorizontalSpeedNorm_f()) * 10;
int16_t photo_nr = -1;
if (dc_photo_nr < DC_IMAGE_BUFFER) {
dc_photo_nr++;
photo_nr = dc_photo_nr;
}
DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
&photo_nr,
&stateGetPositionLla_i()->lat,
&stateGetPositionLla_i()->lon,
&stateGetPositionLla_i()->alt,
&gps.hmsl,
&phi,
&theta,
&psi,
&course,
&speed,
&gps.tow);
}
static void send_cam(struct transport_tx *trans, struct link_device *dev)
{
int16_t x = cam_target_x;
int16_t y = cam_target_y;
int16_t phi = DegOfRad(cam_phi_c);
int16_t theta = DegOfRad(cam_theta_c);
pprz_msg_send_CAM(trans, dev, AC_ID, &phi, &theta, &x, &y);
}
void sky_seg_avoid_run(void) {
static int counter = 0;
counter++;
// Read Latest GST Module Results
if (counter >= (512/15))
{
// Vertical
float dx = sqrt ( (stateGetPositionEnu_f()->x * stateGetPositionEnu_f()->x) + (stateGetPositionEnu_f()->y * stateGetPositionEnu_f()->y) );
float dh = (17.0f - stateGetPositionEnu_f()->z);
float vang = atan2(dh,dx) * 80.9F * 2.0f; // 255 / PI
if (vang < 0.0f)
vang = 0.0f;
if (vang > 255.0f)
vang = 255.0f;
// Horizontal
float bearing = atan2(- stateGetPositionEnu_f()->x, - stateGetPositionEnu_f()->y );
float heading = stateGetNedToBodyEulers_f()->psi;
float diff = bearing - heading;
NormAngleRad(diff);
diff = DegOfRad(diff);
float hang = DegOfRad(atan2(10,dx)); // 255 / PI
float viewangle = 50; // degrees
float range = viewangle/2.0f;
float deg_per_bin = viewangle/ ((float) N_BINS);
float bin_nr_mid = range/deg_per_bin;
float bin_nr_start = bin_nr_mid + diff/deg_per_bin - hang/deg_per_bin;
float bin_nr_stop = bin_nr_mid + diff/deg_per_bin + hang/deg_per_bin;
for (int i=0; i<N_BINS; i++)
{
if ((i > bin_nr_start) && (i <= (bin_nr_stop)))
{
gst2ppz.obstacle_bins[i] = vang;
}
else
{
gst2ppz.obstacle_bins[i] = 0.0f;
}
}
counter = 0;
run_avoid_navigation_onvision();
}
}
gboolean timeout_callback(gpointer data) {
for (int i=0; i<20; i++) {
aos_compute_state();
aos_compute_sensors();
#ifndef DISABLE_PROPAGATE
ahrs_propagate();
#endif
#ifndef DISABLE_ACCEL_UPDATE
ahrs_update_accel();
#endif
#ifndef DISABLE_MAG_UPDATE
if (!(i%5)) ahrs_update_mag();
#endif
}
#if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ
EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler);
#endif
#if AHRS_TYPE == AHRS_TYPE_ICQ
IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d",
ahrs_impl.gyro_bias.p,
ahrs_impl.gyro_bias.q,
ahrs_impl.gyro_bias.r);
#endif
#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2
struct Int32Rates bias_i;
RATES_BFP_OF_REAL(bias_i, ahrs_impl.gyro_bias);
IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d",
bias_i.p,
bias_i.q,
bias_i.r);
#endif
IvySendMsg("183 AHRS_EULER %f %f %f",
ahrs_float.ltp_to_imu_euler.phi,
ahrs_float.ltp_to_imu_euler.theta,
ahrs_float.ltp_to_imu_euler.psi);
IvySendMsg("183 BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f",
DegOfRad(aos.imu_rates.p),
DegOfRad(aos.imu_rates.q),
DegOfRad(aos.imu_rates.r),
DegOfRad(aos.ltp_to_imu_euler.phi),
DegOfRad(aos.ltp_to_imu_euler.theta),
DegOfRad(aos.ltp_to_imu_euler.psi));
IvySendMsg("183 BOOZ_SIM_GYRO_BIAS %f %f %f",
DegOfRad(aos.gyro_bias.p),
DegOfRad(aos.gyro_bias.q),
DegOfRad(aos.gyro_bias.r));
return TRUE;
}
value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, value cl, value t, value m, value lat, value lon) {
gps_mode = (Bool_val(m) ? 3 : 0);
gps_course = DegOfRad(Double_val(c)) * 10.;
gps_alt = Double_val(a) * 100.;
gps_gspeed = Double_val(s) * 100.;
gps_climb = Double_val(cl) * 100.;
gps_week = 0; // FIXME
gps_itow = Double_val(t) * 1000.;
#ifdef GPS_USE_LATLONG
gps_lat = DegOfRad(Double_val(lat))*1e7;
gps_lon = DegOfRad(Double_val(lon))*1e7;
latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0);
gps_utm_east = latlong_utm_x * 100;
gps_utm_north = latlong_utm_y * 100;
gps_utm_zone = nav_utm_zone0;
x = y = z; /* Just to get rid of the "unused arg" warning */
#else // GPS_USE_LATLONG
gps_utm_east = Int_val(x);
gps_utm_north = Int_val(y);
gps_utm_zone = Int_val(z);
lat = lon; /* Just to get rid of the "unused arg" warning */
#endif // GPS_USE_LATLONG
/** Space vehicle info simulation */
gps_nb_channels=7;
int i;
static int time;
time++;
for(i = 0; i < gps_nb_channels; i++) {
gps_svinfos[i].svid = 7 + i;
gps_svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45;
gps_svinfos[i].azim = (time/gps_nb_channels + 50 * i) % 360;
gps_svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.;
gps_svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01);
gps_svinfos[i].qi = (int)((time / 1000.) + i) % 8;
}
gps_PDOP = gps_Sacc = gps_Pacc = 500+200*sin(time/100.);
gps_numSV = 7;
gps_verbose_downlink = !launch;
UseGpsPosNoSend(estimator_update_state_gps);
gps_downlink();
return Val_unit;
}
/** Reset the UTM zone to current GPS fix */
unit_t nav_reset_utm_zone(void) {
struct UtmCoor_f utm0_old;
utm0_old.zone = nav_utm_zone0;
utm0_old.north = nav_utm_north0;
utm0_old.east = nav_utm_east0;
utm0_old.alt = ground_alt;
struct LlaCoor_f lla0;
lla_of_utm_f(&lla0, &utm0_old);
#ifdef GPS_USE_LATLONG
/* Set the real UTM zone */
nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
#else
nav_utm_zone0 = gps.utm_pos.zone;
#endif
struct UtmCoor_f utm0;
utm0.zone = nav_utm_zone0;
utm_of_lla_f(&utm0, &lla0);
nav_utm_east0 = utm0.east;
nav_utm_north0 = utm0.north;
stateSetLocalUtmOrigin_f(&utm0);
return 0;
}
uint8_t dc_info(void)
{
#ifdef DOWNLINK_SEND_DC_INFO
float course = DegOfRad(stateGetNedToBodyEulers_f()->psi);
int16_t mode = dc_autoshoot;
uint8_t shutter = dc_autoshoot_period * 10;
DOWNLINK_SEND_DC_INFO(DefaultChannel, DefaultDevice,
&mode,
&stateGetPositionLla_i()->lat,
&stateGetPositionLla_i()->lon,
&stateGetPositionLla_i()->alt,
&course,
&dc_photo_nr,
&dc_survey_interval,
&dc_gps_next_dist,
&dc_gps_x,
&dc_gps_y,
&dc_circle_start_angle,
&dc_circle_interval,
&dc_circle_last_block,
&dc_gps_count,
&shutter);
#endif
return 0;
}
void mf_daq_send_report(void)
{
// Send report over normal telemetry
if (mf_daq.nb > 0) {
DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 9, mf_daq.values);
}
// Test if log is started
if (pprzLogFile != -1) {
if (log_started == FALSE) {
// Log MD5SUM once
DOWNLINK_SEND_ALIVE(pprzlog_tp, chibios_sdlog, 16, MD5SUM);
log_started = true;
}
// Log GPS for time reference
uint8_t foo = 0;
int16_t climb = -gps.ned_vel.z;
int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));
struct UtmCoor_f utm = *stateGetPositionUtm_f();
int32_t east = utm.east * 100;
int32_t north = utm.north * 100;
DOWNLINK_SEND_GPS(pprzlog_tp, chibios_sdlog, &gps.fix,
&east, &north, &course, &gps.hmsl, &gps.gspeed, &climb,
&gps.week, &gps.tow, &utm.zone, &foo);
}
}
void aoa_pwm_update(void) {
static float prev_aoa = 0.0f;
// raw duty cycle in usec
uint32_t duty_raw = get_pwm_input_duty_in_usec(AOA_PWM_CHANNEL);
// remove some offset if needed
aoa_pwm.raw = duty_raw - AOA_PWM_OFFSET;
// FIXME for some reason, the last value of the MA3 encoder is not 4096 but 4097
// this case is not handled since we don't care about angles close to +- 180 deg
aoa_pwm.angle = AOA_SIGN * (((float)aoa_pwm.raw * aoa_pwm.sens) - aoa_pwm.offset - AOA_ANGLE_OFFSET);
// filter angle
aoa_pwm.angle = aoa_pwm.filter * prev_aoa + (1.0f - aoa_pwm.filter) * aoa_pwm.angle;
prev_aoa = aoa_pwm.angle;
#if USE_AOA
stateSetAngleOfAttack_f(aoa_adc.angle);
#endif
#if SEND_SYNC_AOA
RunOnceEvery(10, DOWNLINK_SEND_AOA(DefaultChannel, DefaultDevice, &aoa_pwm.raw, &aoa_pwm.angle));
#endif
#if LOG_AOA
if(pprzLogFile != -1) {
if (!log_started) {
sdLogWriteLog(pprzLogFile, "AOA_PWM: ANGLE(deg) RAW(int16)\n");
log_started = true;
} else {
float angle = DegOfRad(aoa_pwm.angle);
sdLogWriteLog(pprzLogFile, "AOA_PWM: %.3f %d\n", angle, aoa_pwm.raw);
}
}
#endif
}
bool_t disc_survey( uint8_t center, float radius) {
float wind_dir = atan2(wind_north, wind_east) + M_PI;
/** Not null even if wind_east=wind_north=0 */
float upwind_x = cos(wind_dir);
float upwind_y = sin(wind_dir);
float grid = nav_survey_shift / 2;
switch (status) {
case UTURN:
nav_circle_XY(c.x, c.y, grid*sign);
if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
c1.x = estimator_x;
c1.y = estimator_y;
float d = ScalarProduct(upwind_x, upwind_y, estimator_x-WaypointX(center), estimator_y-WaypointY(center));
if (d > radius) {
status = DOWNWIND;
} else {
float w = sqrt(radius*radius - d*d) - 1.5*grid;
float crosswind_x = - upwind_y;
float crosswind_y = upwind_x;
c2.x = WaypointX(center)+d*upwind_x-w*sign*crosswind_x;
c2.y = WaypointY(center)+d*upwind_y-w*sign*crosswind_y;
status = SEGMENT;
}
nav_init_stage();
}
break;
case DOWNWIND:
c2.x = WaypointX(center) - upwind_x * radius;
c2.y = WaypointY(center) - upwind_y * radius;
status = SEGMENT;
/* No break; */
case SEGMENT:
nav_route_xy(c1.x, c1.y, c2.x, c2.y);
if (nav_approaching_xy(c2.x, c2.y, c1.x, c1.y, CARROT)) {
c.x = c2.x + grid*upwind_x;
c.y = c2.y + grid*upwind_y;
sign = -sign;
status = UTURN;
nav_init_stage();
}
break;
default:
break;
}
NavVerticalAutoThrottleMode(0.); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(center), 0.); /* No preclimb */
return TRUE;
}
void ins_reset_local_origin( void ) {
#if INS_UPDATE_FW_ESTIMATOR
struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
lla.lat = gps.lla_pos.lat / 1e7;
lla.lon = gps.lla_pos.lon / 1e7;
utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
#else
utm.zone = gps.utm_pos.zone;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
#endif
// ground_alt
utm.alt = gps.hmsl / 1000.0f;
// reset state UTM ref
stateSetLocalUtmOrigin_f(&utm);
#else
struct LtpDef_i ltp_def;
ltp_def_from_ecef_i(<p_def, &gps.ecef_pos);
ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(<p_def);
#endif
}
/** Reset the geographic reference to the current GPS fix */
unit_t nav_reset_reference( void ) {
#ifdef GPS_USE_LATLONG
/* Set the real UTM zone */
nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
lla.lat = gps.lla_pos.lat/1e7;
lla.lon = gps.lla_pos.lon/1e7;
struct UtmCoor_f utm;
utm.zone = nav_utm_zone0;
utm_of_lla_f(&utm, &lla);
nav_utm_east0 = utm.east;
nav_utm_north0 = utm.north;
#else
nav_utm_zone0 = gps.utm_pos.zone;
nav_utm_east0 = gps.utm_pos.east/100;
nav_utm_north0 = gps.utm_pos.north/100;
#endif
// reset state UTM ref
struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
stateSetLocalUtmOrigin_f(&utm0);
previous_ground_alt = ground_alt;
ground_alt = gps.hmsl/1000.;
return 0;
}
void ArduIMU_periodicGPS( void ) {
if (ardu_gps_trans.status != I2CTransDone) { return; }
// Test for high acceleration:
// - low speed
// - high thrust
if (estimator_hspeed_dir < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
high_accel_flag = TRUE;
} else {
high_accel_flag = FALSE;
if (estimator_hspeed_dir > HIGH_ACCEL_LOW_SPEED && !high_accel_flag) {
high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
}
if (estimator_hspeed_dir < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
high_accel_done = FALSE; // Activate high accel after landing
}
}
FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps.speed_3d); // speed 3D
FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps.gspeed); // ground speed
FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)DegOfRad(gps.course / 1e6)); // course
ardu_gps_trans.buf[12] = gps.fix; // status gps fix
ardu_gps_trans.buf[13] = gps_ubx.status_flags; // status flags
ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter)
I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15);
}
bool_t snav_circle1(void) {
/* circle around CD until QDR_TD */
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(wp_cd.a, 0.);
nav_circle_XY(wp_cd.x, wp_cd.y, d_radius);
return(! NavQdrCloseTo(DegOfRad(qdr_td)));
}
void dc_send_shot_position(void)
{
// angles in decideg
int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi * 10.0f);
int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta * 10.0f);
int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi * 10.0f);
// course in decideg
int16_t course = DegOfRad(stateGetHorizontalSpeedDir_f()) * 10;
// ground speed in cm/s
uint16_t speed = stateGetHorizontalSpeedNorm_f() * 10;
int16_t photo_nr = -1;
if (dc_photo_nr < DC_IMAGE_BUFFER) {
dc_photo_nr++;
photo_nr = dc_photo_nr;
}
#if DC_SHOT_EXTRA_DL
// send a message on second datalink first
// (for instance an embedded CPU)
DOWNLINK_SEND_DC_SHOT(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
&photo_nr,
&stateGetPositionLla_i()->lat,
&stateGetPositionLla_i()->lon,
&stateGetPositionLla_i()->alt,
&gps.hmsl,
&phi,
&theta,
&psi,
&course,
&speed,
&gps.tow);
#endif
DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
&photo_nr,
&stateGetPositionLla_i()->lat,
&stateGetPositionLla_i()->lon,
&stateGetPositionLla_i()->alt,
&gps.hmsl,
&phi,
&theta,
&psi,
&course,
&speed,
&gps.tow);
}
int main(int argc, char **argv)
{
// Set the default tracking system position and angle
struct EcefCoor_d tracking_ecef;
//alt 45 m because of ellipsoid altitude in Delft
tracking_ecef.x = 3924331.5;
tracking_ecef.y = 300361.7;
tracking_ecef.z = 5002197.1;
tracking_offset_angle = 33.0 / 57.6;
ltp_def_from_ecef_d(&tracking_ltp, &tracking_ecef);
// Parse the options from cmdline
parse_options(argc, argv);
printf_debug("Tracking system Latitude: %f Longitude: %f Offset to North: %f degrees\n", DegOfRad(tracking_ltp.lla.lat),
DegOfRad(tracking_ltp.lla.lon), DegOfRad(tracking_offset_angle));
// Create the network connections
printf_debug("Starting NatNet listening (multicast address: %s, data port: %d, version: %d.%d)\n",
natnet_multicast_addr, natnet_data_port, natnet_major, natnet_minor);
udp_socket_create(&natnet_data, "", -1, natnet_data_port, 0); // Only receiving
udp_socket_subscribe_multicast(&natnet_data, natnet_multicast_addr);
udp_socket_set_recvbuf(&natnet_data, 0x100000); // 1MB
printf_debug("Starting NatNet command socket (server address: %s, command port: %d)\n", natnet_addr, natnet_cmd_port);
udp_socket_create(&natnet_cmd, natnet_addr, natnet_cmd_port, 0, 1);
udp_socket_set_recvbuf(&natnet_cmd, 0x100000); // 1MB
// Create the Ivy Client
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
IvyInit("natnet2ivy", "natnet2ivy READY", 0, 0, 0, 0);
IvyStart(ivy_bus);
// Create the main timers
printf_debug("Starting transmitting and sampling timeouts (transmitting frequency: %dHz, minimum velocity samples: %d)\n",
freq_transmit, min_velocity_samples);
g_timeout_add(1000 / freq_transmit, timeout_transmit_callback, NULL);
GIOChannel *sk = g_io_channel_unix_new(natnet_data.sockfd);
g_io_add_watch(sk, G_IO_IN | G_IO_NVAL | G_IO_HUP,
sample_data, NULL);
// Run the main loop
g_main_loop_run(ml);
return 0;
}
bool snav_circle2(void)
{
/* circle around CA until QDR_A */
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(wp_cd.a, 0.);
nav_circle_XY(wp_ca.x, wp_ca.y, a_radius);
return (! NavQdrCloseTo(DegOfRad(qdr_a)));
}
void test_of_axis_angle(void)
{
struct FloatVect3 axis = { 0., 1., 0.};
FLOAT_VECT3_NORMALIZE(axis);
DISPLAY_FLOAT_VECT3("axis", axis);
const float angle = RadOfDeg(30.);
printf("angle %f\n", DegOfRad(angle));
struct FloatQuat my_q;
FLOAT_QUAT_OF_AXIS_ANGLE(my_q, axis, angle);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("quat", my_q);
struct FloatRMat my_r1;
float_rmat_of_quat(&my_r1, &my_q);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", my_r1);
DISPLAY_FLOAT_RMAT("rmat1", my_r1);
struct FloatRMat my_r;
FLOAT_RMAT_OF_AXIS_ANGLE(my_r, axis, angle);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", my_r);
DISPLAY_FLOAT_RMAT("rmat", my_r);
printf("\n");
struct FloatEulers eul = {RadOfDeg(30.), RadOfDeg(30.), 0.};
struct FloatVect3 uz = { 0., 0., 1.};
struct FloatRMat r_yaw;
FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);
struct FloatVect3 uy = { 0., 1., 0.};
struct FloatRMat r_pitch;
FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);
struct FloatVect3 ux = { 1., 0., 0.};
struct FloatRMat r_roll;
FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);
struct FloatRMat r_yaw_pitch;
float_rmat_comp(&r_yaw_pitch, &r_yaw, &r_pitch);
struct FloatRMat r_yaw_pitch_roll;
float_rmat_comp(&r_yaw_pitch_roll, &r_yaw_pitch, &r_roll);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", r_yaw_pitch_roll);
DISPLAY_FLOAT_RMAT("rmat", r_yaw_pitch_roll);
DISPLAY_FLOAT_EULERS_DEG("eul", eul);
struct FloatRMat rmat1;
float_rmat_of_eulers(&rmat1, &eul);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", rmat1);
DISPLAY_FLOAT_RMAT("rmat1", rmat1);
}
static void send_gps_lla(struct transport_tx *trans, struct link_device *dev)
{
uint8_t err = 0;
int16_t climb = -gps.ned_vel.z;
int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));
pprz_msg_send_GPS_LLA(trans, dev, AC_ID,
&gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt,
&gps.hmsl, &course, &gps.gspeed, &climb,
&gps.week, &gps.tow,
&gps.fix, &err);
}
static void send_gps(struct transport_tx *trans, struct link_device *dev)
{
uint8_t zero = 0;
int16_t climb = -gps.ned_vel.z;
int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));
pprz_msg_send_GPS(trans, dev, AC_ID, &gps.fix,
&gps.utm_pos.east, &gps.utm_pos.north,
&course, &gps.hmsl, &gps.gspeed, &climb,
&gps.week, &gps.tow, &gps.utm_pos.zone, &zero);
// send SVINFO for available satellites that have new data
send_svinfo_available(trans, dev);
}
void handle_ins_msg( void) {
EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
EstimatorSetRate(ins_p,ins_q);
if (gps_mode != 0x03)
gps_gspeed = 0;
//gps_course = ins_psi * 1800 / M_PI;
gps_course = (DegOfRad(atan2f((float)ins_vx, (float)ins_vy))*10.0f);
gps_climb = (int16_t)(-ins_vz * 100);
gps_gspeed = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy) * 100);
EstimatorSetAtt(ins_phi, RadOfDeg(((float)gps_course) / 10.0), ins_theta);
// EstimatorSetSpeedPol(gps_gspeed, gps_course, ins_vz);
// EstimatorSetAlt(ins_z);
estimator_update_state_gps();
reset_gps_watchdog();
}
/* shoot on circle */
uint8_t dc_circle(float interval, float start) {
dc_autoshoot = DC_AUTOSHOOT_CIRCLE;
dc_gps_count = 0;
dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.);
if(start == DC_IGNORE) {
start = DegOfRad(stateGetNedToBodyEulers_f()->psi);
}
dc_circle_start_angle = fmodf(start, 360.);
if (start < 0.)
start += 360.;
//dc_circle_last_block = floorf(dc_circle_start_angle/dc_circle_interval);
dc_circle_last_block = 0;
dc_circle_max_blocks = floorf(360./dc_circle_interval);
dc_info();
return 0;
}
/**
* Send Metrics typically displayed on a HUD for fixed wing aircraft.
*/
static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev)
{
/* Current heading in degrees, in compass units (0..360, 0=north) */
int16_t heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
/* Current throttle setting in integer percent, 0 to 100 */
// is a 16bit unsigned int but supposed to be from 0 to 100??
uint16_t throttle;
#ifdef COMMAND_THRUST
throttle = commands[COMMAND_THRUST] / (MAX_PPRZ / 100);
#elif defined COMMAND_THROTTLE
throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100);
#endif
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0,
stateGetAirspeed_f(),
stateGetHorizontalSpeedNorm_f(), // groundspeed
heading,
throttle,
stateGetPositionLla_f()->alt, // altitude, FIXME: should be MSL
stateGetSpeedNed_f()->z); // climb rate
MAVLinkSendMessage();
}
static void mavlink_send_gps_raw_int(struct transport_tx *trans, struct link_device *dev)
{
#if USE_GPS
int32_t course = DegOfRad(gps.course) / 1e5;
if (course < 0) {
course += 36000;
}
mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
get_sys_time_usec(),
gps.fix,
gps.lla_pos.lat,
gps.lla_pos.lon,
gps.lla_pos.alt,
gps.pdop,
UINT16_MAX, // VDOP
gps.gspeed,
course,
gps.num_sv);
MAVLinkSendMessage();
#endif
}
static void mavlink_send_global_position_int(struct transport_tx *trans, struct link_device *dev)
{
float heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
if (heading < 0.) {
heading += 360;
}
uint16_t compass_heading = heading * 100;
int32_t relative_alt = stateGetPositionLla_i()->alt - state.ned_origin_f.hmsl;
/// TODO: check/ask what coordinate system vel is supposed to be in, not clear from docs
mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetPositionLla_i()->lat,
stateGetPositionLla_i()->lon,
stateGetPositionLla_i()->alt,
relative_alt,
stateGetSpeedNed_f()->x * 100,
stateGetSpeedNed_f()->y * 100,
stateGetSpeedNed_f()->z * 100,
compass_heading);
MAVLinkSendMessage();
}
uint8_t dc_info(void) {
#ifdef DOWNLINK_SEND_DC_INFO
float course = DegOfRad(estimator_psi);
DOWNLINK_SEND_DC_INFO(DefaultChannel,
&dc_autoshoot,
&estimator_x,
&estimator_y,
&course,
&dc_buffer,
&dc_gps_dist,
&dc_gps_next_dist,
&dc_gps_x,
&dc_gps_y,
&dc_circle_start_angle,
&dc_circle_interval,
&dc_circle_last_block,
&dc_gps_count,
&dc_autoshoot_quartersec_period);
#endif
return 0;
}
/** Reset the geographic reference to the current GPS fix */
void ins_reset_local_origin(void) {
struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
lla.lat = gps.lla_pos.lat / 1e7;
lla.lon = gps.lla_pos.lon / 1e7;
utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
#else
utm.zone = gps.utm_pos.zone;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
#endif
// ground_alt
utm.alt = gps.hmsl /1000.0f;
// reset state UTM ref
stateSetLocalUtmOrigin_f(&utm);
// reset filter flag
ins_impl.reset_alt_ref = TRUE;
}
static void test_enu_of_ecef_int(void) {
printf("\n--- enu_of_ecef int ---\n");
struct EcefCoor_f ref_coor_f = { 4624497.0 , 116475.0, 4376563.0};
struct LtpDef_f ltp_def_f;
ltp_def_from_ecef_f(<p_def_f, &ref_coor_f);
struct EcefCoor_i ref_coor_i = { rint(CM_OF_M(ref_coor_f.x)),
rint(CM_OF_M(ref_coor_f.y)),
rint(CM_OF_M(ref_coor_f.z))};
printf("ecef0 : (%d,%d,%d)\n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z);
struct LtpDef_i ltp_def_i;
ltp_def_from_ecef_i(<p_def_i, &ref_coor_i);
printf("lla0 : (%d %d %d) (%f,%f,%f)\n", ltp_def_i.lla.lat, ltp_def_i.lla.lon, ltp_def_i.lla.alt,
DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lat)),
DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lon)),
M_OF_CM((double)ltp_def_i.lla.alt));
#define STEP 1000.
#define RANGE 100000.
double sum_err = 0;
struct FloatVect3 max_err;
FLOAT_VECT3_ZERO(max_err);
struct FloatVect3 offset;
for (offset.x=-RANGE; offset.x<=RANGE; offset.x+=STEP) {
for (offset.y=-RANGE; offset.y<=RANGE; offset.y+=STEP) {
for (offset.z=-RANGE; offset.z<=RANGE; offset.z+=STEP) {
struct EcefCoor_f my_ecef_point_f = ref_coor_f;
VECT3_ADD(my_ecef_point_f, offset);
struct EnuCoor_f my_enu_point_f;
enu_of_ecef_point_f(&my_enu_point_f, <p_def_f, &my_ecef_point_f);
#if DEBUG
printf("ecef to enu float : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n",
my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z,
my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z );
#endif
struct EcefCoor_i my_ecef_point_i = { rint(CM_OF_M(my_ecef_point_f.x)),
rint(CM_OF_M(my_ecef_point_f.y)),
rint(CM_OF_M(my_ecef_point_f.z))};;
struct EnuCoor_i my_enu_point_i;
enu_of_ecef_point_i(&my_enu_point_i, <p_def_i, &my_ecef_point_i);
#if DEBUG
// printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z);
printf("ecef to enu int : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n",
M_OF_CM((double)my_ecef_point_i.x),
M_OF_CM((double)my_ecef_point_i.y),
M_OF_CM((double)my_ecef_point_i.z),
M_OF_CM((double)my_enu_point_i.x),
M_OF_CM((double)my_enu_point_i.y),
M_OF_CM((double)my_enu_point_i.z));
#endif
float ex = my_enu_point_f.x - M_OF_CM((double)my_enu_point_i.x);
if (fabs(ex) > max_err.x) max_err.x = fabs(ex);
float ey = my_enu_point_f.y - M_OF_CM((double)my_enu_point_i.y);
if (fabs(ey) > max_err.y) max_err.y = fabs(ey);
float ez = my_enu_point_f.z - M_OF_CM((double)my_enu_point_i.z);
if (fabs(ez) > max_err.z) max_err.z = fabs(ez);
sum_err += ex*ex + ey*ey + ez*ez;
}
}
}
double nb_samples = (2*RANGE / STEP + 1) * (2*RANGE / STEP + 1) * (2*RANGE / STEP + 1);
printf("enu_of_ecef int/float comparison:\n");
printf("error max (%f,%f,%f) m\n", max_err.x, max_err.y, max_err.z );
printf("error avg (%f ) m \n", sqrt(sum_err) / nb_samples );
printf("\n");
}
