//Chris's last minute demo stuff
void chris_waypoint_init()
{
do
{
gps_update();
} while (gps_get_latitude()==0 || gps_get_longitude()==0);
float center_lat = gps_get_latitude();
float center_long = gps_get_longitude();
float circle_scale = 10000; //100 = radius of ~1000m, 1000 = radius of ~100m, 4000 = radius of ~ 25m
debug_print("Generating circle of waypoints around center latitude ");
debug_printf(center_lat);
debug_print(", longitude ");
debug_printf(center_lat);
debug_println("...");
for(int i = 0; i < NUM_WAYPOINTS; i++)
{
float temp_lat, temp_long;
temp_lat = center_lat-cos((2*PI*i)/NUM_WAYPOINTS)/circle_scale;
temp_long = center_long+sin((2*PI*i)/NUM_WAYPOINTS)/circle_scale;
chris_waypoint_add(temp_lat,temp_long);
debug_printf(temp_lat);
debug_print(", ");
debug_printf(temp_long);
debug_println("");
}
chris_array_index=0;
}
void task_GPS(void) {
user_debug_msg(DBG_MSG, STR_TASK_GPS ": Starting.");
// With GPS silent, now it's time to initialize the NMEA buffer handler.
gps_open();
// Init all the variables we'll be reading from the GPS (e.g. longitude).
gps_init();
while (1) {
gps_update();
//Display current latitude, longitude, velocity and heading
#if 1
sprintf(str_tmp, STR_TASK_GPS ": %09.2f,%9.4f,%10.4f,%6.1f,%6.1f,%4.1f,%7.3f,%7.2f,%7.2f,%u,%u,%u,%2X,%u,%u,%u,%u",
gps_read().time,gps_read().latitude,gps_read().longitude,gps_read().altitude,gps_read().geoid,
gps_read().hdop,gps_read().speed,gps_read().heading,gps_read().mag_var,gps_read().num_sat,
gps_read().stationID,gps_read().date,gps_read().fixflag,
gps_read().rmc_update,gps_read().gga_update,gps_read().rmc_count,gps_read().gga_count);
user_debug_msg(DBG_MSG, str_tmp);
#endif
// Note that this is not in keeping with all the other TAPS -- this should be driven by TAP_delay, etc.
OS_Delay(500);
}
}
int main(void)
{
initialise();
uint16_t last_changed = millis();
while(1)
{
if(gps_demonstration==true)
{
if(gps_update())
{
chris_waypoint_update();
debug_print("H");
debug_print(",");
debug_printf(gps_get_latitude());
debug_print(",");
debug_printf(gps_get_longitude());
debug_print(",");
debug_printi(gps_get_sats());
debug_print(",");
debug_printf(altimeter_get_metres());
debug_print(",");
debug_printf(chris_waypoint_current_bearing()); //target heading
debug_print(",");
float current_heading = compass_get_heading();
debug_printf(current_heading);
debug_print(",");
debug_printf(current_heading-chris_waypoint_current_bearing());
debug_print(",");
debug_printi(chris_waypoint_current_index());
debug_print(",");
debug_printf(chris_waypoint_current_distance());
debug_print(",");
debug_printf(array_get_lat(chris_waypoint_current_index()));
debug_print(",");
debug_printf(array_get_lon(chris_waypoint_current_index()));
debug_println(",");
}
//automatic testing, every 3 seconds change to the next waypoint
if(millis()-last_changed>3000 && 1==2)
{
chris_set_waypoint_current_index(chris_waypoint_current_index()+1);
last_changed=millis();
}
if(chris_waypoint_current_index()>chris_get_max_waypoints())
chris_set_waypoint_current_index(0);
}
else
{
//demonstrate reading receiver
rx_update();
quad_output_passthrough(true,true,true,true,true,true);
}
}
}
void update_all_inputs(void) {
button_update();
cmps10_update_all();
gps_update();
odometer_update();
update_all_nav();
return;
}
/******************************************************************************
**** ****
** **
task_monitor()
This task proves the proper functioning of the passthrough feature of the
GPSRM 1. Passthrough connects the OEM615V's COM1 port to one of three pairs
of IO lines on the CSK bus: IO.[5,4], IO.[17,16] or IO.[33,32]. Which pair
is implemented depends on the GPSRM 1's Assembly Revision (ASSY REV).
task_gps() initially configures the GPSRM 1 with passthrough enabled, but
without any logging active in the OEM615V.
task_monitor() (re-)starts once commanded to by task_gps() ... this happens
after all the basic tests (I2C working, can power OEM615V on and off) are
concluded, the OEM615V is on and not in reset, and is ready to talk via COM1.
task_monitor() then commands the OEM615V to start logging, and then scans
the resulting NMEA strings from the OEM615V for valid GPS fix and GPS data.
** **
**** ****
******************************************************************************/
void task_monitor(void) {
// Initially we're stopped, waiting for task_gps to configure the GPSRM1 and OEM615V.
user_debug_msg(STR_TASK_MONITOR "Stopped.");
OS_Stop();
// Eventually task_gps() starts this task.
user_debug_msg(STR_TASK_MONITOR "Starting.");
// With GPS silent, now it's time to initialize the NMEA buffer handler.
gps_open();
// Init all the variables we'll be reading from the GPS (e.g. longitude).
gps_init();
// Now that we're ready for NMEA messages from the OEM615V, tell it to
// start logging them at 1Hz on its COM1.
// We need to send this out to all three possible paths into the GPSRM 1's
// passthrough port.
// Don't forget to terminate the string with CRLF!
csk_uart1_puts("LOG COM1 GPGGA ONTIME 1\r\n");
csk_uart2_puts("LOG COM1 GPGGA ONTIME 1\r\n");
csk_uart3_puts("LOG COM1 GPGGA ONTIME 1\r\n");
// Additionally, tell the OEM615V to output a pulsetrain (2ms @ 125kHz) via the VARF output
csk_uart1_puts("FREQUENCYOUT ENABLE 200 800\r\n");
csk_uart2_puts("FREQUENCYOUT ENABLE 200 800\r\n");
csk_uart3_puts("FREQUENCYOUT ENABLE 200 800\r\n");
// Wait a few seconds for the NMEA buffers to fill ...
OS_Delay(200);
OS_Delay(200);
// We remain here forever, parsing NMEA strings from the OEM615 for GPS status
// and data. While this is happening (and after a GPS fix has been achieved),
// it's a good time to test disconnecting and reconnecting the GPS antenna from
// the OEM615V, to see that its GPS Position Valid LED goes out when the
// antenna is disconnected.
while(1) {
// Update gps values from incoming NMEA strings.
gps_update();
// Display current sats in view, HDOP, altitude, latitude & longitude if we have a fix.
if((gps_read().fixflag&gps_fix)||(gps_read().fixflag&diffgps_fix)) {
sprintf(strTmp, STR_TASK_MONITOR " GMT Sat HDOP Alt. Latitude Longitude\r\n" \
"\t\t\t\t-----------------------------------------------\r\n" \
"\t\t\t\t%s %02u %3.1f %6.1fm %8.4f %8.4f\r\n",
gps_NMEA_GMT_time_hhmmss(), gps_read().num_sat, gps_read().hdop, gps_read().altitude, gps_read().latitude, gps_read().longitude);
} /* if() */
// O/wise indicate that we do not have a fix.
else {
sprintf(strTmp, STR_TASK_MONITOR "No valid GPS position -- check antenna.\r\n");
} /* else() */
user_debug_msg(strTmp);
// Repeat every 5s.
OS_Delay(250);
OS_Delay(250);
} /* while() */
} /* task_monitor() */
// This gets called by the world update start event.
void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
gzerr << "[gazebo_mavlink_interface] Please specify a robotNamespace.\n";
if(!received_first_referenc_)
return;
common::Time now = world_->GetSimTime();
mav_msgs::CommandMotorSpeedPtr turning_velocities_msg(new mav_msgs::CommandMotorSpeed);
for (int i = 0; i < input_reference_.size(); i++)
turning_velocities_msg->motor_speed.push_back(input_reference_[i]);
turning_velocities_msg->header.stamp.sec = now.sec;
turning_velocities_msg->header.stamp.nsec = now.nsec;
motor_velocity_reference_pub_.publish(turning_velocities_msg);
turning_velocities_msg.reset();
//send gps
common::Time current_time = now;
double dt = (current_time - last_time_).Double();
last_time_ = current_time;
double t = current_time.Double();
math::Pose T_W_I = model_->GetWorldPose(); //TODO(burrimi): Check tf.
math::Vector3 pos_W_I = T_W_I.pos; // Use the models' world position for GPS and pressure alt.
math::Vector3 velocity_current_W = model_->GetWorldLinearVel(); // Use the models' world position for GPS velocity.
math::Vector3 velocity_current_W_xy = velocity_current_W;
velocity_current_W_xy.z = 0.0;
// TODO: Remove GPS message from IMU plugin. Added gazebo GPS plugin. This is temp here.
float lat_zurich = 47.3667; // deg
float long_zurich = 8.5500; // deg
float earth_radius = 6353000; // m
common::Time gps_update(gps_update_interval_);
if(current_time - last_gps_time_ > gps_update){ // 5Hz
mavlink_message_t gps_mmsg;
hil_gps_msg_.time_usec = current_time.nsec*1000;
hil_gps_msg_.fix_type = 3;
hil_gps_msg_.lat = (lat_zurich + (pos_W_I.x/earth_radius)*180/3.1416) * 10000000;
hil_gps_msg_.lon = (long_zurich + (-pos_W_I.y/earth_radius)*180/3.1416) * 10000000;
hil_gps_msg_.alt = pos_W_I.z * 1000;
hil_gps_msg_.eph = 100;
hil_gps_msg_.epv = 100;
hil_gps_msg_.vel = velocity_current_W_xy.GetLength() * 100;
hil_gps_msg_.vn = velocity_current_W.x * 100;
hil_gps_msg_.ve = -velocity_current_W.y * 100;
hil_gps_msg_.vd = -velocity_current_W.z * 100;
hil_gps_msg_.cog = atan2(hil_gps_msg_.ve, hil_gps_msg_.vn) * 180.0/3.1416 * 100.0;
hil_gps_msg_.satellites_visible = 10;
mavlink_hil_gps_t* hil_gps_msg = &hil_gps_msg_;
mavlink_msg_hil_gps_encode(1, 240, &gps_mmsg, hil_gps_msg);
mavlink_message_t* gps_msg = &gps_mmsg;
mavros::MavlinkPtr gps_rmsg = boost::make_shared<mavros::Mavlink>();
gps_rmsg->header.stamp = ros::Time::now();
mavutils::copy_mavlink_to_ros(gps_msg, gps_rmsg);
hil_sensor_pub_.publish(gps_rmsg);
last_gps_time_ = current_time;
}
}
// This gets called by the world update start event.
void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
pollForMAVLinkMessages();
common::Time now = world_->GetSimTime();
if(received_first_referenc_) {
mav_msgs::msgs::CommandMotorSpeed turning_velocities_msg;
for (int i = 0; i < input_reference_.size(); i++){
turning_velocities_msg.add_motor_speed(input_reference_[i]);
}
// TODO Add timestamp and Header
// turning_velocities_msg->header.stamp.sec = now.sec;
// turning_velocities_msg->header.stamp.nsec = now.nsec;
motor_velocity_reference_pub_->Publish(turning_velocities_msg);
}
//send gps
common::Time current_time = now;
double dt = (current_time - last_time_).Double();
last_time_ = current_time;
double t = current_time.Double();
math::Pose T_W_I = model_->GetWorldPose(); //TODO(burrimi): Check tf.
math::Vector3 pos_W_I = T_W_I.pos; // Use the models' world position for GPS and pressure alt.
math::Vector3 velocity_current_W = model_->GetWorldLinearVel(); // Use the models' world position for GPS velocity.
math::Vector3 velocity_current_W_xy = velocity_current_W;
velocity_current_W_xy.z = 0.0;
// TODO: Remove GPS message from IMU plugin. Added gazebo GPS plugin. This is temp here.
const double lat_zurich = 47.3667 * M_PI / 180 ; // rad
const double lon_zurich = 8.5500 * M_PI / 180; // rad
const float earth_radius = 6353000; // m
// reproject local position to gps coordinates
double x_rad = pos_W_I.x / earth_radius;
double y_rad = -pos_W_I.y / earth_radius;
double c = sqrt(x_rad * x_rad + y_rad * y_rad);
double sin_c = sin(c);
double cos_c = cos(c);
double lat_rad;
double lon_rad;
if (c != 0.0) {
lat_rad = asin(cos_c * sin(lat_zurich) + (x_rad * sin_c * cos(lat_zurich)) / c);
lon_rad = (lon_zurich + atan2(y_rad * sin_c, c * cos(lat_zurich) * cos_c - x_rad * sin(lat_zurich) * sin_c));
} else {
lat_rad = lat_zurich;
lon_rad = lon_zurich;
}
common::Time gps_update(gps_update_interval_);
if(current_time - last_gps_time_ > gps_update){ // 5Hz
if(use_mavlink_udp){
// Raw UDP mavlink
mavlink_hil_gps_t hil_gps_msg;
hil_gps_msg.time_usec = current_time.nsec*1000;
hil_gps_msg.fix_type = 3;
hil_gps_msg.lat = lat_rad * 180 / M_PI * 1e7;
hil_gps_msg.lon = lon_rad * 180 / M_PI * 1e7;
hil_gps_msg.alt = pos_W_I.z * 1000;
hil_gps_msg.eph = 100;
hil_gps_msg.epv = 100;
hil_gps_msg.vel = velocity_current_W_xy.GetLength() * 100;
hil_gps_msg.vn = velocity_current_W.x * 100;
hil_gps_msg.ve = -velocity_current_W.y * 100;
hil_gps_msg.vd = -velocity_current_W.z * 100;
hil_gps_msg.cog = atan2(hil_gps_msg.ve, hil_gps_msg.vn) * 180.0/3.1416 * 100.0;
hil_gps_msg.satellites_visible = 10;
send_mavlink_message(MAVLINK_MSG_ID_HIL_GPS, &hil_gps_msg, 200);
} else{
// Send via protobuf
hil_gps_msg_.set_time_usec(current_time.nsec*1000);
hil_gps_msg_.set_fix_type(3);
hil_gps_msg_.set_lat(lat_rad * 180 / M_PI * 1e7);
hil_gps_msg_.set_lon(lon_rad * 180 / M_PI * 1e7);
hil_gps_msg_.set_alt(pos_W_I.z * 1000);
hil_gps_msg_.set_eph(100);
hil_gps_msg_.set_epv(100);
hil_gps_msg_.set_vel(velocity_current_W_xy.GetLength() * 100);
hil_gps_msg_.set_vn(velocity_current_W.x * 100);
hil_gps_msg_.set_ve(-velocity_current_W.y * 100);
hil_gps_msg_.set_vd(-velocity_current_W.z * 100);
hil_gps_msg_.set_cog(atan2(-velocity_current_W.y * 100, velocity_current_W.x * 100) * 180.0/3.1416 * 100.0);
hil_gps_msg_.set_satellites_visible(10);
hil_gps_pub_->Publish(hil_gps_msg_);
}
last_gps_time_ = current_time;
}
}
// This gets called by the world update start event.
void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
if(!received_first_referenc_)
return;
common::Time now = world_->GetSimTime();
mav_msgs::ActuatorsPtr turning_velocities_msg(new mav_msgs::Actuators);
for (int i = 0; i < input_reference_.size(); i++)
turning_velocities_msg->angular_velocities.push_back(input_reference_[i]);
turning_velocities_msg->header.stamp.sec = now.sec;
turning_velocities_msg->header.stamp.nsec = now.nsec;
motor_velocity_reference_pub_.publish(turning_velocities_msg);
turning_velocities_msg.reset();
//send gps
common::Time current_time = now;
double dt = (current_time - last_time_).Double();
last_time_ = current_time;
double t = current_time.Double();
math::Pose T_W_I = model_->GetWorldPose(); //TODO(burrimi): Check tf.
math::Vector3 pos_W_I = T_W_I.pos; // Use the models' world position for GPS and pressure alt.
math::Vector3 velocity_current_W = model_->GetWorldLinearVel(); // Use the models' world position for GPS velocity.
math::Vector3 velocity_current_W_xy = velocity_current_W;
velocity_current_W_xy.z = 0.0;
// TODO: Remove GPS message from IMU plugin. Added gazebo GPS plugin. This is temp here.
double lat_zurich = 47.3667 * M_PI / 180 ; // rad
double lon_zurich = 8.5500 * M_PI / 180; // rad
float earth_radius = 6353000; // m
// reproject local position to gps coordinates
double x_rad = pos_W_I.x / earth_radius;
double y_rad = -pos_W_I.y / earth_radius;
double c = sqrt(x_rad * x_rad + y_rad * y_rad);
double sin_c = sin(c);
double cos_c = cos(c);
double lat_rad;
double lon_rad;
if (c != 0.0) {
lat_rad = asin(cos_c * sin(lat_zurich) + (x_rad * sin_c * cos(lat_zurich)) / c);
lon_rad = (lon_zurich + atan2(y_rad * sin_c, c * cos(lat_zurich) * cos_c - x_rad * sin(lat_zurich) * sin_c));
} else {
lat_rad = lat_zurich;
lon_rad = lon_zurich;
}
common::Time gps_update(gps_update_interval_);
if(current_time - last_gps_time_ > gps_update){ // 5Hz
mavlink_message_t gps_mmsg;
hil_gps_msg_.time_usec = current_time.nsec*1000;
hil_gps_msg_.fix_type = 3;
hil_gps_msg_.lat = lat_rad * 180 / M_PI * 1e7;
hil_gps_msg_.lon = lon_rad * 180 / M_PI * 1e7;
hil_gps_msg_.alt = pos_W_I.z * 1000;
hil_gps_msg_.eph = 100;
hil_gps_msg_.epv = 100;
hil_gps_msg_.vel = velocity_current_W_xy.GetLength() * 100;
hil_gps_msg_.vn = velocity_current_W.x * 100;
hil_gps_msg_.ve = -velocity_current_W.y * 100;
hil_gps_msg_.vd = -velocity_current_W.z * 100;
hil_gps_msg_.cog = atan2(hil_gps_msg_.ve, hil_gps_msg_.vn) * 180.0/3.1416 * 100.0;
hil_gps_msg_.satellites_visible = 10;
mavlink_hil_gps_t* hil_gps_msg = &hil_gps_msg_;
mavlink_msg_hil_gps_encode(1, 240, &gps_mmsg, hil_gps_msg);
mavlink_message_t* gps_msg = &gps_mmsg;
mavros_msgs::MavlinkPtr gps_rmsg = boost::make_shared<mavros_msgs::Mavlink>();
gps_rmsg->header.stamp = ros::Time::now();
mavros_msgs::mavlink::convert(*gps_msg, *gps_rmsg);
hil_sensor_pub_.publish(gps_rmsg);
last_gps_time_ = current_time;
}
}
